pinocchiorobot.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2016,
3  *
4  * Maximilien Naveau
5  * Olivier Stasse
6  *
7  * JRL, CNRS/AIST
8  *
9  * This file is part of jrl-walkgen.
10  * jrl-walkgen is free software: you can redistribute it and/or modify
11  * it under the terms of the GNU Lesser General Public License as published by
12  * the Free Software Foundation, either version 3 of the License, or
13  * (at your option) any later version.
14  *
15  * jrl-walkgen is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU General Lesser Public License for more details.
19  * You should have received a copy of the GNU Lesser General Public License
20  * along with jrl-walkgen. If not, see <http://www.gnu.org/licenses/>.
21  *
22  * Research carried out within the scope of the
23  * Joint Japanese-French Robotics Laboratory (JRL)
24  */
29 #ifndef PinocchioRobot_HH
30 #define PinocchioRobot_HH
31 
32 #include "pinocchio/multibody/data.hpp"
33 #include "pinocchio/multibody/model.hpp"
34 #include "pinocchio/parsers/urdf.hpp"
35 #include "pinocchio/spatial/se3.hpp"
36 
37 namespace PatternGeneratorJRL {
39  pinocchio::JointIndex associatedAnkle;
40  double soleDepth; // z axis
41  double soleWidth; // y axis
42  double soleHeight; // x axis
43  Eigen::Vector3d anklePosition;
44  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45 
47  : associatedAnkle(0), soleDepth(0.0), soleWidth(0.0), soleHeight(0.0),
48  anklePosition(0.0, 0.0, 0.0) {}
49 };
51 
52 namespace pinocchio_robot {
53 const int RPY_SIZE = 6;
54 const int QUATERNION_SIZE = 7;
55 } // namespace pinocchio_robot
56 
58 public:
59  // overload the new[] eigen operator
60  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
61 
64  virtual ~PinocchioRobot();
65 
69 
75  void computeInverseDynamics(Eigen::VectorXd &q, Eigen::VectorXd &v,
76  Eigen::VectorXd &a);
77 
80 
81  void RPYToSpatialFreeFlyer(Eigen::Vector3d &rpy, Eigen::Vector3d &drpy,
82  Eigen::Vector3d &ddrpy, Eigen::Quaterniond &quat,
83  Eigen::Vector3d &omega, Eigen::Vector3d &domega);
84 
101  virtual bool
102  ComputeSpecializedInverseKinematics(const pinocchio::JointIndex &jointRoot,
103  const pinocchio::JointIndex &jointEnd,
104  const Eigen::Matrix4d &jointRootPosition,
105  const Eigen::Matrix4d &jointEndPosition,
106  Eigen::VectorXd &q);
107 
116  virtual bool testArmsInverseKinematics();
127  virtual bool testLegsInverseKinematics();
136  std::vector<std::string> &leftLegJointNames,
137  std::vector<std::string> &rightLegJointNames);
138 
145  virtual void initializeLegsInverseKinematics();
146 
147 public:
149  std::vector<pinocchio::JointIndex>
150  jointsBetween(pinocchio::JointIndex first, pinocchio::JointIndex second);
151  std::vector<pinocchio::JointIndex> fromRootToIt(pinocchio::JointIndex it);
152 
153 private:
154  // needed for the inverse geometry (ComputeSpecializedInverseKinematics)
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);
165 
170  void ComputeRootSize();
171 
172 public:
175  inline pinocchio::Data *Data() { return m_robotData; }
176  inline pinocchio::Data *DataInInitialePose() {
177  return m_robotDataInInitialePose;
178  }
179  inline pinocchio::Model *Model() { return m_robotModel; }
180 
181  inline PRFoot *leftFoot() { return &m_leftFoot; }
182  inline PRFoot *rightFoot() { return &m_rightFoot; }
183 
184  inline pinocchio::JointIndex leftWrist() { return m_leftWrist; }
185  inline pinocchio::JointIndex rightWrist() { return m_rightWrist; }
186 
187  inline pinocchio::JointIndex chest() { return m_chest; }
188  inline pinocchio::JointIndex waist() { return m_waist; }
189 
190  inline double mass() { return m_mass; }
191 
192  inline pinocchio::JointModelVector &getActuatedJoints() {
193  return m_robotModel->joints;
194  }
195 
196  inline Eigen::VectorXd &currentPinoConfiguration() { return m_qpino; }
197  inline Eigen::VectorXd &currentRPYConfiguration() { return m_qrpy; }
198  inline Eigen::VectorXd &currentPinoVelocity() { return m_vpino; }
199  inline Eigen::VectorXd &currentPinoAcceleration() { return m_apino; }
200  inline Eigen::VectorXd &currentRPYVelocity() { return m_vrpy; }
201  inline Eigen::VectorXd &currentRPYAcceleration() { return m_arpy; }
202  inline Eigen::VectorXd &currentTau() { return m_tau; }
203 
204  inline unsigned numberDof() { return (unsigned)m_robotModel->nq; }
205 
206  inline unsigned numberVelDof() { return (unsigned)m_robotModel->nv; }
207 
208  inline void zeroMomentumPoint(Eigen::Vector3d &zmp) {
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);
214  zmp(2) = 0.0; // by default
215  }
216 
217  inline void positionCenterOfMass(Eigen::Vector3d &com) {
218  m_com = m_robotData->com[0];
219  com(0) = m_com(0);
220  com(1) = m_com(1);
221  com(2) = m_com(2);
222  }
223  inline void CenterOfMass(Eigen::Vector3d &com, Eigen::Vector3d &dcom,
224  Eigen::Vector3d &ddcom) {
225  m_com = m_robotData->acom[0];
226  ddcom(0) = m_com(0);
227  ddcom(1) = m_com(1);
228  ddcom(2) = m_com(2);
229 
230  m_com = m_robotData->vcom[0];
231  dcom(0) = m_com(0);
232  dcom(1) = m_com(1);
233  dcom(2) = m_com(2);
234 
235  m_com = m_robotData->com[0];
236  com(0) = m_com(0);
237  com(1) = m_com(1);
238  com(2) = m_com(2);
239  }
240 
243  void currentPinoConfiguration(Eigen::VectorXd &conf);
244  void currentRPYConfiguration(Eigen::VectorXd &);
245  inline void currentPinoVelocity(Eigen::VectorXd &vel) { m_vpino = vel; }
246  inline void currentPinoAcceleration(Eigen::VectorXd &acc) { m_apino = acc; }
247  inline void currentRPYVelocity(Eigen::VectorXd &vel) { m_vrpy = vel; }
248  inline void currentRPYAcceleration(Eigen::VectorXd &acc) { m_arpy = acc; }
249 
250  inline pinocchio::JointIndex getFreeFlyerSize() const {
251  return m_PinoFreeFlyerSize;
252  }
253 
254  inline pinocchio::JointIndex getFreeFlyerVelSize() const {
255  return m_PinoFreeFlyerVelSize;
256  }
257 
260  inline bool isInitialized() {
261  return m_boolModel && m_boolData && m_boolLeftFoot && m_boolRightFoot;
262  }
263  bool checkModel(pinocchio::Model *robotModel);
264  bool initializeRobotModelAndData(pinocchio::Model *robotModel,
265  pinocchio::Data *robotData);
268 
269  const std::string &getName() const;
272 private:
273  pinocchio::Model *m_robotModel;
274  pinocchio::Data *m_robotDataInInitialePose; // internal variable
275  pinocchio::Data *m_robotData;
276  PRFoot m_leftFoot, m_rightFoot;
277  double m_mass;
278  pinocchio::JointIndex m_chest, m_waist, m_leftShoulder, m_rightShoulder;
279  pinocchio::JointIndex m_leftWrist, m_rightWrist;
280 
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;
294  // @}
295 
296  // tmp variables
297  Eigen::Quaterniond m_quat;
298  Eigen::Matrix3d m_rot;
299  pinocchio::Force m_externalForces; // external forces and torques
300  Eigen::VectorXd m_tau; // external forces and torques
301  Eigen::Vector3d m_f, m_n; // external forces and torques
302  Eigen::Vector3d m_com; // multibody CoM
303  Eigen::Matrix3d m_S;
304  Eigen::Vector3d m_rpy, m_drpy, m_ddrpy, m_omega, m_domega;
305 
306  // Variables extracted form the urdf used for the analitycal inverse
307  // kinematic
308  bool m_isLegInverseKinematic;
309  unsigned int m_modeLegInverseKinematic;
310  bool m_isArmInverseKinematic;
311 
312  // length between the waist and the hip
313  Eigen::Vector3d m_leftDt, m_rightDt;
314  double m_femurLength;
315  double m_tibiaLengthZ;
316  double m_tibiaLengthY;
317 
318  bool m_boolModel;
319  bool m_boolData;
320  bool m_boolLeftFoot;
321  bool m_boolRightFoot;
322 
324  pinocchio::JointIndex m_PinoFreeFlyerSize;
325 
327  pinocchio::JointIndex m_PinoFreeFlyerVelSize;
328 
329 }; // PinocchioRobot
330 } // namespace PatternGeneratorJRL
331 #endif // PinocchioRobot_HH
PatternGeneratorJRL::PinocchioRobot::waist
pinocchio::JointIndex waist()
Definition: pinocchiorobot.hh:188
PatternGeneratorJRL::PinocchioRobot::positionCenterOfMass
void positionCenterOfMass(Eigen::Vector3d &com)
Definition: pinocchiorobot.hh:217
PatternGeneratorJRL::PinocchioRobot::currentRPYAcceleration
Eigen::VectorXd & currentRPYAcceleration()
Definition: pinocchiorobot.hh:201
PatternGeneratorJRL::PinocchioRobot::getName
const std::string & getName() const
PatternGeneratorJRL::PinocchioRobot::Data
pinocchio::Data * Data()
Definition: pinocchiorobot.hh:175
PatternGeneratorJRL::PinocchioRobot::currentRPYAcceleration
void currentRPYAcceleration(Eigen::VectorXd &acc)
Definition: pinocchiorobot.hh:248
PatternGeneratorJRL::PinocchioRobot::testLegsInverseKinematics
virtual bool testLegsInverseKinematics()
testLegsInverseKinematics : test if the robot legs has the good joint configuration to use the analit...
PatternGeneratorJRL::PinocchioRobot::testOneModeOfLegsInverseKinematics
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...
PatternGeneratorJRL::PinocchioRobot::initializeRightFoot
bool initializeRightFoot(PRFoot rightFoot)
PatternGeneratorJRL::PinocchioRobotFoot_t::soleHeight
double soleHeight
Definition: pinocchiorobot.hh:42
a
doublereal * a
Definition: qld.cpp:386
PatternGeneratorJRL::PinocchioRobot::leftFoot
PRFoot * leftFoot()
Definition: pinocchiorobot.hh:181
PatternGeneratorJRL::PinocchioRobotFoot_t::soleDepth
double soleDepth
Definition: pinocchiorobot.hh:40
PatternGeneratorJRL::PinocchioRobotFoot_t
Definition: pinocchiorobot.hh:38
PatternGeneratorJRL::PinocchioRobotFoot_t::anklePosition
Eigen::Vector3d anklePosition
Definition: pinocchiorobot.hh:43
PatternGeneratorJRL::PinocchioRobot::getActuatedJoints
pinocchio::JointModelVector & getActuatedJoints()
Definition: pinocchiorobot.hh:192
PatternGeneratorJRL::PinocchioRobot::currentPinoConfiguration
Eigen::VectorXd & currentPinoConfiguration()
Definition: pinocchiorobot.hh:196
PatternGeneratorJRL::PinocchioRobot::DataInInitialePose
pinocchio::Data * DataInInitialePose()
Definition: pinocchiorobot.hh:176
PatternGeneratorJRL::PRFoot
PinocchioRobotFoot_t PRFoot
Definition: pinocchiorobot.hh:50
PatternGeneratorJRL::PinocchioRobot::getFreeFlyerSize
pinocchio::JointIndex getFreeFlyerSize() const
Definition: pinocchiorobot.hh:250
PatternGeneratorJRL::PinocchioRobot::currentRPYVelocity
Eigen::VectorXd & currentRPYVelocity()
Definition: pinocchiorobot.hh:200
PatternGeneratorJRL::PinocchioRobot::currentTau
Eigen::VectorXd & currentTau()
Definition: pinocchiorobot.hh:202
PatternGeneratorJRL::PinocchioRobot::currentPinoVelocity
Eigen::VectorXd & currentPinoVelocity()
Definition: pinocchiorobot.hh:198
PatternGeneratorJRL::PinocchioRobot::initializeLegsInverseKinematics
virtual void initializeLegsInverseKinematics()
initializeInverseKinematics : initialize the internal data for the inverse kinematic e....
PatternGeneratorJRL::PinocchioRobot::ComputeSpecializedInverseKinematics
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...
PatternGeneratorJRL::PinocchioRobotFoot_t::PinocchioRobotFoot_t
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PinocchioRobotFoot_t()
Definition: pinocchiorobot.hh:46
PatternGeneratorJRL::PinocchioRobot::mass
double mass()
Definition: pinocchiorobot.hh:190
PatternGeneratorJRL::PinocchioRobot::RPYToSpatialFreeFlyer
void RPYToSpatialFreeFlyer(Eigen::Vector3d &rpy, Eigen::Vector3d &drpy, Eigen::Vector3d &ddrpy, Eigen::Quaterniond &quat, Eigen::Vector3d &omega, Eigen::Vector3d &domega)
PatternGeneratorJRL::PinocchioRobotFoot_t::associatedAnkle
pinocchio::JointIndex associatedAnkle
Definition: pinocchiorobot.hh:39
PatternGeneratorJRL::PinocchioRobotFoot_t::soleWidth
double soleWidth
Definition: pinocchiorobot.hh:41
PatternGeneratorJRL::PinocchioRobot::computeInverseDynamics
void computeInverseDynamics()
PatternGeneratorJRL::PinocchioRobot::chest
pinocchio::JointIndex chest()
Definition: pinocchiorobot.hh:187
PatternGeneratorJRL::PinocchioRobot::currentPinoAcceleration
Eigen::VectorXd & currentPinoAcceleration()
Definition: pinocchiorobot.hh:199
PatternGeneratorJRL::PinocchioRobot::currentPinoAcceleration
void currentPinoAcceleration(Eigen::VectorXd &acc)
Definition: pinocchiorobot.hh:246
PatternGeneratorJRL::PinocchioRobot::computeForwardKinematics
void computeForwardKinematics()
Compute the geometry of the robot.
PatternGeneratorJRL::PinocchioRobot::fromRootToIt
std::vector< pinocchio::JointIndex > fromRootToIt(pinocchio::JointIndex it)
PatternGeneratorJRL::PinocchioRobot::currentRPYVelocity
void currentRPYVelocity(Eigen::VectorXd &vel)
Definition: pinocchiorobot.hh:247
PatternGeneratorJRL::PinocchioRobot::Model
pinocchio::Model * Model()
Definition: pinocchiorobot.hh:179
PatternGeneratorJRL::PinocchioRobot::initializeRobotModelAndData
bool initializeRobotModelAndData(pinocchio::Model *robotModel, pinocchio::Data *robotData)
PatternGeneratorJRL::PinocchioRobot::rightWrist
pinocchio::JointIndex rightWrist()
Definition: pinocchiorobot.hh:185
PatternGeneratorJRL::PinocchioRobot::initializeLeftFoot
bool initializeLeftFoot(PRFoot leftFoot)
PatternGeneratorJRL::PinocchioRobot::leftWrist
pinocchio::JointIndex leftWrist()
Definition: pinocchiorobot.hh:184
PatternGeneratorJRL::PinocchioRobot::~PinocchioRobot
virtual ~PinocchioRobot()
PatternGeneratorJRL::pinocchio_robot::QUATERNION_SIZE
const int QUATERNION_SIZE
Definition: pinocchiorobot.hh:54
PatternGeneratorJRL::PinocchioRobot
Definition: pinocchiorobot.hh:57
PatternGeneratorJRL::PinocchioRobot::zeroMomentumPoint
void zeroMomentumPoint(Eigen::Vector3d &zmp)
Definition: pinocchiorobot.hh:208
PatternGeneratorJRL::PinocchioRobot::rightFoot
PRFoot * rightFoot()
Definition: pinocchiorobot.hh:182
PatternGeneratorJRL::PinocchioRobot::getFreeFlyerVelSize
pinocchio::JointIndex getFreeFlyerVelSize() const
Definition: pinocchiorobot.hh:254
PatternGeneratorJRL
\doc Simulate a rigid body
Definition: patterngeneratorinterface.hh:41
PatternGeneratorJRL::PinocchioRobot::testArmsInverseKinematics
virtual bool testArmsInverseKinematics()
testArmsInverseKinematics : test if the robot arms has the good joint configuration to use the analit...
PatternGeneratorJRL::PinocchioRobot::jointsBetween
std::vector< pinocchio::JointIndex > jointsBetween(pinocchio::JointIndex first, pinocchio::JointIndex second)
tools :
PatternGeneratorJRL::pinocchio_robot::RPY_SIZE
const int RPY_SIZE
Definition: pinocchiorobot.hh:53
PatternGeneratorJRL::PinocchioRobot::numberVelDof
unsigned numberVelDof()
Definition: pinocchiorobot.hh:206
PatternGeneratorJRL::PinocchioRobot::PinocchioRobot
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PinocchioRobot()
Constructor and Destructor.
PatternGeneratorJRL::PinocchioRobot::numberDof
unsigned numberDof()
Definition: pinocchiorobot.hh:204
PatternGeneratorJRL::PinocchioRobot::currentPinoVelocity
void currentPinoVelocity(Eigen::VectorXd &vel)
Definition: pinocchiorobot.hh:245
PatternGeneratorJRL::PinocchioRobot::currentRPYConfiguration
Eigen::VectorXd & currentRPYConfiguration()
Definition: pinocchiorobot.hh:197
PatternGeneratorJRL::PinocchioRobot::checkModel
bool checkModel(pinocchio::Model *robotModel)
PatternGeneratorJRL::PinocchioRobot::isInitialized
bool isInitialized()
Definition: pinocchiorobot.hh:260
PatternGeneratorJRL::PinocchioRobot::CenterOfMass
void CenterOfMass(Eigen::Vector3d &com, Eigen::Vector3d &dcom, Eigen::Vector3d &ddcom)
Definition: pinocchiorobot.hh:223