Go to the documentation of this file.
29 #ifndef PinocchioRobot_HH
30 #define PinocchioRobot_HH
32 #include "pinocchio/multibody/data.hpp"
33 #include "pinocchio/multibody/model.hpp"
34 #include "pinocchio/parsers/urdf.hpp"
35 #include "pinocchio/spatial/se3.hpp"
44 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
52 namespace pinocchio_robot {
60 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
82 Eigen::Vector3d &ddrpy, Eigen::Quaterniond &quat,
83 Eigen::Vector3d &omega, Eigen::Vector3d &domega);
103 const pinocchio::JointIndex &jointEnd,
104 const Eigen::Matrix4d &jointRootPosition,
105 const Eigen::Matrix4d &jointEndPosition,
136 std::vector<std::string> &leftLegJointNames,
137 std::vector<std::string> &rightLegJointNames);
149 std::vector<pinocchio::JointIndex>
150 jointsBetween(pinocchio::JointIndex first, pinocchio::JointIndex second);
151 std::vector<pinocchio::JointIndex>
fromRootToIt(pinocchio::JointIndex it);
155 void getWaistFootKinematics(
const Eigen::Matrix4d &jointRootPosition,
156 const Eigen::Matrix4d &jointEndPosition,
157 Eigen::VectorXd &q, Eigen::Vector3d &Dt)
const;
158 double ComputeXmax(
double &Z);
159 void getShoulderWristKinematics(
const Eigen::Matrix4d &jointRootPosition,
160 const Eigen::Matrix4d &jointEndPosition,
161 Eigen::VectorXd &q,
int side);
162 void DetectAutomaticallyShoulders();
163 void DetectAutomaticallyOneShoulder(pinocchio::JointIndex aWrist,
164 pinocchio::JointIndex &aShoulder);
170 void ComputeRootSize();
175 inline pinocchio::Data *
Data() {
return m_robotData; }
177 return m_robotDataInInitialePose;
179 inline pinocchio::Model *
Model() {
return m_robotModel; }
184 inline pinocchio::JointIndex
leftWrist() {
return m_leftWrist; }
185 inline pinocchio::JointIndex
rightWrist() {
return m_rightWrist; }
187 inline pinocchio::JointIndex
chest() {
return m_chest; }
188 inline pinocchio::JointIndex
waist() {
return m_waist; }
190 inline double mass() {
return m_mass; }
193 return m_robotModel->joints;
204 inline unsigned numberDof() {
return (
unsigned)m_robotModel->nq; }
209 m_externalForces = m_robotData->liMi[1].act(m_robotData->f[1]);
210 m_f = m_externalForces.linear();
211 m_n = m_externalForces.angular();
212 zmp(0) = -m_n(1) / m_f(2);
213 zmp(1) = m_n(0) / m_f(2);
218 m_com = m_robotData->com[0];
224 Eigen::Vector3d &ddcom) {
225 m_com = m_robotData->acom[0];
230 m_com = m_robotData->vcom[0];
235 m_com = m_robotData->com[0];
251 return m_PinoFreeFlyerSize;
255 return m_PinoFreeFlyerVelSize;
261 return m_boolModel && m_boolData && m_boolLeftFoot && m_boolRightFoot;
263 bool checkModel(pinocchio::Model *robotModel);
265 pinocchio::Data *robotData);
269 const std::string &
getName()
const;
273 pinocchio::Model *m_robotModel;
274 pinocchio::Data *m_robotDataInInitialePose;
275 pinocchio::Data *m_robotData;
276 PRFoot m_leftFoot, m_rightFoot;
278 pinocchio::JointIndex m_chest, m_waist, m_leftShoulder, m_rightShoulder;
279 pinocchio::JointIndex m_leftWrist, m_rightWrist;
284 Eigen::VectorXd m_qpino;
286 Eigen::VectorXd m_qrpy;
288 Eigen::VectorXd m_vpino;
290 Eigen::VectorXd m_vrpy;
292 Eigen::VectorXd m_apino;
293 Eigen::VectorXd m_arpy;
297 Eigen::Quaterniond m_quat;
298 Eigen::Matrix3d m_rot;
299 pinocchio::Force m_externalForces;
300 Eigen::VectorXd m_tau;
301 Eigen::Vector3d m_f, m_n;
302 Eigen::Vector3d m_com;
304 Eigen::Vector3d m_rpy, m_drpy, m_ddrpy, m_omega, m_domega;
308 bool m_isLegInverseKinematic;
309 unsigned int m_modeLegInverseKinematic;
310 bool m_isArmInverseKinematic;
313 Eigen::Vector3d m_leftDt, m_rightDt;
314 double m_femurLength;
315 double m_tibiaLengthZ;
316 double m_tibiaLengthY;
321 bool m_boolRightFoot;
324 pinocchio::JointIndex m_PinoFreeFlyerSize;
327 pinocchio::JointIndex m_PinoFreeFlyerVelSize;
331 #endif // PinocchioRobot_HH
pinocchio::JointIndex waist()
Definition: pinocchiorobot.hh:188
void positionCenterOfMass(Eigen::Vector3d &com)
Definition: pinocchiorobot.hh:217
Eigen::VectorXd & currentRPYAcceleration()
Definition: pinocchiorobot.hh:201
const std::string & getName() const
pinocchio::Data * Data()
Definition: pinocchiorobot.hh:175
void currentRPYAcceleration(Eigen::VectorXd &acc)
Definition: pinocchiorobot.hh:248
virtual bool testLegsInverseKinematics()
testLegsInverseKinematics : test if the robot legs has the good joint configuration to use the analit...
virtual bool testOneModeOfLegsInverseKinematics(std::vector< std::string > &leftLegJointNames, std::vector< std::string > &rightLegJointNames)
testOneModefOfLegInverseKinematics : test if the robot legs has one good joint configuration to use t...
bool initializeRightFoot(PRFoot rightFoot)
doublereal * a
Definition: qld.cpp:386
PRFoot * leftFoot()
Definition: pinocchiorobot.hh:181
pinocchio::JointModelVector & getActuatedJoints()
Definition: pinocchiorobot.hh:192
Eigen::VectorXd & currentPinoConfiguration()
Definition: pinocchiorobot.hh:196
pinocchio::Data * DataInInitialePose()
Definition: pinocchiorobot.hh:176
PinocchioRobotFoot_t PRFoot
Definition: pinocchiorobot.hh:50
pinocchio::JointIndex getFreeFlyerSize() const
Definition: pinocchiorobot.hh:250
Eigen::VectorXd & currentRPYVelocity()
Definition: pinocchiorobot.hh:200
Eigen::VectorXd & currentTau()
Definition: pinocchiorobot.hh:202
Eigen::VectorXd & currentPinoVelocity()
Definition: pinocchiorobot.hh:198
virtual void initializeLegsInverseKinematics()
initializeInverseKinematics : initialize the internal data for the inverse kinematic e....
virtual bool ComputeSpecializedInverseKinematics(const pinocchio::JointIndex &jointRoot, const pinocchio::JointIndex &jointEnd, const Eigen::Matrix4d &jointRootPosition, const Eigen::Matrix4d &jointEndPosition, Eigen::VectorXd &q)
ComputeSpecializedInverseKinematics : compute POSITION (not velocity) of the joints from end effector...
double mass()
Definition: pinocchiorobot.hh:190
void RPYToSpatialFreeFlyer(Eigen::Vector3d &rpy, Eigen::Vector3d &drpy, Eigen::Vector3d &ddrpy, Eigen::Quaterniond &quat, Eigen::Vector3d &omega, Eigen::Vector3d &domega)
void computeInverseDynamics()
pinocchio::JointIndex chest()
Definition: pinocchiorobot.hh:187
Eigen::VectorXd & currentPinoAcceleration()
Definition: pinocchiorobot.hh:199
void currentPinoAcceleration(Eigen::VectorXd &acc)
Definition: pinocchiorobot.hh:246
void computeForwardKinematics()
Compute the geometry of the robot.
std::vector< pinocchio::JointIndex > fromRootToIt(pinocchio::JointIndex it)
void currentRPYVelocity(Eigen::VectorXd &vel)
Definition: pinocchiorobot.hh:247
pinocchio::Model * Model()
Definition: pinocchiorobot.hh:179
bool initializeRobotModelAndData(pinocchio::Model *robotModel, pinocchio::Data *robotData)
pinocchio::JointIndex rightWrist()
Definition: pinocchiorobot.hh:185
bool initializeLeftFoot(PRFoot leftFoot)
pinocchio::JointIndex leftWrist()
Definition: pinocchiorobot.hh:184
virtual ~PinocchioRobot()
const int QUATERNION_SIZE
Definition: pinocchiorobot.hh:54
Definition: pinocchiorobot.hh:57
void zeroMomentumPoint(Eigen::Vector3d &zmp)
Definition: pinocchiorobot.hh:208
PRFoot * rightFoot()
Definition: pinocchiorobot.hh:182
pinocchio::JointIndex getFreeFlyerVelSize() const
Definition: pinocchiorobot.hh:254
\doc Simulate a rigid body
Definition: patterngeneratorinterface.hh:41
virtual bool testArmsInverseKinematics()
testArmsInverseKinematics : test if the robot arms has the good joint configuration to use the analit...
std::vector< pinocchio::JointIndex > jointsBetween(pinocchio::JointIndex first, pinocchio::JointIndex second)
tools :
const int RPY_SIZE
Definition: pinocchiorobot.hh:53
unsigned numberVelDof()
Definition: pinocchiorobot.hh:206
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PinocchioRobot()
Constructor and Destructor.
unsigned numberDof()
Definition: pinocchiorobot.hh:204
void currentPinoVelocity(Eigen::VectorXd &vel)
Definition: pinocchiorobot.hh:245
Eigen::VectorXd & currentRPYConfiguration()
Definition: pinocchiorobot.hh:197
bool checkModel(pinocchio::Model *robotModel)
bool isInitialized()
Definition: pinocchiorobot.hh:260
void CenterOfMass(Eigen::Vector3d &com, Eigen::Vector3d &dcom, Eigen::Vector3d &ddcom)
Definition: pinocchiorobot.hh:223