3 import pinocchio
as pin
4 from test_case
import PinocchioTestCase
as TestCase
9 model = pin.buildSampleModelHumanoidRandom()
11 joint_name =
"larm6_joint"
12 joint_id = model.getJointId(joint_name)
13 frame_id = model.addBodyFrame(
"test_body", joint_id, pin.SE3.Identity(), -1)
15 data = model.createData()
17 model.lowerPositionLimit[:7] = -1.0
18 model.upperPositionLimit[:7] = 1.0
20 q = pin.randomConfiguration(model)
21 pin.forwardKinematics(model, data, q)
23 R1 = pin.computeJointKinematicRegressor(
24 model, data, joint_id, pin.ReferenceFrame.LOCAL, pin.SE3.Identity()
26 R2 = pin.computeJointKinematicRegressor(
27 model, data, joint_id, pin.ReferenceFrame.LOCAL
30 self.assertApprox(R1, R2)
32 R3 = pin.computeFrameKinematicRegressor(
33 model, data, frame_id, pin.ReferenceFrame.LOCAL
35 self.assertApprox(R1, R3)
38 if __name__ ==
"__main__":