20 #include <pinocchio/multibody/model.hpp> 21 #include <pinocchio/parsers/urdf.hpp> 22 #include <pinocchio/algorithm/center-of-mass.hpp> 23 #include <pinocchio/algorithm/compute-all-terms.hpp> 24 #include <pinocchio/algorithm/jacobian.hpp> 25 #include <pinocchio/algorithm/frames.hpp> 34 namespace talos_balance
39 RobotWrapper::RobotWrapper(
const std::string & filename,
40 const std::vector<std::string> & ,
53 const std::vector<std::string> & ,
54 const pinocchio::JointModelVariant & rootJoint,
74 pinocchio::computeAllTerms(
m_model, data, q, v);
75 data.M.triangularView<Eigen::StrictlyLower>()
76 = data.M.transpose().triangularView<Eigen::StrictlyLower>();
78 pinocchio::centerOfMass(
m_model, data, 2,
false);
79 pinocchio::framesForwardKinematics(
m_model, data);
80 pinocchio::centerOfMass(
m_model, data, q, v, Eigen::VectorXd::Zero(
nv()));
118 com_pos = data.com[0];
119 com_vel = data.vcom[0];
120 com_acc = data.acom[0];
156 const Model::JointIndex index)
const 158 return data.oMi[index];
162 const Model::JointIndex index)
const 164 return data.v[index];
168 const Model::JointIndex index)
const 170 return data.a[index];
174 const Model::JointIndex index,
175 Data::Matrix6x & J)
const 177 return pinocchio::getJointJacobian(
m_model, data, index, pinocchio::WORLD, J);
181 const Model::JointIndex index,
182 Data::Matrix6x & J)
const 184 return pinocchio::getJointJacobian(
m_model, data, index, pinocchio::LOCAL, J);
188 const Model::FrameIndex index)
const 191 return data.oMi[f.parent].act(f.placement);
195 const Model::FrameIndex index,
199 framePosition = data.oMi[f.parent].act(f.placement);
203 const Model::FrameIndex index)
const 205 return pinocchio::getFrameVelocity(
m_model,data,index);
209 const Model::FrameIndex index,
212 frameVelocity = pinocchio::getFrameVelocity(
m_model,data,index);
216 const Model::FrameIndex index)
const 218 return pinocchio::getFrameAcceleration(
m_model,data,index);
222 const Model::FrameIndex index,
225 frameAcceleration = pinocchio::getFrameAcceleration(
m_model,data,index);
229 const Model::FrameIndex index)
const 231 Motion a = pinocchio::getFrameAcceleration(
m_model,data,index);
233 a.linear() += v.angular().cross(v.linear());
238 const Model::FrameIndex index,
241 frameAcceleration = pinocchio::getFrameAcceleration(
m_model,data,index);
243 frameAcceleration.linear() += v.angular().cross(v.linear());
247 const Model::FrameIndex index,
248 Data::Matrix6x & J)
const 250 return pinocchio::getFrameJacobian(
m_model, data, index, pinocchio::WORLD, J);
254 const Model::FrameIndex index,
255 Data::Matrix6x & J)
const 257 return pinocchio::getFrameJacobian(
m_model, data, index, pinocchio::LOCAL, J);
void frameJacobianWorld(const Data &data, const Model::FrameIndex index, Data::Matrix6x &J) const
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
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
SE3 framePosition(const Data &data, const Model::FrameIndex index) const
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
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
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
void frameJacobianLocal(const Data &data, const Model::FrameIndex index, Data::Matrix6x &J) const
const Vector3 & com_vel(const Data &data) const