5 import pinocchio
as pin
9 from os.path import dirname, join, abspath
15 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),
"models")
17 model_path = join(pinocchio_model_dir,
"example-robot-data/robots")
18 mesh_dir = pinocchio_model_dir
21 urdf_filename =
"solo.urdf"
22 urdf_model_path = join(join(model_path,
"solo_description/robots"), urdf_filename)
24 model, collision_model, visual_model = pin.buildModelsFromUrdf(
25 urdf_model_path, mesh_dir, pin.JointModelFreeFlyer()
36 viz.initViewer(open=
True)
37 except ImportError
as err:
39 "Error while initializing the viewer. It seems you should install Python meshcat"
48 q0 = pin.neutral(model)
50 viz.displayVisuals(
True)
53 mesh = visual_model.geometryObjects[0].geometry
54 mesh.buildConvexRepresentation(
True)
58 if convex
is not None:
59 placement = pin.SE3.Identity()
60 placement.translation[0] = 2.0
61 geometry = pin.GeometryObject(
"convex", 0, placement, convex)
62 geometry.meshColor = np.ones((4))
64 geometry.overrideMaterial =
True
65 geometry.meshMaterial = pin.GeometryPhongMaterial()
66 geometry.meshMaterial.meshEmissionColor = np.array([1.0, 0.1, 0.1, 1.0])
67 geometry.meshMaterial.meshSpecularColor = np.array([0.1, 1.0, 0.1, 1.0])
68 geometry.meshMaterial.meshShininess = 0.8
69 visual_model.addGeometryObject(geometry)
76 viz2.initViewer(viz.viewer)
77 viz2.loadViewerModel(rootNodeName=
"pinocchio2")
84 [0.0, 0.0, 0.235, 0.0, 0.0, 0.0, 1.0, 0.8, -1.6, 0.8, -1.6, -0.8, 1.6, -0.8, 1.6]
87 v0 = np.random.randn(model.nv) * 2
89 pin.forwardKinematics(model, data, q1, v0)
90 frame_id = model.getFrameId(
"HR_FOOT")
92 viz.drawFrameVelocities(frame_id=frame_id)
94 model.gravity.linear[:] = 0.0
99 tau0 = np.zeros(model.nv)
103 for i
in range(nsteps):
106 a1 = pin.aba(model, data, q, v, tau0)
108 qnext = pin.integrate(model, q, dt * vnext)
112 viz.drawFrameVelocities(frame_id=frame_id)
118 fid2 = model.getFrameId(
"FL_FOOT")
122 viz.drawFrameVelocities(frame_id)
123 viz.drawFrameVelocities(fid2)
126 with viz.create_video_ctx(
"../leap.mp4"):
127 viz.play(qs, dt, callback=my_callback)