Coverage for src/pyroboplan/models/marble.py: 0%

1"""Utilities to load a 2d spherical body resembling a marble labyrinth.""" 

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(): 

13 """ 

14 Gets the example 2-DOF 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, "marble_description") 

23 urdf_filename = os.path.join(package_dir, "marble.urdf") 

24 

25 return pinocchio.buildModelsFromUrdf(urdf_filename, package_dirs=models_folder) 

26 

27 

28def add_object_collisions(model, collision_model, visual_model): 

29 ground_plane = pinocchio.GeometryObject( 

30 "ground", 

31 0, 

32 pinocchio.SE3(np.eye(3), np.array([1.0, 0.5, -0.05 - 0.02])), 

33 coal.Box(2.1, 1.1, 0.1), 

34 ) 

35 ground_plane.meshColor = np.array([0.8, 0.8, 0.8, 1.0]) 

36 visual_model.addGeometryObject(ground_plane) 

37 collision_model.addGeometryObject(ground_plane) 

38 

39 box_idx = 0 

40 

41 def box_on_plane(x, y, w, h): 

42 nonlocal box_idx 

43 box_idx += 1 

44 box = pinocchio.GeometryObject( 

45 f"box{box_idx}", 

46 0, 

47 pinocchio.SE3(np.eye(3), np.array([x + w / 2, y + h / 2, 0.0])), 

48 coal.Box(w, h, 0.04), 

49 ) 

50 box.meshColor = np.array([0.3, 0.3, 0.3, 1.0]) 

51 visual_model.addGeometryObject(box) 

52 collision_model.addGeometryObject(box) 

53 

54 # outer walls 

55 box_on_plane(-0.05, -0.05, 0.05, 1.1) 

56 box_on_plane(2.0, -0.05, 0.05, 1.1) 

57 box_on_plane(-0.05, -0.05, 2.1, 0.05) 

58 box_on_plane(-0.05, 1.0, 2.1, 0.05) 

59 

60 # obstacles 

61 box_on_plane(0.3, 0.0, 0.05, 0.7) 

62 box_on_plane(0.75, 0.2, 0.05, 0.8) 

63 box_on_plane(1.3, 0.0, 0.05, 0.3) 

64 box_on_plane(0.8, 0.5, 0.5, 0.05) 

65 box_on_plane(1.6, 0.45, 0.05, 0.4) 

66 box_on_plane(1.65, 0.45, 0.35, 0.05) 

67 

68 # split workspace so that many plans will not be feasible and the search trees fill the plane 

69 # box_on_plane(1.0, 0.0, 0.05, 0.5) 

70 

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

72 obstacle_names = [ 

73 cobj.name 

74 for cobj in collision_model.geometryObjects 

75 if cobj.name.startswith("box") 

76 ] 

77 for obstacle_name in obstacle_names: 

78 set_collisions(model, collision_model, obstacle_name, "body", True)