frames-derivatives.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2020 INRIA
3 //
4 
5 #include "pinocchio/algorithm/jacobian.hpp"
6 #include "pinocchio/algorithm/joint-configuration.hpp"
7 #include "pinocchio/algorithm/kinematics.hpp"
8 #include "pinocchio/algorithm/kinematics-derivatives.hpp"
9 #include "pinocchio/algorithm/frames.hpp"
10 #include "pinocchio/algorithm/frames-derivatives.hpp"
11 #include "pinocchio/parsers/sample-models.hpp"
12 
13 #include <iostream>
14 
15 #include <boost/test/unit_test.hpp>
16 #include <boost/utility/binary.hpp>
17 
18 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
19 
20 BOOST_AUTO_TEST_CASE(test_frames_derivatives_velocity)
21 {
22  using namespace Eigen;
23  using namespace pinocchio;
24 
25  Model model;
26  buildModels::humanoidRandom(model);
27 
28  const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
29  Frame frame("rand",jointId,0,SE3::Random(),OP_FRAME);
30  FrameIndex frameId = model.addFrame(frame);
31 
32  BOOST_CHECK(model.getFrameId("rand") == frameId);
33  BOOST_CHECK(model.frames[frameId].parent == jointId);
34 
35  Data data(model), data_ref(model);
36 
37  model.lowerPositionLimit.head<3>().fill(-1.);
38  model.upperPositionLimit.head<3>().fill(1.);
39  VectorXd q = randomConfiguration(model);
40  VectorXd v(VectorXd::Random(model.nv));
41  VectorXd a(VectorXd::Random(model.nv));
42 
43  computeForwardKinematicsDerivatives(model,data,q,v,a);
44 
45  Data::Matrix6x partial_dq(6,model.nv); partial_dq.setZero();
46  Data::Matrix6x partial_dq_local_world_aligned(6,model.nv); partial_dq_local_world_aligned.setZero();
47  Data::Matrix6x partial_dq_local(6,model.nv); partial_dq_local.setZero();
48  Data::Matrix6x partial_dv(6,model.nv); partial_dv.setZero();
49  Data::Matrix6x partial_dv_local_world_aligned(6,model.nv); partial_dv_local_world_aligned.setZero();
50  Data::Matrix6x partial_dv_local(6,model.nv); partial_dv_local.setZero();
51 
52  getFrameVelocityDerivatives(model,data,frameId,WORLD,
53  partial_dq,partial_dv);
54 
55  getFrameVelocityDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED,
56  partial_dq_local_world_aligned,partial_dv_local_world_aligned);
57 
58  getFrameVelocityDerivatives(model,data,frameId,LOCAL,
59  partial_dq_local,partial_dv_local);
60 
61  Data::Matrix6x J_ref(6,model.nv); J_ref.setZero();
62  Data::Matrix6x J_ref_local_world_aligned(6,model.nv); J_ref_local_world_aligned.setZero();
63  Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero();
64  computeJointJacobians(model,data_ref,q);
65  getFrameJacobian(model,data_ref,frameId,WORLD,J_ref);
66  getFrameJacobian(model,data_ref,frameId,LOCAL_WORLD_ALIGNED,J_ref_local_world_aligned);
67  getFrameJacobian(model,data_ref,frameId,LOCAL,J_ref_local);
68 
69  BOOST_CHECK(data_ref.oMf[frameId].isApprox(data.oMf[frameId]));
70  BOOST_CHECK(partial_dv.isApprox(J_ref));
71  BOOST_CHECK(partial_dv_local_world_aligned.isApprox(J_ref_local_world_aligned));
72  BOOST_CHECK(partial_dv_local.isApprox(J_ref_local));
73 
74  // Check against finite differences
75  Data::Matrix6x partial_dq_fd(6,model.nv); partial_dq_fd.setZero();
76  Data::Matrix6x partial_dq_fd_local_world_aligned(6,model.nv); partial_dq_fd_local_world_aligned.setZero();
77  Data::Matrix6x partial_dq_fd_local(6,model.nv); partial_dq_fd_local.setZero();
78  Data::Matrix6x partial_dv_fd(6,model.nv); partial_dv_fd.setZero();
79  Data::Matrix6x partial_dv_fd_local_world_aligned(6,model.nv); partial_dv_fd_local_world_aligned.setZero();
80  Data::Matrix6x partial_dv_fd_local(6,model.nv); partial_dv_fd_local.setZero();
81  const double alpha = 1e-8;
82 
83  // dvel/dv
84  Eigen::VectorXd v_plus(v);
85  Data data_plus(model);
86  forwardKinematics(model,data_ref,q,v);
87  Motion v0 = getFrameVelocity(model,data,frameId,WORLD);
88  Motion v0_local_world_aligned = getFrameVelocity(model,data,frameId,LOCAL_WORLD_ALIGNED);
89  Motion v0_local = getFrameVelocity(model,data,frameId,LOCAL);
90  for(int k = 0; k < model.nv; ++k)
91  {
92  v_plus[k] += alpha;
93  forwardKinematics(model,data_plus,q,v_plus);
94 
95  partial_dv_fd.col(k) = (getFrameVelocity(model,data_plus,frameId,WORLD) - v0).toVector()/alpha;
96  partial_dv_fd_local_world_aligned.col(k) = (getFrameVelocity(model,data_plus,frameId,LOCAL_WORLD_ALIGNED) - v0_local_world_aligned).toVector()/alpha;
97  partial_dv_fd_local.col(k) = (getFrameVelocity(model,data_plus,frameId,LOCAL) - v0_local).toVector()/alpha;
98  v_plus[k] -= alpha;
99  }
100 
101  BOOST_CHECK(partial_dv.isApprox(partial_dv_fd,sqrt(alpha)));
102  BOOST_CHECK(partial_dv_local_world_aligned.isApprox(partial_dv_fd_local_world_aligned,sqrt(alpha)));
103  BOOST_CHECK(partial_dv_local.isApprox(partial_dv_fd_local,sqrt(alpha)));
104 
105  // dvel/dq
106  Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
107  forwardKinematics(model,data_ref,q,v);
108  updateFramePlacements(model,data_ref);
109 
110  for(int k = 0; k < model.nv; ++k)
111  {
112  v_eps[k] += alpha;
113  q_plus = integrate(model,q,v_eps);
114  forwardKinematics(model,data_plus,q_plus,v);
115  updateFramePlacements(model,data_plus);
116 
117  Motion v_plus_local_world_aligned = getFrameVelocity(model,data_plus,frameId,LOCAL_WORLD_ALIGNED);
118  SE3::Vector3 trans = data_plus.oMf[frameId].translation() - data_ref.oMf[frameId].translation();
119  v_plus_local_world_aligned.linear() -= v_plus_local_world_aligned.angular().cross(trans);
120  partial_dq_fd.col(k) = (getFrameVelocity(model,data_plus,frameId,WORLD) - v0).toVector()/alpha;
121  partial_dq_fd_local_world_aligned.col(k) = (v_plus_local_world_aligned - v0_local_world_aligned).toVector()/alpha;
122  partial_dq_fd_local.col(k) = (getFrameVelocity(model,data_plus,frameId,LOCAL) - v0_local).toVector()/alpha;
123  v_eps[k] -= alpha;
124  }
125 
126  BOOST_CHECK(partial_dq.isApprox(partial_dq_fd,sqrt(alpha)));
127  BOOST_CHECK(partial_dq_local_world_aligned.isApprox(partial_dq_fd_local_world_aligned,sqrt(alpha)));
128  BOOST_CHECK(partial_dq_local.isApprox(partial_dq_fd_local,sqrt(alpha)));
129 
130 }
131 
132 BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_acceleration)
133 {
134  using namespace Eigen;
135  using namespace pinocchio;
136 
137  Model model;
138  buildModels::humanoidRandom(model);
139 
140  const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
141  Frame frame("rand",jointId,0,SE3::Random(),OP_FRAME);
142  FrameIndex frameId = model.addFrame(frame);
143 
144  BOOST_CHECK(model.getFrameId("rand") == frameId);
145  BOOST_CHECK(model.frames[frameId].parent == jointId);
146 
147  Data data(model), data_ref(model);
148 
149  model.lowerPositionLimit.head<3>().fill(-1.);
150  model.upperPositionLimit.head<3>().fill(1.);
151  VectorXd q = randomConfiguration(model);
152  VectorXd v(VectorXd::Random(model.nv));
153  VectorXd a(VectorXd::Random(model.nv));
154 
155  computeForwardKinematicsDerivatives(model,data,q,v,a);
156 
157  Data::Matrix6x v_partial_dq(6,model.nv); v_partial_dq.setZero();
158  Data::Matrix6x v_partial_dq_local(6,model.nv); v_partial_dq_local.setZero();
159  Data::Matrix6x v_partial_dq_local_world_aligned(6,model.nv); v_partial_dq_local_world_aligned.setZero();
160  Data::Matrix6x a_partial_dq(6,model.nv); a_partial_dq.setZero();
161  Data::Matrix6x a_partial_dq_local_world_aligned(6,model.nv); a_partial_dq_local_world_aligned.setZero();
162  Data::Matrix6x a_partial_dq_local(6,model.nv); a_partial_dq_local.setZero();
163  Data::Matrix6x a_partial_dv(6,model.nv); a_partial_dv.setZero();
164  Data::Matrix6x a_partial_dv_local_world_aligned(6,model.nv); a_partial_dv_local_world_aligned.setZero();
165  Data::Matrix6x a_partial_dv_local(6,model.nv); a_partial_dv_local.setZero();
166  Data::Matrix6x a_partial_da(6,model.nv); a_partial_da.setZero();
167  Data::Matrix6x a_partial_da_local_world_aligned(6,model.nv); a_partial_da_local_world_aligned.setZero();
168  Data::Matrix6x a_partial_da_local(6,model.nv); a_partial_da_local.setZero();
169 
170  getFrameAccelerationDerivatives(model,data,frameId,WORLD,
171  v_partial_dq,
172  a_partial_dq,a_partial_dv,a_partial_da);
173 
174  getFrameAccelerationDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED,
175  v_partial_dq_local_world_aligned,
176  a_partial_dq_local_world_aligned,
177  a_partial_dv_local_world_aligned,
178  a_partial_da_local_world_aligned);
179 
180  getFrameAccelerationDerivatives(model,data,frameId,LOCAL,
181  v_partial_dq_local,
182  a_partial_dq_local,a_partial_dv_local,a_partial_da_local);
183 
184  // Check v_partial_dq against getFrameVelocityDerivatives
185  {
186  Data data_v(model);
187  computeForwardKinematicsDerivatives(model,data_v,q,v,a);
188 
189  Data::Matrix6x v_partial_dq_ref(6,model.nv); v_partial_dq_ref.setZero();
190  Data::Matrix6x v_partial_dq_ref_local_world_aligned(6,model.nv); v_partial_dq_ref_local_world_aligned.setZero();
191  Data::Matrix6x v_partial_dq_ref_local(6,model.nv); v_partial_dq_ref_local.setZero();
192  Data::Matrix6x v_partial_dv_ref(6,model.nv); v_partial_dv_ref.setZero();
193  Data::Matrix6x v_partial_dv_ref_local_world_aligned(6,model.nv); v_partial_dv_ref_local_world_aligned.setZero();
194  Data::Matrix6x v_partial_dv_ref_local(6,model.nv); v_partial_dv_ref_local.setZero();
195 
196  getFrameVelocityDerivatives(model,data_v,frameId,WORLD,
197  v_partial_dq_ref,v_partial_dv_ref);
198 
199  BOOST_CHECK(v_partial_dq.isApprox(v_partial_dq_ref));
200  BOOST_CHECK(a_partial_da.isApprox(v_partial_dv_ref));
201 
202  getFrameVelocityDerivatives(model,data_v,frameId,LOCAL_WORLD_ALIGNED,
203  v_partial_dq_ref_local_world_aligned,v_partial_dv_ref_local_world_aligned);
204 
205  BOOST_CHECK(v_partial_dq_local_world_aligned.isApprox(v_partial_dq_ref_local_world_aligned));
206  BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(v_partial_dv_ref_local_world_aligned));
207 
208  getFrameVelocityDerivatives(model,data_v,frameId,LOCAL,
209  v_partial_dq_ref_local,v_partial_dv_ref_local);
210 
211  BOOST_CHECK(v_partial_dq_local.isApprox(v_partial_dq_ref_local));
212  BOOST_CHECK(a_partial_da_local.isApprox(v_partial_dv_ref_local));
213  }
214 
215  Data::Matrix6x J_ref(6,model.nv); J_ref.setZero();
216  Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero();
217  Data::Matrix6x J_ref_local_world_aligned(6,model.nv); J_ref_local_world_aligned.setZero();
218  computeJointJacobians(model,data_ref,q);
219  getFrameJacobian(model,data_ref,frameId,WORLD,J_ref);
220  getFrameJacobian(model,data_ref,frameId,LOCAL_WORLD_ALIGNED,J_ref_local_world_aligned);
221  getFrameJacobian(model,data_ref,frameId,LOCAL,J_ref_local);
222 
223  BOOST_CHECK(a_partial_da.isApprox(J_ref));
224  BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(J_ref_local_world_aligned));
225  BOOST_CHECK(a_partial_da_local.isApprox(J_ref_local));
226 
227  // Check against finite differences
228  Data::Matrix6x a_partial_da_fd(6,model.nv); a_partial_da_fd.setZero();
229  Data::Matrix6x a_partial_da_fd_local_world_aligned(6,model.nv); a_partial_da_fd_local_world_aligned.setZero();
230  Data::Matrix6x a_partial_da_fd_local(6,model.nv); a_partial_da_fd_local.setZero();
231  const double alpha = 1e-8;
232 
233  Eigen::VectorXd v_plus(v), a_plus(a);
234  Data data_plus(model);
235  forwardKinematics(model,data_ref,q,v,a);
236 
237  // dacc/da
238  Motion a0 = getFrameAcceleration(model,data,frameId,WORLD);
239  Motion a0_local_world_aligned = getFrameAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED);
240  Motion a0_local = getFrameAcceleration(model,data,frameId,LOCAL);
241  for(int k = 0; k < model.nv; ++k)
242  {
243  a_plus[k] += alpha;
244  forwardKinematics(model,data_plus,q,v,a_plus);
245 
246  a_partial_da_fd.col(k) = (getFrameAcceleration(model,data_plus,frameId,WORLD) - a0).toVector()/alpha;
247  a_partial_da_fd_local_world_aligned.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL_WORLD_ALIGNED) - a0_local_world_aligned).toVector()/alpha;
248  a_partial_da_fd_local.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL) - a0_local).toVector()/alpha;
249  a_plus[k] -= alpha;
250  }
251  BOOST_CHECK(a_partial_da.isApprox(a_partial_da_fd,sqrt(alpha)));
252  BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(a_partial_da_fd_local_world_aligned,sqrt(alpha)));
253  BOOST_CHECK(a_partial_da_local.isApprox(a_partial_da_fd_local,sqrt(alpha)));
254 
255  // dacc/dv
256  Data::Matrix6x a_partial_dv_fd(6,model.nv); a_partial_dv_fd.setZero();
257  Data::Matrix6x a_partial_dv_fd_local_world_aligned(6,model.nv); a_partial_dv_fd_local_world_aligned.setZero();
258  Data::Matrix6x a_partial_dv_fd_local(6,model.nv); a_partial_dv_fd_local.setZero();
259  for(int k = 0; k < model.nv; ++k)
260  {
261  v_plus[k] += alpha;
262  forwardKinematics(model,data_plus,q,v_plus,a);
263 
264  a_partial_dv_fd.col(k) = (getFrameAcceleration(model,data_plus,frameId,WORLD) - a0).toVector()/alpha; a_partial_dv_fd_local_world_aligned.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL_WORLD_ALIGNED) - a0_local_world_aligned).toVector()/alpha;
265  a_partial_dv_fd_local.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL) - a0_local).toVector()/alpha;
266  v_plus[k] -= alpha;
267  }
268 
269  BOOST_CHECK(a_partial_dv.isApprox(a_partial_dv_fd,sqrt(alpha)));
270  BOOST_CHECK(a_partial_dv_local_world_aligned.isApprox(a_partial_dv_fd_local_world_aligned,sqrt(alpha)));
271  BOOST_CHECK(a_partial_dv_local.isApprox(a_partial_dv_fd_local,sqrt(alpha)));
272 
273  // dacc/dq
274  a_partial_dq.setZero();
275  a_partial_dv.setZero();
276  a_partial_da.setZero();
277 
278  a_partial_dq_local_world_aligned.setZero();
279  a_partial_dv_local_world_aligned.setZero();
280  a_partial_da_local_world_aligned.setZero();
281 
282  a_partial_dq_local.setZero();
283  a_partial_dv_local.setZero();
284  a_partial_da_local.setZero();
285 
286  Data::Matrix6x a_partial_dq_fd(6,model.nv); a_partial_dq_fd.setZero();
287  Data::Matrix6x a_partial_dq_fd_local_world_aligned(6,model.nv); a_partial_dq_fd_local_world_aligned.setZero();
288  Data::Matrix6x a_partial_dq_fd_local(6,model.nv); a_partial_dq_fd_local.setZero();
289 
290  computeForwardKinematicsDerivatives(model,data,q,v,a);
291  getFrameAccelerationDerivatives(model,data,frameId,WORLD,
292  v_partial_dq,
293  a_partial_dq,a_partial_dv,a_partial_da);
294 
295  getFrameAccelerationDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED,
296  v_partial_dq_local_world_aligned,
297  a_partial_dq_local_world_aligned,
298  a_partial_dv_local_world_aligned,
299  a_partial_da_local_world_aligned);
300 
301  getFrameAccelerationDerivatives(model,data,frameId,LOCAL,
302  v_partial_dq_local,
303  a_partial_dq_local,a_partial_dv_local,a_partial_da_local);
304 
305  Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
306  forwardKinematics(model,data_ref,q,v,a);
307  updateFramePlacements(model,data_ref);
308  a0 = getFrameAcceleration(model,data,frameId,WORLD);
309  a0_local_world_aligned = getFrameAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED);
310  a0_local = getFrameAcceleration(model,data,frameId,LOCAL);
311 
312  for(int k = 0; k < model.nv; ++k)
313  {
314  v_eps[k] += alpha;
315  q_plus = integrate(model,q,v_eps);
316  forwardKinematics(model,data_plus,q_plus,v,a);
317  updateFramePlacements(model,data_plus);
318 
319  a_partial_dq_fd.col(k) = (getFrameAcceleration(model,data_plus,frameId,WORLD) - a0).toVector()/alpha;
320  Motion a_plus_local_world_aligned = getFrameAcceleration(model,data_plus,frameId,LOCAL_WORLD_ALIGNED);
321  const SE3::Vector3 trans = data_plus.oMf[frameId].translation() - data_ref.oMf[frameId].translation();
322  a_plus_local_world_aligned.linear() -= a_plus_local_world_aligned.angular().cross(trans);
323  a_partial_dq_fd_local_world_aligned.col(k) = (a_plus_local_world_aligned - a0_local_world_aligned).toVector()/alpha;
324  a_partial_dq_fd_local.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL) - a0_local).toVector()/alpha;
325  v_eps[k] -= alpha;
326  }
327 
328  BOOST_CHECK(a_partial_dq.isApprox(a_partial_dq_fd,sqrt(alpha)));
329  BOOST_CHECK(a_partial_dq_local_world_aligned.isApprox(a_partial_dq_fd_local_world_aligned,sqrt(alpha)));
330  BOOST_CHECK(a_partial_dq_local.isApprox(a_partial_dq_fd_local,sqrt(alpha)));
331 
332  // Test other signatures
333  Data::Matrix6x v_partial_dq_other(6,model.nv); v_partial_dq_other.setZero();
334  Data::Matrix6x v_partial_dq_local_other(6,model.nv); v_partial_dq_local_other.setZero();
335  Data::Matrix6x v_partial_dq_local_world_aligned_other(6,model.nv); v_partial_dq_local_world_aligned_other.setZero();
336  Data::Matrix6x v_partial_dv_other(6,model.nv); v_partial_dv_other.setZero();
337  Data::Matrix6x v_partial_dv_local_other(6,model.nv); v_partial_dv_local_other.setZero();
338  Data::Matrix6x v_partial_dv_local_world_aligned_other(6,model.nv); v_partial_dv_local_world_aligned_other.setZero();
339  Data::Matrix6x a_partial_dq_other(6,model.nv); a_partial_dq_other.setZero();
340  Data::Matrix6x a_partial_dq_local_world_aligned_other(6,model.nv); a_partial_dq_local_world_aligned_other.setZero();
341  Data::Matrix6x a_partial_dq_local_other(6,model.nv); a_partial_dq_local_other.setZero();
342  Data::Matrix6x a_partial_dv_other(6,model.nv); a_partial_dv_other.setZero();
343  Data::Matrix6x a_partial_dv_local_world_aligned_other(6,model.nv); a_partial_dv_local_world_aligned_other.setZero();
344  Data::Matrix6x a_partial_dv_local_other(6,model.nv); a_partial_dv_local_other.setZero();
345  Data::Matrix6x a_partial_da_other(6,model.nv); a_partial_da_other.setZero();
346  Data::Matrix6x a_partial_da_local_world_aligned_other(6,model.nv); a_partial_da_local_world_aligned_other.setZero();
347  Data::Matrix6x a_partial_da_local_other(6,model.nv); a_partial_da_local_other.setZero();
348 
349  getFrameAccelerationDerivatives(model,data,frameId,WORLD,
350  v_partial_dq_other,
351  v_partial_dv_other,
352  a_partial_dq_other,
353  a_partial_dv_other,
354  a_partial_da_other);
355 
356  BOOST_CHECK(v_partial_dq_other.isApprox(v_partial_dq));
357  BOOST_CHECK(v_partial_dv_other.isApprox(a_partial_da));
358  BOOST_CHECK(a_partial_dq_other.isApprox(a_partial_dq));
359  BOOST_CHECK(a_partial_dv_other.isApprox(a_partial_dv));
360  BOOST_CHECK(a_partial_da_other.isApprox(a_partial_da));
361 
362  getFrameAccelerationDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED,
363  v_partial_dq_local_world_aligned_other,
364  v_partial_dv_local_world_aligned_other,
365  a_partial_dq_local_world_aligned_other,
366  a_partial_dv_local_world_aligned_other,
367  a_partial_da_local_world_aligned_other);
368 
369  BOOST_CHECK(v_partial_dq_local_world_aligned_other.isApprox(v_partial_dq_local_world_aligned));
370  BOOST_CHECK(v_partial_dv_local_world_aligned_other.isApprox(a_partial_da_local_world_aligned));
371  BOOST_CHECK(a_partial_dq_local_world_aligned_other.isApprox(a_partial_dq_local_world_aligned));
372  BOOST_CHECK(a_partial_dv_local_world_aligned_other.isApprox(a_partial_dv_local_world_aligned));
373  BOOST_CHECK(a_partial_da_local_world_aligned_other.isApprox(a_partial_da_local_world_aligned));
374 
375  getFrameAccelerationDerivatives(model,data,frameId,LOCAL,
376  v_partial_dq_local_other,
377  v_partial_dv_local_other,
378  a_partial_dq_local_other,
379  a_partial_dv_local_other,
380  a_partial_da_local_other);
381 
382  BOOST_CHECK(v_partial_dq_local_other.isApprox(v_partial_dq_local));
383  BOOST_CHECK(v_partial_dv_local_other.isApprox(a_partial_da_local));
384  BOOST_CHECK(a_partial_dq_local_other.isApprox(a_partial_dq_local));
385  BOOST_CHECK(a_partial_dv_local_other.isApprox(a_partial_dv_local));
386  BOOST_CHECK(a_partial_da_local_other.isApprox(a_partial_da_local));
387 }
388 
389 BOOST_AUTO_TEST_SUITE_END()
pinocchio::WORLD
The WORLD frame convention corresponds to the frame concident with the Universe/Inertial frame but mo...
Definition: src/multibody/fwd.hpp:46
pinocchio::FrameIndex
Index FrameIndex
Definition: src/multibody/fwd.hpp:30
q
q
pinocchio::integrate
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
Definition: joint-configuration.hpp:54
pinocchio::FrameTpl
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
Definition: src/multibody/frame.hpp:41
pinocchio::ModelTpl::njoints
int njoints
Number of joints.
Definition: src/multibody/model.hpp:82
pinocchio::getFrameVelocity
MotionTpl< Scalar, Options > getFrameVelocity(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf=LOCAL)
Returns the spatial velocity of the Frame expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement and velocity values in data structure.
pinocchio::ModelTpl::lowerPositionLimit
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
Definition: src/multibody/model.hpp:139
pinocchio::ModelTpl::frames
FrameVector frames
Vector of operational frames registered on the model.
Definition: src/multibody/model.hpp:145
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_frames_derivatives_velocity)
Definition: frames-derivatives.cpp:20
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:224
pinocchio::ModelTpl::existJointName
bool existJointName(const std::string &name) const
Check if a joint given by its name exists.
pinocchio::MotionBase::linear
ConstLinearType linear() const
Definition: motion-base.hpp:22
fill
fill
pinocchio::MotionBase::angular
ConstAngularType angular() const
Definition: motion-base.hpp:21
pinocchio::OP_FRAME
operational frame: user-defined frames that are defined at runtime
Definition: src/multibody/frame.hpp:30
pinocchio::getFrameAcceleration
MotionTpl< Scalar, Options > getFrameAcceleration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf=LOCAL)
Returns the spatial acceleration of the Frame expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in data structure.
pinocchio::forwardKinematics
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
pinocchio::LOCAL
The LOCAL frame convention corresponds to the frame directly attached to the moving part (Joint...
Definition: src/multibody/fwd.hpp:47
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: src/multibody/model.hpp:49
pinocchio::computeForwardKinematicsDerivatives
void computeForwardKinematicsDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a)
Computes all the terms required to compute the derivatives of the placement, spatial velocity and acc...
pinocchio::ModelTpl::getFrameId
FrameIndex getFrameId(const std::string &name, const FrameType &type=(FrameType)(JOINT|FIXED_JOINT|BODY|OP_FRAME|SENSOR)) const
Returns the index of a frame given by its name.
a
Vec3f a
pinocchio::getFrameAccelerationDerivatives
void getFrameAccelerationDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &a_partial_dq, const Eigen::MatrixBase< Matrix6xOut3 > &a_partial_dv, const Eigen::MatrixBase< Matrix6xOut4 > &a_partial_da)
Computes the partial derivatives of the frame acceleration quantity with respect to q...
pinocchio::SE3Tpl< double, 0 >::Vector3
traits< SE3Tpl >::Vector3 Vector3
Definition: spatial/se3-tpl.hpp:53
pinocchio::ModelTpl::addFrame
FrameIndex addFrame(const Frame &frame, const bool append_inertia=true)
Adds a frame to the kinematic tree. The inertia stored within the frame will be happened to the inert...
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:28
pinocchio::getFrameVelocityDerivatives
void getFrameVelocityDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &v_partial_dv)
Computes the partial derivatives of the frame velocity quantity with respect to q and v...
pinocchio::ModelTpl::getJointId
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
pinocchio::computeJointJacobians
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobians(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame...
pinocchio::LOCAL_WORLD_ALIGNED
The LOCAL_WORLD_ALIGNED frame convention corresponds to the frame centered on the moving part (Joint...
Definition: src/multibody/fwd.hpp:48
pinocchio::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition: src/multibody/model.hpp:79
pinocchio::updateFramePlacements
void updateFramePlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Updates the position of each frame contained in the model.
pinocchio::Index
std::size_t Index
Definition: src/multibody/fwd.hpp:25
pinocchio::ModelTpl::upperPositionLimit
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
Definition: src/multibody/model.hpp:142
pinocchio::getFrameJacobian
void getFrameJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xLike > &J)
Returns the jacobian of the frame expressed either expressed in the LOCAL frame coordinate system or ...
pinocchio::buildModels::humanoidRandom
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
pinocchio::python::VectorXd
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: src/multibody/data.hpp:73

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