Jacobian.h
Go to the documentation of this file.
1 /*
2  * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
3  */
4 
5 #pragma once
6 
7 // includes
8 // RBDyn
9 #include <rbdyn/config.hh>
10 
11 #include "MultiBody.h"
12 
13 namespace rbd
14 {
15 struct MultiBodyConfig;
16 
18 struct Block
19 {
20  Block() = default;
21  Block(Eigen::DenseIndex startDof, Eigen::DenseIndex startJac, Eigen::DenseIndex length)
22  : startDof(startDof), startJac(startJac), length(length)
23  {
24  }
26  Eigen::DenseIndex startDof;
28  Eigen::DenseIndex startJac;
30  Eigen::DenseIndex length;
31 };
32 
33 using Blocks = std::vector<Block>;
34 
38 class RBDYN_DLLAPI Jacobian
39 {
40 public:
41  Jacobian();
42 
50  Jacobian(const MultiBody & mb, const std::string & bodyName, const Eigen::Vector3d & point = Eigen::Vector3d::Zero());
51 
60  Jacobian(const MultiBody & mb,
61  const std::string & bodyName,
62  const std::string & refBodyName,
63  const Eigen::Vector3d & point = Eigen::Vector3d::Zero());
64 
72  const Eigen::MatrixXd & jacobian(const MultiBody & mb, const MultiBodyConfig & mbc, const sva::PTransformd & X_0_p);
73 
80  const Eigen::MatrixXd & jacobian(const MultiBody & mb, const MultiBodyConfig & mbc);
81 
88  const Eigen::MatrixXd & bodyJacobian(const MultiBody & mb, const MultiBodyConfig & mbc);
89 
98  const Eigen::MatrixXd & vectorJacobian(const MultiBody & mb,
99  const MultiBodyConfig & mbc,
100  const Eigen::Vector3d & vector);
101 
110  const Eigen::MatrixXd & vectorBodyJacobian(const MultiBody & mb,
111  const MultiBodyConfig & mbc,
112  const Eigen::Vector3d & vector);
113 
120  const Eigen::MatrixXd & jacobianDot(const MultiBody & mb, const MultiBodyConfig & mbc);
121 
128  const Eigen::MatrixXd & bodyJacobianDot(const MultiBody & mb, const MultiBodyConfig & mbc);
129 
137  sva::MotionVecd velocity(const MultiBody & mb, const MultiBodyConfig & mbc, const sva::PTransformd & X_b_p) const;
138 
145  sva::MotionVecd velocity(const MultiBody & mb, const MultiBodyConfig & mbc) const;
146 
153  sva::MotionVecd bodyVelocity(const MultiBody & mb, const MultiBodyConfig & mbc) const;
154 
163  sva::MotionVecd normalAcceleration(const MultiBody & mb,
164  const MultiBodyConfig & mbc,
165  const sva::PTransformd & X_b_p,
166  const sva::MotionVecd & V_b_p) const;
167 
174  sva::MotionVecd normalAcceleration(const MultiBody & mb, const MultiBodyConfig & mbc) const;
175 
182  sva::MotionVecd bodyNormalAcceleration(const MultiBody & mb, const MultiBodyConfig & mbc) const;
183 
193  sva::MotionVecd normalAcceleration(const MultiBody & mb,
194  const MultiBodyConfig & mbc,
195  const std::vector<sva::MotionVecd> & normalAccB,
196  const sva::PTransformd & X_b_p,
197  const sva::MotionVecd & V_b_p) const;
198 
206  sva::MotionVecd normalAcceleration(const MultiBody & mb,
207  const MultiBodyConfig & mbc,
208  const std::vector<sva::MotionVecd> & normalAccB) const;
209 
217  sva::MotionVecd bodyNormalAcceleration(const MultiBody & mb,
218  const MultiBodyConfig & mbc,
219  const std::vector<sva::MotionVecd> & normalAccB) const;
220 
228  void translateJacobian(const Eigen::Ref<const Eigen::MatrixXd> & jac,
229  const MultiBodyConfig & mbc,
230  const Eigen::Vector3d & point,
231  Eigen::MatrixXd & res);
232 
240  void translateBodyJacobian(const Eigen::Ref<const Eigen::MatrixXd> & jac,
241  const MultiBodyConfig & mbc,
242  const Eigen::Vector3d & point,
243  Eigen::MatrixXd & res);
244 
251  void fullJacobian(const MultiBody & mb, const Eigen::Ref<const Eigen::MatrixXd> & jac, Eigen::MatrixXd & res) const;
252 
260  void addFullJacobian(const MultiBody & mb,
261  const Eigen::Ref<const Eigen::MatrixXd> & jac,
262  Eigen::MatrixXd & res) const;
263 
272  void addFullJacobian(const Blocks & compactPath,
273  const Eigen::Ref<const Eigen::MatrixXd> & jac,
274  Eigen::MatrixXd & res) const;
275 
282  Eigen::MatrixXd expand(const rbd::MultiBody & mb, const Eigen::Ref<const Eigen::MatrixXd> & jac) const;
283 
291  void expandAdd(const rbd::MultiBody & mb, const Eigen::Ref<const Eigen::MatrixXd> & jac, Eigen::MatrixXd & res) const;
292 
298  Blocks compactPath(const rbd::MultiBody & mb) const;
299 
309  void expandAdd(const Blocks & compactPath,
310  const Eigen::Ref<const Eigen::MatrixXd> & jac,
311  Eigen::MatrixXd & res) const;
312 
315  MultiBody subMultiBody(const MultiBody & mb) const;
316 
318  const std::vector<int> & jointsPath() const
319  {
320  return jointsPath_;
321  }
322 
324  int dof() const
325  {
326  return static_cast<int>(jac_.cols());
327  }
328 
330  const Eigen::Vector3d & point() const
331  {
332  return point_.translation();
333  }
334 
336  void point(const Eigen::Vector3d & point)
337  {
338  point_ = sva::PTransformd(point);
339  }
340 
341  // safe version for python binding
342 
346  const Eigen::MatrixXd & sJacobian(const MultiBody & mb, const MultiBodyConfig & mbc, const sva::PTransformd & X_0_p);
347 
351  const Eigen::MatrixXd & sJacobian(const MultiBody & mb, const MultiBodyConfig & mbc);
352 
356  const Eigen::MatrixXd & sBodyJacobian(const MultiBody & mb, const MultiBodyConfig & mbc);
357 
361  const Eigen::MatrixXd & sVectorJacobian(const MultiBody & mb,
362  const MultiBodyConfig & mbc,
363  const Eigen::Vector3d & vec);
364 
368  const Eigen::MatrixXd & sVectorBodyJacobian(const MultiBody & mb,
369  const MultiBodyConfig & mbc,
370  const Eigen::Vector3d & vec);
371 
375  MultiBody sSubMultiBody(const MultiBody & mb) const;
376 
380  const Eigen::MatrixXd & sJacobianDot(const MultiBody & mb, const MultiBodyConfig & mbc);
381 
385  const Eigen::MatrixXd & sBodyJacobianDot(const MultiBody & mb, const MultiBodyConfig & mbc);
386 
391  void sTranslateJacobian(const Eigen::MatrixXd & jac,
392  const MultiBodyConfig & mbc,
393  const Eigen::Vector3d & point,
394  Eigen::MatrixXd & res);
395 
400  void sFullJacobian(const MultiBody & mb, const Eigen::MatrixXd & jac, Eigen::MatrixXd & res) const;
401 
405  sva::MotionVecd sVelocity(const MultiBody & mb, const MultiBodyConfig & mbc, const sva::PTransformd & X_b_p) const;
406 
410  sva::MotionVecd sVelocity(const MultiBody & mb, const MultiBodyConfig & mbc) const;
411 
415  sva::MotionVecd sNormalAcceleration(const MultiBody & mb,
416  const MultiBodyConfig & mbc,
417  const sva::PTransformd & X_b_p,
418  const sva::MotionVecd & V_b_p) const;
419 
423  sva::MotionVecd sNormalAcceleration(const MultiBody & mb,
424  const MultiBodyConfig & mbc,
425  const std::vector<sva::MotionVecd> & normalAccB,
426  const sva::PTransformd & X_b_p,
427  const sva::MotionVecd & V_b_p) const;
428 
432  sva::MotionVecd sNormalAcceleration(const MultiBody & mb, const MultiBodyConfig & mbc) const;
433 
437  sva::MotionVecd sBodyVelocity(const MultiBody & mb, const MultiBodyConfig & mbc) const;
438 
442  sva::MotionVecd sBodyNormalAcceleration(const MultiBody & mb, const MultiBodyConfig & mbc) const;
443 
447  sva::MotionVecd sNormalAcceleration(const MultiBody & mb,
448  const MultiBodyConfig & mbc,
449  const std::vector<sva::MotionVecd> & normalAccB) const;
450 
454  sva::MotionVecd sBodyNormalAcceleration(const MultiBody & mb,
455  const MultiBodyConfig & mbc,
456  const std::vector<sva::MotionVecd> & normalAccB) const;
457 
458 private:
459  sva::MotionVecd normalAcceleration(const MultiBodyConfig & mbc,
460  const sva::MotionVecd & bodyNNormalAcc,
461  const sva::PTransformd & X_b_p,
462  const sva::MotionVecd & V_b_p) const;
463  sva::MotionVecd normalAcceleration(const MultiBodyConfig & mbc, const sva::MotionVecd & bodyNNormalAcc) const;
464  sva::MotionVecd bodyNormalAcceleration(const MultiBodyConfig & mbc, const sva::MotionVecd & bodyNNormalAcc) const;
465 
466 private:
467  std::vector<int> jointsPath_;
468  std::vector<double> jointsSign_;
469  sva::PTransformd point_;
470 
471  Eigen::MatrixXd jac_;
472  Eigen::MatrixXd jacDot_;
473 
474  int bodyIndex_;
475  int refBodyIndex_;
476 };
477 
478 } // namespace rbd
rbd::Block::length
Eigen::DenseIndex length
Definition: Jacobian.h:30
rbd::Block::Block
Block()=default
rbd::Block::Block
Block(Eigen::DenseIndex startDof, Eigen::DenseIndex startJac, Eigen::DenseIndex length)
Definition: Jacobian.h:21
rbd::Jacobian
Definition: Jacobian.h:38
rbd::Block::startJac
Eigen::DenseIndex startJac
Definition: Jacobian.h:28
rbd::MultiBody
Definition: MultiBody.h:29
rbd::Jacobian::point
const Eigen::Vector3d & point() const
Definition: Jacobian.h:330
rbd::Blocks
std::vector< Block > Blocks
Definition: Jacobian.h:33
rbd
Definition: common.h:20
rbd::MultiBodyConfig
Definition: MultiBodyConfig.h:23
rbd::Jacobian::jointsPath
const std::vector< int > & jointsPath() const
Definition: Jacobian.h:318
MultiBody.h
rbd::Jacobian::dof
int dof() const
Definition: Jacobian.h:324
rbd::Jacobian::point
void point(const Eigen::Vector3d &point)
Definition: Jacobian.h:336
rbd::Block::startDof
Eigen::DenseIndex startDof
Definition: Jacobian.h:26
rbd::Block
Definition: Jacobian.h:18