4 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)
19 q = pin.randomConfiguration(self.
model)
20 v = np.random.rand(self.
model.nv)
21 a = np.random.rand(self.
model.nv)
22 pin.forwardKinematics(self.
model, data, q, v, a)
28 v = pin.getVelocity(self.
model, data, self.
joint_idx, pin.ReferenceFrame.LOCAL)
30 v = pin.getVelocity(self.
model, data, self.
joint_idx, pin.ReferenceFrame.WORLD)
33 self.
model, data, self.
joint_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
36 v, pin.SE3(T.rotation, np.zeros(3)).
act(data.v[self.
joint_idx])
41 a = pin.getAcceleration(
45 a = pin.getAcceleration(
49 a = pin.getAcceleration(
50 self.
model, data, self.
joint_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
53 a, pin.SE3(T.rotation, np.zeros(3)).
act(data.a[self.
joint_idx])
57 a = pin.getClassicalAcceleration(
60 a = pin.getClassicalAcceleration(
63 a = pin.getClassicalAcceleration(
64 self.
model, data, self.
joint_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
68 if __name__ ==
"__main__":