bindings_kinematics.py
Go to the documentation of this file.
1 import unittest
2 import pinocchio as pin
3 import numpy as np
4 
5 from test_case import PinocchioTestCase
6 
7 
9  def setUp(self):
10  self.model = pin.buildSampleModelHumanoidRandom()
11  self.joint_idx = (
12  self.model.getJointId("rarm2_joint")
13  if self.model.existJointName("rarm2_joint")
14  else (self.model.njoints - 1)
15  )
16 
17  def test_getters(self):
18  data = self.model.createData()
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)
23 
24  T = data.oMi[self.joint_idx]
25 
26  v = pin.getVelocity(self.model, data, self.joint_idx)
27  self.assertApprox(v, data.v[self.joint_idx])
28  v = pin.getVelocity(self.model, data, self.joint_idx, pin.ReferenceFrame.LOCAL)
29  self.assertApprox(v, data.v[self.joint_idx])
30  v = pin.getVelocity(self.model, data, self.joint_idx, pin.ReferenceFrame.WORLD)
31  self.assertApprox(v, data.oMi[self.joint_idx].act(data.v[self.joint_idx]))
32  v = pin.getVelocity(
33  self.model, data, self.joint_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
34  )
35  self.assertApprox(
36  v, pin.SE3(T.rotation, np.zeros(3)).act(data.v[self.joint_idx])
37  )
38 
39  a = pin.getAcceleration(self.model, data, self.joint_idx)
40  self.assertApprox(a, data.a[self.joint_idx])
41  a = pin.getAcceleration(
42  self.model, data, self.joint_idx, pin.ReferenceFrame.LOCAL
43  )
44  self.assertApprox(a, data.a[self.joint_idx])
45  a = pin.getAcceleration(
46  self.model, data, self.joint_idx, pin.ReferenceFrame.WORLD
47  )
48  self.assertApprox(a, data.oMi[self.joint_idx].act(data.a[self.joint_idx]))
49  a = pin.getAcceleration(
50  self.model, data, self.joint_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
51  )
52  self.assertApprox(
53  a, pin.SE3(T.rotation, np.zeros(3)).act(data.a[self.joint_idx])
54  )
55 
56  a = pin.getClassicalAcceleration(self.model, data, self.joint_idx)
57  a = pin.getClassicalAcceleration(
58  self.model, data, self.joint_idx, pin.ReferenceFrame.LOCAL
59  )
60  a = pin.getClassicalAcceleration(
61  self.model, data, self.joint_idx, pin.ReferenceFrame.WORLD
62  )
63  a = pin.getClassicalAcceleration(
64  self.model, data, self.joint_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
65  )
66 
67 
68 if __name__ == "__main__":
69  unittest.main()
pinocchio::motionSet::act
static void act(const Eigen::MatrixBase< Mat > &iV, const ForceDense< ForceDerived > &f, Eigen::MatrixBase< MatRet > const &jF)
Action of a motion set on a force object. The input motion set is represented by a 6xN matrix whose e...
bindings_kinematics.TestKinematicsBindings
Definition: bindings_kinematics.py:8
bindings_kinematics.TestKinematicsBindings.test_getters
def test_getters(self)
Definition: bindings_kinematics.py:17
pinocchio::createData
ConstraintDataTpl< Scalar, Options, ConstraintCollectionTpl > createData(const ConstraintModelTpl< Scalar, Options, ConstraintCollectionTpl > &cmodel)
Definition: constraint-model-visitor.hpp:248
test_case.PinocchioTestCase.assertApprox
def assertApprox(self, a, b, eps=1e-6)
Definition: test_case.py:12
bindings_kinematics.TestKinematicsBindings.joint_idx
joint_idx
Definition: bindings_kinematics.py:11
bindings_kinematics.TestKinematicsBindings.model
model
Definition: bindings_kinematics.py:10
bindings_kinematics.TestKinematicsBindings.setUp
def setUp(self)
Definition: bindings_kinematics.py:9
test_case.PinocchioTestCase
Definition: test_case.py:11


pinocchio
Author(s):
autogenerated on Sun Jun 16 2024 02:43:06