2 import pinocchio
as pin
5 from test_case
import PinocchioTestCase
10 self.
model = pin.buildSampleModelHumanoidRandom()
12 self.
model.getJointId(
"rarm2_joint")
13 if self.
model.existJointName(
"rarm2_joint")
14 else (self.
model.njoints - 1)
36 f.type = pin.FrameType.BODY
37 self.assertTrue(f.type == pin.FrameType.BODY)
42 f.name =
"new_hip_frame"
43 self.assertTrue(f.name ==
"new_hip_frame")
47 self.assertTrue(f.parentJoint == self.
parent_idx)
49 f.parentJoint = newparent
50 self.assertTrue(f.parentJoint == newparent)
57 new_placement = pin.SE3.Random()
58 f.placement = new_placement
59 self.assertTrue(np.allclose(f.placement.homogeneous, new_placement.homogeneous))
63 frame1 = pin.Frame(
"name", 1, 2, M, pin.OP_FRAME)
64 frame2 = pin.Frame(
"name", 1, 2, M, pin.OP_FRAME)
65 frame3 = pin.Frame(
"othername", 3, 4, pin.SE3.Random(), pin.BODY)
67 self.assertTrue(frame1 == frame2)
68 self.assertFalse(frame1 != frame2)
69 self.assertTrue(frame1 != frame3)
70 self.assertFalse(frame1 == frame3)
75 frame = pin.Frame(
"name", 1, 2, pin.SE3.Random(), pin.OP_FRAME)
76 filename =
"frame.pickle"
77 with open(filename,
"wb")
as f:
80 with open(filename,
"rb")
as f:
81 frame_copy = pickle.load(f)
83 self.assertEqual(frame, frame_copy)
87 q = pin.randomConfiguration(self.
model)
88 v = np.random.rand(self.
model.nv)
89 a = np.random.rand(self.
model.nv)
90 pin.forwardKinematics(self.
model, data, q, v, a)
97 v = pin.getFrameVelocity(
101 v = pin.getFrameVelocity(
105 v = pin.getFrameVelocity(
106 self.
model, data, self.
frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
110 pin.SE3(T.rotation, np.zeros(3)).
act(
117 a = pin.getFrameAcceleration(
121 a = pin.getFrameAcceleration(
125 a = pin.getFrameAcceleration(
126 self.
model, data, self.
frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
130 pin.SE3(T.rotation, np.zeros(3)).
act(
135 a = pin.getFrameClassicalAcceleration(self.
model, data, self.
frame_idx)
136 a = pin.getFrameClassicalAcceleration(
139 a = pin.getFrameClassicalAcceleration(
142 a = pin.getFrameClassicalAcceleration(
143 self.
model, data, self.
frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
148 data = model.createData()
150 q = pin.neutral(model)
151 v = np.random.rand((model.nv))
154 J1 = pin.computeFrameJacobian(model, data, q, frame_id)
155 J2 = pin.computeFrameJacobian(model, data, q, frame_id, pin.LOCAL)
158 data2 = model.createData()
160 pin.computeJointJacobians(model, data2, q)
161 J3 = pin.getFrameJacobian(model, data2, frame_id, pin.LOCAL)
164 dJ1 = pin.frameJacobianTimeVariation(model, data, q, v, frame_id, pin.LOCAL)
166 data3 = model.createData()
167 pin.computeJointJacobiansTimeVariation(model, data3, q, v)
169 dJ2 = pin.getFrameJacobianTimeVariation(model, data3, frame_id, pin.LOCAL)
173 if __name__ ==
"__main__":