Kinematics.cc
Go to the documentation of this file.
1 /*
2  * Original Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
3  *
4  *
5  * RDL - Robot Dynamics Library
6  * Modifications Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
7  *
8  * Licensed under the zlib license. See LICENSE for more details.
9  */
10 
11 #include <boost/bind.hpp>
12 #include <iostream>
13 #include <limits>
14 #include <cstring>
15 #include <assert.h>
16 
17 #include "rdl_dynamics/FramePoint.hpp"
18 #include "rdl_dynamics/Kinematics.h"
19 #include "rdl_dynamics/rdl_mathutils.h"
20 
21 namespace RobotDynamics
22 {
23  using namespace Math;
24 
25  void updateKinematics(Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &QDDot)
26  {
27  model.a[0].setZero();
28 
29  for(unsigned int i = 1; i < model.mBodies.size(); i++)
30  {
31  unsigned int q_index = model.mJoints[i].q_index;
32  ReferenceFramePtr bodyFrame = model.bodyFrames[i];
33 
34  unsigned int lambda = model.lambda[i];
35 
36  jcalc(model, i, Q, QDot);
37 
38  if(lambda != 0)
39  {
40  model.v[i].set(model.v[lambda].transform_copy(bodyFrame->getTransformFromParent()) + model.v_J[i]);
41  }
42  else
43  {
44  model.v[i] = model.v_J[i];
45  }
46 
47  model.c[i] = model.c_J[i] + model.v[i] % model.v_J[i];
48  model.a[i].set(model.a[lambda].transform_copy(bodyFrame->getTransformFromParent())+ model.c[i]);
49 
50  if(model.mJoints[i].mJointType != JointTypeCustom)
51  {
52  if(model.mJoints[i].mDoFCount == 1)
53  {
54  model.a[i].set(model.a[i] + model.S[i] * QDDot[q_index]);
55  }
56  else if(model.mJoints[i].mDoFCount == 3)
57  {
58  model.a[i].set(model.a[i] + model.multdof3_S[i] * Vector3d(QDDot[q_index], QDDot[q_index + 1], QDDot[q_index + 2]));
59  }
60  }
61  else
62  {
63  unsigned int custom_index = model.mJoints[i].custom_joint_index;
64  const CustomJoint *custom_joint = model.mCustomJoints[custom_index];
65  unsigned int joint_dof_count = custom_joint->mDoFCount;
66 
67  model.a[i].set(model.a[i] + (model.mCustomJoints[custom_index]->S * QDDot.block(q_index, 0, joint_dof_count, 1)));
68  }
69  }
70  }
71 
72  void updateKinematicsCustom(Model &model, const VectorNd *Q, const VectorNd *QDot,
73  const VectorNd *QDDot)
74  {
75  model.a[0].setZero();
76  if(Q && !QDot && !QDDot)
77  {
78  for(unsigned int i = 1; i < model.mBodies.size(); i++)
79  {
80  jcalc(model, i, (*Q), model.q0_vec);
81  }
82  }
83  else if(Q && QDot && !QDDot)
84  {
85  for(unsigned int i = 1; i < model.mBodies.size(); i++)
86  {
87  jcalc(model, i, (*Q), (*QDot));
88 
89  ReferenceFramePtr bodyFrame = model.bodyFrames[i];
90  unsigned int lambda = model.lambda[i];
91 
92  if(lambda != 0)
93  {
94  model.v[i].set(model.v[lambda].transform_copy(bodyFrame->getTransformFromParent()) + model.v_J[i]);
95  model.c[i] = model.c_J[i] + model.v[i] % model.v_J[i];
96  }
97  else
98  {
99  model.v[i].set(model.v_J[i]);
100  model.c[i] = model.c_J[i] + model.v[i] % model.v_J[i];
101  }
102  }
103  }
104  else if(Q && QDot && QDDot)
105  {
106  for(unsigned int i = 1; i < model.mBodies.size(); i++)
107  {
108  jcalc(model, i, (*Q), (*QDot));
109 
110  ReferenceFramePtr bodyFrame = model.bodyFrames[i];
111  unsigned int lambda = model.lambda[i];
112 
113  if(lambda != 0)
114  {
115  model.v[i].set(model.v[lambda].transform_copy(bodyFrame->getTransformFromParent()) + model.v_J[i]);
116  model.c[i] = model.c_J[i] + model.v[i] % model.v_J[i];
117  model.a[i].set(model.a[lambda].transform_copy(bodyFrame->getTransformFromParent()) + model.c[i]);
118  }
119  else
120  {
121  model.v[i].set(model.v_J[i]);
122  model.c[i] = model.c_J[i] + model.v[i] % model.v_J[i];
123  model.a[i].set(model.c[i]);
124  }
125 
126  unsigned int q_index = model.mJoints[i].q_index;
127 
128  if(model.mJoints[i].mJointType != JointTypeCustom)
129  {
130  if(model.mJoints[i].mDoFCount == 1)
131  {
132  model.a[i].set(model.a[i] + model.S[i] * (*QDDot)[q_index]);
133  }
134  else if(model.mJoints[i].mDoFCount == 3)
135  {
136  Vector3d omegadot_temp((*QDDot)[q_index], (*QDDot)[q_index + 1], (*QDDot)[q_index + 2]);
137  model.a[i].set(model.a[i] + model.multdof3_S[i] * omegadot_temp);
138  }
139  }
140  else
141  {
142  unsigned int custom_index = model.mJoints[i].custom_joint_index;
143  const CustomJoint *custom_joint = model.mCustomJoints[custom_index];
144  unsigned int joint_dof_count = custom_joint->mDoFCount;
145 
146  model.a[i].set(model.a[i] + (model.mCustomJoints[custom_index]->S * (QDDot->block(q_index, 0, joint_dof_count, 1))));
147  }
148  }
149  }
150  }
151 
152  void updateAccelerations(Model &model, const VectorNd& QDDot)
153  {
154  model.a[0].setZero();
155 
156  for(unsigned int i = 1; i < model.mBodies.size(); i++)
157  {
158  unsigned int q_index = model.mJoints[i].q_index;
159  ReferenceFramePtr bodyFrame = model.bodyFrames[i];
160  unsigned int lambda = model.lambda[i];
161 
162  if(lambda != 0)
163  {
164  model.a[i].set(model.a[lambda].transform_copy(bodyFrame->getTransformFromParent()) + model.c[i]);
165  }
166  else
167  {
168  model.a[i].set(model.c[i]);
169  }
170 
171  if(model.mJoints[i].mJointType != JointTypeCustom)
172  {
173  if(model.mJoints[i].mDoFCount == 1)
174  {
175  model.a[i].set(model.a[i] + model.S[i] * QDDot[q_index]);
176  }
177  else if(model.mJoints[i].mDoFCount == 3)
178  {
179  Vector3d omegadot_temp(QDDot[q_index], QDDot[q_index + 1], QDDot[q_index + 2]);
180  model.a[i].set(model.a[i] + model.multdof3_S[i] * omegadot_temp);
181  }
182  }
183  else
184  {
185  unsigned int custom_index = model.mJoints[i].custom_joint_index;
186  const CustomJoint *custom_joint = model.mCustomJoints[custom_index];
187  unsigned int joint_dof_count = custom_joint->mDoFCount;
188 
189  model.a[i].set(model.a[i] + (model.mCustomJoints[custom_index]->S * (QDDot.block(q_index, 0, joint_dof_count, 1))));
190  }
191  }
192  }
193 
194  void updateKinematicsCustomParallel(Model& model, const VectorNd *Q, const VectorNd *QDot,
195  const VectorNd *QDDot)
196  {
197  model.a[0].setZero();
198  if(model.threads.size() == 0)
199  {
200  updateKinematicsCustom(model, Q, QDot, QDDot);
201  }
202  else
203  {
204  std::atomic<unsigned int> branch_ends = 0;
205 
206  model.crawlChainKinematics(0, &branch_ends, Q, QDot, QDDot);
207  while(branch_ends != model.num_branch_ends)
208  {
209 
210  }
211  }
212  }
213 
214  void updateKinematicsParallel(Model& model, const VectorNd& Q, const VectorNd& QDot,
215  const VectorNd& QDDot)
216  {
217  model.a[0].setZero();
218  if(model.threads.size() == 0)
219  {
220  updateKinematics(model, Q, QDot, QDDot);
221  }
222  else
223  {
224  std::atomic<unsigned int> branch_ends = 0;
225 
226  model.crawlChainKinematics(0, &branch_ends, &Q, &QDot, &QDDot);
227  while(branch_ends != model.num_branch_ends)
228  {
229 
230  }
231  }
232  }
233 
234  void calcRelativeBodySpatialJacobian(Model & model, const Math::VectorNd& Q, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame, bool update_kinematics)
235  {
236  ReferenceFramePtr expressedInFrameRef = expressedInFrame==nullptr ? baseFrame : expressedInFrame;
237 
238  if(update_kinematics)
239  {
240  updateKinematicsCustom(model, &Q, NULL, NULL);
241  }
242 
243  unsigned int common_parent_id = model.getCommonMovableParentId(baseFrame->getMovableBodyId(),relativeFrame->getMovableBodyId());
244  unsigned int j = baseFrame->getMovableBodyId();
245 
246  while(j > common_parent_id)
247  {
248  if(model.mJoints[j].mJointType != JointTypeCustom)
249  {
250  if(model.mJoints[j].mDoFCount == 1)
251  {
252  G.col(model.mJoints[j].q_index) = model.S[j].transform_copy(model.bodyFrames[j]->getTransformToDesiredFrame(expressedInFrameRef));
253  }
254  else if(model.mJoints[j].mDoFCount == 3)
255  {
256  for(int k = 0;k<3;k++)
257  {
258  G.col(model.mJoints[j].q_index+k) = MotionVector(model.multdof3_S[j].col(k)).transform_copy(model.bodyFrames[j]->getTransformToDesiredFrame(expressedInFrameRef));
259  }
260  }
261  }
262  else
263  {
264  unsigned int k = model.mJoints[j].custom_joint_index;
265 
266  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) = model.bodyFrames[j]->getTransformToDesiredFrame(expressedInFrameRef).toMatrix() * model.mCustomJoints[k]->S;
267  }
268 
269  j = model.lambda[j];
270  }
271 
272  j = relativeFrame->getMovableBodyId();
273 
274  while(j > common_parent_id)
275  {
276  if(model.mJoints[j].mJointType != JointTypeCustom)
277  {
278  if(model.mJoints[j].mDoFCount == 1)
279  {
280  G.col(model.mJoints[j].q_index) -= model.S[j].transform_copy(model.bodyFrames[j]->getTransformToDesiredFrame(expressedInFrameRef));
281  }
282  else if(model.mJoints[j].mDoFCount == 3)
283  {
284  for(int k = 0;k<3;k++)
285  {
286  G.col(model.mJoints[j].q_index+k) -= MotionVector(model.multdof3_S[j].col(k)).transform_copy(model.bodyFrames[j]->getTransformToDesiredFrame(expressedInFrameRef));
287  }
288  }
289  }
290  else
291  {
292  unsigned int k = model.mJoints[j].custom_joint_index;
293 
294  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) -= model.bodyFrames[j]->getTransformToDesiredFrame(expressedInFrameRef).toMatrix() * model.mCustomJoints[k]->S;
295  }
296 
297  j = model.lambda[j];
298  }
299  }
300 
301  void calcRelativeBodySpatialJacobianDot(Model & model, const Math::VectorNd& Q, const Math::VectorNd& QDot, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame, bool update_kinematics)
302  {
303  assert(G.rows()==6 && G.cols()==model.qdot_size);
304 
305  ReferenceFramePtr expressedInFrameRef = expressedInFrame==nullptr ? baseFrame : expressedInFrame;
306 
307  if(update_kinematics)
308  {
309  updateKinematicsCustom(model, &Q, &QDot, NULL);
310  }
311 
312  unsigned int common_parent_id = model.getCommonMovableParentId(baseFrame->getMovableBodyId(),relativeFrame->getMovableBodyId());
313  unsigned int j = baseFrame->getMovableBodyId();
314 
315  MotionVector v_cur_wrt_new_expr_in_new = calcSpatialVelocity(model,Q,QDot,baseFrame,expressedInFrameRef,expressedInFrameRef,false);
316  while(j != 0)
317  {
318  SpatialTransform X_j_to_expressedInFrame = model.bodyFrames[j]->getTransformToDesiredFrame(expressedInFrameRef);
319  if(model.mJoints[j].mJointType != JointTypeCustom)
320  {
321  if(model.mJoints[j].mDoFCount == 1)
322  {
323  G.col(model.mJoints[j].q_index) = v_cur_wrt_new_expr_in_new%model.S[j].transform_copy(X_j_to_expressedInFrame);
324  // The reason this check is here is bc it's a waste to add this bit on to then subtract it off below, which is
325  // what would be happening. We could add this bit in always, but it would result in a bunch of redundant operations.
326  // Same goes with the loop below, you will see the same check being done.
327  if(j>common_parent_id)
328  {
329  G.col(model.mJoints[j].q_index) += MotionVector(model.S_o[j] + model.v[j]%model.S[j]).transform_copy(X_j_to_expressedInFrame);
330  }
331  }
332  else if(model.mJoints[j].mDoFCount == 3)
333  {
334  for(int k = 0;k<3;k++)
335  {
336  G.col(model.mJoints[j].q_index+k) = v_cur_wrt_new_expr_in_new.crossm()*MotionVector(model.multdof3_S[j].col(k)).transform_copy(X_j_to_expressedInFrame);
337 
338  if(j>common_parent_id)
339  {
340  G.col(model.mJoints[j].q_index+k) += MotionVector(model.multdof3_S_o[j].col(k) + model.v[j].crossm()*model.multdof3_S[j].col(k)).transform_copy(X_j_to_expressedInFrame);
341  }
342  }
343  }
344  }
345  else
346  {
347  unsigned int k = model.mJoints[j].custom_joint_index;
348 
349  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) = v_cur_wrt_new_expr_in_new.crossm()*X_j_to_expressedInFrame.toMatrix()*model.mCustomJoints[k]->S;
350 
351  if(j>common_parent_id)
352  {
353  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) += X_j_to_expressedInFrame.toMatrix() * (model.mCustomJoints[k]->S_o + model.v[j].crossm()*model.mCustomJoints[k]->S);
354  }
355  }
356 
357  j = model.lambda[j];
358  }
359 
360  j = relativeFrame->getMovableBodyId();
361  v_cur_wrt_new_expr_in_new = calcSpatialVelocity(model,Q,QDot,relativeFrame,expressedInFrameRef,expressedInFrameRef,false);
362  while(j != 0)
363  {
364  SpatialTransform X_j_to_expressedInFrame = model.bodyFrames[j]->getTransformToDesiredFrame(expressedInFrameRef);
365  if(model.mJoints[j].mJointType != JointTypeCustom)
366  {
367  if(model.mJoints[j].mDoFCount == 1)
368  {
369  G.col(model.mJoints[j].q_index) -= v_cur_wrt_new_expr_in_new%model.S[j].transform_copy(X_j_to_expressedInFrame);
370  if(j>common_parent_id)
371  {
372  G.col(model.mJoints[j].q_index) -= MotionVector(model.S_o[j] + model.v[j]%model.S[j]).transform_copy(X_j_to_expressedInFrame);
373  }
374  }
375  else if(model.mJoints[j].mDoFCount == 3)
376  {
377  for(int k = 0;k<3;k++)
378  {
379  G.col(model.mJoints[j].q_index+k) -= v_cur_wrt_new_expr_in_new.crossm()*MotionVector(model.multdof3_S[j].col(k)).transform_copy(X_j_to_expressedInFrame);
380  if(j>common_parent_id)
381  {
382  G.col(model.mJoints[j].q_index+k) -= MotionVector(model.multdof3_S_o[j].col(k) + model.v[j].crossm()*model.multdof3_S[j].col(k)).transform_copy(X_j_to_expressedInFrame);
383  }
384  }
385  }
386  }
387  else
388  {
389  unsigned int k = model.mJoints[j].custom_joint_index;
390 
391  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) -= v_cur_wrt_new_expr_in_new.crossm()*X_j_to_expressedInFrame.toMatrix()*model.mCustomJoints[k]->S;
392  if(j>common_parent_id)
393  {
394  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) -= X_j_to_expressedInFrame.toMatrix() * (model.mCustomJoints[k]->S_o + model.v[j].crossm()*model.mCustomJoints[k]->S);
395  }
396  }
397 
398  j = model.lambda[j];
399  }
400  }
401 
402  void calcRelativeBodySpatialJacobianAndJacobianDot(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, Math::MatrixNd& G, Math::MatrixNd& GDot, ReferenceFramePtr baseFrame,
403  ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame, bool update_kinematics)
404  {
405  assert(G.rows()==6 && G.cols()==model.qdot_size && GDot.rows()==6 && GDot.cols()==model.qdot_size);
406  ReferenceFramePtr expressedInFrameRef = expressedInFrame==nullptr ? baseFrame : expressedInFrame;
407 
408  if(update_kinematics)
409  {
410  updateKinematicsCustom(model, &Q, &QDot, NULL);
411  }
412 
413  unsigned int common_parent_id = model.getCommonMovableParentId(baseFrame->getMovableBodyId(),relativeFrame->getMovableBodyId());
414  unsigned int j = baseFrame->getMovableBodyId();
415 
416  MotionVector v_cur_wrt_new_expr_in_new = calcSpatialVelocity(model,Q,QDot,baseFrame,expressedInFrameRef,expressedInFrameRef,false);
417  while(j != 0)
418  {
419  SpatialTransform X_j_to_expressedInFrame = model.bodyFrames[j]->getTransformToDesiredFrame(expressedInFrameRef);
420  if(model.mJoints[j].mJointType != JointTypeCustom)
421  {
422  if(model.mJoints[j].mDoFCount == 1)
423  {
424  GDot.col(model.mJoints[j].q_index) = v_cur_wrt_new_expr_in_new%model.S[j].transform_copy(X_j_to_expressedInFrame);
425  // The reason this check is here is bc it's a waste to add this bit on to then subtract it off below, which is
426  // what would be happening. We could add this bit in always, but it would result in a bunch of redundant operations.
427  // Same goes with the loop below, you will see the same check being done.
428  if(j>common_parent_id)
429  {
430  GDot.col(model.mJoints[j].q_index) += MotionVector(model.S_o[j] + model.v[j]%model.S[j]).transform_copy(X_j_to_expressedInFrame);
431  G.col(model.mJoints[j].q_index) = model.S[j].transform_copy(model.bodyFrames[j]->getTransformToDesiredFrame(expressedInFrameRef));
432  }
433  }
434  else if(model.mJoints[j].mDoFCount == 3)
435  {
436  for(int k = 0;k<3;k++)
437  {
438  GDot.col(model.mJoints[j].q_index+k) = v_cur_wrt_new_expr_in_new.crossm()*MotionVector(model.multdof3_S[j].col(k)).transform_copy(X_j_to_expressedInFrame);
439 
440  if(j>common_parent_id)
441  {
442  GDot.col(model.mJoints[j].q_index+k) += MotionVector(model.multdof3_S_o[j].col(k) + model.v[j].crossm()*model.multdof3_S[j].col(k)).transform_copy(X_j_to_expressedInFrame);
443 
444  G.col(model.mJoints[j].q_index+k) = MotionVector(model.multdof3_S[j].col(k)).transform_copy(model.bodyFrames[j]->getTransformToDesiredFrame(expressedInFrameRef));
445  }
446  }
447  }
448  }
449  else
450  {
451  unsigned int k = model.mJoints[j].custom_joint_index;
452 
453  GDot.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) = v_cur_wrt_new_expr_in_new.crossm()*X_j_to_expressedInFrame.toMatrix()*model.mCustomJoints[k]->S;
454 
455  if(j>common_parent_id)
456  {
457  GDot.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) += X_j_to_expressedInFrame.toMatrix() * (model.mCustomJoints[k]->S_o + model.v[j].crossm()*model.mCustomJoints[k]->S);
458  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) = model.bodyFrames[j]->getTransformToDesiredFrame(expressedInFrameRef).toMatrix() * model.mCustomJoints[k]->S;
459  }
460  }
461 
462  j = model.lambda[j];
463  }
464 
465  j = relativeFrame->getMovableBodyId();
466  v_cur_wrt_new_expr_in_new = calcSpatialVelocity(model,Q,QDot,relativeFrame,expressedInFrameRef,expressedInFrameRef,false);
467  while(j != 0)
468  {
469  SpatialTransform X_j_to_expressedInFrame = model.bodyFrames[j]->getTransformToDesiredFrame(expressedInFrameRef);
470  if(model.mJoints[j].mJointType != JointTypeCustom)
471  {
472  if(model.mJoints[j].mDoFCount == 1)
473  {
474  GDot.col(model.mJoints[j].q_index) -= v_cur_wrt_new_expr_in_new%model.S[j].transform_copy(X_j_to_expressedInFrame);
475  if(j>common_parent_id)
476  {
477  GDot.col(model.mJoints[j].q_index) -= MotionVector(model.S_o[j] + model.v[j]%model.S[j]).transform_copy(X_j_to_expressedInFrame);
478  G.col(model.mJoints[j].q_index) -= model.S[j].transform_copy(model.bodyFrames[j]->getTransformToDesiredFrame(expressedInFrameRef));
479  }
480  }
481  else if(model.mJoints[j].mDoFCount == 3)
482  {
483  for(int k = 0;k<3;k++)
484  {
485  GDot.col(model.mJoints[j].q_index+k) -= v_cur_wrt_new_expr_in_new.crossm()*MotionVector(model.multdof3_S[j].col(k)).transform_copy(X_j_to_expressedInFrame);
486  if(j>common_parent_id)
487  {
488  GDot.col(model.mJoints[j].q_index+k) -= MotionVector(model.multdof3_S_o[j].col(k) + model.v[j].crossm()*model.multdof3_S[j].col(k)).transform_copy(X_j_to_expressedInFrame);
489  G.col(model.mJoints[j].q_index+k) -= MotionVector(model.multdof3_S[j].col(k)).transform_copy(model.bodyFrames[j]->getTransformToDesiredFrame(expressedInFrameRef));
490  }
491  }
492  }
493  }
494  else
495  {
496  unsigned int k = model.mJoints[j].custom_joint_index;
497 
498  GDot.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) -= v_cur_wrt_new_expr_in_new.crossm()*X_j_to_expressedInFrame.toMatrix()*model.mCustomJoints[k]->S;
499  if(j>common_parent_id)
500  {
501  GDot.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) -= X_j_to_expressedInFrame.toMatrix() * (model.mCustomJoints[k]->S_o + model.v[j].crossm()*model.mCustomJoints[k]->S);
502  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) -= model.bodyFrames[j]->getTransformToDesiredFrame(expressedInFrameRef).toMatrix() * model.mCustomJoints[k]->S;
503  }
504  }
505 
506  j = model.lambda[j];
507  }
508  }
509 
510  void calcPointJacobian(Model &model, const VectorNd &Q, unsigned int body_id,
511  const Vector3d &point_position, MatrixNd &G, bool update_kinematics)
512  {
513  // update the Kinematics if necessary
514  if(update_kinematics)
515  {
516  updateKinematicsCustom(model, &Q, NULL, NULL);
517  }
518 
519  unsigned int reference_body_id = body_id;
520 
521  FramePoint p;
522  if(model.IsFixedBodyId(body_id))
523  {
524  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
525  reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
526 
527  p.setIncludingFrame(point_position,model.fixedBodyFrames[fbody_id]);
528  }
529  else
530  {
531  p.setIncludingFrame(point_position,model.bodyFrames[body_id]);
532  }
533 
534  p.changeFrame(model.worldFrame);
535  SpatialTransform point_trans = SpatialTransform(Matrix3d::Identity(3, 3), p.vec());
536 
537  assert (G.rows() == 3 && G.cols() == model.qdot_size);
538 
539  unsigned int j = reference_body_id;
540 
541  while(j != 0)
542  {
543  if(model.mJoints[j].mJointType != JointTypeCustom)
544  {
545  if(model.mJoints[j].mDoFCount == 1)
546  {
547  G.col(model.mJoints[j].q_index) = model.S[j].transform_copy(point_trans*model.bodyFrames[j]->getTransformToRoot()).getLinearPart();
548  }
549  else if(model.mJoints[j].mDoFCount == 3)
550  {
551  G.block(0, model.mJoints[j].q_index, 3, 3) = ((point_trans * model.bodyFrames[j]->getTransformToRoot()).toMatrix() * model.multdof3_S[j]).block(3, 0, 3, 3);
552  }
553  }
554  else
555  {
556  unsigned int k = model.mJoints[j].custom_joint_index;
557 
558  G.block(0, model.mJoints[j].q_index, 3, model.mCustomJoints[k]->mDoFCount) = ((point_trans * model.bodyFrames[j]->getTransformToRoot()).toMatrix() * model.mCustomJoints[k]->S).block(3, 0, 3, model.mCustomJoints[k]->mDoFCount);
559  }
560 
561  j = model.lambda[j];
562  }
563  }
564 
565  void calcPointJacobian(Model & model, const Math::VectorNd& Q, Math::MatrixNd & G, ReferenceFramePtr frame,
566  bool update_kinematics)
567  {
568  // update the Kinematics if necessary
569  if(update_kinematics)
570  {
571  updateKinematicsCustom(model, &Q, NULL, NULL);
572  }
573 
574  SpatialTransform point_trans = SpatialTransform(Matrix3d::Identity(3, 3), frame->getInverseTransformToRoot().r);
575 
576  assert (G.rows() == 3 && G.cols() == model.qdot_size);
577 
578  unsigned int j = frame->getMovableBodyId();
579 
580  while(j != 0)
581  {
582  if(model.mJoints[j].mJointType != JointTypeCustom)
583  {
584  if(model.mJoints[j].mDoFCount == 1)
585  {
586  G.col(model.mJoints[j].q_index) = model.S[j].transform_copy(point_trans*model.bodyFrames[j]->getTransformToRoot()).getLinearPart();
587  }
588  else if(model.mJoints[j].mDoFCount == 3)
589  {
590  G.block(0, model.mJoints[j].q_index, 3, 3) = ((point_trans * model.bodyFrames[j]->getTransformToRoot()).toMatrix() * model.multdof3_S[j]).block(3, 0, 3, 3);
591  }
592  }
593  else
594  {
595  unsigned int k = model.mJoints[j].custom_joint_index;
596 
597  G.block(0, model.mJoints[j].q_index, 3, model.mCustomJoints[k]->mDoFCount) = ((point_trans * model.bodyFrames[j]->getTransformToRoot()).toMatrix() * model.mCustomJoints[k]->S).block(3, 0, 3, model.mCustomJoints[k]->mDoFCount);
598  }
599 
600  j = model.lambda[j];
601  }
602  }
603 
604  void calcPointJacobian6D(Model & model, const Math::VectorNd& Q, Math::MatrixNd & G, ReferenceFramePtr frame,
605  bool update_kinematics)
606  {
607  // update the Kinematics if necessary
608  if(update_kinematics)
609  {
610  updateKinematicsCustom(model, &Q, NULL, NULL);
611  }
612 
613  SpatialTransform point_trans = SpatialTransform(Matrix3d::Identity(3, 3), frame->getInverseTransformToRoot().r);
614 
615  assert (G.rows() == 6 && G.cols() == model.qdot_size);
616 
617  unsigned int j = frame->getMovableBodyId();
618 
619  while(j != 0)
620  {
621  if(model.mJoints[j].mJointType != JointTypeCustom)
622  {
623  if(model.mJoints[j].mDoFCount == 1)
624  {
625  G.col(model.mJoints[j].q_index) = model.S[j].transform_copy(point_trans*model.bodyFrames[j]->getTransformToRoot());
626  }
627  else if(model.mJoints[j].mDoFCount == 3)
628  {
629  G.block(0, model.mJoints[j].q_index, 6, 3) = ((point_trans * model.bodyFrames[j]->getTransformToRoot()).toMatrix() * model.multdof3_S[j]);
630  }
631  }
632  else
633  {
634  unsigned int k = model.mJoints[j].custom_joint_index;
635 
636  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) = ((point_trans * model.bodyFrames[j]->getTransformToRoot()).toMatrix() * model.mCustomJoints[k]->S);
637  }
638 
639  j = model.lambda[j];
640  }
641  }
642 
643  void calcRelativePointJacobian6D(Model & model,
644  const Math::VectorNd& Q,
645  Math::MatrixNd & G,
646  ReferenceFramePtr baseFrame,
647  ReferenceFramePtr relativeFrame,
648  ReferenceFramePtr expressedInFrame,
649  bool update_kinematics)
650  {
651  assert(baseFrame!=nullptr && relativeFrame!=nullptr && expressedInFrame!=nullptr);
652  assert(G.rows()==6 && G.cols()==model.qdot_size);
653  // update the Kinematics if necessary
654  if(update_kinematics)
655  {
656  updateKinematicsCustom(model, &Q, NULL, NULL);
657  }
658 
659  assert(G.rows()==6 && G.cols() == model.qdot_size);
660 
661  unsigned int common_parent_id = model.getCommonMovableParentId(baseFrame->getMovableBodyId(),relativeFrame->getMovableBodyId());
662 
663  SpatialTransform X_rot = model.worldFrame->getTransformToDesiredFrame(expressedInFrame);
664  X_rot.r.setZero();
665 
666  SpatialTransform point_trans = X_rot*Xtrans(baseFrame->getInverseTransformToRoot().r);
667  unsigned int j = baseFrame->getMovableBodyId();
668  while(j > common_parent_id)
669  {
670  if(model.mJoints[j].mJointType != JointTypeCustom)
671  {
672  if(model.mJoints[j].mDoFCount == 1)
673  {
674  G.col(model.mJoints[j].q_index) = model.S[j].transform_copy(point_trans*model.bodyFrames[j]->getTransformToRoot());
675  }
676  else if(model.mJoints[j].mDoFCount == 3)
677  {
678  G.block(0, model.mJoints[j].q_index, 6, 3) = ((point_trans*model.bodyFrames[j]->getTransformToRoot()).toMatrix() * model.multdof3_S[j]);
679  }
680  }
681  else
682  {
683  unsigned int k = model.mJoints[j].custom_joint_index;
684 
685  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) = ((point_trans*model.bodyFrames[j]->getTransformToRoot()).toMatrix() * model.mCustomJoints[k]->S);
686  }
687 
688  j = model.lambda[j];
689  }
690 
691  point_trans = X_rot*Xtrans(relativeFrame->getInverseTransformToRoot().r);
692  j = relativeFrame->getMovableBodyId();
693 
694  while(j > common_parent_id)
695  {
696  if(model.mJoints[j].mJointType != JointTypeCustom)
697  {
698  if(model.mJoints[j].mDoFCount == 1)
699  {
700  G.col(model.mJoints[j].q_index) = -model.S[j].transform_copy(point_trans*model.bodyFrames[j]->getTransformToRoot());
701  }
702  else if(model.mJoints[j].mDoFCount == 3)
703  {
704  G.block(0, model.mJoints[j].q_index, 6, 3) = -((point_trans*model.bodyFrames[j]->getTransformToRoot()).toMatrix() * model.multdof3_S[j]);
705  }
706  }
707  else
708  {
709  unsigned int k = model.mJoints[j].custom_joint_index;
710 
711  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) = -((point_trans*model.bodyFrames[j]->getTransformToRoot()).toMatrix() * model.mCustomJoints[k]->S);
712  }
713 
714  j = model.lambda[j];
715  }
716 
722  RobotDynamics::Math::SpatialMatrix matrix_trans;
723  matrix_trans.block<3,3>(3,0) = toTildeForm(-baseFrame->getInverseTransformToRoot().r+relativeFrame->getInverseTransformToRoot().r);
724  RobotDynamics::Math::SpatialMatrix expr_X_world = X_rot.toMatrix();
725  j = common_parent_id;
726 
727  while(j!=0)
728  {
729  if(model.mJoints[j].mJointType != JointTypeCustom)
730  {
731  if(model.mJoints[j].mDoFCount == 1)
732  {
733  G.col(model.mJoints[j].q_index) = expr_X_world*matrix_trans*model.bodyFrames[j]->getTransformToRoot().toMatrix()*model.S[j];
734  }
735  else if(model.mJoints[j].mDoFCount == 3)
736  {
737  G.block(0, model.mJoints[j].q_index, 6, 3) = expr_X_world*matrix_trans*model.bodyFrames[j]->getTransformToRoot().toMatrix() * model.multdof3_S[j];
738  }
739  }
740  else
741  {
742  unsigned int k = model.mJoints[j].custom_joint_index;
743 
744  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) = expr_X_world*matrix_trans*model.bodyFrames[j]->getTransformToRoot().toMatrix() * model.mCustomJoints[k]->S;
745  }
746 
747  j = model.lambda[j];
748  }
749  }
750 
751  void calcPointJacobianDot(Model &model, const VectorNd &Q, const VectorNd &QDot, unsigned int body_id,
752  const Vector3d &point_position, MatrixNd &G, bool update_kinematics)
753  {
754  assert (G.rows() == 3 && G.cols() == model.qdot_size);
755 
756  MatrixNd GDot6D = MatrixNd::Constant(6,model.qdot_size,0.);
757  calcPointJacobianDot6D(model,Q,QDot,body_id,point_position,GDot6D,update_kinematics);
758 
759  G = GDot6D.block(3,0,3,model.qdot_size);
760  }
761 
762  void calcPointJacobian6D(Model &model, const VectorNd &Q, unsigned int body_id,
763  const Vector3d &point_position, MatrixNd &G, bool update_kinematics)
764  {
765  // update the Kinematics if necessary
766  if(update_kinematics)
767  {
768  updateKinematicsCustom(model, &Q, nullptr, nullptr);
769  }
770 
771  Math::FramePoint p;
772 
773  unsigned int reference_body_id = body_id;
774 
775  if(model.IsFixedBodyId(body_id))
776  {
777  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
778  reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
779 
780  p.setIncludingFrame(point_position,model.fixedBodyFrames[fbody_id]);
781  }
782  else
783  {
784  p.setIncludingFrame(point_position,model.bodyFrames[body_id]);
785  }
786 
787  p.changeFrame(ReferenceFrame::getWorldFrame());
788  SpatialTransform point_trans(Matrix3dIdentity,p.vec());
789 
790  assert (G.rows() == 6 && G.cols() == model.qdot_size);
791 
792  unsigned int j = reference_body_id;
793 
794  MotionVector m_vec;
795  while(j != 0)
796  {
797  if(model.mJoints[j].mJointType != JointTypeCustom)
798  {
799  if(model.mJoints[j].mDoFCount == 1)
800  {
801  G.col(model.mJoints[j].q_index) = model.S[j].transform_copy(model.bodyFrames[j]->getTransformToRoot()).transform_copy(point_trans);
802  }
803  else if(model.mJoints[j].mDoFCount == 3)
804  {
805  ReferenceFramePtr frame = model.bodyFrames[j];
806  Matrix63 m = model.multdof3_S[j];
807  for(int k = 0;k<3;k++)
808  {
809  m_vec = m.col(k);
810  m_vec.transform(frame->getTransformToRoot());
811  G.col(model.mJoints[j].q_index+k) = m_vec.transform_copy(point_trans);
812  }
813  }
814  }
815  else
816  {
817  unsigned int k = model.mJoints[j].custom_joint_index;
818 
819  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) = ((point_trans * model.bodyFrames[j]->getTransformToRoot()).toMatrix() * model.mCustomJoints[k]->S).block(0, 0, 6, model.mCustomJoints[k]->mDoFCount);
820  }
821 
822  j = model.lambda[j];
823  }
824  }
825 
826  void calcPointJacobianDot6D(Model & model, const Math::VectorNd &Q, const Math::VectorNd &QDot,
827  RobotDynamics::ReferenceFramePtr frame, Math::MatrixNd &G,bool update_kinematics)
828  {
829  // update the Kinematics if necessary
830  if(update_kinematics)
831  {
832  updateKinematicsCustom(model, &Q, &QDot, nullptr);
833  }
834 
835  SpatialTransform point_trans(Matrix3dIdentity,frame->getInverseTransformToRoot().r);
836  SpatialMotion v = model.v[frame->getMovableBodyId()];
837  v.changeFrame(model.worldFrame);
838 
839  assert (G.rows() == 6 && G.cols() == model.qdot_size);
840 
841  unsigned int j = frame->getMovableBodyId();
842 
843  while(j != 0)
844  {
845  SpatialTransform X_tmp = point_trans*model.bodyFrames[j]->getTransformToRoot();
846  if(model.mJoints[j].mJointType != JointTypeCustom)
847  {
848  if(model.mJoints[j].mDoFCount == 1)
849  {
850  G.block<3,1>(3,model.mJoints[j].q_index) = v.getAngularPart().cross(model.S[j].transform_copy(X_tmp).getLinearPart());
851  G.col(model.mJoints[j].q_index) += MotionVector(model.S_o[j] + model.v[j]%model.S[j]).transform_copy(X_tmp);
852  }
853  else if(model.mJoints[j].mDoFCount == 3)
854  {
855  for(int k = 0;k<3;k++)
856  {
857  G.block<3,1>(3,model.mJoints[j].q_index+k) = v.getAngularPart().cross(MotionVector(model.multdof3_S[j].col(k)).transform_copy(X_tmp).getLinearPart());
858  G.col(model.mJoints[j].q_index+k) += MotionVector(model.multdof3_S_o[j].col(k) + model.v[j].crossm()*model.multdof3_S[j].col(k)).transform_copy(X_tmp);
859  }
860  }
861  }
862  else
863  {
864  unsigned int k = model.mJoints[j].custom_joint_index;
865 
866  for(unsigned int l = 0; l<model.mCustomJoints[k]->mDoFCount;l++)
867  {
868  G.block<3,1>(3,model.mJoints[j].q_index+l) = v.getAngularPart().cross(MotionVector(model.mCustomJoints[k]->S.col(l)).transform_copy(X_tmp).getLinearPart());
869  G.col(model.mJoints[j].q_index+l) += X_tmp.toMatrix() * (model.mCustomJoints[k]->S_o.col(l) + model.v[j].crossm()*model.mCustomJoints[k]->S.col(l));
870  }
871  }
872 
873  j = model.lambda[j];
874  }
875  }
876 
877  void calcRelativePointJacobianDot6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, Math::MatrixNd& G,
878  ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame, bool update_kinematics)
879  {
880  assert(baseFrame && relativeFrame && expressedInFrame);
881  assert (G.rows() == 6 && G.cols() == model.qdot_size);
882  // update the Kinematics if necessary
883  if(update_kinematics)
884  {
885  updateKinematicsCustom(model, &Q, &QDot, nullptr);
886  }
887 
888  SpatialTransform X_rot_to_expressed_in_frame = model.worldFrame->getTransformToDesiredFrame(expressedInFrame);
889  // Set r to zero bc it's two 3d vector transforms, not a spatial vector transform
890  X_rot_to_expressed_in_frame.r.setZero();
891 
892  SpatialTransform base_point_trans(Matrix3dIdentity,baseFrame->getInverseTransformToRoot().r);
893  SpatialMotion v_base = model.v[baseFrame->getMovableBodyId()];
894  v_base.changeFrame(model.worldFrame);
895 
896  unsigned int j = baseFrame->getMovableBodyId();
897  unsigned int common_parent_id = model.getCommonMovableParentId(baseFrame->getMovableBodyId(),relativeFrame->getMovableBodyId());
898  while(j > common_parent_id)
899  {
900  SpatialTransform X_tmp = base_point_trans*model.bodyFrames[j]->getTransformToRoot();
901  if(model.mJoints[j].mJointType != JointTypeCustom)
902  {
903  if(model.mJoints[j].mDoFCount == 1)
904  {
905  G.block<3,1>(3,model.mJoints[j].q_index) = v_base.getAngularPart().cross(model.S[j].transform_copy(X_tmp).getLinearPart());
906  G.col(model.mJoints[j].q_index) += MotionVector(model.S_o[j] + model.v[j]%model.S[j]).transform_copy(X_tmp);
907  G.col(model.mJoints[j].q_index) = X_rot_to_expressed_in_frame.toMatrix()*G.col(model.mJoints[j].q_index);
908  }
909  else if(model.mJoints[j].mDoFCount == 3)
910  {
911  for(int k = 0;k<3;k++)
912  {
913  G.block<3,1>(3,model.mJoints[j].q_index+k) = v_base.getAngularPart().cross(MotionVector(model.multdof3_S[j].col(k)).transform_copy(X_tmp).getLinearPart());
914  G.col(model.mJoints[j].q_index+k) += MotionVector(model.multdof3_S_o[j].col(k) + model.v[j]%MotionVector(model.multdof3_S[j].col(k))).transform_copy(X_tmp);
915  }
916 
917  G.block<6,3>(0,model.mJoints[j].q_index) = X_rot_to_expressed_in_frame.toMatrix()*G.block<6,3>(0,model.mJoints[j].q_index);
918  }
919  }
920  else
921  {
922  unsigned int k = model.mJoints[j].custom_joint_index;
923 
924  for(unsigned int l = 0; l<model.mCustomJoints[k]->mDoFCount;l++)
925  {
926  G.block<3,1>(3,model.mJoints[j].q_index+l) = v_base.getAngularPart().cross(MotionVector(model.mCustomJoints[k]->S.col(l)).transform_copy(X_tmp).getLinearPart());
927  G.col(model.mJoints[j].q_index+l) += X_tmp.toMatrix() * (model.mCustomJoints[k]->S_o.col(l) + model.v[j]%MotionVector(model.mCustomJoints[k]->S.col(l)));
928  G.col(model.mJoints[j].q_index+l) = X_rot_to_expressed_in_frame.toMatrix()*G.col(model.mJoints[j].q_index+l);
929  }
930  }
931 
932  j = model.lambda[j];
933  }
934 
935  SpatialTransform relative_point_trans = SpatialTransform(Matrix3dIdentity,relativeFrame->getInverseTransformToRoot().r);
936  SpatialMotion v_relative = model.v[relativeFrame->getMovableBodyId()];
937  v_relative.changeFrame(model.worldFrame);
938 
939  j = relativeFrame->getMovableBodyId();
940 
944  while(j > common_parent_id)
945  {
946  SpatialTransform X_tmp = relative_point_trans*model.bodyFrames[j]->getTransformToRoot();
947  if(model.mJoints[j].mJointType != JointTypeCustom)
948  {
949  if(model.mJoints[j].mDoFCount == 1)
950  {
951  G.block<3,1>(3,model.mJoints[j].q_index) = -v_relative.getAngularPart().cross(model.S[j].transform_copy(X_tmp).getLinearPart());
952  G.col(model.mJoints[j].q_index) -= MotionVector(model.S_o[j] + model.v[j]%model.S[j]).transform_copy(X_tmp);
953  G.col(model.mJoints[j].q_index) = X_rot_to_expressed_in_frame.toMatrix()*G.col(model.mJoints[j].q_index);
954  }
955  else if(model.mJoints[j].mDoFCount == 3)
956  {
957  for(int k = 0;k<3;k++)
958  {
959  G.block<3,1>(3,model.mJoints[j].q_index+k) = -v_relative.getAngularPart().cross(MotionVector(model.multdof3_S[j].col(k)).transform_copy(X_tmp).getLinearPart());
960  G.col(model.mJoints[j].q_index+k) -= MotionVector(model.multdof3_S_o[j].col(k) + model.v[j]%MotionVector(model.multdof3_S[j].col(k))).transform_copy(X_tmp);
961  }
962 
963  G.block<6,3>(0,model.mJoints[j].q_index) = X_rot_to_expressed_in_frame.toMatrix()*G.block<6,3>(0,model.mJoints[j].q_index);
964  }
965  }
966  else
967  {
968  unsigned int k = model.mJoints[j].custom_joint_index;
969 
970  for(unsigned int l = 0; l<model.mCustomJoints[k]->mDoFCount;l++)
971  {
972  G.block<3,1>(3,model.mJoints[j].q_index+l) = -v_relative.getAngularPart().cross(MotionVector(model.mCustomJoints[k]->S.col(l)).transform_copy(X_tmp).getLinearPart());
973  G.col(model.mJoints[j].q_index+l) -= X_tmp.toMatrix() * (model.mCustomJoints[k]->S_o.col(l) + model.v[j]%MotionVector(model.mCustomJoints[k]->S.col(l)));
974  G.col(model.mJoints[j].q_index+l) = X_rot_to_expressed_in_frame.toMatrix()*G.col(model.mJoints[j].q_index+l);
975  }
976  }
977 
978  j = model.lambda[j];
979  }
980 
981  j = common_parent_id;
982 
986  SpatialMatrix X;
987  X.block<3,3>(3,0) = toTildeForm(v_base.getAngularPart())*base_point_trans.toMatrix().block<3,3>(3,0) -
988  toTildeForm(v_relative.getAngularPart())*relative_point_trans.toMatrix().block<3,3>(3,0);
989  X.block<3,3>(3,3) = toTildeForm(v_base.getAngularPart()) - toTildeForm(v_relative.getAngularPart());
990  SpatialMatrix X_tmp;
991  while(j != 0)
992  {
997  X_tmp = (base_point_trans.toMatrix()-relative_point_trans.toMatrix())*model.bodyFrames[j]->getTransformToRoot().toMatrix();
998  if(model.mJoints[j].mJointType != JointTypeCustom)
999  {
1000  if(model.mJoints[j].mDoFCount == 1)
1001  {
1002  G.block<3,1>(3,model.mJoints[j].q_index) = (X*model.S[j].transform_copy(model.bodyFrames[j]->getTransformToRoot())).block<3,1>(3,0);
1003  G.col(model.mJoints[j].q_index) += X_tmp*MotionVector(model.S_o[j] + model.v[j]%model.S[j]);
1004  G.col(model.mJoints[j].q_index) = X_rot_to_expressed_in_frame.toMatrix()*G.col(model.mJoints[j].q_index);
1005  }
1006  else if(model.mJoints[j].mDoFCount == 3)
1007  {
1008  for(int k = 0;k<3;k++)
1009  {
1010  G.block<3,1>(3,model.mJoints[j].q_index+k) = (X*model.bodyFrames[j]->getTransformToRoot().toMatrix()*MotionVector(model.multdof3_S[j].col(k))).block<3,1>(3,0);
1011  G.col(model.mJoints[j].q_index+k) += X_tmp*MotionVector(model.multdof3_S_o[j].col(k) + model.v[j]%MotionVector(model.multdof3_S[j].col(k)));
1012  }
1013 
1014  G.block<6,3>(0,model.mJoints[j].q_index) = X_rot_to_expressed_in_frame.toMatrix()*G.block<6,3>(0,model.mJoints[j].q_index);
1015  }
1016  }
1017  else
1018  {
1019  unsigned int k = model.mJoints[j].custom_joint_index;
1020 
1021  for(unsigned int l = 0; l<model.mCustomJoints[k]->mDoFCount;l++)
1022  {
1023  G.block<3,1>(3,model.mJoints[j].q_index+l) = (X*model.bodyFrames[j]->getTransformToRoot().toMatrix()*MotionVector(model.mCustomJoints[k]->S.col(l))).block<3,1>(3,0);
1024  G.col(model.mJoints[j].q_index+l) += X_tmp * (model.mCustomJoints[k]->S_o.col(l) + model.v[j]%MotionVector(model.mCustomJoints[k]->S.col(l)));
1025  G.col(model.mJoints[j].q_index+l) = X_rot_to_expressed_in_frame.toMatrix()*G.col(model.mJoints[j].q_index+l);
1026  }
1027  }
1028 
1029  j = model.lambda[j];
1030  }
1031  }
1032 
1033 void calcRelativePointJacobianAndJacobianDot6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, Math::MatrixNd& G, Math::MatrixNd& GDot, ReferenceFramePtr baseFrame,
1034  ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame, bool update_kinematics)
1035 {
1036  assert(baseFrame && relativeFrame && expressedInFrame);
1037  assert (GDot.rows() == 6 && GDot.cols() == model.qdot_size);
1038  assert (G.rows() == 6 && G.cols() == model.qdot_size);
1039  // update the Kinematics if necessary
1040  if(update_kinematics)
1041  {
1042  updateKinematicsCustom(model, &Q, &QDot, nullptr);
1043  }
1044 
1045  SpatialTransform X_rot_to_expressed_in_frame = expressedInFrame->getInverseTransformToRoot();
1046  // Set r to zero bc it's two 3d vector transforms, not a spatial vector transform
1047  X_rot_to_expressed_in_frame.r.setZero();
1048 
1049  SpatialTransform base_point_trans(Matrix3dIdentity,baseFrame->getInverseTransformToRoot().r);
1050  SpatialMotion v_base = model.v[baseFrame->getMovableBodyId()];
1051  v_base.changeFrame(model.worldFrame);
1052 
1053  SpatialTransform point_trans = X_rot_to_expressed_in_frame*base_point_trans;
1054 
1055  unsigned int j = baseFrame->getMovableBodyId();
1056  unsigned int common_parent_id = model.getCommonMovableParentId(baseFrame->getMovableBodyId(),relativeFrame->getMovableBodyId());
1057  while(j > common_parent_id)
1058  {
1059  SpatialTransform X_tmp = base_point_trans*model.bodyFrames[j]->getTransformToRoot();
1060  if(model.mJoints[j].mJointType != JointTypeCustom)
1061  {
1062  if(model.mJoints[j].mDoFCount == 1)
1063  {
1064  G.col(model.mJoints[j].q_index) = model.S[j].transform_copy(point_trans*model.bodyFrames[j]->getTransformToRoot());
1065 
1066  GDot.block<3,1>(3,model.mJoints[j].q_index) = v_base.getAngularPart().cross(model.S[j].transform_copy(X_tmp).getLinearPart());
1067  GDot.col(model.mJoints[j].q_index) += MotionVector(model.S_o[j] + model.v[j]%model.S[j]).transform_copy(X_tmp);
1068  GDot.col(model.mJoints[j].q_index) = X_rot_to_expressed_in_frame.toMatrix()*GDot.col(model.mJoints[j].q_index);
1069  }
1070  else if(model.mJoints[j].mDoFCount == 3)
1071  {
1072  G.block(0, model.mJoints[j].q_index, 6, 3) = ((point_trans*model.bodyFrames[j]->getTransformToRoot()).toMatrix() * model.multdof3_S[j]);
1073 
1074  for(int k = 0;k<3;k++)
1075  {
1076  GDot.block<3,1>(3,model.mJoints[j].q_index+k) = v_base.getAngularPart().cross(MotionVector(model.multdof3_S[j].col(k)).transform_copy(X_tmp).getLinearPart());
1077  GDot.col(model.mJoints[j].q_index+k) += MotionVector(model.multdof3_S_o[j].col(k) + model.v[j]%MotionVector(model.multdof3_S[j].col(k))).transform_copy(X_tmp);
1078  }
1079 
1080  GDot.block<6,3>(0,model.mJoints[j].q_index) = X_rot_to_expressed_in_frame.toMatrix()*GDot.block<6,3>(0,model.mJoints[j].q_index);
1081  }
1082  }
1083  else
1084  {
1085  unsigned int k = model.mJoints[j].custom_joint_index;
1086 
1087  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) = ((point_trans*model.bodyFrames[j]->getTransformToRoot()).toMatrix() * model.mCustomJoints[k]->S);
1088 
1089  for(unsigned int l = 0; l<model.mCustomJoints[k]->mDoFCount;l++)
1090  {
1091  GDot.block<3,1>(3,model.mJoints[j].q_index+l) = v_base.getAngularPart().cross(MotionVector(model.mCustomJoints[k]->S.col(l)).transform_copy(X_tmp).getLinearPart());
1092  GDot.col(model.mJoints[j].q_index+l) += X_tmp.toMatrix() * (model.mCustomJoints[k]->S_o.col(l) + model.v[j]%MotionVector(model.mCustomJoints[k]->S.col(l)));
1093  GDot.col(model.mJoints[j].q_index+l) = X_rot_to_expressed_in_frame.toMatrix()*GDot.col(model.mJoints[j].q_index+l);
1094  }
1095  }
1096 
1097  j = model.lambda[j];
1098  }
1099 
1100  SpatialTransform relative_point_trans = SpatialTransform(Matrix3dIdentity,relativeFrame->getInverseTransformToRoot().r);
1101  SpatialMotion v_relative = model.v[relativeFrame->getMovableBodyId()];
1102  v_relative.changeFrame(model.worldFrame);
1103 
1104  point_trans = X_rot_to_expressed_in_frame*relative_point_trans;
1105 
1106  j = relativeFrame->getMovableBodyId();
1107 
1111  while(j > common_parent_id)
1112  {
1113  SpatialTransform X_tmp = relative_point_trans*model.bodyFrames[j]->getTransformToRoot();
1114  if(model.mJoints[j].mJointType != JointTypeCustom)
1115  {
1116  if(model.mJoints[j].mDoFCount == 1)
1117  {
1118  G.col(model.mJoints[j].q_index) = -model.S[j].transform_copy(point_trans*model.bodyFrames[j]->getTransformToRoot());
1119 
1120  GDot.block<3,1>(3,model.mJoints[j].q_index) = -v_relative.getAngularPart().cross(model.S[j].transform_copy(X_tmp).getLinearPart());
1121  GDot.col(model.mJoints[j].q_index) -= MotionVector(model.S_o[j] + model.v[j]%model.S[j]).transform_copy(X_tmp);
1122  GDot.col(model.mJoints[j].q_index) = X_rot_to_expressed_in_frame.toMatrix()*GDot.col(model.mJoints[j].q_index);
1123  }
1124  else if(model.mJoints[j].mDoFCount == 3)
1125  {
1126  G.block(0, model.mJoints[j].q_index, 6, 3) = -((point_trans*model.bodyFrames[j]->getTransformToRoot()).toMatrix() * model.multdof3_S[j]);
1127 
1128  for(int k = 0;k<3;k++)
1129  {
1130  GDot.block<3,1>(3,model.mJoints[j].q_index+k) = -v_relative.getAngularPart().cross(MotionVector(model.multdof3_S[j].col(k)).transform_copy(X_tmp).getLinearPart());
1131  GDot.col(model.mJoints[j].q_index+k) -= MotionVector(model.multdof3_S_o[j].col(k) + model.v[j]%MotionVector(model.multdof3_S[j].col(k))).transform_copy(X_tmp);
1132  }
1133 
1134  GDot.block<6,3>(0,model.mJoints[j].q_index) = X_rot_to_expressed_in_frame.toMatrix()*GDot.block<6,3>(0,model.mJoints[j].q_index);
1135  }
1136  }
1137  else
1138  {
1139  unsigned int k = model.mJoints[j].custom_joint_index;
1140 
1141  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) = -((point_trans*model.bodyFrames[j]->getTransformToRoot()).toMatrix() * model.mCustomJoints[k]->S);
1142 
1143  for(unsigned int l = 0; l<model.mCustomJoints[k]->mDoFCount;l++)
1144  {
1145  GDot.block<3,1>(3,model.mJoints[j].q_index+l) = -v_relative.getAngularPart().cross(MotionVector(model.mCustomJoints[k]->S.col(l)).transform_copy(X_tmp).getLinearPart());
1146  GDot.col(model.mJoints[j].q_index+l) -= X_tmp.toMatrix() * (model.mCustomJoints[k]->S_o.col(l) + model.v[j]%MotionVector(model.mCustomJoints[k]->S.col(l)));
1147  GDot.col(model.mJoints[j].q_index+l) = X_rot_to_expressed_in_frame.toMatrix()*GDot.col(model.mJoints[j].q_index+l);
1148  }
1149  }
1150 
1151  j = model.lambda[j];
1152  }
1153 
1157  SpatialMatrix X;
1158  X.block<3,3>(3,0) = toTildeForm(v_base.getAngularPart())*base_point_trans.toMatrix().block<3,3>(3,0) -
1159  toTildeForm(v_relative.getAngularPart())*relative_point_trans.toMatrix().block<3,3>(3,0);
1160  X.block<3,3>(3,3) = toTildeForm(v_base.getAngularPart()) - toTildeForm(v_relative.getAngularPart());
1161  SpatialMatrix X_tmp;
1162 
1168  RobotDynamics::Math::SpatialMatrix matrix_trans;
1169  matrix_trans.block<3,3>(3,0) = toTildeForm(-baseFrame->getInverseTransformToRoot().r+relativeFrame->getInverseTransformToRoot().r);
1170  RobotDynamics::Math::SpatialMatrix expr_X_world = X_rot_to_expressed_in_frame.toMatrix();
1171 
1172  j = common_parent_id;
1173  while(j != 0)
1174  {
1179  X_tmp = (base_point_trans.toMatrix()-relative_point_trans.toMatrix())*model.bodyFrames[j]->getTransformToRoot().toMatrix();
1180  if(model.mJoints[j].mJointType != JointTypeCustom)
1181  {
1182  if(model.mJoints[j].mDoFCount == 1)
1183  {
1184  G.col(model.mJoints[j].q_index) = expr_X_world*matrix_trans*model.bodyFrames[j]->getTransformToRoot().toMatrix()*model.S[j];
1185 
1186  GDot.block<3,1>(3,model.mJoints[j].q_index) = (X*model.S[j].transform_copy(model.bodyFrames[j]->getTransformToRoot())).block<3,1>(3,0);
1187  GDot.col(model.mJoints[j].q_index) += X_tmp*MotionVector(model.S_o[j] + model.v[j]%model.S[j]);
1188  GDot.col(model.mJoints[j].q_index) = X_rot_to_expressed_in_frame.toMatrix()*GDot.col(model.mJoints[j].q_index);
1189  }
1190  else if(model.mJoints[j].mDoFCount == 3)
1191  {
1192  G.block(0, model.mJoints[j].q_index, 6, 3) = expr_X_world*matrix_trans*model.bodyFrames[j]->getTransformToRoot().toMatrix() * model.multdof3_S[j];
1193 
1194  for(int k = 0;k<3;k++)
1195  {
1196  GDot.block<3,1>(3,model.mJoints[j].q_index+k) = (X*model.bodyFrames[j]->getTransformToRoot().toMatrix()*MotionVector(model.multdof3_S[j].col(k))).block<3,1>(3,0);
1197  GDot.col(model.mJoints[j].q_index+k) += X_tmp*MotionVector(model.multdof3_S_o[j].col(k) + model.v[j]%MotionVector(model.multdof3_S[j].col(k)));
1198  }
1199 
1200  GDot.block<6,3>(0,model.mJoints[j].q_index) = X_rot_to_expressed_in_frame.toMatrix()*GDot.block<6,3>(0,model.mJoints[j].q_index);
1201  }
1202  }
1203  else
1204  {
1205  unsigned int k = model.mJoints[j].custom_joint_index;
1206 
1207  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) = expr_X_world*matrix_trans*model.bodyFrames[j]->getTransformToRoot().toMatrix() * model.mCustomJoints[k]->S;
1208 
1209  for(unsigned int l = 0; l<model.mCustomJoints[k]->mDoFCount;l++)
1210  {
1211  GDot.block<3,1>(3,model.mJoints[j].q_index+l) = (X*model.bodyFrames[j]->getTransformToRoot().toMatrix()*MotionVector(model.mCustomJoints[k]->S.col(l))).block<3,1>(3,0);
1212  GDot.col(model.mJoints[j].q_index+l) += X_tmp * (model.mCustomJoints[k]->S_o.col(l) + model.v[j]%MotionVector(model.mCustomJoints[k]->S.col(l)));
1213  GDot.col(model.mJoints[j].q_index+l) = X_rot_to_expressed_in_frame.toMatrix()*GDot.col(model.mJoints[j].q_index+l);
1214  }
1215  }
1216 
1217  j = model.lambda[j];
1218  }
1219 }
1220 
1221  void calcPointJacobianDot(Model & model, const Math::VectorNd &Q, const Math::VectorNd &QDot,
1222  RobotDynamics::ReferenceFramePtr frame, Math::MatrixNd &G,bool update_kinematics)
1223  {
1224  assert (G.rows() == 3 && G.cols() == model.qdot_size);
1225 
1226  MatrixNd GDot6D = MatrixNd::Constant(6,model.qdot_size,0.);
1227  calcPointJacobianDot6D(model,Q,QDot,frame,GDot6D,update_kinematics);
1228 
1229  G = GDot6D.block(3,0,3,model.qdot_size);
1230  }
1231 
1232  void calcPointJacobianDot6D(Model &model, const VectorNd &Q, const VectorNd &QDot, unsigned int body_id,
1233  const Vector3d &point_position, MatrixNd &G, bool update_kinematics)
1234  {
1235  // update the Kinematics if necessary
1236  if(update_kinematics)
1237  {
1238  updateKinematicsCustom(model, &Q, &QDot, nullptr);
1239  }
1240 
1241  Math::FramePoint p;
1242 
1243  unsigned int reference_body_id = body_id;
1244 
1245  if(model.IsFixedBodyId(body_id))
1246  {
1247  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
1248  reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
1249 
1250  p.setIncludingFrame(point_position,model.fixedBodyFrames[fbody_id]);
1251  }
1252  else
1253  {
1254  p.setIncludingFrame(point_position,model.bodyFrames[body_id]);
1255  }
1256 
1257  p.changeFrame(ReferenceFrame::getWorldFrame());
1258  SpatialTransform point_trans(Matrix3dIdentity,p.vec());
1259  FrameVectorPair v_p = -1.*calcPointVelocity6D(model,Q,QDot,body_id,point_position,false);
1260  MotionVector vw_x_S;
1261 
1262  assert (G.rows() == 6 && G.cols() == model.qdot_size);
1263 
1264  unsigned int j = reference_body_id;
1265 
1266  while(j != 0)
1267  {
1268  if(model.mJoints[j].mJointType != JointTypeCustom)
1269  {
1270  if(model.mJoints[j].mDoFCount == 1)
1271  {
1276  vw_x_S.setLinearPart(model.S[j].transform_copy(model.bodyFrames[j]->getTransformToRoot()).transform_copy(point_trans).getLinearPart());
1277  vw_x_S.setLinearPart(-v_p.angular().cross(vw_x_S.getLinearPart()));
1278 
1279  G.col(model.mJoints[j].q_index) = MotionVector(model.S_o[j] + model.v[j]%model.S[j]).transform_copy(model.bodyFrames[j]->getTransformToRoot()).transform_copy(point_trans) +
1280  vw_x_S;
1281  }
1282  else if(model.mJoints[j].mDoFCount == 3)
1283  {
1284  for(int k = 0;k<3;k++)
1285  {
1286  vw_x_S.setLinearPart(MotionVector(model.multdof3_S[j].col(k)).transform_copy(model.bodyFrames[j]->getTransformToRoot()).transform_copy(point_trans).getLinearPart());
1287  vw_x_S.setLinearPart(-v_p.angular().cross(vw_x_S.getLinearPart()));
1288  G.col(model.mJoints[j].q_index+k) = MotionVector(model.multdof3_S_o[j].col(k) + model.v[j].crossm()*model.multdof3_S[j].col(k)).transform_copy(model.bodyFrames[j]->getTransformToRoot()).transform_copy(point_trans) +
1289  vw_x_S;
1290  }
1291  }
1292  }
1293  else
1294  {
1295  unsigned int k = model.mJoints[j].custom_joint_index;
1296 
1297  for(unsigned int l = 0; l<model.mCustomJoints[k]->mDoFCount;l++)
1298  {
1299  vw_x_S.setLinearPart(MotionVector(model.mCustomJoints[k]->S.col(l)).transform_copy(model.bodyFrames[j]->getTransformToRoot()).transform_copy(point_trans).getLinearPart());
1300  vw_x_S.setLinearPart(-v_p.angular().cross(vw_x_S.getLinearPart()));
1301  G.col(model.mJoints[j].q_index+l) = (point_trans * model.bodyFrames[j]->getTransformToRoot()).toMatrix() * (model.mCustomJoints[k]->S_o.col(l) + model.v[j].crossm()*model.mCustomJoints[k]->S.col(l)) +
1302  vw_x_S;
1303  }
1304  }
1305 
1306  j = model.lambda[j];
1307  }
1308  }
1309 
1310  void calcBodySpatialJacobian(Model &model, const VectorNd &Q, unsigned int body_id, MatrixNd &G,
1311  bool update_kinematics)
1312  {
1313  // update the Kinematics if necessary
1314  if(update_kinematics)
1315  {
1316  updateKinematicsCustom(model, &Q, NULL, NULL);
1317  }
1318 
1319  assert (G.rows() == 6 && G.cols() == model.qdot_size);
1320 
1321  unsigned int reference_body_id = body_id;
1322 
1323  SpatialTransform base_to_body;
1324  ReferenceFramePtr bodyFrame;
1325 
1326  if(model.IsFixedBodyId(body_id))
1327  {
1328  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
1329 
1330  reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
1331  bodyFrame = model.fixedBodyFrames[fbody_id];
1332  }
1333  else
1334  {
1335  bodyFrame = model.bodyFrames[reference_body_id];
1336  }
1337 
1338  unsigned int j = reference_body_id;
1339 
1340  while(j != 0)
1341  {
1342  if(model.mJoints[j].mJointType != JointTypeCustom)
1343  {
1344  if(model.mJoints[j].mDoFCount == 1)
1345  {
1346  G.col(model.mJoints[j].q_index) = model.S[j].transform_copy(model.bodyFrames[j]->getTransformToDesiredFrame(bodyFrame));
1347  }
1348  else if(model.mJoints[j].mDoFCount == 3)
1349  {
1350  for(int k = 0;k<3;k++)
1351  {
1352  G.col(model.mJoints[j].q_index+k) = MotionVector(model.multdof3_S[j].col(k)).transform_copy(model.bodyFrames[j]->getTransformToDesiredFrame(bodyFrame));
1353  }
1354  }
1355  }
1356  else
1357  {
1358  unsigned int k = model.mJoints[j].custom_joint_index;
1359 
1360  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) = model.bodyFrames[j]->getTransformToDesiredFrame(bodyFrame).toMatrix() * model.mCustomJoints[k]->S;
1361  }
1362 
1363  j = model.lambda[j];
1364  }
1365  }
1366 
1367  void calcBodySpatialJacobianDot(Model &model, const Math::VectorNd &Q, const Math::VectorNd QDot, unsigned int body_id, Math::MatrixNd &G, const bool update_kinematics)
1368  {
1369  // update the Kinematics if necessary
1370  if(update_kinematics)
1371  {
1372  updateKinematicsCustom(model, &Q, &QDot, NULL);
1373  }
1374 
1375  assert (G.rows() == 6 && G.cols() == model.qdot_size);
1376 
1377  unsigned int reference_body_id = body_id;
1378 
1379  ReferenceFramePtr bodyFrame;
1380 
1381  if(model.IsFixedBodyId(body_id))
1382  {
1383  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
1384 
1385  reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
1386  bodyFrame = model.fixedBodyFrames[fbody_id];
1387  }
1388  else
1389  {
1390  bodyFrame = model.bodyFrames[reference_body_id];
1391  }
1392 
1393  unsigned int j = reference_body_id;
1394 
1395  while(j != 0)
1396  {
1397  if(model.mJoints[j].mJointType != JointTypeCustom)
1398  {
1399  if(model.mJoints[j].mDoFCount == 1)
1400  {
1401  G.col(model.mJoints[j].q_index) = MotionVector(model.S_o[j] + model.v[j]%model.S[j]).transform_copy(model.bodyFrames[j]->getTransformToDesiredFrame(bodyFrame));
1402  }
1403  else if(model.mJoints[j].mDoFCount == 3)
1404  {
1405  for(int k = 0;k<3;k++)
1406  {
1407  G.col(model.mJoints[j].q_index+k) = MotionVector(model.multdof3_S_o[j].col(k) + model.v[j].crossm()*model.multdof3_S[j].col(k)).transform_copy(model.bodyFrames[j]->getTransformToDesiredFrame(bodyFrame));
1408  }
1409  }
1410  }
1411  else
1412  {
1413  unsigned int k = model.mJoints[j].custom_joint_index;
1414 
1415  G.block(0, model.mJoints[j].q_index, 6, model.mCustomJoints[k]->mDoFCount) = model.bodyFrames[j]->getTransformToDesiredFrame(bodyFrame).toMatrix() * (model.mCustomJoints[k]->S_o + model.v[j].crossm()*model.mCustomJoints[k]->S);
1416  }
1417 
1418  j = model.lambda[j];
1419  }
1420  }
1421 
1422  Math::FrameVector calcPointVelocity(Model &model, const VectorNd &Q, const VectorNd &QDot, unsigned int body_id,
1423  const Vector3d &point_position, bool update_kinematics)
1424  {
1425  assert (model.IsBodyId(body_id));
1426  assert (model.q_size == Q.size());
1427  assert (model.qdot_size == QDot.size());
1428 
1429  // Reset the velocity of the root body
1430  model.v[0].setZero();
1431 
1432  if(update_kinematics)
1433  {
1434  updateKinematicsCustom(model, &Q, &QDot, nullptr);
1435  }
1436 
1437  unsigned int reference_body_id = body_id;
1438  Math::FramePoint p;
1439 
1440  if(model.IsFixedBodyId(body_id))
1441  {
1442  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
1443  reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
1444 
1445  p.setIncludingFrame(point_position,model.fixedBodyFrames[fbody_id]);
1446 
1447  p.changeFrame(model.bodyFrames[reference_body_id]);
1448  }
1449  else
1450  {
1451  p.setIncludingFrame(point_position,model.bodyFrames[body_id]);
1452  }
1453 
1454  // E*(v-rxw)
1455  SpatialVector v = model.v[reference_body_id];
1456  return Math::FrameVector(model.worldFrame,model.bodyFrames[reference_body_id]->getTransformToRoot().E*(v.getLinearPart()-p.vec().cross(v.getAngularPart())));
1457  }
1458 
1459  Math::FrameVectorPair calcPointVelocity6D(Model &model, const Math::VectorNd &Q,
1460  const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position,
1461  bool update_kinematics)
1462  {
1463  assert (model.IsBodyId(body_id));
1464  assert (model.q_size == Q.size());
1465  assert (model.qdot_size == QDot.size());
1466 
1467  // Reset the velocity of the root body
1468  model.v[0].setZero();
1469 
1470  if(update_kinematics)
1471  {
1472  updateKinematicsCustom(model, &Q, &QDot, NULL);
1473  }
1474 
1475  unsigned int reference_body_id = body_id;
1476  Math::FramePoint p;
1477 
1478  if(model.IsFixedBodyId(body_id))
1479  {
1480  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
1481  reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
1482  p.setIncludingFrame(point_position,model.fixedBodyFrames[fbody_id]);
1483  p.changeFrame(model.bodyFrames[reference_body_id]);
1484  }
1485  else
1486  {
1487  p.setIncludingFrame(point_position,model.bodyFrames[body_id]);
1488  }
1489 
1490  FrameVectorPair point_velocity(model.v[reference_body_id]);
1491  point_velocity.setLinearPart((Vector3d)point_velocity.linear()-p.cross(point_velocity.angular()));
1492  point_velocity.changeFrame(model.worldFrame);
1493 
1494  return point_velocity;
1495  }
1496 
1497  Math::FrameVectorPair calcPointVelocity6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot,
1498  RobotDynamics::ReferenceFramePtr frame, bool update_kinematics)
1499  {
1500  assert (model.q_size == Q.size());
1501  assert (model.qdot_size == QDot.size());
1502 
1503  // Reset the velocity of the root body
1504  model.v[0].setZero();
1505 
1506  if(update_kinematics)
1507  {
1508  updateKinematicsCustom(model, &Q, &QDot, NULL);
1509  }
1510 
1511  RobotDynamics::Math::Vector3d p(0.,0.,0.);
1512  unsigned int body_id = frame->getMovableBodyId();
1513  if(!frame->getIsBodyFrame())
1514  {
1515  p = frame->getTransformFromParent().r;
1516  }
1517 
1518  FrameVectorPair point_velocity(model.v[body_id]);
1519  point_velocity.setLinearPart((Vector3d)point_velocity.linear()-p.cross(point_velocity.angular()));
1520  point_velocity.changeFrame(model.worldFrame);
1521 
1522  return point_velocity;
1523  }
1524 
1525  FrameVectorPair calcPointVelocity6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot,
1526  RobotDynamics::ReferenceFramePtr baseFrame, RobotDynamics::ReferenceFramePtr relativeFrame,
1527  RobotDynamics::ReferenceFramePtr expressedInFrame,
1528  bool update_kinematics)
1529  {
1530  if(update_kinematics)
1531  {
1532  updateKinematicsCustom(model, &Q, &QDot, NULL);
1533  }
1534 
1535  FrameVectorPair v = calcPointVelocity6D(model, Q, QDot, baseFrame, false);
1536  v-=calcPointVelocity6D(model, Q, QDot, relativeFrame, false);
1537 
1538  v.changeFrame(expressedInFrame);
1539 
1540  return v;
1541  }
1542 
1543  FrameVectorPair calcPointAcceleration6D(Model & model,
1544  const Math::VectorNd& Q,
1545  const Math::VectorNd& QDot,
1546  const Math::VectorNd& QDDot,
1547  ReferenceFramePtr body_frame,
1548  ReferenceFramePtr relative_body_frame,
1549  ReferenceFramePtr expressedInFrame,
1550  const bool update_kinematics)
1551  {
1552  // Reset the velocity of the root body
1553  model.v[0].setZero();
1554  model.a[0].setZero();
1555 
1556  if(update_kinematics)
1557  {
1558  updateKinematics(model, Q, QDot, QDDot);
1559  }
1560 
1561  RobotDynamics::Math::Vector3d p(0.,0.,0.);
1562  ReferenceFramePtr moveable_body_frame = body_frame;
1563  if(!body_frame->getIsBodyFrame())
1564  {
1565  p = body_frame->getTransformFromParent().r;
1566  moveable_body_frame = model.bodyFrames[body_frame->getMovableBodyId()];
1567  }
1568 
1569  SpatialTransform p_X_i(moveable_body_frame->getTransformToRoot().E,p);
1570  FrameVectorPair accel(model.worldFrame, model.a[moveable_body_frame->getMovableBodyId()].transform_copy(p_X_i));
1571 
1572  FrameVectorPair point_velocity = calcPointVelocity6D(model,Q,QDot,body_frame,false);
1573 
1574  accel.setLinearPart(accel.linear()+point_velocity.angular().cross(point_velocity.linear()));
1575 
1576  p.setZero();
1577  ReferenceFramePtr movable_relative_body_frame = relative_body_frame;
1578  if(!relative_body_frame->getIsBodyFrame())
1579  {
1580  p = relative_body_frame->getTransformFromParent().r;
1581  movable_relative_body_frame = model.bodyFrames[relative_body_frame->getMovableBodyId()];
1582  }
1583 
1584  p_X_i = SpatialTransform(movable_relative_body_frame->getTransformToRoot().E,p);
1585  accel -= FrameVectorPair(model.worldFrame,model.a[movable_relative_body_frame->getMovableBodyId()].transform_copy(p_X_i));
1586  point_velocity = calcPointVelocity6D(model,Q,QDot,relative_body_frame,false);
1587  // This cross is subtracted bc it's part of the relative_to accel of which the whole thing is subtracted off
1588  accel.setLinearPart(accel.linear()-point_velocity.angular().cross(point_velocity.linear()));
1589 
1590  return accel.changeFrameAndCopy(expressedInFrame);
1591  }
1592 
1593  Math::FrameVector calcPointAcceleration(Model &model, const VectorNd &Q, const VectorNd &QDot,
1594  const VectorNd &QDDot, unsigned int body_id, const Vector3d &point_position, bool update_kinematics)
1595  {
1596 
1597  // Reset the velocity of the root body
1598  model.v[0].setZero();
1599  model.a[0].setZero();
1600 
1601  if(update_kinematics)
1602  {
1603  updateKinematics(model, Q, QDot, QDDot);
1604  }
1605 
1606  unsigned int reference_body_id = body_id;
1607  Math::FramePoint p;
1608 
1609  if(model.IsFixedBodyId(body_id))
1610  {
1611  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
1612  reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
1613 
1614  p.setIncludingFrame(point_position,model.fixedBodyFrames[fbody_id]);
1615  p.changeFrame(model.bodyFrames[reference_body_id]);
1616  }
1617  else
1618  {
1619  p.setIncludingFrame(point_position,model.bodyFrames[body_id]);
1620  }
1621 
1622  SpatialTransform p_X_i(model.bodyFrames[reference_body_id]->getTransformToRoot().E, p.vec());
1623 
1624  MotionVector p_v_i = model.v[reference_body_id].transform_copy(p_X_i);
1625  Vector3d a_dash = Vector3d(p_v_i[0], p_v_i[1], p_v_i[2]).cross(Vector3d(p_v_i[3], p_v_i[4], p_v_i[5]));
1626  MotionVector p_a_i = model.a[reference_body_id].transform_copy(p_X_i);
1627 
1628  return Math::FrameVector(model.worldFrame,p_a_i[3] + a_dash[0], p_a_i[4] + a_dash[1], p_a_i[5] + a_dash[2]);
1629  }
1630 
1631  Math::FrameVectorPair calcPointAcceleration6D(Model &model, const VectorNd &Q, const VectorNd &QDot,
1632  const VectorNd &QDDot, unsigned int body_id, const Vector3d &point_position, bool update_kinematics)
1633  {
1634  // Reset the velocity of the root body
1635  model.v[0].setZero();
1636  model.a[0].setZero();
1637 
1638  if(update_kinematics)
1639  {
1640  updateKinematics(model, Q, QDot, QDDot);
1641  }
1642 
1643  unsigned int reference_body_id = body_id;
1644  Math::FramePoint p;
1645 
1646  if(model.IsFixedBodyId(body_id))
1647  {
1648  unsigned int fbody_id = body_id - model.fixed_body_discriminator;
1649  reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
1650 
1651  p.setIncludingFrame(point_position,model.fixedBodyFrames[fbody_id]);
1652  p.changeFrame(model.bodyFrames[reference_body_id]);
1653  }
1654  else
1655  {
1656  p.setIncludingFrame(point_position,model.bodyFrames[body_id]);
1657  }
1658 
1659  SpatialTransform p_X_i(model.bodyFrames[reference_body_id]->getTransformToRoot().E, p.vec());
1660 
1661  FrameVectorPair accel(model.worldFrame, model.a[reference_body_id].transform_copy(p_X_i));
1662 
1663  MotionVector p_v_i = model.v[reference_body_id].transform_copy(p_X_i);
1664  Vector3d a_dash = Vector3d(p_v_i[0], p_v_i[1], p_v_i[2]).cross(Vector3d(p_v_i[3], p_v_i[4], p_v_i[5]));
1665  accel.setLinearPart(accel.linear()+a_dash);
1666  return accel;
1667 // return MotionVector(model.a[reference_body_id].transform_copy(p_X_i) + MotionVector(0, 0, 0, a_dash[0], a_dash[1], a_dash[2]));
1668  }
1669 
1670  Math::SpatialMotion calcSpatialVelocity(Model & model,
1671  const Math::VectorNd& Q,
1672  const Math::VectorNd& QDot,
1673  ReferenceFramePtr body_frame,
1674  ReferenceFramePtr relative_body_frame,
1675  ReferenceFramePtr expressedInFrame,
1676  const bool update_kinematics)
1677  {
1678 
1679  ReferenceFramePtr expressedInFrameRef;
1680  expressedInFrameRef = expressedInFrame==nullptr ? body_frame : expressedInFrame;
1681  assert(body_frame!=nullptr && relative_body_frame!=nullptr && expressedInFrameRef!=nullptr);
1682  if(body_frame==relative_body_frame)
1683  {
1684  Math::SpatialMotion m(body_frame,relative_body_frame,body_frame,Math::SpatialVectorZero);
1685  m.changeFrame(expressedInFrameRef);
1686  return m;
1687  }
1688 
1689  if(update_kinematics)
1690  {
1691  updateKinematicsCustom(model,&Q,&QDot,nullptr);
1692  }
1693 
1694  Math::SpatialMotion v_body = model.v[body_frame->getMovableBodyId()];
1695  if(!body_frame->getIsBodyFrame())
1696  {
1697  // Change the frame if the reference frame is a fixed frame
1698  v_body.changeFrame(body_frame);
1699  v_body.setBodyFrame(body_frame);
1700  }
1701 
1702  Math::SpatialMotion v_relative_body=model.v[relative_body_frame->getMovableBodyId()];
1703  if(!relative_body_frame->getIsBodyFrame())
1704  {
1705  // Change the frame if the reference frame is a fixed frame
1706  v_relative_body.changeFrame(relative_body_frame);
1707  v_relative_body.setBodyFrame(relative_body_frame);
1708  }
1709 
1710  v_relative_body.changeFrame(expressedInFrameRef);
1711  v_body.changeFrame(expressedInFrameRef);
1712 
1713  return (v_body-v_relative_body);
1714  }
1715 
1716  Math::SpatialMotion calcSpatialVelocity(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const unsigned int body_id, const unsigned int relative_body_id,
1717  ReferenceFramePtr expressedInFrame, const bool update_kinematics)
1718  {
1719  ReferenceFramePtr body_frame;
1720  ReferenceFramePtr relative_body_frame;
1721  if(model.IsFixedBodyId(body_id))
1722  {
1723  body_frame = model.fixedBodyFrames[body_id-model.fixed_body_discriminator];
1724  }
1725  else
1726  {
1727  body_frame = model.bodyFrames[body_id];
1728  }
1729 
1730  ReferenceFramePtr expressedInFrameRef;
1731  expressedInFrameRef = expressedInFrame==nullptr ? body_frame : expressedInFrame;
1732 
1733  if(model.IsFixedBodyId(relative_body_id))
1734  {
1735  relative_body_frame = model.fixedBodyFrames[relative_body_id-model.fixed_body_discriminator];
1736  }
1737  else
1738  {
1739  relative_body_frame = model.bodyFrames[relative_body_id];
1740  }
1741 
1742  assert(body_frame!=nullptr && relative_body_frame!=nullptr && expressedInFrameRef!=nullptr);
1743 
1744  if(body_id==relative_body_id)
1745  {
1746  return Math::SpatialMotion(body_frame,relative_body_frame,body_frame,Math::SpatialVectorZero);
1747  }
1748 
1749  if(update_kinematics)
1750  {
1751  updateKinematicsCustom(model,&Q,&QDot,nullptr);
1752  }
1753 
1754  Math::SpatialMotion v_body;
1755  if(model.IsFixedBodyId(body_id))
1756  {
1757  v_body = model.v[model.mFixedBodies[body_id-model.fixed_body_discriminator].mMovableParent];
1758  v_body.changeFrame(model.fixedBodyFrames[body_id-model.fixed_body_discriminator]);
1759  v_body.setBodyFrame(body_frame);
1760  }
1761  else
1762  {
1763  v_body = model.v[body_id];
1764  }
1765 
1766  Math::SpatialMotion v_relative_body;
1767  if(model.IsFixedBodyId(relative_body_id))
1768  {
1769  v_relative_body = model.v[model.mFixedBodies[relative_body_id-model.fixed_body_discriminator].mMovableParent];
1770  v_relative_body.changeFrame(model.fixedBodyFrames[relative_body_id-model.fixed_body_discriminator]);
1771  v_relative_body.setBodyFrame(relative_body_frame);
1772  }
1773  else
1774  {
1775  v_relative_body = model.v[relative_body_id];
1776  }
1777 
1778  v_relative_body.changeFrame(expressedInFrameRef);
1779  v_body.changeFrame(expressedInFrameRef);
1780 
1781  return (v_body-v_relative_body);
1782  }
1783 
1784  Math::SpatialAcceleration calcSpatialAcceleration(Model & model,
1785  const Math::VectorNd& Q,
1786  const Math::VectorNd& QDot,
1787  const Math::VectorNd& QDDot,
1788  ReferenceFramePtr body_frame,
1789  ReferenceFramePtr relative_body_frame,
1790  ReferenceFramePtr expressedInFrame,
1791  const bool update_kinematics)
1792  {
1793  if(body_frame==relative_body_frame)
1794  {
1795  return Math::SpatialAcceleration(body_frame,relative_body_frame,body_frame,Math::SpatialVectorZero);
1796  }
1797 
1798  if(update_kinematics)
1799  {
1800  updateKinematics(model,Q,QDot,QDDot);
1801  }
1802 
1803  Math::SpatialAcceleration a_body = model.a[body_frame->getMovableBodyId()];
1804  if(!body_frame->getIsBodyFrame())
1805  {
1806  // There is no relative motion btw fixed body and its moveable parent, so this is kosher
1807  a_body.changeFrame(body_frame);
1808  a_body.setBodyFrame(body_frame);
1809  }
1810 
1811  Math::SpatialAcceleration a_relative_body = model.a[relative_body_frame->getMovableBodyId()];
1812  if(!relative_body_frame->getIsBodyFrame())
1813  {
1814  // @todo - Can add in the velocity thing here to save cycles
1815  // There is no relative motion btw fixed body and its moveable parent, so this is kosher
1816  a_relative_body.changeFrame(relative_body_frame);
1817  a_relative_body.setBodyFrame(relative_body_frame);
1818  }
1819 
1820  SpatialMotion twistOfCurrentFrameWithRespectToNewFrame =
1821  calcSpatialVelocity(model,Q,QDot,a_relative_body.getReferenceFrame(),a_body.getReferenceFrame(),a_relative_body.getReferenceFrame(),false);
1822  SpatialMotion twistOfBodyWrtBaseExpressedInCurrent =
1823  calcSpatialVelocity(model,Q,QDot,a_relative_body.getBodyFrame(),a_relative_body.getBaseFrame(),a_relative_body.getReferenceFrame(),false);
1824 
1825  a_relative_body.changeFrameWithRelativeMotion(a_body.getReferenceFrame(),twistOfCurrentFrameWithRespectToNewFrame,twistOfBodyWrtBaseExpressedInCurrent);
1826 
1827  a_body-=a_relative_body;
1828 
1829  if(expressedInFrame==nullptr || expressedInFrame==a_body.getReferenceFrame())
1830  {
1834  return a_body;
1835  }
1836  else
1837  {
1842  twistOfCurrentFrameWithRespectToNewFrame =
1843  calcSpatialVelocity(model,Q,QDot,a_body.getReferenceFrame(),expressedInFrame,a_body.getReferenceFrame(),false);
1844  twistOfBodyWrtBaseExpressedInCurrent =
1845  calcSpatialVelocity(model,Q,QDot,a_body.getBodyFrame(),a_body.getBaseFrame(),a_body.getReferenceFrame(),false);
1846 
1847  a_body.changeFrameWithRelativeMotion(expressedInFrame,twistOfCurrentFrameWithRespectToNewFrame,twistOfBodyWrtBaseExpressedInCurrent);
1848  return a_body;
1849  }
1850  }
1851 
1852  Math::SpatialAcceleration calcSpatialAcceleration(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, const unsigned int body_id, const unsigned int relative_body_id,
1853  ReferenceFramePtr expressedInFrame, const bool update_kinematics)
1854  {
1855  ReferenceFramePtr body_frame;
1856  ReferenceFramePtr relative_body_frame;
1857  if(model.IsFixedBodyId(body_id))
1858  {
1859  body_frame = model.fixedBodyFrames[body_id-model.fixed_body_discriminator];
1860  }
1861  else
1862  {
1863  body_frame = model.bodyFrames[body_id];
1864  }
1865 
1866  if(model.IsFixedBodyId(relative_body_id))
1867  {
1868  relative_body_frame = model.fixedBodyFrames[relative_body_id-model.fixed_body_discriminator];
1869  }
1870  else
1871  {
1872  relative_body_frame = model.bodyFrames[relative_body_id];
1873  }
1874 
1875  return calcSpatialAcceleration(model,Q,QDot,QDDot,body_frame,relative_body_frame,expressedInFrame,update_kinematics);
1876  }
1877 
1878  bool inverseKinematics(Model &model, const VectorNd &Qinit, const std::vector<unsigned int> &body_id,
1879  const std::vector<Vector3d> &body_point, const std::vector<Vector3d> &target_pos, VectorNd &Qres,
1880  double step_tol, double lambda, unsigned int max_iter)
1881  {
1882  assert (Qinit.size() == model.q_size);
1883  assert (body_id.size() == body_point.size());
1884  assert (body_id.size() == target_pos.size());
1885 
1886  MatrixNd J = MatrixNd::Zero(3 * body_id.size(), model.qdot_size);
1887  VectorNd e = VectorNd::Zero(3 * body_id.size());
1888  MatrixNd G = model.three_x_qd0;
1889  Math::FramePoint p;
1890  Vector3d tmp_vec;
1891 
1892  Qres = Qinit;
1893 
1894  for(unsigned int ik_iter = 0; ik_iter < max_iter; ik_iter++)
1895  {
1896  updateKinematicsCustom(model, &Qres, NULL, NULL);
1897 
1898  for(unsigned int k = 0; k < body_id.size(); k++)
1899  {
1900  G.setZero();
1901  calcPointJacobian(model, Qres, body_id[k], body_point[k], G, false);
1902  p.setIncludingFrame(body_point[k],model.bodyFrames[body_id[k]]);
1903  p.changeFrame(model.worldFrame);
1904  tmp_vec = p.vec();
1905 
1906  for(unsigned int i = 0; i < 3; i++)
1907  {
1908  for(unsigned int j = 0; j < model.qdot_size; j++)
1909  {
1910  unsigned int row = k * 3 + i;
1911  J(row, j) = G(i, j);
1912  }
1913 
1914  e[k * 3 + i] = target_pos[k][i] - tmp_vec[i];
1915  }
1916  }
1917 
1918  // abort if we are getting "close"
1919  if(e.norm() < step_tol)
1920  {
1921  return true;
1922  }
1923 
1924  MatrixNd JJTe_lambda2_I = J * J.transpose() + lambda * lambda * MatrixNd::Identity(e.size(), e.size());
1925 
1926  VectorNd z(body_id.size() * 3);
1927  bool solve_successful = linSolveGaussElimPivot (JJTe_lambda2_I, e, z);
1928  assert (solve_successful);
1929 
1930  VectorNd delta_theta = J.transpose() * z;
1931 
1932  Qres = Qres + delta_theta;
1933 
1934  if(delta_theta.norm() < step_tol)
1935  {
1936  return true;
1937  }
1938 
1939  VectorNd test_1(z.size());
1940  VectorNd test_res(z.size());
1941 
1942  test_1.setZero();
1943 
1944  for(unsigned int i = 0; i < z.size(); i++)
1945  {
1946  test_1[i] = 1.;
1947 
1948  VectorNd test_delta = J.transpose() * test_1;
1949 
1950  test_res[i] = test_delta.squaredNorm();
1951 
1952  test_1[i] = 0.;
1953  }
1954  }
1955 
1956  return false;
1957  }
1958 
1959 }
RobotDynamics::CustomJoint::mDoFCount
unsigned int mDoFCount
Definition: Joint.h:688
RobotDynamics::Model::S_o
Math::MotionVectorV S_o
Definition: Model.h:212
RobotDynamics::Model::IsFixedBodyId
bool IsFixedBodyId(unsigned int body_id) const
Checks whether the body is rigidly attached to another body.
Definition: Model.h:483
FramePoint.hpp
File containing the FramePoint<T> object definition.
RobotDynamics::calcPointJacobian
void calcPointJacobian(Model &model, const Math::VectorNd &Q, unsigned int body_id, const Math::Vector3d &point_position, Math::MatrixNd &G, bool update_kinematics=true)
Computes the 3D point jacobian for a point on a body.
Definition: Kinematics.cc:510
RobotDynamics::Model::mJoints
std::vector< Joint > mJoints
All joints.
Definition: Model.h:209
RobotDynamics::FrameObject::getReferenceFrame
ReferenceFramePtr getReferenceFrame() const
Get a pointer to the reference frame this FrameObject is expressed in.
Definition: FrameObject.hpp:52
RobotDynamics::Math::MotionVector::crossm
SpatialMatrix crossm()
Get the spatial motion cross matrix, .
Definition: MotionVector.hpp:266
RobotDynamics::Math::SpatialVectorZero
SpatialVector SpatialVectorZero
RobotDynamics::Math::FramePoint
A FramePoint is a 3D point that is expressed in a ReferenceFrame. To change the ReferenceFrame a Fram...
Definition: FramePoint.hpp:40
RobotDynamics::Model::c_J
std::vector< Math::SpatialVector > c_J
Definition: Model.h:217
RobotDynamics::calcRelativeBodySpatialJacobianAndJacobianDot
void calcRelativeBodySpatialJacobianAndJacobianDot(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, Math::MatrixNd &GDot, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=nullptr, bool update_kinematics=true)
Computes both the body spatial jacobian and its time derivative. This function will be a bit more eff...
Definition: Kinematics.cc:402
RobotDynamics::JointTypeCustom
User defined joints of varying size.
Definition: Joint.h:213
QDDot
VectorNd QDDot
Definition: RdlTwoLegModelTests.cpp:31
RobotDynamics::Math::Matrix3dIdentity
Matrix3d Matrix3dIdentity
RobotDynamics::CustomJoint
CustomJoint is a struct used to create a joint with user defined parameters. This is accomplished by ...
Definition: Joint.h:662
RobotDynamics::Math::FrameVectorPair::linear
FrameVector linear() const
Get copy of linear component.
Definition: FrameVectorPair.hpp:222
RobotDynamics::Model::mFixedBodies
std::vector< FixedBody > mFixedBodies
All bodies that are attached to a body via a fixed joint.
Definition: Model.h:298
RobotDynamics::Model::lambda
std::vector< unsigned int > lambda
Definition: Model.h:156
RobotDynamics::Model::num_branch_ends
unsigned int num_branch_ends
Definition: Model.h:328
RobotDynamics::Math::linSolveGaussElimPivot
bool linSolveGaussElimPivot(MatrixNd A, VectorNd b, VectorNd &x)
Solves a linear system using gaussian elimination with pivoting.
Definition: rdl_mathutils.cc:36
RobotDynamics::calcSpatialVelocity
Math::SpatialMotion calcSpatialVelocity(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, ReferenceFramePtr body_frame, ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame=nullptr, const bool update_kinematics=true)
Compute the spatial velocity of any frame with respect to any other frame, expressed in an arbirtary ...
Definition: Kinematics.cc:1670
QDot
VectorNd QDot
Definition: RdlTwoLegModelTests.cpp:30
RobotDynamics::Math::MotionVector::transform
void transform(const SpatialTransform &X)
Transforms a motion vector. Performs .
Definition: MotionVector.hpp:201
RobotDynamics::calcRelativeBodySpatialJacobian
void calcRelativeBodySpatialJacobian(Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=nullptr, bool update_kinematics=true)
Computes the jacobian of a frame with being the "base" frame, being the "relative" frame...
Definition: Kinematics.cc:234
RobotDynamics::Math::SpatialVector::setLinearPart
void setLinearPart(const Vector3d &v)
Definition: rdl_eigenmath.h:224
RobotDynamics::Math::SpatialTransform::toMatrix
SpatialMatrix toMatrix() const
Return transform as 6x6 spatial matrix.
Definition: rdl_eigenmath.h:434
RobotDynamics::Math::Xtrans
SpatialTransform Xtrans(const Vector3d &r)
Get pure translation transform.
Definition: SpatialAlgebraOperators.h:369
Q
VectorNd Q
Definition: RdlTwoLegModelTests.cpp:29
RobotDynamics::calcPointJacobianDot
void calcPointJacobianDot(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, Math::MatrixNd &G, bool update_kinematics=true)
Computes the time derivative of the linear components the a point jacobian on a body.
Definition: Kinematics.cc:751
RobotDynamics::Math::FrameVectorPair::angular
FrameVector angular() const
Get copy of angular component.
Definition: FrameVectorPair.hpp:231
RobotDynamics::calcRelativePointJacobian6D
void calcRelativePointJacobian6D(Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics=true)
Compute the 6D jacobian of the origin of a reference frame relative to the origin of another referenc...
Definition: Kinematics.cc:643
RobotDynamics::jcalc
void jcalc(Model &model, unsigned int joint_id, const Math::VectorNd &q, const Math::VectorNd &qdot)
Computes all variables for a joint model and updates the body frame as well as the body&#39;s center of m...
Definition: Joint.cc:23
rdl_mathutils.h
RobotDynamics::Model::threads
std::vector< boost::thread * > threads
Definition: Model.h:335
RobotDynamics::calcPointAcceleration
Math::FrameVector calcPointAcceleration(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
Computes the linear acceleration of a point on a body.
Definition: Kinematics.cc:1593
RobotDynamics::Math::toTildeForm
static Matrix3d toTildeForm(const Point3d &p)
Definition: Point3.hpp:370
RobotDynamics::calcRelativePointJacobianAndJacobianDot6D
void calcRelativePointJacobianAndJacobianDot6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, Math::MatrixNd &GDot, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics=true)
Compute the point Jacobian of the origin of baseFrame, relative to the origin of relativeFrame, expressed in expressedInFrame. Also compute that point Jacobians time derivative.
Definition: Kinematics.cc:1033
RobotDynamics::ReferenceFrame::getWorldFrame
static ReferenceFramePtr getWorldFrame()
Get a pointer to the world frame.
Definition: ReferenceFrame.hpp:267
RobotDynamics::Math::FrameVector::cross
FrameVector cross(const FrameVector &vector) const
Cross product between two FrameVectors, i.e. .
Definition: FrameVector.hpp:163
RobotDynamics::calcSpatialAcceleration
Math::SpatialAcceleration calcSpatialAcceleration(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, const unsigned int body_id, const unsigned int relative_body_id, ReferenceFramePtr expressedInFrame=nullptr, const bool update_kinematics=true)
Compute the spatial acceleration of any body with respect to any other body and express the result in...
Definition: Kinematics.cc:1852
RobotDynamics::Model::bodyFrames
std::vector< ReferenceFramePtr > bodyFrames
Definition: Model.h:145
RobotDynamics::Model::worldFrame
ReferenceFramePtr worldFrame
Definition: Model.h:141
RobotDynamics::calcPointJacobian6D
void calcPointJacobian6D(Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr frame, bool update_kinematics=true)
Compute the 6D point jacobian of the origin of a reference frame If a position of a point is computed...
Definition: Kinematics.cc:604
RobotDynamics::Math::FramePoint::setIncludingFrame
EIGEN_STRONG_INLINE void setIncludingFrame(const Math::Vector3d &v, ReferenceFramePtr referenceFrame)
Set both the ReferenceFrame this object is expressed in as well as the (x,y,z) coordinates of the poi...
Definition: FramePoint.hpp:138
RobotDynamics::Math::Point3d::vec
EIGEN_STRONG_INLINE Math::Vector3d vec() const
Definition: Point3.hpp:269
RobotDynamics::Model::v
Math::SpatialMotionV v
Definition: Model.h:202
RobotDynamics::ReferenceFramePtr
std::shared_ptr< ReferenceFrame > ReferenceFramePtr
Definition: ReferenceFrame.hpp:72
RobotDynamics::calcPointVelocity
Math::FrameVector calcPointVelocity(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
Computes the velocity of a point on a body.
Definition: Kinematics.cc:1422
RobotDynamics::Model::q_size
unsigned int q_size
The size of the -vector. For models without spherical joints the value is the same as Model::dof_coun...
Definition: Model.h:178
RobotDynamics::Model::IsBodyId
bool IsBodyId(unsigned int id) const
Definition: Model.h:492
RobotDynamics::Math::SpatialTransform
Compact representation of spatial transformations.
Definition: rdl_eigenmath.h:337
RobotDynamics::calcRelativePointJacobianDot6D
void calcRelativePointJacobianDot6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics=true)
Compute the time derivative of the 6D jacobian of the origin of a reference frame relative to the ori...
Definition: Kinematics.cc:877
RobotDynamics::FrameObject::changeFrame
virtual void changeFrame(ReferenceFramePtr desiredFrame)
Change the ReferenceFrame this FrameObject is expressed in.
Definition: FrameObject.cpp:12
RobotDynamics::calcPointJacobianDot6D
void calcPointJacobianDot6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, RobotDynamics::ReferenceFramePtr frame, Math::MatrixNd &G, bool update_kinematics=true)
Computes the time derivative of a point jacobian of a point on a body.
Definition: Kinematics.cc:826
RobotDynamics::Model::multdof3_S_o
std::vector< Math::Matrix63 > multdof3_S_o
Definition: Model.h:236
RobotDynamics::Model::mBodies
std::vector< Body > mBodies
All bodies 0 ... N_B, including the base.
Definition: Model.h:321
RobotDynamics::Math::FrameVector
A FrameVector is a 3D vector with a ReferenceFrame, and all operations between FrameVectors and other...
Definition: FrameVector.hpp:33
RobotDynamics::Math::SpatialMotion
A SpatialMotion vector is a MotionVector with a RobotDynamics::ReferenceFrame it is expressed in...
Definition: SpatialMotion.hpp:35
RobotDynamics::Math::FrameVectorPair::changeFrame
void changeFrame(ReferenceFramePtr referenceFrame)
Change the frame of the two 3d vectors. Equivalent to the following math expression ...
Definition: FrameVectorPair.hpp:112
RobotDynamics::calcBodySpatialJacobianDot
void calcBodySpatialJacobianDot(Model &model, const Math::VectorNd &Q, const Math::VectorNd QDot, unsigned int body_id, Math::MatrixNd &G, const bool update_kinematics=true)
Computes the time derivative of the spatial jacobian for a body. The result will be returned via the ...
Definition: Kinematics.cc:1367
RobotDynamics::Model::q0_vec
Math::VectorNd q0_vec
Definition: Model.h:283
RobotDynamics::updateKinematicsParallel
void updateKinematicsParallel(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot)
Updates and computes velocities and accelerations of the bodies.
Definition: Kinematics.cc:214
RobotDynamics::Math::SpatialMotion::getBaseFrame
ReferenceFramePtr getBaseFrame() const
Get a SpatialMotions SpatialMotion::baseFrame.
Definition: SpatialMotion.hpp:155
RobotDynamics::Math::VectorNd
Eigen::VectorXd VectorNd
Definition: rdl_eigenmath.h:25
RobotDynamics::Model::crawlChainKinematics
void crawlChainKinematics(unsigned int b_id, std::atomic< unsigned int > *branch_ends, const Math::VectorNd *Q, const Math::VectorNd *QDot, const Math::VectorNd *QDDot)
Definition: Model.cc:539
RobotDynamics::calcBodySpatialJacobian
void calcBodySpatialJacobian(Model &model, const Math::VectorNd &Q, unsigned int body_id, Math::MatrixNd &G, bool update_kinematics=true)
Computes the spatial jacobian for a body. The result will be returned via the G argument and represen...
Definition: Kinematics.cc:1310
RobotDynamics::updateKinematics
void updateKinematics(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot)
Updates and computes velocities and accelerations of the bodies.
Definition: Kinematics.cc:25
RobotDynamics::Model::a
Math::SpatialAccelerationV a
The spatial velocity of the bodies.
Definition: Model.h:200
RobotDynamics::Model::mCustomJoints
std::vector< CustomJoint * > mCustomJoints
Definition: Model.h:242
RobotDynamics::Math::SpatialMotion::getBodyFrame
ReferenceFramePtr getBodyFrame() const
Get a SpatialMotions SpatialMotion::bodyFrame.
Definition: SpatialMotion.hpp:146
RobotDynamics::Math::SpatialVector::getAngularPart
EIGEN_STRONG_INLINE Vector3d getAngularPart() const
Definition: rdl_eigenmath.h:207
RobotDynamics::inverseKinematics
bool inverseKinematics(Model &model, const Math::VectorNd &Qinit, const std::vector< unsigned int > &body_id, const std::vector< Math::Vector3d > &body_point, const std::vector< Math::Vector3d > &target_pos, Math::VectorNd &Qres, double step_tol=1.0e-12, double lambda=0.01, unsigned int max_iter=50)
Computes the inverse kinematics iteratively using a damped Levenberg-Marquardt method (also known as ...
RobotDynamics::Model
Contains all information about the rigid body model.
Definition: Model.h:121
RobotDynamics::calcRelativeBodySpatialJacobianDot
void calcRelativeBodySpatialJacobianDot(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=nullptr, bool update_kinematics=true)
Computes the rate of change of the jacobian, , with being the "base" frame, being the relative" fra...
Definition: Kinematics.cc:301
RobotDynamics::Math::FrameVectorPair::setLinearPart
void setLinearPart(const Vector3d &v)
Set the linear vector.
Definition: FrameVectorPair.hpp:204
RobotDynamics::updateKinematicsCustom
void updateKinematicsCustom(Model &model, const Math::VectorNd *Q, const Math::VectorNd *QDot=nullptr, const Math::VectorNd *QDDot=nullptr)
Selectively updates model internal states of body positions, velocities and/or accelerations.
Definition: Kinematics.cc:72
RobotDynamics::Model::getCommonMovableParentId
unsigned int getCommonMovableParentId(unsigned int id_1, unsigned int id_2) const
Definition: Model.h:620
RobotDynamics::Math::MatrixNd
Eigen::MatrixXd MatrixNd
Definition: rdl_eigenmath.h:26
RobotDynamics::Model::c
std::vector< Math::SpatialVector > c
The velocity dependent spatial acceleration.
Definition: Model.h:248
RobotDynamics::Math::Matrix63
Eigen::Matrix< double, 6, 3 > Matrix63
Definition: rdl_eigenmath.h:24
RobotDynamics::Math::Point3d::cross
Vector3d cross(const Vector3d &v)
Cross product between a point and vector.
Definition: Point3.hpp:213
RobotDynamics::Model::fixed_body_discriminator
unsigned int fixed_body_discriminator
Value that is used to discriminate between fixed and movable bodies.
Definition: Model.h:312
RobotDynamics::Math::SpatialAcceleration
SpatialAcceleration. For clarity, the ReferenceFrames are stated as follows. A spatial acceleration i...
Definition: SpatialAcceleration.hpp:37
RobotDynamics::Math::SpatialAcceleration::changeFrameWithRelativeMotion
void changeFrameWithRelativeMotion(ReferenceFramePtr newFrame, SpatialMotion twistOfCurrentFrameWithRespectToNewFrame, const SpatialMotion &twistOfBodyWrtBaseExpressedInCurrent)
Use this method to change the ReferenceFrame::expressedInFrame of a SpatialAcceleration if there is r...
Definition: SpatialAcceleration.cpp:14
RobotDynamics::Math::FrameVectorPair
A FrameVector is a pair of 3D vector with a ReferenceFrame.
Definition: FrameVectorPair.hpp:33
RobotDynamics::Model::three_x_qd0
Math::MatrixNd three_x_qd0
Definition: Model.h:282
RobotDynamics::Model::v_J
Math::SpatialMotionV v_J
Definition: Model.h:219
RobotDynamics::calcPointAcceleration6D
Math::FrameVectorPair calcPointAcceleration6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
Computes linear and angular acceleration of a point on a body.
Definition: Kinematics.cc:1631
RobotDynamics::calcPointVelocity6D
Math::FrameVectorPair calcPointVelocity6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
Computes angular and linear velocity of a point that is fixed on a body.
Definition: Kinematics.cc:1459
RobotDynamics::Math::SpatialVector::getLinearPart
EIGEN_STRONG_INLINE Vector3d getLinearPart() const
Definition: rdl_eigenmath.h:212
RobotDynamics::Model::multdof3_S
std::vector< Math::Matrix63 > multdof3_S
Motion subspace for joints with 3 degrees of freedom.
Definition: Model.h:235
RobotDynamics::updateAccelerations
void updateAccelerations(Model &model, const Math::VectorNd &QDDot)
Computes only the accelerations of the bodies.
Definition: Kinematics.cc:152
RobotDynamics::Model::fixedBodyFrames
std::vector< ReferenceFramePtr > fixedBodyFrames
Definition: Model.h:154
RobotDynamics::Math::MotionVector::transform_copy
MotionVector transform_copy(const SpatialTransform &X) const
Copies, transforms, and returns a MotionVector. Performs .
Definition: MotionVector.hpp:217
RobotDynamics::updateKinematicsCustomParallel
void updateKinematicsCustomParallel(Model &model, const Math::VectorNd *Q, const Math::VectorNd *QDot=nullptr, const Math::VectorNd *QDDot=nullptr)
Selectively updates model internal states of body positions, velocities and/or accelerations and spaw...
Definition: Kinematics.cc:194
RobotDynamics::Math::toMatrix
static Matrix3d toMatrix(const Quaternion &q)
Convert quaternion to rotation matrix.
Definition: SpatialAlgebraOperators.h:187
RobotDynamics::Math::SpatialMotion::setBodyFrame
void setBodyFrame(ReferenceFramePtr referenceFrame)
Sets the body frame of a spatial motion.
Definition: SpatialMotion.hpp:224
RobotDynamics
Namespace for all structures of the RobotDynamics library.
Definition: Body.h:21
RobotDynamics::Model::qdot_size
unsigned int qdot_size
The size of the.
Definition: Model.h:187
RobotDynamics::Model::S
Math::MotionVectorV S
Definition: Model.h:211

rdl_dynamics
Author(s):
autogenerated on Tue Apr 20 2021 02:25:27