5 import pinocchio
as pin
13 m = pin.SE3.Identity()
14 m.translation = np.array([x, y, z])
15 m.rotation *= rotate(
"x", rx)
16 m.rotation *= rotate(
"y", ry)
17 m.rotation *= rotate(
"z", rz)
22 return [int(i)
for i
in "%03d" % int(bin(body_number % 8)[2:])] + [1]
28 (
"universe", pin.SE3.Identity(), pin.SE3.Identity().translation)
30 self.
name = self.__class__.__name__
if name
is None else name
36 for joint
in self.joints:
51 if joint_model
is None:
52 joint_model = pin.JointModelFreeFlyer()
53 elif isinstance(joint_model, str):
54 joint_model = pin.__dict__[
"JointModel" + joint_model]()
55 if joint_placement
is None:
56 joint_placement = pin.SE3.Identity()
57 elif isinstance(joint_placement, dict):
58 joint_placement =
placement(**joint_placement)
60 lever = pin.SE3.Identity()
61 elif isinstance(lever, dict):
63 joint_name, body_name = (
64 "world/%s_%s_%s" % (self.
name, joint_name, i)
for i
in (
"joint",
"body")
66 body_inertia = pin.Inertia.Random()
69 (float(i)
for i
in dimensions)
70 if isinstance(dimensions, tuple)
71 else [float(dimensions)] * 3
74 mass = w * h * d * DENSITY
75 body_inertia = pin.Inertia.FromBox(mass, w, h, d)
77 self.
display.viewer.gui.addBox(body_name, w, h, d,
color(body_color))
78 elif shape ==
"cylinder":
81 mass = pi * r**2 * h * DENSITY
82 body_inertia = pin.Inertia.FromCylinder(mass, r, h)
84 self.
display.viewer.gui.addCylinder(body_name, r, h,
color(body_color))
85 elif shape ==
"sphere":
87 (float(i)
for i
in dimensions)
88 if isinstance(dimensions, tuple)
89 else [float(dimensions)] * 3
92 mass = 4.0 / 3.0 * pi * w * h * d * DENSITY
93 body_inertia = pin.Inertia.FromEllipsoid(mass, w, h, d)
95 self.
display.viewer.gui.addSphere(
96 body_name, dimensions,
color(body_color)
98 body_inertia.lever = lever.translation
99 joint_id = self.
model.addJoint(parent, joint_model, joint_placement, joint_name)
100 self.
model.appendBodyToJoint(joint_id, body_inertia, pin.SE3.Identity())
101 self.
model.addJointFrame(joint_id, -1)
102 self.
model.addBodyFrame(body_name, joint_id, pin.SE3.Identity(), -1)
109 for i, (name, placement, lever)
in enumerate(self.
visuals):
113 self.
display.viewer.gui.refresh()
118 from gepetto
import corbaserver
122 window_id = self.
viewer.gui.getWindowID(window_name)
124 window_id = self.
viewer.gui.createWindow(window_name)
125 self.
viewer.gui.createSceneWithFloor(
"world")
126 self.
viewer.gui.addSceneToWindow(
"world", window_id)
127 self.
viewer.gui.setLightingMode(
"world",
"OFF")
128 self.
viewer.gui.setVisibility(
"world/floor",
"OFF")
132 self.
viewer.gui.applyConfiguration(obj_name, pin.se3ToXYZQUATtuple(m))
138 "joint_name":
"pelvis",
139 "dimensions": (0.1, 0.2, 0.1),
143 "joint_name":
"left_leg",
145 "joint_placement": {
"y": -0.15},
146 "lever": {
"z": -0.45},
148 "dimensions": (0.1, 0.9),
154 "joint_name":
"right_leg",
156 "joint_placement": {
"y": 0.15},
157 "lever": {
"z": -0.45},
159 "dimensions": (0.1, 0.9),