Coverage for src/pyroboplan/models/panda.py: 98%

1"""Utilities to load example Franka Emika Panda model.""" 

2 

3import coal 

4import numpy as np 

5import os 

6import pinocchio 

7 

8from ..core.utils import set_collisions 

9from .utils import get_example_models_folder 

10 

11 

12def load_models(use_sphere_collisions=False): 

13 """ 

14 Gets the example Panda models. 

15 

16 Returns 

17 ------- 

18 tuple[`pinocchio.Model`] 

19 A 3-tuple containing the model, collision geometry model, and visual geometry model. 

20 """ 

21 models_folder = get_example_models_folder() 

22 package_dir = os.path.join(models_folder, "panda_description") 

23 urdf_filename = "panda_spheres.urdf" if use_sphere_collisions else "panda.urdf" 

24 urdf_filepath = os.path.join(package_dir, "urdf", urdf_filename) 

25 

26 return pinocchio.buildModelsFromUrdf(urdf_filepath, package_dirs=models_folder) 

27 

28 

29def add_self_collisions(model, collision_model, srdf_filename=None): 

30 """ 

31 Adds link self-collisions to the Panda collision model. 

32 

33 This uses an SRDF file to remove any excluded collision pairs. 

34 

35 Parameters 

36 ---------- 

37 model : `pinocchio.Model` 

38 The Panda model. 

39 collision_model : `pinocchio.Model` 

40 The Panda collision geometry model. 

41 srdf_filename : str, optional 

42 Path to the SRDF file describing the excluded collision pairs. 

43 If not specified, uses a default file included with the Panda model. 

44 """ 

45 if srdf_filename is None: 45 ↛ 50line 45 didn't jump to line 50 because the condition on line 45 was always true

46 models_folder = get_example_models_folder() 

47 package_dir = os.path.join(models_folder, "panda_description") 

48 srdf_filename = os.path.join(package_dir, "srdf", "panda.srdf") 

49 

50 collision_model.addAllCollisionPairs() 

51 pinocchio.removeCollisionPairs(model, collision_model, srdf_filename) 

52 

53 

54def add_object_collisions(model, collision_model, visual_model, inflation_radius=0.0): 

55 """ 

56 Adds obstacles and collisions to the Panda collision model. 

57 

58 Parameters 

59 ---------- 

60 model : `pinocchio.Model` 

61 The Panda model. 

62 collision_model : `pinocchio.Model` 

63 The Panda collision geometry model. 

64 visual_model : `pinocchio.Model` 

65 The Panda visual geometry model. 

66 inflation_radius : float, optional 

67 An inflation radius, in meters, around the objects. 

68 """ 

69 # Add the collision objects 

70 ground_plane = pinocchio.GeometryObject( 

71 "ground_plane", 

72 0, 

73 pinocchio.SE3(np.eye(3), np.array([0.0, 0.0, -0.151])), 

74 coal.Box(2.0, 2.0, 0.3), 

75 ) 

76 ground_plane.meshColor = np.array([0.5, 0.5, 0.5, 0.5]) 

77 visual_model.addGeometryObject(ground_plane) 

78 collision_model.addGeometryObject(ground_plane) 

79 

80 obstacle_sphere_1 = pinocchio.GeometryObject( 

81 "obstacle_sphere_1", 

82 0, 

83 pinocchio.SE3(np.eye(3), np.array([0.0, 0.1, 1.1])), 

84 coal.Sphere(0.2 + inflation_radius), 

85 ) 

86 obstacle_sphere_1.meshColor = np.array([0.0, 1.0, 0.0, 0.5]) 

87 visual_model.addGeometryObject(obstacle_sphere_1) 

88 collision_model.addGeometryObject(obstacle_sphere_1) 

89 

90 obstacle_sphere_2 = pinocchio.GeometryObject( 

91 "obstacle_sphere_2", 

92 0, 

93 pinocchio.SE3(np.eye(3), np.array([0.5, 0.5, 0.5])), 

94 coal.Sphere(0.25 + inflation_radius), 

95 ) 

96 obstacle_sphere_2.meshColor = np.array([1.0, 1.0, 0.0, 0.5]) 

97 visual_model.addGeometryObject(obstacle_sphere_2) 

98 collision_model.addGeometryObject(obstacle_sphere_2) 

99 

100 obstacle_box_1 = pinocchio.GeometryObject( 

101 "obstacle_box_1", 

102 0, 

103 pinocchio.SE3(np.eye(3), np.array([-0.5, 0.5, 0.7])), 

104 coal.Box( 

105 0.25 + 2.0 * inflation_radius, 

106 0.55 + 2.0 * inflation_radius, 

107 0.55 + 2.0 * inflation_radius, 

108 ), 

109 ) 

110 obstacle_box_1.meshColor = np.array([1.0, 0.0, 0.0, 0.5]) 

111 visual_model.addGeometryObject(obstacle_box_1) 

112 collision_model.addGeometryObject(obstacle_box_1) 

113 

114 obstacle_box_2 = pinocchio.GeometryObject( 

115 "obstacle_box_2", 

116 0, 

117 pinocchio.SE3(np.eye(3), np.array([-0.5, -0.5, 0.75])), 

118 coal.Box( 

119 0.33 + 2.0 * inflation_radius, 

120 0.33 + 2.0 * inflation_radius, 

121 0.33 + 2.0 * inflation_radius, 

122 ), 

123 ) 

124 obstacle_box_2.meshColor = np.array([0.0, 0.0, 1.0, 0.5]) 

125 visual_model.addGeometryObject(obstacle_box_2) 

126 collision_model.addGeometryObject(obstacle_box_2) 

127 

128 # Define the active collision pairs between the robot and obstacle links. 

129 collision_names = [ 

130 cobj.name for cobj in collision_model.geometryObjects if "panda" in cobj.name 

131 ] 

132 obstacle_names = [ 

133 "ground_plane", 

134 "obstacle_box_1", 

135 "obstacle_box_2", 

136 "obstacle_sphere_1", 

137 "obstacle_sphere_2", 

138 ] 

139 for obstacle_name in obstacle_names: 

140 for collision_name in collision_names: 

141 set_collisions(model, collision_model, obstacle_name, collision_name, True) 

142 

143 # Exclude the collision between the ground and the base link 

144 set_collisions(model, collision_model, "panda_link0", "ground_plane", False)