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 
8 
9  def setUp(self):
10  self.model = pin.buildSampleModelHumanoidRandom()
11  self.parent_idx = self.model.getJointId("rarm2_joint") if self.model.existJointName("rarm2_joint") else (self.model.njoints-1)
12  self.frame_name = self.model.names[self.parent_idx] + "_frame"
13  self.frame_placement = pin.SE3.Random()
14  self.frame_type = pin.FrameType.OP_FRAME
15  self.model.addFrame(pin.Frame(self.frame_name, self.parent_idx, 0, self.frame_placement, self.frame_type))
16  self.frame_idx = self.model.getFrameId(self.frame_name)
17 
18  def tearDown(self):
19  del self.model
20 
21  def test_type_get_set(self):
22  f = self.model.frames[self.frame_idx]
23  self.assertTrue(f.type == self.frame_type)
24  f.type = pin.FrameType.BODY
25  self.assertTrue(f.type == pin.FrameType.BODY)
26 
27  def test_name_get_set(self):
28  f = self.model.frames[self.frame_idx]
29  self.assertTrue(f.name == self.frame_name)
30  f.name = 'new_hip_frame'
31  self.assertTrue(f.name == 'new_hip_frame')
32 
34  f = self.model.frames[self.frame_idx]
35  self.assertTrue(f.parent == self.parent_idx)
36  newparent = self.parent_idx-1
37  f.parent = newparent
38  self.assertTrue(f.parent == newparent)
39 
41  f = self.model.frames[self.frame_idx]
42  self.assertTrue(np.allclose(f.placement.homogeneous, self.frame_placement.homogeneous))
43  new_placement = pin.SE3.Random()
44  f.placement = new_placement
45  self.assertTrue(np.allclose(f.placement.homogeneous, new_placement.homogeneous))
46 
48  M = pin.SE3.Random()
49  frame1 = pin.Frame("name", 1, 2, M, pin.OP_FRAME)
50  frame2 = pin.Frame("name", 1, 2, M, pin.OP_FRAME)
51  frame3 = pin.Frame("othername", 3, 4, pin.SE3.Random(), pin.BODY)
52 
53  self.assertTrue(frame1 == frame2)
54  self.assertFalse(frame1 != frame2)
55  self.assertTrue(frame1 != frame3)
56  self.assertFalse(frame1 == frame3)
57 
58  def test_pickle(self):
59  import pickle
60 
61  frame = pin.Frame("name", 1, 2, pin.SE3.Random(), pin.OP_FRAME)
62  filename = "frame.pickle"
63  with open(filename, 'wb') as f:
64  pickle.dump(frame, f)
65 
66  with open(filename, 'rb') as f:
67  frame_copy = pickle.load(f)
68 
69  self.assertEqual(frame, frame_copy)
70 
71  def test_getters(self):
72  data = self.model.createData()
73  q = pin.randomConfiguration(self.model)
74  v = np.random.rand(self.model.nv)
75  a = np.random.rand(self.model.nv)
76  pin.forwardKinematics(self.model, data, q, v, a)
77 
78  T = pin.updateFramePlacement(self.model, data, self.frame_idx)
79  self.assertApprox(T, data.oMi[self.parent_idx].act(self.frame_placement))
80 
81  v = pin.getFrameVelocity(self.model, data, self.frame_idx)
82  self.assertApprox(v, self.frame_placement.actInv(data.v[self.parent_idx]))
83  v = pin.getFrameVelocity(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL)
84  self.assertApprox(v, self.frame_placement.actInv(data.v[self.parent_idx]))
85  v = pin.getFrameVelocity(self.model, data, self.frame_idx, pin.ReferenceFrame.WORLD)
86  self.assertApprox(v, data.oMi[self.parent_idx].act(data.v[self.parent_idx]))
87  v = pin.getFrameVelocity(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
88  self.assertApprox(v, pin.SE3(T.rotation, np.zeros(3)).act(self.frame_placement.actInv(data.v[self.parent_idx])))
89 
90  a = pin.getFrameAcceleration(self.model, data, self.frame_idx)
91  self.assertApprox(a, self.frame_placement.actInv(data.a[self.parent_idx]))
92  a = pin.getFrameAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL)
93  self.assertApprox(a, self.frame_placement.actInv(data.a[self.parent_idx]))
94  a = pin.getFrameAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.WORLD)
95  self.assertApprox(a, data.oMi[self.parent_idx].act(data.a[self.parent_idx]))
96  a = pin.getFrameAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
97  self.assertApprox(a, pin.SE3(T.rotation, np.zeros(3)).act(self.frame_placement.actInv(data.a[self.parent_idx])))
98 
99  a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx)
100  a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL)
101  a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.WORLD)
102  a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
103 
104  def test_frame_algo(self):
105  model = self.model
106  data = model.createData()
107 
108  q = pin.neutral(model)
109  v = np.random.rand((model.nv))
110  frame_id = self.frame_idx
111 
112  J1 = pin.computeFrameJacobian(model,data,q,frame_id)
113  J2 = pin.computeFrameJacobian(model,data,q,frame_id,pin.LOCAL)
114 
115  self.assertApprox(J1,J2)
116  data2 = model.createData()
117 
118  pin.computeJointJacobians(model,data2,q)
119  J3 = pin.getFrameJacobian(model,data2,frame_id,pin.LOCAL)
120  self.assertApprox(J1,J3)
121 
122  dJ1 = pin.frameJacobianTimeVariation(model,data,q,v,frame_id,pin.LOCAL)
123 
124  data3 = model.createData()
125  pin.computeJointJacobiansTimeVariation(model,data3,q,v)
126 
127  dJ2 = pin.getFrameJacobianTimeVariation(model,data3,frame_id,pin.LOCAL)
128  self.assertApprox(dJ1,dJ2)
129 
130 if __name__ == '__main__':
131  unittest.main()
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
def assertApprox(self, a, b, eps=1e-6)
Definition: test_case.py:10
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...


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