Jacobian.h

Go to the documentation of this file.

/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
// includes
// RBDyn
#include <rbdyn/config.hh>
 
#include "MultiBody.h"
 
namespace rbd
{
struct MultiBodyConfig;
 
struct Block
{
  Block() = default;
  Block(Eigen::DenseIndex startDof, Eigen::DenseIndex startJac, Eigen::DenseIndex length)
  : startDof(startDof), startJac(startJac), length(length)
  {
  }
  Eigen::DenseIndex startDof;
  Eigen::DenseIndex startJac;
  Eigen::DenseIndex length;
};
 
using Blocks = std::vector<Block>;
 
class RBDYN_DLLAPI Jacobian
{
public:
  Jacobian();
 
  Jacobian(const MultiBody & mb, const std::string & bodyName, const Eigen::Vector3d & point = Eigen::Vector3d::Zero());
 
  Jacobian(const MultiBody & mb,
           const std::string & bodyName,
           const std::string & refBodyName,
           const Eigen::Vector3d & point = Eigen::Vector3d::Zero());
 
  const Eigen::MatrixXd & jacobian(const MultiBody & mb, const MultiBodyConfig & mbc, const sva::PTransformd & X_0_p);
 
  const Eigen::MatrixXd & jacobian(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  const Eigen::MatrixXd & bodyJacobian(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  const Eigen::MatrixXd & vectorJacobian(const MultiBody & mb,
                                         const MultiBodyConfig & mbc,
                                         const Eigen::Vector3d & vector);
 
  const Eigen::MatrixXd & vectorBodyJacobian(const MultiBody & mb,
                                             const MultiBodyConfig & mbc,
                                             const Eigen::Vector3d & vector);
 
  const Eigen::MatrixXd & jacobianDot(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  const Eigen::MatrixXd & bodyJacobianDot(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  sva::MotionVecd velocity(const MultiBody & mb, const MultiBodyConfig & mbc, const sva::PTransformd & X_b_p) const;
 
  sva::MotionVecd velocity(const MultiBody & mb, const MultiBodyConfig & mbc) const;
 
  sva::MotionVecd bodyVelocity(const MultiBody & mb, const MultiBodyConfig & mbc) const;
 
  sva::MotionVecd normalAcceleration(const MultiBody & mb,
                                     const MultiBodyConfig & mbc,
                                     const sva::PTransformd & X_b_p,
                                     const sva::MotionVecd & V_b_p) const;
 
  sva::MotionVecd normalAcceleration(const MultiBody & mb, const MultiBodyConfig & mbc) const;
 
  sva::MotionVecd bodyNormalAcceleration(const MultiBody & mb, const MultiBodyConfig & mbc) const;
 
  sva::MotionVecd normalAcceleration(const MultiBody & mb,
                                     const MultiBodyConfig & mbc,
                                     const std::vector<sva::MotionVecd> & normalAccB,
                                     const sva::PTransformd & X_b_p,
                                     const sva::MotionVecd & V_b_p) const;
 
  sva::MotionVecd normalAcceleration(const MultiBody & mb,
                                     const MultiBodyConfig & mbc,
                                     const std::vector<sva::MotionVecd> & normalAccB) const;
 
  sva::MotionVecd bodyNormalAcceleration(const MultiBody & mb,
                                         const MultiBodyConfig & mbc,
                                         const std::vector<sva::MotionVecd> & normalAccB) const;
 
  void translateJacobian(const Eigen::Ref<const Eigen::MatrixXd> & jac,
                         const MultiBodyConfig & mbc,
                         const Eigen::Vector3d & point,
                         Eigen::MatrixXd & res);
 
  void translateBodyJacobian(const Eigen::Ref<const Eigen::MatrixXd> & jac,
                             const MultiBodyConfig & mbc,
                             const Eigen::Vector3d & point,
                             Eigen::MatrixXd & res);
 
  void fullJacobian(const MultiBody & mb, const Eigen::Ref<const Eigen::MatrixXd> & jac, Eigen::MatrixXd & res) const;
 
  void addFullJacobian(const MultiBody & mb,
                       const Eigen::Ref<const Eigen::MatrixXd> & jac,
                       Eigen::MatrixXd & res) const;
 
  void addFullJacobian(const Blocks & compactPath,
                       const Eigen::Ref<const Eigen::MatrixXd> & jac,
                       Eigen::MatrixXd & res) const;
 
  Eigen::MatrixXd expand(const rbd::MultiBody & mb, const Eigen::Ref<const Eigen::MatrixXd> & jac) const;
 
  void expandAdd(const rbd::MultiBody & mb, const Eigen::Ref<const Eigen::MatrixXd> & jac, Eigen::MatrixXd & res) const;
 
  Blocks compactPath(const rbd::MultiBody & mb) const;
 
  void expandAdd(const Blocks & compactPath,
                 const Eigen::Ref<const Eigen::MatrixXd> & jac,
                 Eigen::MatrixXd & res) const;
 
  MultiBody subMultiBody(const MultiBody & mb) const;
 
  const std::vector<int> & jointsPath() const
  {
    return jointsPath_;
  }
 
  int dof() const
  {
    return static_cast<int>(jac_.cols());
  }
 
  const Eigen::Vector3d & point() const
  {
    return point_.translation();
  }
 
  void point(const Eigen::Vector3d & point)
  {
    point_ = sva::PTransformd(point);
  }
 
  // safe version for python binding
 
  const Eigen::MatrixXd & sJacobian(const MultiBody & mb, const MultiBodyConfig & mbc, const sva::PTransformd & X_0_p);
 
  const Eigen::MatrixXd & sJacobian(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  const Eigen::MatrixXd & sBodyJacobian(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  const Eigen::MatrixXd & sVectorJacobian(const MultiBody & mb,
                                          const MultiBodyConfig & mbc,
                                          const Eigen::Vector3d & vec);
 
  const Eigen::MatrixXd & sVectorBodyJacobian(const MultiBody & mb,
                                              const MultiBodyConfig & mbc,
                                              const Eigen::Vector3d & vec);
 
  MultiBody sSubMultiBody(const MultiBody & mb) const;
 
  const Eigen::MatrixXd & sJacobianDot(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  const Eigen::MatrixXd & sBodyJacobianDot(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  void sTranslateJacobian(const Eigen::MatrixXd & jac,
                          const MultiBodyConfig & mbc,
                          const Eigen::Vector3d & point,
                          Eigen::MatrixXd & res);
 
  void sFullJacobian(const MultiBody & mb, const Eigen::MatrixXd & jac, Eigen::MatrixXd & res) const;
 
  sva::MotionVecd sVelocity(const MultiBody & mb, const MultiBodyConfig & mbc, const sva::PTransformd & X_b_p) const;
 
  sva::MotionVecd sVelocity(const MultiBody & mb, const MultiBodyConfig & mbc) const;
 
  sva::MotionVecd sNormalAcceleration(const MultiBody & mb,
                                      const MultiBodyConfig & mbc,
                                      const sva::PTransformd & X_b_p,
                                      const sva::MotionVecd & V_b_p) const;
 
  sva::MotionVecd sNormalAcceleration(const MultiBody & mb,
                                      const MultiBodyConfig & mbc,
                                      const std::vector<sva::MotionVecd> & normalAccB,
                                      const sva::PTransformd & X_b_p,
                                      const sva::MotionVecd & V_b_p) const;
 
  sva::MotionVecd sNormalAcceleration(const MultiBody & mb, const MultiBodyConfig & mbc) const;
 
  sva::MotionVecd sBodyVelocity(const MultiBody & mb, const MultiBodyConfig & mbc) const;
 
  sva::MotionVecd sBodyNormalAcceleration(const MultiBody & mb, const MultiBodyConfig & mbc) const;
 
  sva::MotionVecd sNormalAcceleration(const MultiBody & mb,
                                      const MultiBodyConfig & mbc,
                                      const std::vector<sva::MotionVecd> & normalAccB) const;
 
  sva::MotionVecd sBodyNormalAcceleration(const MultiBody & mb,
                                          const MultiBodyConfig & mbc,
                                          const std::vector<sva::MotionVecd> & normalAccB) const;
 
private:
  sva::MotionVecd normalAcceleration(const MultiBodyConfig & mbc,
                                     const sva::MotionVecd & bodyNNormalAcc,
                                     const sva::PTransformd & X_b_p,
                                     const sva::MotionVecd & V_b_p) const;
  sva::MotionVecd normalAcceleration(const MultiBodyConfig & mbc, const sva::MotionVecd & bodyNNormalAcc) const;
  sva::MotionVecd bodyNormalAcceleration(const MultiBodyConfig & mbc, const sva::MotionVecd & bodyNNormalAcc) const;
 
private:
  std::vector<int> jointsPath_;
  std::vector<double> jointsSign_;
  sva::PTransformd point_;
 
  Eigen::MatrixXd jac_;
  Eigen::MatrixXd jacDot_;
 
  int bodyIndex_;
  int refBodyIndex_;
};
 
} // namespace rbd