append-urdf-model-with-another-model.py
Go to the documentation of this file.
1 from os.path import dirname, join, abspath
2 import math
3 import sys
4 
5 import numpy as np
6 
7 import pinocchio as pin
8 from pinocchio.visualize import MeshcatVisualizer as Visualizer
9 import hppfcl as fcl
10 
11 # load model from example-robot urdf
12 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models/")
13 
14 model_path = join(pinocchio_model_dir, "example-robot-data/robots")
15 mesh_dir = pinocchio_model_dir
16 urdf_filename = "ur5_robot.urdf"
17 urdf_model_path = join(join(model_path, "ur_description/urdf/"), urdf_filename)
18 
19 model1, collision_model1, visual_model1 = pin.buildModelsFromUrdf(
20  urdf_model_path, package_dirs=mesh_dir)
21 
22 # build model from scratch
23 model2 = pin.Model()
24 model2.name = "pendulum"
25 geom_model = pin.GeometryModel()
26 
27 parent_id = 0
28 joint_placement = pin.SE3.Identity()
29 body_mass = 1.
30 body_radius = 1e-2
31 
32 joint_name = "joint_spherical"
33 joint_id = model2.addJoint(parent_id, pin.JointModelSpherical(), joint_placement, joint_name)
34 
35 body_inertia = pin.Inertia.FromSphere(body_mass, body_radius)
36 body_placement = joint_placement.copy()
37 body_placement.translation[2] = 0.1
38 model2.appendBodyToJoint(joint_id, body_inertia, body_placement)
39 
40 geom1_name = "ball"
41 shape1 = fcl.Sphere(body_radius)
42 geom1_obj = pin.GeometryObject(geom1_name, joint_id, shape1, body_placement)
43 geom1_obj.meshColor = np.ones((4))
44 geom_model.addGeometryObject(geom1_obj)
45 
46 geom2_name = "bar"
47 shape2 = fcl.Cylinder(body_radius/4., body_placement.translation[2])
48 shape2_placement = body_placement.copy()
49 shape2_placement.translation[2] /= 2.
50 
51 geom2_obj = pin.GeometryObject(geom2_name, joint_id, shape2, shape2_placement)
52 geom2_obj.meshColor = np.array([0.,0.,0.,1.])
53 geom_model.addGeometryObject(geom2_obj)
54 
55 visual_model2 = geom_model
56 
57 # join the two models, append pendulum to end effector
58 frame_id_end_effector = model1.getFrameId("tool0")
59 model, visual_model = pin.appendModel(
60  model1, model2, visual_model1, visual_model2, frame_id_end_effector, pin.SE3.Identity())
61 
62 print("Check the joints of the appended model:\n %s \n ->Notice the spherical joint at the end." % model)
63 viz = Visualizer(model, visual_model, visual_model)
64 
65 try:
66  viz.initViewer(open=True)
67 except ImportError as err:
68  print("Error while initializing the viewer. It seems you should install Python meshcat")
69  print(err)
70  sys.exit(0)
71 
72 # Load the robot in the viewer.
73 viz.viewer.open()
74 viz.loadViewerModel()
75 
76 # Display a random robot configuration.
77 model.lowerPositionLimit.fill(-math.pi/2)
78 model.upperPositionLimit.fill(+math.pi/2)
79 q = pin.randomConfiguration(model)
80 viz.display(q)
81 


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:28