bindings_com.py
Go to the documentation of this file.
1 import unittest
2 from test_case import PinocchioTestCase as TestCase
3 import pinocchio as pin
4 from pinocchio.utils import rand
5 import numpy as np
6 
7 
8 class TestComBindings(TestCase):
9  def setUp(self):
10  self.model = pin.buildSampleModelHumanoidRandom()
11  self.data = self.model.createData()
12 
13  qmax = np.full((self.model.nq, 1), np.pi)
14  self.q = pin.randomConfiguration(self.model, -qmax, qmax)
15 
16  def test_mass(self):
17  mass = pin.computeTotalMass(self.model)
18  self.assertIsNot(mass, np.nan)
19 
20  mass_check = sum([inertia.mass for inertia in self.model.inertias[1:]])
21  self.assertApprox(mass, mass_check)
22 
23  mass_data = pin.computeTotalMass(self.model, self.data)
24  self.assertIsNot(mass_data, np.nan)
25  self.assertApprox(mass, mass_data)
26  self.assertApprox(mass_data, self.data.mass[0])
27 
28  data2 = self.model.createData()
29  pin.centerOfMass(self.model, data2, self.q)
30  self.assertApprox(mass, data2.mass[0])
31 
32  def test_subtree_masses(self):
33  pin.computeSubtreeMasses(self.model, self.data)
34 
35  data2 = self.model.createData()
36  pin.centerOfMass(self.model, data2, self.q)
37 
38  for i in range(self.model.njoints):
39  self.assertApprox(self.data.mass[i], data2.mass[i])
40 
41  def test_com_0(self):
42  data = self.data
43 
44  c0 = pin.centerOfMass(self.model, self.data, self.q)
45  c0_bis = pin.centerOfMass(self.model, self.data, self.q, False)
46 
47  self.assertApprox(c0, c0_bis)
48 
49  data2 = self.model.createData()
50  pin.forwardKinematics(self.model, data, self.q)
51  c0 = pin.centerOfMass(self.model, data, pin.POSITION)
52  pin.forwardKinematics(self.model, data2, self.q)
53  c0_bis = pin.centerOfMass(self.model, data2, pin.POSITION, False)
54 
55  self.assertApprox(c0, c0_bis)
56 
57  c0_bis = pin.centerOfMass(self.model, self.data, 0)
58 
59  self.assertApprox(c0, c0_bis)
60 
61  self.assertApprox(c0, data2.com[0])
62  self.assertApprox(self.data.com[0], data2.com[0])
63 
64  def test_com_1(self):
65  data = self.data
66 
67  v = rand(self.model.nv)
68  c0 = pin.centerOfMass(self.model, self.data, self.q, v)
69  c0_bis = pin.centerOfMass(self.model, self.data, self.q, v, False)
70 
71  self.assertApprox(c0, c0_bis)
72 
73  data2 = self.model.createData()
74  pin.forwardKinematics(self.model, data, self.q, v)
75  c0 = pin.centerOfMass(self.model, data, pin.VELOCITY)
76  pin.forwardKinematics(self.model, data2, self.q, v)
77  c0_bis = pin.centerOfMass(self.model, data2, pin.VELOCITY, False)
78 
79  self.assertApprox(c0, c0_bis)
80 
81  c0_bis = pin.centerOfMass(self.model, data2, 1)
82 
83  self.assertApprox(c0, c0_bis)
84 
85  data3 = self.model.createData()
86  pin.centerOfMass(self.model, data3, self.q)
87 
88  self.assertApprox(self.data.com[0], data2.com[0])
89  self.assertApprox(self.data.vcom[0], data2.vcom[0])
90 
91  self.assertApprox(self.data.com[0], data3.com[0])
92 
93  def test_com_2(self):
94  data = self.data
95 
96  v = rand(self.model.nv)
97  a = rand(self.model.nv)
98  c0 = pin.centerOfMass(self.model, self.data, self.q, v, a)
99  c0_bis = pin.centerOfMass(self.model, self.data, self.q, v, a, False)
100 
101  self.assertApprox(c0, c0_bis)
102 
103  data2 = self.model.createData()
104  pin.forwardKinematics(self.model, data, self.q, v, a)
105  c0 = pin.centerOfMass(self.model, data, pin.ACCELERATION)
106  pin.forwardKinematics(self.model, data2, self.q, v, a)
107  c0_bis = pin.centerOfMass(self.model, data2, pin.ACCELERATION, False)
108 
109  self.assertApprox(c0, c0_bis)
110 
111  c0_bis = pin.centerOfMass(self.model, data2, 2)
112  self.assertApprox(c0, c0_bis)
113 
114  data3 = self.model.createData()
115  pin.centerOfMass(self.model, data3, self.q)
116 
117  data4 = self.model.createData()
118  pin.centerOfMass(self.model, data4, self.q, v)
119 
120  self.assertApprox(self.data.com[0], data2.com[0])
121  self.assertApprox(self.data.vcom[0], data2.vcom[0])
122  self.assertApprox(self.data.acom[0], data2.acom[0])
123 
124  self.assertApprox(self.data.com[0], data3.com[0])
125 
126  self.assertApprox(self.data.com[0], data4.com[0])
127  self.assertApprox(self.data.vcom[0], data4.vcom[0])
128 
129  def test_com_default(self):
130  v = rand(self.model.nv)
131  a = rand(self.model.nv)
132  pin.centerOfMass(self.model, self.data, self.q, v, a)
133 
134  data2 = self.model.createData()
135  pin.forwardKinematics(self.model, data2, self.q, v, a)
136  pin.centerOfMass(self.model, data2)
137 
138  for i in range(self.model.njoints):
139  self.assertApprox(self.data.com[i], data2.com[i])
140  self.assertApprox(self.data.vcom[i], data2.vcom[i])
141  self.assertApprox(self.data.acom[i], data2.acom[i])
142 
143  def test_Jcom_update3(self):
144  Jcom = pin.jacobianCenterOfMass(self.model, self.data, self.q)
145  self.assertFalse(np.isnan(Jcom).any())
146 
147  def test_Jcom_update4(self):
148  Jcom = pin.jacobianCenterOfMass(self.model, self.data, self.q, True)
149  self.assertFalse(np.isnan(Jcom).any())
150  self.assertFalse(np.isnan(self.data.com[1]).any())
151 
152  def test_Jcom_noupdate2(self):
153  data_no = self.data
154  data_up = self.model.createData()
155 
156  pin.forwardKinematics(self.model, data_no, self.q)
157  Jcom_no = pin.jacobianCenterOfMass(self.model, data_no)
158 
159  Jcom_up = pin.jacobianCenterOfMass(self.model, data_up, self.q)
160 
161  self.assertTrue((Jcom_no == Jcom_up).all())
162 
163  def test_Jcom_noupdate3(self):
164  data_no = self.data
165  data_up = self.model.createData()
166 
167  pin.forwardKinematics(self.model, data_no, self.q)
168  Jcom_no = pin.jacobianCenterOfMass(self.model, data_no, True)
169 
170  Jcom_up = pin.jacobianCenterOfMass(self.model, data_up, self.q, True)
171 
172  self.assertTrue((Jcom_no == Jcom_up).all())
173  self.assertTrue((data_no.com[1] == data_up.com[1]).all())
174 
175  def test_subtree_jacobian(self):
176  model = self.model
177  data = self.data
178 
179  Jcom = pin.jacobianCenterOfMass(model, data, self.q)
180  Jcom_subtree = pin.getJacobianSubtreeCenterOfMass(model, data, 0)
181  self.assertApprox(Jcom, Jcom_subtree)
182 
183  data2 = model.createData()
184  Jcom_subtree2 = pin.jacobianSubtreeCenterOfMass(model, data2, self.q, 0)
185  self.assertApprox(Jcom_subtree, Jcom_subtree2)
186 
187  data3 = model.createData()
188  Jcom_subtree3 = pin.jacobianSubtreeCenterOfMass(model, data3, self.q, 0)
189  self.assertApprox(Jcom_subtree3, Jcom_subtree2)
190 
191  Jcom_subtree4 = pin.jacobianSubtreeCenterOfMass(model, data3, 0)
192  self.assertApprox(Jcom_subtree3, Jcom_subtree4)
193 
194 
195 if __name__ == "__main__":
196  unittest.main()
bindings_com.TestComBindings
Definition: bindings_com.py:8
bindings_com.TestComBindings.test_com_default
def test_com_default(self)
Definition: bindings_com.py:129
bindings_com.TestComBindings.test_com_2
def test_com_2(self)
Definition: bindings_com.py:93
bindings_com.TestComBindings.setUp
def setUp(self)
Definition: bindings_com.py:9
bindings_com.TestComBindings.test_mass
def test_mass(self)
Definition: bindings_com.py:16
bindings_com.TestComBindings.test_Jcom_noupdate3
def test_Jcom_noupdate3(self)
Definition: bindings_com.py:163
bindings_com.TestComBindings.test_com_0
def test_com_0(self)
Definition: bindings_com.py:41
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:248
bindings_com.TestComBindings.test_subtree_masses
def test_subtree_masses(self)
Definition: bindings_com.py:32
bindings_com.TestComBindings.model
model
Definition: bindings_com.py:10
bindings_com.TestComBindings.test_Jcom_noupdate2
def test_Jcom_noupdate2(self)
Definition: bindings_com.py:152
bindings_com.TestComBindings.test_com_1
def test_com_1(self)
Definition: bindings_com.py:64
bindings_com.TestComBindings.test_Jcom_update4
def test_Jcom_update4(self)
Definition: bindings_com.py:147
bindings_com.TestComBindings.q
q
Definition: bindings_com.py:14
pinocchio.utils.rand
def rand(n)
Definition: bindings/python/pinocchio/utils.py:42
bindings_com.TestComBindings.test_subtree_jacobian
def test_subtree_jacobian(self)
Definition: bindings_com.py:175
bindings_com.TestComBindings.test_Jcom_update3
def test_Jcom_update3(self)
Definition: bindings_com.py:143
bindings_com.TestComBindings.data
data
Definition: bindings_com.py:11

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