1 import pinocchio
as pin
10 geom_model = pin.GeometryModel()
13 joint_placement = pin.SE3.Identity()
17 shape0 = fcl.Sphere(body_radius)
18 geom0_obj = pin.GeometryObject(
"base", 0, pin.SE3.Identity(), shape0)
19 geom0_obj.meshColor = np.array([1.0, 0.1, 0.1, 1.0])
20 geom_model.addGeometryObject(geom0_obj)
23 joint_name =
"joint_" + str(k + 1)
24 joint_id = model.addJoint(
25 parent_id, pin.JointModelRY(), joint_placement, joint_name
28 body_inertia = pin.Inertia.FromSphere(body_mass, body_radius)
29 body_placement = joint_placement.copy()
30 body_placement.translation[2] = 1.0
31 model.appendBodyToJoint(joint_id, body_inertia, body_placement)
33 geom1_name =
"ball_" + str(k + 1)
34 shape1 = fcl.Sphere(body_radius)
35 geom1_obj = pin.GeometryObject(geom1_name, joint_id, body_placement, shape1)
36 geom1_obj.meshColor = np.ones((4))
37 geom_model.addGeometryObject(geom1_obj)
39 geom2_name =
"bar_" + str(k + 1)
40 shape2 = fcl.Cylinder(body_radius / 4.0, body_placement.translation[2])
41 shape2_placement = body_placement.copy()
42 shape2_placement.translation[2] /= 2.0
44 geom2_obj = pin.GeometryObject(geom2_name, joint_id, shape2_placement, shape2)
45 geom2_obj.meshColor = np.array([0.0, 0.0, 0.0, 1.0])
46 geom_model.addGeometryObject(geom2_obj)
49 joint_placement = body_placement.copy()
53 visual_model = geom_model
59 except ImportError
as err:
61 "Error while initializing the viewer. It seems you should install gepetto-viewer"
67 viz.loadViewerModel(
"pinocchio")
68 except AttributeError
as err:
70 "Error while loading the viewer model. It seems you should start gepetto-viewer"
76 q0 = pin.neutral(model)
83 N = math.floor(T / dt)
85 model.lowerPositionLimit.fill(-math.pi)
86 model.upperPositionLimit.fill(+math.pi)
87 q = pin.randomConfiguration(model)
88 v = np.zeros((model.nv))
91 data_sim = model.createData()
93 tau_control = np.zeros((model.nv))
94 a = pin.aba(model, data_sim, q, v, tau_control)
98 q = pin.integrate(model, q, v * dt)