bindings_model.py
Go to the documentation of this file.
1 import unittest
2 import pinocchio as pin
3 from pinocchio.utils import np, zero
4 
5 from test_case import PinocchioTestCase as TestCase
6 
7 
8 class TestModel(TestCase):
9  def setUp(self):
10  self.model = pin.buildSampleModelHumanoidRandom()
11 
13  model = pin.Model()
14  self.assertEqual(model.nbodies, 1)
15  self.assertEqual(model.nq, 0)
16  self.assertEqual(model.nv, 0)
17  model.name = "empty_model"
18 
19  def test_add_joint(self):
20  model = pin.Model()
21  idx = 0
22  idx = model.addJoint(
23  idx, pin.JointModelRY(), pin.SE3.Identity(), "joint_" + str(idx + 1)
24  )
25 
26  MAX_EFF = 100.0
27  MAX_VEL = 10.0
28  MIN_POS = -1.0
29  MAX_POS = 1.0
30 
31  me = np.array([MAX_EFF])
32  mv = np.array([MAX_VEL])
33  lb = np.array([MIN_POS])
34  ub = np.array([MAX_POS])
35  idx = model.addJoint(
36  idx,
37  pin.JointModelRY(),
38  pin.SE3.Identity(),
39  "joint_" + str(idx + 1),
40  me,
41  mv,
42  lb,
43  ub,
44  )
45 
46  self.assertEqual(model.nbodies, 1)
47  self.assertEqual(model.njoints, 3)
48  self.assertEqual(model.nq, 2)
49  self.assertEqual(model.nv, 2)
50 
51  self.assertEqual(float(model.effortLimit[1]), MAX_EFF)
52  self.assertEqual(float(model.velocityLimit[1]), MAX_VEL)
53  self.assertEqual(float(model.lowerPositionLimit[1]), MIN_POS)
54  self.assertEqual(float(model.upperPositionLimit[1]), MAX_POS)
55 
56  def test_model(self):
57  model = self.model
58  nb = 28 # We should have 28 bodies, thus 27 joints, one of them a free-flyer.
59  self.assertEqual(model.nbodies, nb)
60  self.assertEqual(model.nq, nb - 1 + 6)
61  self.assertEqual(model.nv, nb - 1 + 5)
62 
63  def test_inertias(self):
64  model = self.model
65  model.inertias[1] = model.inertias[2]
66  self.assertApprox(model.inertias[1].np, model.inertias[2].np)
67 
68  def test_placements(self):
69  model = self.model
70  model.jointPlacements[1] = model.jointPlacements[2]
71  self.assertApprox(model.jointPlacements[1].np, model.jointPlacements[2].np)
72  self.assertEqual(model.parents[0], 0)
73  self.assertEqual(model.parents[1], 0)
74  model.parents[2] = model.parents[1]
75  self.assertEqual(model.parents[2], model.parents[1])
76  self.assertEqual(model.names[0], "universe")
77 
78  def test_gravity(self):
79  self.assertApprox(self.model.gravity.np, np.array([0, 0, -9.81, 0, 0, 0]).T)
80 
81  def test_rnea(self):
82  model = self.model
83  data = model.createData()
84 
85  q = zero(model.nq)
86  qdot = zero(model.nv)
87  qddot = zero(model.nv)
88  for i in range(model.nbodies):
89  data.a[i] = pin.Motion.Zero()
90 
91  pin.rnea(model, data, q, qdot, qddot)
92  for i in range(model.nbodies):
93  self.assertApprox(data.v[i].np, zero(6))
94  self.assertApprox(data.a_gf[0].np, -model.gravity.np)
95  self.assertApprox(data.f[-1], model.inertias[-1] * data.a_gf[-1])
96 
98  model = self.model
99  model.referenceConfigurations["neutral"] = pin.neutral(model)
100  q_neutral = model.referenceConfigurations["neutral"]
101 
102  q_neutral.fill(1.0)
103  self.assertApprox(model.referenceConfigurations["neutral"], q_neutral)
104 
105  def test_pickle(self):
106  import pickle
107 
108  model = self.model
109  filename = "model.pickle"
110  with open(filename, "wb") as f:
111  pickle.dump(model, f)
112 
113  with open(filename, "rb") as f:
114  model_copy = pickle.load(f)
115 
116  self.assertTrue(model == model_copy)
117 
118 
119 if __name__ == "__main__":
120  unittest.main()
bindings_model.TestModel.test_inertias
def test_inertias(self)
Definition: bindings_model.py:63
bindings_model.TestModel.setUp
def setUp(self)
Definition: bindings_model.py:9
bindings_model.TestModel.model
model
Definition: bindings_model.py:10
bindings_model.TestModel
Definition: bindings_model.py:8
bindings_model.TestModel.test_empty_model_sizes
def test_empty_model_sizes(self)
Definition: bindings_model.py:12
bindings_model.TestModel.test_rnea
def test_rnea(self)
Definition: bindings_model.py:81
bindings_model.TestModel.test_add_joint
def test_add_joint(self)
Definition: bindings_model.py:19
bindings_model.TestModel.test_placements
def test_placements(self)
Definition: bindings_model.py:68
bindings_model.TestModel.test_pickle
def test_pickle(self)
Definition: bindings_model.py:105
bindings_model.TestModel.test_std_map_fields
def test_std_map_fields(self)
Definition: bindings_model.py:97
pinocchio.utils
Definition: bindings/python/pinocchio/utils.py:1
append-urdf-model-with-another-model.open
open
Definition: append-urdf-model-with-another-model.py:78
bindings_model.TestModel.test_gravity
def test_gravity(self)
Definition: bindings_model.py:78
bindings_model.TestModel.test_model
def test_model(self)
Definition: bindings_model.py:56
pinocchio.utils.zero
def zero(n)
Definition: bindings/python/pinocchio/utils.py:38


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