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
51 anklePosition(0.0, 0.0, 0.0) {}
55 namespace pinocchio_robot {
63 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
67 virtual ~PinocchioRobot();
71 void computeInverseDynamics();
78 void computeInverseDynamics(Eigen::VectorXd &q, Eigen::VectorXd &v,
82 void computeForwardKinematics();
84 void RPYToSpatialFreeFlyer(Eigen::Vector3d &rpy, Eigen::Vector3d &drpy,
85 Eigen::Vector3d &ddrpy, Eigen::Quaterniond &quat,
86 Eigen::Vector3d &omega, Eigen::Vector3d &domega);
104 virtual bool ComputeSpecializedInverseKinematics(
105 const pinocchio::JointIndex &jointRoot,
106 const pinocchio::JointIndex &jointEnd,
107 const Eigen::Matrix4d &jointRootPosition,
108 const Eigen::Matrix4d &jointEndPosition, Eigen::VectorXd &q);
118 virtual bool testArmsInverseKinematics();
129 virtual bool testLegsInverseKinematics();
137 virtual bool testOneModeOfLegsInverseKinematics(
138 std::vector<std::string> &leftLegJointNames,
139 std::vector<std::string> &rightLegJointNames);
147 virtual void initializeLegsInverseKinematics();
151 std::vector<pinocchio::JointIndex> jointsBetween(
152 pinocchio::JointIndex first, pinocchio::JointIndex second);
153 std::vector<pinocchio::JointIndex> fromRootToIt(pinocchio::JointIndex it);
157 void getWaistFootKinematics(
const Eigen::Matrix4d &jointRootPosition,
158 const Eigen::Matrix4d &jointEndPosition,
159 Eigen::VectorXd &q, Eigen::Vector3d &Dt)
const;
160 double ComputeXmax(
double &Z);
161 void getShoulderWristKinematics(
const Eigen::Matrix4d &jointRootPosition,
162 const Eigen::Matrix4d &jointEndPosition,
163 Eigen::VectorXd &q,
int side);
164 void DetectAutomaticallyShoulders();
165 void DetectAutomaticallyOneShoulder(pinocchio::JointIndex aWrist,
166 pinocchio::JointIndex &aShoulder);
172 void ComputeRootSize();
177 inline pinocchio::Data *
Data() {
return m_robotData; }
179 return m_robotDataInInitialePose;
181 inline pinocchio::Model *
Model() {
return m_robotModel; }
186 inline pinocchio::JointIndex
leftWrist() {
return m_leftWrist; }
187 inline pinocchio::JointIndex
rightWrist() {
return m_rightWrist; }
189 inline pinocchio::JointIndex
chest() {
return m_chest; }
190 inline pinocchio::JointIndex
waist() {
return m_waist; }
192 inline double mass() {
return m_mass; }
195 return m_robotModel->joints;
206 inline unsigned numberDof() {
return (
unsigned)m_robotModel->nq; }
211 m_externalForces = m_robotData->liMi[1].act(m_robotData->f[1]);
212 m_f = m_externalForces.linear();
213 m_n = m_externalForces.angular();
214 zmp(0) = -m_n(1) / m_f(2);
215 zmp(1) = m_n(0) / m_f(2);
220 m_com = m_robotData->com[0];
226 Eigen::Vector3d &ddcom) {
227 m_com = m_robotData->acom[0];
232 m_com = m_robotData->vcom[0];
237 m_com = m_robotData->com[0];
245 void currentPinoConfiguration(Eigen::VectorXd &conf);
246 void currentRPYConfiguration(Eigen::VectorXd &);
253 return m_PinoFreeFlyerSize;
257 return m_PinoFreeFlyerVelSize;
263 return m_boolModel && m_boolData && m_boolLeftFoot && m_boolRightFoot;
265 bool checkModel(pinocchio::Model *robotModel);
266 bool initializeRobotModelAndData(pinocchio::Model *robotModel,
267 pinocchio::Data *robotData);
268 bool initializeLeftFoot(PRFoot leftFoot);
269 bool initializeRightFoot(PRFoot rightFoot);
271 const std::string &getName()
const;
275 pinocchio::Model *m_robotModel;
276 pinocchio::Data *m_robotDataInInitialePose;
277 pinocchio::Data *m_robotData;
278 PRFoot m_leftFoot, m_rightFoot;
280 pinocchio::JointIndex m_chest, m_waist, m_leftShoulder, m_rightShoulder;
281 pinocchio::JointIndex m_leftWrist, m_rightWrist;
286 Eigen::VectorXd m_qpino;
288 Eigen::VectorXd m_qrpy;
290 Eigen::VectorXd m_vpino;
292 Eigen::VectorXd m_vrpy;
294 Eigen::VectorXd m_apino;
295 Eigen::VectorXd m_arpy;
299 Eigen::Quaterniond m_quat;
300 Eigen::Matrix3d m_rot;
301 pinocchio::Force m_externalForces;
302 Eigen::VectorXd m_tau;
303 Eigen::Vector3d m_f, m_n;
304 Eigen::Vector3d m_com;
306 Eigen::Vector3d m_rpy, m_drpy, m_ddrpy, m_omega, m_domega;
310 bool m_isLegInverseKinematic;
311 unsigned int m_modeLegInverseKinematic;
312 bool m_isArmInverseKinematic;
315 Eigen::Vector3d m_leftDt, m_rightDt;
316 double m_femurLength;
317 double m_tibiaLengthZ;
318 double m_tibiaLengthY;
323 bool m_boolRightFoot;
326 pinocchio::JointIndex m_PinoFreeFlyerSize;
329 pinocchio::JointIndex m_PinoFreeFlyerVelSize;
333 #endif // PinocchioRobot_HH
pinocchio::JointIndex waist()
Definition: pinocchiorobot.hh:190
Eigen::VectorXd & currentPinoConfiguration()
Definition: pinocchiorobot.hh:198
pinocchio::JointIndex getFreeFlyerSize() const
Definition: pinocchiorobot.hh:252
Eigen::VectorXd & currentPinoVelocity()
Definition: pinocchiorobot.hh:200
pinocchio::Data * Data()
Definition: pinocchiorobot.hh:177
void currentPinoVelocity(Eigen::VectorXd &vel)
Definition: pinocchiorobot.hh:247
void positionCenterOfMass(Eigen::Vector3d &com)
Definition: pinocchiorobot.hh:219
void zeroMomentumPoint(Eigen::Vector3d &zmp)
Definition: pinocchiorobot.hh:210
double mass()
Definition: pinocchiorobot.hh:192
void currentRPYAcceleration(Eigen::VectorXd &acc)
Definition: pinocchiorobot.hh:250
void currentRPYVelocity(Eigen::VectorXd &vel)
Definition: pinocchiorobot.hh:249
pinocchio::JointIndex getFreeFlyerVelSize() const
Definition: pinocchiorobot.hh:256
pinocchio::JointIndex chest()
Definition: pinocchiorobot.hh:189
const int RPY_SIZE
Definition: pinocchiorobot.hh:56
PinocchioRobotFoot_t PRFoot
Definition: pinocchiorobot.hh:53
PRFoot * leftFoot()
Definition: pinocchiorobot.hh:183
Eigen::VectorXd & currentRPYConfiguration()
Definition: pinocchiorobot.hh:199
pinocchio::JointIndex leftWrist()
Definition: pinocchiorobot.hh:186
void currentPinoAcceleration(Eigen::VectorXd &acc)
Definition: pinocchiorobot.hh:248
unsigned numberVelDof()
Definition: pinocchiorobot.hh:208
pinocchio::JointIndex rightWrist()
Definition: pinocchiorobot.hh:187
Definition: pinocchiorobot.hh:60
const int QUATERNION_SIZE
Definition: pinocchiorobot.hh:57
pinocchio::JointModelVector & getActuatedJoints()
Definition: pinocchiorobot.hh:194
Eigen::VectorXd & currentRPYVelocity()
Definition: pinocchiorobot.hh:202
pinocchio::Model * Model()
Definition: pinocchiorobot.hh:181
bool isInitialized()
Definition: pinocchiorobot.hh:262
pinocchio::Data * DataInInitialePose()
Definition: pinocchiorobot.hh:178
doublereal * a
Definition: qld.cpp:386
Eigen::VectorXd & currentTau()
Definition: pinocchiorobot.hh:204
PRFoot * rightFoot()
Definition: pinocchiorobot.hh:184
Eigen::VectorXd & currentRPYAcceleration()
Definition: pinocchiorobot.hh:203
Eigen::VectorXd & currentPinoAcceleration()
Definition: pinocchiorobot.hh:201
void CenterOfMass(Eigen::Vector3d &com, Eigen::Vector3d &dcom, Eigen::Vector3d &ddcom)
Definition: pinocchiorobot.hh:225
Simulate a rigid body
Definition: patterngeneratorinterface.hh:41
unsigned numberDof()
Definition: pinocchiorobot.hh:206