18 #ifndef __invdyn_robot_wrapper_hpp__ 19 #define __invdyn_robot_wrapper_hpp__ 24 #include <pinocchio/multibody/model.hpp> 25 #include <pinocchio/multibody/data.hpp> 26 #include <pinocchio/spatial/fwd.hpp> 34 namespace talos_balance
44 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
48 typedef pinocchio::Data
Data;
51 typedef pinocchio::SE3
SE3;
62 const std::vector<std::string> & package_dirs,
66 const std::vector<std::string> & package_dirs,
67 const pinocchio::JointModelVariant & rootJoint,
70 virtual int nq()
const;
71 virtual int nv()
const;
78 const Model &
model()
const;
81 void computeAllTerms(Data & data,
const Vector & q,
const Vector & v)
const;
89 void com(
const Data & data,
94 const Vector3 &
com(
const Data & data)
const;
96 const Vector3 &
com_vel(
const Data & data)
const;
98 const Vector3 &
com_acc(
const Data & data)
const;
100 const Matrix3x &
Jcom(
const Data & data)
const;
102 const Matrix &
mass(
const Data & data);
106 const SE3 &
position(
const Data & data,
107 const Model::JointIndex index)
const;
109 const Motion &
velocity(
const Data & data,
110 const Model::JointIndex index)
const;
113 const Model::JointIndex index)
const;
116 const Model::JointIndex index,
117 Data::Matrix6x & J)
const;
120 const Model::JointIndex index,
121 Data::Matrix6x & J)
const;
124 const Model::FrameIndex index)
const;
127 const Model::FrameIndex index,
131 const Model::FrameIndex index)
const;
134 const Model::FrameIndex index,
138 const Model::FrameIndex index)
const;
141 const Model::FrameIndex index,
145 const Model::FrameIndex index)
const;
148 const Model::FrameIndex index,
152 const Model::FrameIndex index,
153 Data::Matrix6x & J)
const;
156 const Model::FrameIndex index,
157 Data::Matrix6x & J)
const;
181 #endif // ifndef __invdyn_robot_wrapper_hpp__ void frameJacobianWorld(const Data &data, const Model::FrameIndex index, Data::Matrix6x &J) const
Eigen::Matrix< Scalar, 3, 1 > Vector3
Eigen::Matrix< Scalar, 3, Eigen::Dynamic > Matrix3x
void jacobianWorld(const Data &data, const Model::JointIndex index, Data::Matrix6x &J) const
const Matrix3x & Jcom(const Data &data) const
const Vector & gear_ratios() const
Eigen::Ref< Vector > RefVector
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
void jacobianLocal(const Data &data, const Model::JointIndex index, Data::Matrix6x &J) const
void com(const Data &data, RefVector com_pos, RefVector com_vel, RefVector com_acc) const
Wrapper for a robot based on pinocchio.
SE3 framePosition(const Data &data, const Model::FrameIndex index) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::Scalar Scalar
std::string m_model_filename
const Vector3 & com_acc(const Data &data) const
const Vector & nonLinearEffects(const Data &data) const
Motion frameClassicAcceleration(const Data &data, const Model::FrameIndex index) const
const Eigen::Ref< const Vector > ConstRefVector
RobotWrapper(const std::string &filename, const std::vector< std::string > &package_dirs, bool verbose=false)
Motion frameVelocity(const Data &data, const Model::FrameIndex index) const
math::ConstRefVector ConstRefVector
const Matrix & mass(const Data &data)
const Motion & velocity(const Data &data, const Model::JointIndex index) const
void computeAllTerms(Data &data, const Vector &q, const Vector &v) const
Model m_model
Robot model.
const Model & model() const
Accessor to model.
const SE3 & position(const Data &data, const Model::JointIndex index) const
math::RefVector RefVector
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > Matrix
const Vector & rotor_inertias() const
Matrix m_M
diagonal part of inertia matrix due to rotor inertias
Motion frameAcceleration(const Data &data, const Model::FrameIndex index) const
const Motion & acceleration(const Data &data, const Model::JointIndex index) const
Eigen::Matrix< Scalar, 6, 1 > Vector6
void frameJacobianLocal(const Data &data, const Model::FrameIndex index, Data::Matrix6x &J) const
const Vector3 & com_vel(const Data &data) const