2 from test_case
import PinocchioTestCase
as TestCase
3 import pinocchio
as pin
8 model = pin.buildSampleModelHumanoidRandom()
10 joint_name =
"larm6_joint"
11 joint_id = model.getJointId(joint_name)
12 frame_id = model.addBodyFrame(
"test_body", joint_id, pin.SE3.Identity(), -1)
14 data = model.createData()
16 model.lowerPositionLimit[:7] = -1.0
17 model.upperPositionLimit[:7] = 1.0
19 q = pin.randomConfiguration(model)
20 pin.forwardKinematics(model, data, q)
22 R1 = pin.computeJointKinematicRegressor(
23 model, data, joint_id, pin.ReferenceFrame.LOCAL, pin.SE3.Identity()
25 R2 = pin.computeJointKinematicRegressor(
26 model, data, joint_id, pin.ReferenceFrame.LOCAL
29 self.assertApprox(R1, R2)
31 R3 = pin.computeFrameKinematicRegressor(
32 model, data, frame_id, pin.ReferenceFrame.LOCAL
34 self.assertApprox(R1, R3)
37 if __name__ ==
"__main__":