bindings_frame.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.parent_idx = (
12  self.model.getJointId("rarm2_joint")
13  if self.model.existJointName("rarm2_joint")
14  else (self.model.njoints - 1)
15  )
16  self.frame_name = self.model.names[self.parent_idx] + "_frame"
17  self.frame_placement = pin.SE3.Random()
18  self.frame_type = pin.FrameType.OP_FRAME
19  self.model.addFrame(
20  pin.Frame(
21  self.frame_name,
22  self.parent_idx,
23  0,
24  self.frame_placement,
25  self.frame_type,
26  )
27  )
28  self.frame_idx = self.model.getFrameId(self.frame_name)
29 
30  def tearDown(self):
31  del self.model
32 
33  def test_type_get_set(self):
34  f = self.model.frames[self.frame_idx]
35  self.assertTrue(f.type == self.frame_type)
36  f.type = pin.FrameType.BODY
37  self.assertTrue(f.type == pin.FrameType.BODY)
38 
39  def test_name_get_set(self):
40  f = self.model.frames[self.frame_idx]
41  self.assertTrue(f.name == self.frame_name)
42  f.name = "new_hip_frame"
43  self.assertTrue(f.name == "new_hip_frame")
44 
46  f = self.model.frames[self.frame_idx]
47  self.assertTrue(f.parentJoint == self.parent_idx)
48  newparent = self.parent_idx - 1
49  f.parentJoint = newparent
50  self.assertTrue(f.parentJoint == newparent)
51 
53  f = self.model.frames[self.frame_idx]
54  self.assertTrue(
55  np.allclose(f.placement.homogeneous, self.frame_placement.homogeneous)
56  )
57  new_placement = pin.SE3.Random()
58  f.placement = new_placement
59  self.assertTrue(np.allclose(f.placement.homogeneous, new_placement.homogeneous))
60 
62  M = pin.SE3.Random()
63  frame1 = pin.Frame("name", 1, 2, M, pin.OP_FRAME)
64  frame2 = pin.Frame("name", 1, 2, M, pin.OP_FRAME)
65  frame3 = pin.Frame("othername", 3, 4, pin.SE3.Random(), pin.BODY)
66 
67  self.assertTrue(frame1 == frame2)
68  self.assertFalse(frame1 != frame2)
69  self.assertTrue(frame1 != frame3)
70  self.assertFalse(frame1 == frame3)
71 
72  def test_pickle(self):
73  import pickle
74 
75  frame = pin.Frame("name", 1, 2, pin.SE3.Random(), pin.OP_FRAME)
76  filename = "frame.pickle"
77  with open(filename, "wb") as f:
78  pickle.dump(frame, f)
79 
80  with open(filename, "rb") as f:
81  frame_copy = pickle.load(f)
82 
83  self.assertEqual(frame, frame_copy)
84 
85  def test_getters(self):
86  data = self.model.createData()
87  q = pin.randomConfiguration(self.model)
88  v = np.random.rand(self.model.nv)
89  a = np.random.rand(self.model.nv)
90  pin.forwardKinematics(self.model, data, q, v, a)
91 
92  T = pin.updateFramePlacement(self.model, data, self.frame_idx)
93  self.assertApprox(T, data.oMi[self.parent_idx].act(self.frame_placement))
94 
95  v = pin.getFrameVelocity(self.model, data, self.frame_idx)
96  self.assertApprox(v, self.frame_placement.actInv(data.v[self.parent_idx]))
97  v = pin.getFrameVelocity(
98  self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL
99  )
100  self.assertApprox(v, self.frame_placement.actInv(data.v[self.parent_idx]))
101  v = pin.getFrameVelocity(
102  self.model, data, self.frame_idx, pin.ReferenceFrame.WORLD
103  )
104  self.assertApprox(v, data.oMi[self.parent_idx].act(data.v[self.parent_idx]))
105  v = pin.getFrameVelocity(
106  self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
107  )
108  self.assertApprox(
109  v,
110  pin.SE3(T.rotation, np.zeros(3)).act(
111  self.frame_placement.actInv(data.v[self.parent_idx])
112  ),
113  )
114 
115  a = pin.getFrameAcceleration(self.model, data, self.frame_idx)
116  self.assertApprox(a, self.frame_placement.actInv(data.a[self.parent_idx]))
117  a = pin.getFrameAcceleration(
118  self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL
119  )
120  self.assertApprox(a, self.frame_placement.actInv(data.a[self.parent_idx]))
121  a = pin.getFrameAcceleration(
122  self.model, data, self.frame_idx, pin.ReferenceFrame.WORLD
123  )
124  self.assertApprox(a, data.oMi[self.parent_idx].act(data.a[self.parent_idx]))
125  a = pin.getFrameAcceleration(
126  self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
127  )
128  self.assertApprox(
129  a,
130  pin.SE3(T.rotation, np.zeros(3)).act(
131  self.frame_placement.actInv(data.a[self.parent_idx])
132  ),
133  )
134 
135  a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx)
136  a = pin.getFrameClassicalAcceleration(
137  self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL
138  )
139  a = pin.getFrameClassicalAcceleration(
140  self.model, data, self.frame_idx, pin.ReferenceFrame.WORLD
141  )
142  a = pin.getFrameClassicalAcceleration(
143  self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
144  )
145 
146  def test_frame_algo(self):
147  model = self.model
148  data = model.createData()
149 
150  q = pin.neutral(model)
151  v = np.random.rand((model.nv))
152  frame_id = self.frame_idx
153 
154  J1 = pin.computeFrameJacobian(model, data, q, frame_id)
155  J2 = pin.computeFrameJacobian(model, data, q, frame_id, pin.LOCAL)
156 
157  self.assertApprox(J1, J2)
158  data2 = model.createData()
159 
160  pin.computeJointJacobians(model, data2, q)
161  J3 = pin.getFrameJacobian(model, data2, frame_id, pin.LOCAL)
162  self.assertApprox(J1, J3)
163 
164  dJ1 = pin.frameJacobianTimeVariation(model, data, q, v, frame_id, pin.LOCAL)
165 
166  data3 = model.createData()
167  pin.computeJointJacobiansTimeVariation(model, data3, q, v)
168 
169  dJ2 = pin.getFrameJacobianTimeVariation(model, data3, frame_id, pin.LOCAL)
170  self.assertApprox(dJ1, dJ2)
171 
172 
173 if __name__ == "__main__":
174  unittest.main()
bindings_frame.TestFrameBindings.tearDown
def tearDown(self)
Definition: bindings_frame.py:30
bindings_frame.TestFrameBindings.model
model
Definition: bindings_frame.py:10
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_frame.TestFrameBindings.test_type_get_set
def test_type_get_set(self)
Definition: bindings_frame.py:33
bindings_frame.TestFrameBindings.test_getters
def test_getters(self)
Definition: bindings_frame.py:85
bindings_frame.TestFrameBindings.test_pickle
def test_pickle(self)
Definition: bindings_frame.py:72
bindings_frame.TestFrameBindings.frame_name
frame_name
Definition: bindings_frame.py:16
bindings_frame.TestFrameBindings.test_frame_equality
def test_frame_equality(self)
Definition: bindings_frame.py:61
bindings_frame.TestFrameBindings.test_name_get_set
def test_name_get_set(self)
Definition: bindings_frame.py:39
bindings_frame.TestFrameBindings.setUp
def setUp(self)
Definition: bindings_frame.py:9
bindings_frame.TestFrameBindings.parent_idx
parent_idx
Definition: bindings_frame.py:11
bindings_frame.TestFrameBindings
Definition: bindings_frame.py:8
pinocchio::createData
ConstraintDataTpl< Scalar, Options, ConstraintCollectionTpl > createData(const ConstraintModelTpl< Scalar, Options, ConstraintCollectionTpl > &cmodel)
Definition: constraint-model-visitor.hpp:248
bindings_frame.TestFrameBindings.frame_placement
frame_placement
Definition: bindings_frame.py:17
bindings_frame.TestFrameBindings.test_placement_get_set
def test_placement_get_set(self)
Definition: bindings_frame.py:52
test_case.PinocchioTestCase.assertApprox
def assertApprox(self, a, b, eps=1e-6)
Definition: test_case.py:12
bindings_frame.TestFrameBindings.frame_idx
frame_idx
Definition: bindings_frame.py:28
bindings_frame.TestFrameBindings.frame_type
frame_type
Definition: bindings_frame.py:18
append-urdf-model-with-another-model.open
open
Definition: append-urdf-model-with-another-model.py:78
bindings_frame.TestFrameBindings.test_parent_get_set
def test_parent_get_set(self)
Definition: bindings_frame.py:45
test_case.PinocchioTestCase
Definition: test_case.py:11
bindings_frame.TestFrameBindings.test_frame_algo
def test_frame_algo(self)
Definition: bindings_frame.py:146


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