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 class TestComBindings(TestCase):
8 
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.jacobianSubtreeCoMJacobian(model,data3,self.q,0)
189  self.assertApprox(Jcom_subtree3,Jcom_subtree2)
190 
191  Jcom_subtree4 = pin.jacobianSubtreeCoMJacobian(model,data3,0)
192  self.assertApprox(Jcom_subtree3,Jcom_subtree4)
193 
194 if __name__ == '__main__':
195  unittest.main()
bindings_com.TestComBindings.test_Jcom_noupdate3
def test_Jcom_noupdate3(self)
Definition: bindings_com.py:163
bindings_com.TestComBindings.test_Jcom_update4
def test_Jcom_update4(self)
Definition: bindings_com.py:147
bindings_com.TestComBindings.test_subtree_masses
def test_subtree_masses(self)
Definition: bindings_com.py:32
bindings_com.TestComBindings.test_mass
def test_mass(self)
Definition: bindings_com.py:16
bindings_com.TestComBindings.test_Jcom_noupdate2
def test_Jcom_noupdate2(self)
Definition: bindings_com.py:152
pinocchio::createData
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
bindings_com.TestComBindings.model
model
Definition: bindings_com.py:10
bindings_com.TestComBindings
Definition: bindings_com.py:7
bindings_com.TestComBindings.test_com_0
def test_com_0(self)
Definition: bindings_com.py:41
bindings_com.TestComBindings.q
q
Definition: bindings_com.py:14
bindings_com.TestComBindings.data
data
Definition: bindings_com.py:11
bindings_com.TestComBindings.test_Jcom_update3
def test_Jcom_update3(self)
Definition: bindings_com.py:143
bindings_com.TestComBindings.setUp
def setUp(self)
Definition: bindings_com.py:9
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.test_subtree_jacobian
def test_subtree_jacobian(self)
Definition: bindings_com.py:175
bindings_com.TestComBindings.test_com_1
def test_com_1(self)
Definition: bindings_com.py:64

pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:28