bindings_com.py
Go to the documentation of this file.
1 import unittest
2 
3 import numpy as np
4 import pinocchio as pin
5 from pinocchio.utils import rand
6 from test_case import PinocchioTestCase as TestCase
7 
8 
9 class TestComBindings(TestCase):
10  def setUp(self):
11  self.model = pin.buildSampleModelHumanoidRandom()
12  self.data = self.model.createData()
13 
14  qmax = np.full((self.model.nq, 1), np.pi)
15  self.q = pin.randomConfiguration(self.model, -qmax, qmax)
16 
17  def test_mass(self):
18  mass = pin.computeTotalMass(self.model)
19  self.assertIsNot(mass, np.nan)
20 
21  mass_check = sum([inertia.mass for inertia in self.model.inertias[1:]])
22  self.assertApprox(mass, mass_check)
23 
24  mass_data = pin.computeTotalMass(self.model, self.data)
25  self.assertIsNot(mass_data, np.nan)
26  self.assertApprox(mass, mass_data)
27  self.assertApprox(mass_data, self.data.mass[0])
28 
29  data2 = self.model.createData()
30  pin.centerOfMass(self.model, data2, self.q)
31  self.assertApprox(mass, data2.mass[0])
32 
33  def test_subtree_masses(self):
34  pin.computeSubtreeMasses(self.model, self.data)
35 
36  data2 = self.model.createData()
37  pin.centerOfMass(self.model, data2, self.q)
38 
39  for i in range(self.model.njoints):
40  self.assertApprox(self.data.mass[i], data2.mass[i])
41 
42  def test_com_0(self):
43  data = self.data
44 
45  c0 = pin.centerOfMass(self.model, self.data, self.q)
46  c0_bis = pin.centerOfMass(self.model, self.data, self.q, False)
47 
48  self.assertApprox(c0, c0_bis)
49 
50  data2 = self.model.createData()
51  pin.forwardKinematics(self.model, data, self.q)
52  c0 = pin.centerOfMass(self.model, data, pin.POSITION)
53  pin.forwardKinematics(self.model, data2, self.q)
54  c0_bis = pin.centerOfMass(self.model, data2, pin.POSITION, False)
55 
56  self.assertApprox(c0, c0_bis)
57 
58  c0_bis = pin.centerOfMass(self.model, self.data, 0)
59 
60  self.assertApprox(c0, c0_bis)
61 
62  self.assertApprox(c0, data2.com[0])
63  self.assertApprox(self.data.com[0], data2.com[0])
64 
65  def test_com_1(self):
66  data = self.data
67 
68  v = rand(self.model.nv)
69  c0 = pin.centerOfMass(self.model, self.data, self.q, v)
70  c0_bis = pin.centerOfMass(self.model, self.data, self.q, v, False)
71 
72  self.assertApprox(c0, c0_bis)
73 
74  data2 = self.model.createData()
75  pin.forwardKinematics(self.model, data, self.q, v)
76  c0 = pin.centerOfMass(self.model, data, pin.VELOCITY)
77  pin.forwardKinematics(self.model, data2, self.q, v)
78  c0_bis = pin.centerOfMass(self.model, data2, pin.VELOCITY, False)
79 
80  self.assertApprox(c0, c0_bis)
81 
82  c0_bis = pin.centerOfMass(self.model, data2, 1)
83 
84  self.assertApprox(c0, c0_bis)
85 
86  data3 = self.model.createData()
87  pin.centerOfMass(self.model, data3, self.q)
88 
89  self.assertApprox(self.data.com[0], data2.com[0])
90  self.assertApprox(self.data.vcom[0], data2.vcom[0])
91 
92  self.assertApprox(self.data.com[0], data3.com[0])
93 
94  def test_com_2(self):
95  data = self.data
96 
97  v = rand(self.model.nv)
98  a = rand(self.model.nv)
99  c0 = pin.centerOfMass(self.model, self.data, self.q, v, a)
100  c0_bis = pin.centerOfMass(self.model, self.data, self.q, v, a, False)
101 
102  self.assertApprox(c0, c0_bis)
103 
104  data2 = self.model.createData()
105  pin.forwardKinematics(self.model, data, self.q, v, a)
106  c0 = pin.centerOfMass(self.model, data, pin.ACCELERATION)
107  pin.forwardKinematics(self.model, data2, self.q, v, a)
108  c0_bis = pin.centerOfMass(self.model, data2, pin.ACCELERATION, False)
109 
110  self.assertApprox(c0, c0_bis)
111 
112  c0_bis = pin.centerOfMass(self.model, data2, 2)
113  self.assertApprox(c0, c0_bis)
114 
115  data3 = self.model.createData()
116  pin.centerOfMass(self.model, data3, self.q)
117 
118  data4 = self.model.createData()
119  pin.centerOfMass(self.model, data4, self.q, v)
120 
121  self.assertApprox(self.data.com[0], data2.com[0])
122  self.assertApprox(self.data.vcom[0], data2.vcom[0])
123  self.assertApprox(self.data.acom[0], data2.acom[0])
124 
125  self.assertApprox(self.data.com[0], data3.com[0])
126 
127  self.assertApprox(self.data.com[0], data4.com[0])
128  self.assertApprox(self.data.vcom[0], data4.vcom[0])
129 
130  def test_com_default(self):
131  v = rand(self.model.nv)
132  a = rand(self.model.nv)
133  pin.centerOfMass(self.model, self.data, self.q, v, a)
134 
135  data2 = self.model.createData()
136  pin.forwardKinematics(self.model, data2, self.q, v, a)
137  pin.centerOfMass(self.model, data2)
138 
139  for i in range(self.model.njoints):
140  self.assertApprox(self.data.com[i], data2.com[i])
141  self.assertApprox(self.data.vcom[i], data2.vcom[i])
142  self.assertApprox(self.data.acom[i], data2.acom[i])
143 
144  def test_Jcom_update3(self):
145  Jcom = pin.jacobianCenterOfMass(self.model, self.data, self.q)
146  self.assertFalse(np.isnan(Jcom).any())
147 
148  def test_Jcom_update4(self):
149  Jcom = pin.jacobianCenterOfMass(self.model, self.data, self.q, True)
150  self.assertFalse(np.isnan(Jcom).any())
151  self.assertFalse(np.isnan(self.data.com[1]).any())
152 
153  def test_Jcom_noupdate2(self):
154  data_no = self.data
155  data_up = self.model.createData()
156 
157  pin.forwardKinematics(self.model, data_no, self.q)
158  Jcom_no = pin.jacobianCenterOfMass(self.model, data_no)
159 
160  Jcom_up = pin.jacobianCenterOfMass(self.model, data_up, self.q)
161 
162  self.assertTrue((Jcom_no == Jcom_up).all())
163 
164  def test_Jcom_noupdate3(self):
165  data_no = self.data
166  data_up = self.model.createData()
167 
168  pin.forwardKinematics(self.model, data_no, self.q)
169  Jcom_no = pin.jacobianCenterOfMass(self.model, data_no, True)
170 
171  Jcom_up = pin.jacobianCenterOfMass(self.model, data_up, self.q, True)
172 
173  self.assertTrue((Jcom_no == Jcom_up).all())
174  self.assertTrue((data_no.com[1] == data_up.com[1]).all())
175 
176  def test_subtree_jacobian(self):
177  model = self.model
178  data = self.data
179 
180  Jcom = pin.jacobianCenterOfMass(model, data, self.q)
181  Jcom_subtree = pin.getJacobianSubtreeCenterOfMass(model, data, 0)
182  self.assertApprox(Jcom, Jcom_subtree)
183 
184  data2 = model.createData()
185  Jcom_subtree2 = pin.jacobianSubtreeCenterOfMass(model, data2, self.q, 0)
186  self.assertApprox(Jcom_subtree, Jcom_subtree2)
187 
188  data3 = model.createData()
189  Jcom_subtree3 = pin.jacobianSubtreeCenterOfMass(model, data3, self.q, 0)
190  self.assertApprox(Jcom_subtree3, Jcom_subtree2)
191 
192  Jcom_subtree4 = pin.jacobianSubtreeCenterOfMass(model, data3, 0)
193  self.assertApprox(Jcom_subtree3, Jcom_subtree4)
194 
195 
196 if __name__ == "__main__":
197  unittest.main()
bindings_com.TestComBindings
Definition: bindings_com.py:9
bindings_com.TestComBindings.test_com_default
def test_com_default(self)
Definition: bindings_com.py:130
pinocchio::randomConfiguration
void randomConfiguration(const LieGroupGenericTpl< LieGroupCollection > &lg, const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< ConfigOut_t > &qout)
bindings_com.TestComBindings.test_com_2
def test_com_2(self)
Definition: bindings_com.py:94
bindings_com.TestComBindings.setUp
def setUp(self)
Definition: bindings_com.py:10
pinocchio::forwardKinematics
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a)
Update the joint placements, spatial velocities and spatial accelerations according to the current jo...
pinocchio::jacobianSubtreeCenterOfMass
void jacobianSubtreeCenterOfMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex &rootSubtreeId, const Eigen::MatrixBase< Matrix3xLike > &res)
Computes the Jacobian of the center of mass of the given subtree according to the current value store...
bindings_com.TestComBindings.test_mass
def test_mass(self)
Definition: bindings_com.py:17
bindings_com.TestComBindings.test_Jcom_noupdate3
def test_Jcom_noupdate3(self)
Definition: bindings_com.py:164
bindings_com.TestComBindings.test_com_0
def test_com_0(self)
Definition: bindings_com.py:42
pinocchio::computeTotalMass
Scalar computeTotalMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Compute the total mass of the model, put it in data.mass[0] and return it.
pinocchio.utils
Definition: bindings/python/pinocchio/utils.py:1
pinocchio::createData
ConstraintDataTpl< Scalar, Options, ConstraintCollectionTpl > createData(const ConstraintModelTpl< Scalar, Options, ConstraintCollectionTpl > &cmodel)
Definition: constraint-model-visitor.hpp:239
pinocchio::computeSubtreeMasses
void computeSubtreeMasses(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Compute the mass of each kinematic subtree and store it in data.mass. The element mass[0] corresponds...
bindings_com.TestComBindings.test_subtree_masses
def test_subtree_masses(self)
Definition: bindings_com.py:33
bindings_com.TestComBindings.model
model
Definition: bindings_com.py:11
bindings_com.TestComBindings.test_Jcom_noupdate2
def test_Jcom_noupdate2(self)
Definition: bindings_com.py:153
bindings_com.TestComBindings.test_com_1
def test_com_1(self)
Definition: bindings_com.py:65
pinocchio::getJacobianSubtreeCenterOfMass
void getJacobianSubtreeCenterOfMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex &rootSubtreeId, const Eigen::MatrixBase< Matrix3xLike > &res)
Retrieves the Jacobian of the center of mass of the given subtree according to the current value stor...
pinocchio::centerOfMass
const DataTpl< Scalar, Options, JointCollectionTpl >::Vector3 & centerOfMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const bool computeSubtreeComs=true)
Computes the center of mass position, velocity and acceleration of a given model according to the cur...
Definition: center-of-mass.hpp:194
pinocchio::jacobianCenterOfMass
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix3x & jacobianCenterOfMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const bool computeSubtreeComs=true)
Computes both the jacobian and the the center of mass position of a given model according to the curr...
bindings_com.TestComBindings.test_Jcom_update4
def test_Jcom_update4(self)
Definition: bindings_com.py:148
bindings_com.TestComBindings.q
q
Definition: bindings_com.py:15
pinocchio.utils.rand
def rand(n)
Definition: bindings/python/pinocchio/utils.py:41
bindings_com.TestComBindings.test_subtree_jacobian
def test_subtree_jacobian(self)
Definition: bindings_com.py:176
bindings_com.TestComBindings.test_Jcom_update3
def test_Jcom_update3(self)
Definition: bindings_com.py:144
bindings_com.TestComBindings.data
data
Definition: bindings_com.py:12

pinocchio
Author(s):
autogenerated on Wed May 28 2025 02:41:14