pinocchiorobot.hh 12 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
/*
 * Copyright 2016,
 *
 * Maximilien Naveau
 * Olivier Stasse
 *
 * JRL, CNRS/AIST
 *
 * This file is part of jrl-walkgen.
 * jrl-walkgen is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * jrl-walkgen is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Lesser Public License for more details.
 * You should have received a copy of the GNU Lesser General Public License
 * along with jrl-walkgen.  If not, see <http://www.gnu.org/licenses/>.
 *
 *  Research carried out within the scope of the
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */
/*! \file PinocchioRobot.hh
  \brief This object defines a humanoid robot model based on the PinocchioRobot
frame work */

#ifndef PinocchioRobot_HH
#define PinocchioRobot_HH

#include "pinocchio/spatial/se3.hpp"
Olivier Stasse's avatar
Olivier Stasse committed
33
#include "pinocchio/multibody/data.hpp"
34
#include "pinocchio/multibody/model.hpp"
35
#include "pinocchio/parsers/urdf.hpp"
Olivier Stasse's avatar
Olivier Stasse committed
36

37
38
39
namespace PatternGeneratorJRL
{
  struct PinocchioRobotFoot_t{
Olivier Stasse's avatar
Olivier Stasse committed
40
    pinocchio::JointIndex associatedAnkle ;
41
42
43
    double soleDepth ; // z axis
    double soleWidth ; // y axis
    double soleHeight ;// x axis
Olivier Stasse's avatar
Olivier Stasse committed
44
    Eigen::Vector3d anklePosition ;
45
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
46
47
48
  };
  typedef PinocchioRobotFoot_t PRFoot ;

49
50
51
52
53
  namespace pinocchio_robot
  {
    const int RPY_SIZE=6;
    const int QUATERNION_SIZE=7;
  }
54

55
56
57
  class PinocchioRobot
  {
  public:
mnaveau's avatar
mnaveau committed
58
59
    // overload the new[] eigen operator
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
60

mnaveau's avatar
mnaveau committed
61
62
    /// Constructor and Destructor
    PinocchioRobot();
63
    virtual ~PinocchioRobot();
64

65
66
    /// Compute RNEA algorithm
    /// This is a front end for computeInverseDynamics(q,v,a).
67
    void computeInverseDynamics();
68
69
70

    /// Compute RNEA algorithm from
    /// \param[in] q: \f$q=[r,\theta,\hat{q}]\f$ with \f$r\f$ the position
71
72
    /// of the free floating base (usually the waist), \f$theta\f$ the
    /// free floating
73
    /// orientation in RPY format, $\hat{q}$ the motor angles position.
Olivier Stasse's avatar
Olivier Stasse committed
74
75
76
    void computeInverseDynamics(Eigen::VectorXd & q,
                                Eigen::VectorXd & v,
                                Eigen::VectorXd & a);
77

78
    /// Compute the geometry of the robot.
79
80
    void computeForwardKinematics();

81
82
83
84
85
86
87
    void RPYToSpatialFreeFlyer(Eigen::Vector3d & rpy,
                               Eigen::Vector3d & drpy,
                               Eigen::Vector3d & ddrpy,
                               Eigen::Quaterniond & quat,
                               Eigen::Vector3d & omega,
                               Eigen::Vector3d & domega);

88
89
    /// \brief ComputeSpecializedInverseKinematics :
    /// compute POSITION (not velocity) of the joints from end effector pose
90
91
    /// This is the implementation of the analitycal inverse kinematic extracted
    /// from the book of Kajita
92
93
    /// Authors Shuuji Kajita ; Hirohisa Hirukawa ; Kensuke Harada ;
    /// Kazuhito Yokoi
94
95
96
97
98
99
100
101
    /// ISBN 9782287877162 ; 9782287877155
    /// It needs a specific distribution of the joint to work.
    /// a test at the initialization of the class is [should be] done to check
    /// if everything is correct
    /// param root joint index, i.e. the waist or the shoulder
    /// param end joint index, i.e, the wrist or ankle indexes
    /// param root joint homogenous matrix position,
    /// param root joint homogenous matrix index,
102
    /// param 6D vector output, filled with zeros if the robot is not compatible
103
    ///
104
    virtual bool ComputeSpecializedInverseKinematics(
Olivier Stasse's avatar
Olivier Stasse committed
105
106
        const pinocchio::JointIndex &jointRoot,
        const pinocchio::JointIndex &jointEnd,
Olivier Stasse's avatar
Olivier Stasse committed
107
108
109
        const Eigen::Matrix4d & jointRootPosition,
        const Eigen::Matrix4d & jointEndPosition,
        Eigen::VectorXd &q);
110

111
    ///
112
113
    /// \brief testArmsInverseKinematics :
    /// test if the robot arms has the good joint
114
    /// configuration to use the analitical inverse geometry
115
    /// to be overloaded if the user wants another inverse kinematics
116
    /// RY-RX-RZ-RY-RZ-RY
117
118
    /// \return
    ///
119
120
121
122
    virtual bool testArmsInverseKinematics();
    ///
    /// \brief testLegsInverseKinematics :
    /// test if the robot legs has the good joint
123
    /// configuration to use the analitical inverse geometry
124
    /// to be overloaded if the user wants another inverse kinematics
125
126
127
    /// Two modes are available:
    /// FF-RZ-RX-RY-RY-RY-RX
    /// FF-RX-RZ-RY-RY-RY-RX
128
    /// \return
129
    ///
130
    virtual bool testLegsInverseKinematics();
131
    ///
132
133
134
135
136
    /// \brief testOneModefOfLegInverseKinematics :
    /// test if the robot legs has one good joint
    /// configuration to use the analitical inverse geometry
    /// to be overloaded if the user wants another inverse kinematics
    /// \return
137
    ///
138
139
140
    virtual bool testOneModeOfLegsInverseKinematics
    (std::vector<std::string> &leftLegJointNames,
     std::vector<std::string> &rightLegJointNames);
141
142
143
144
145
146
147

    ///
    /// \brief initializeInverseKinematics :
    /// initialize the internal data for the inverse kinematic e.g. the femur
    /// length
    /// \return
    ///
148
    virtual void initializeLegsInverseKinematics();
149

150
151
  public :
    /// tools :
Olivier Stasse's avatar
Olivier Stasse committed
152
153
154
    std::vector<pinocchio::JointIndex> jointsBetween
    ( pinocchio::JointIndex first, pinocchio::JointIndex second);
    std::vector<pinocchio::JointIndex> fromRootToIt (pinocchio::JointIndex it);
155
156

  private :
157
    // needed for the inverse geometry (ComputeSpecializedInverseKinematics)
Olivier Stasse's avatar
Olivier Stasse committed
158
159
160
    void getWaistFootKinematics(const Eigen::Matrix4d & jointRootPosition,
                                const Eigen::Matrix4d & jointEndPosition,
                                Eigen::VectorXd &q,
161
                                Eigen::Vector3d &Dt) const;
162
    double ComputeXmax(double & Z);
Olivier Stasse's avatar
Olivier Stasse committed
163
164
165
    void getShoulderWristKinematics(const Eigen::Matrix4d & jointRootPosition,
                                    const Eigen::Matrix4d & jointEndPosition,
                                    Eigen::VectorXd &q,
166
167
                                    int side);
    void DetectAutomaticallyShoulders();
Olivier Stasse's avatar
Olivier Stasse committed
168
169
    void DetectAutomaticallyOneShoulder(pinocchio::JointIndex aWrist,
                                        pinocchio::JointIndex & aShoulder);
170

171
172
173
174
175
    /*! \brief Computes the size of the free flyer/root robot
      loads by the urdf.
      Set the value of m_PinoFreeFlyerSize.
    */
    void ComputeRootSize();
176
177
178
179

  public :
    /// Getters
    /// ///////
Olivier Stasse's avatar
Olivier Stasse committed
180
    inline pinocchio::Data * Data()
181
    {return m_robotData;}
Olivier Stasse's avatar
Olivier Stasse committed
182
    inline pinocchio::Data * DataInInitialePose()
mnaveau's avatar
mnaveau committed
183
    {return m_robotDataInInitialePose;}
Olivier Stasse's avatar
Olivier Stasse committed
184
    inline pinocchio::Model * Model()
185
186
187
188
189
190
191
    {return m_robotModel;}

    inline PRFoot * leftFoot()
    {return &m_leftFoot;}
    inline PRFoot * rightFoot()
    {return &m_rightFoot;}

Olivier Stasse's avatar
Olivier Stasse committed
192
    inline pinocchio::JointIndex leftWrist()
mnaveau's avatar
mnaveau committed
193
    {return m_leftWrist;}
Olivier Stasse's avatar
Olivier Stasse committed
194
    inline pinocchio::JointIndex rightWrist()
mnaveau's avatar
mnaveau committed
195
    {return m_rightWrist;}
196

Olivier Stasse's avatar
Olivier Stasse committed
197
    inline pinocchio::JointIndex chest()
198
    {return m_chest;}
Olivier Stasse's avatar
Olivier Stasse committed
199
    inline pinocchio::JointIndex waist()
200
201
202
203
204
    {return m_waist;}

    inline double mass()
    {return m_mass;}

Olivier Stasse's avatar
Olivier Stasse committed
205
    inline pinocchio::JointModelVector & getActuatedJoints()
206
207
    {return m_robotModel->joints;}

208
209
210
    inline Eigen::VectorXd & currentPinoConfiguration()
    {return m_qpino;}
    inline Eigen::VectorXd & currentRPYConfiguration()
211
    {return m_qrpy;}
212
213
214
215
216
217
218
219
    inline Eigen::VectorXd & currentPinoVelocity()
    {return m_vpino;}
    inline Eigen::VectorXd & currentPinoAcceleration()
    {return m_apino;}
    inline Eigen::VectorXd & currentRPYVelocity()
    {return m_vrpy;}
    inline Eigen::VectorXd & currentRPYAcceleration()
    {return m_arpy;}
220
221
    inline Eigen::VectorXd & currentTau()
    {return m_tau;}
222
223

    inline unsigned numberDof()
224
225
226
    {return m_robotModel->nq;}

    inline unsigned numberVelDof()
227
228
    {return m_robotModel->nv;}

Olivier Stasse's avatar
Olivier Stasse committed
229
    inline void zeroMomentumPoint(Eigen::Vector3d & zmp)
230
    {
231
      m_externalForces = m_robotData->liMi[1].act(m_robotData->f[1]);
232
233
      m_f = m_externalForces.linear() ;
      m_n = m_externalForces.angular() ;
234
235
236
237
238
      zmp(0) = -m_n(1)/m_f(2) ;
      zmp(1) =  m_n(0)/m_f(2) ;
      zmp(2) = 0.0 ; // by default
    }

Olivier Stasse's avatar
Olivier Stasse committed
239
    inline void positionCenterOfMass(Eigen::Vector3d & com)
240
241
242
243
244
245
    {
      m_com = m_robotData->com[0] ;
      com(0) = m_com(0) ;
      com(1) = m_com(1) ;
      com(2) = m_com(2) ;
    }
Olivier Stasse's avatar
Olivier Stasse committed
246
247
248
    inline void CenterOfMass(Eigen::Vector3d &   com,
                             Eigen::Vector3d &  dcom,
                             Eigen::Vector3d & ddcom)
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
    {
      m_com = m_robotData->acom[0] ;
      ddcom(0) = m_com(0) ;
      ddcom(1) = m_com(1) ;
      ddcom(2) = m_com(2) ;

      m_com = m_robotData->vcom[0] ;
      dcom(0) = m_com(0) ;
      dcom(1) = m_com(1) ;
      dcom(2) = m_com(2) ;

      m_com = m_robotData->com[0] ;
      com(0) = m_com(0) ;
      com(1) = m_com(1) ;
      com(2) = m_com(2) ;
    }
265
266
267

    /// SETTERS
    /// ///////
268
269
270
271
272
273
274
275
276
277
    void currentPinoConfiguration(Eigen::VectorXd & conf);
    void currentRPYConfiguration(Eigen::VectorXd &);
    inline void currentPinoVelocity(Eigen::VectorXd & vel)
    {m_vpino=vel;}
    inline void currentPinoAcceleration(Eigen::VectorXd & acc)
    {m_apino=acc;}
    inline void currentRPYVelocity(Eigen::VectorXd & vel)
    {m_vrpy=vel;}
    inline void currentRPYAcceleration(Eigen::VectorXd & acc)
    {m_arpy=acc;}
278

279
280
281
282
283
284
    inline pinocchio::JointIndex getFreeFlyerSize() const
    { return m_PinoFreeFlyerSize;}

    inline pinocchio::JointIndex getFreeFlyerVelSize() const
    { return m_PinoFreeFlyerVelSize;}

mnaveau's avatar
mnaveau committed
285
286
287
288
289
290
    /// Initialization functions
    /// ////////////////////////
    inline bool isInitialized()
    {
      return m_boolModel && m_boolData && m_boolLeftFoot && m_boolRightFoot;
    }
Olivier Stasse's avatar
Olivier Stasse committed
291
292
293
    bool checkModel(pinocchio::Model * robotModel);
    bool initializeRobotModelAndData(pinocchio::Model * robotModel,
                                     pinocchio::Data * robotData);
mnaveau's avatar
mnaveau committed
294
295
296
    bool initializeLeftFoot(PRFoot leftFoot);
    bool initializeRightFoot(PRFoot rightFoot);

297
    const std::string & getName() const;
mnaveau's avatar
mnaveau committed
298
299
    /// Attributes
    /// //////////
mnaveau's avatar
mnaveau committed
300
  private :
Olivier Stasse's avatar
Olivier Stasse committed
301
302
303
    pinocchio::Model * m_robotModel ;
    pinocchio::Data * m_robotDataInInitialePose ; // internal variable
    pinocchio::Data * m_robotData ;
mnaveau's avatar
mnaveau committed
304
305
    PRFoot m_leftFoot , m_rightFoot ;
    double m_mass ;
Olivier Stasse's avatar
Olivier Stasse committed
306
307
    pinocchio::JointIndex m_chest, m_waist, m_leftShoulder, m_rightShoulder ;
    pinocchio::JointIndex m_leftWrist , m_rightWrist ;
mnaveau's avatar
mnaveau committed
308

309
310
311
312
    /// Fields member to store  configurations, velocity and acceleration
    /// @{
    /// Configuration SE(3) position + quaternion + NbDofs
    Eigen::VectorXd m_qpino ;
313
    /// Configuration SE(3) position + RPY + NbDofs
314
315
316
317
318
319
320
321
322
323
    Eigen::VectorXd m_qrpy ;
    /// Velocity se(3) + NbDofs
    Eigen::VectorXd m_vpino ;
    /// Velocity se(3) + NbDofs
    Eigen::VectorXd m_vrpy ;
    /// Acceleration acc + NbDofs
    Eigen::VectorXd m_apino ;
    Eigen::VectorXd m_arpy ;
    // @}

324

mnaveau's avatar
mnaveau committed
325
326
    // tmp variables
    Eigen::Quaterniond m_quat ;
327
    Eigen::Matrix3d m_rot ;
Olivier Stasse's avatar
Olivier Stasse committed
328
    pinocchio::Force m_externalForces ; // external forces and torques
329
    Eigen::VectorXd m_tau ; // external forces and torques
mnaveau's avatar
mnaveau committed
330
331
    Eigen::Vector3d m_f,m_n; // external forces and torques
    Eigen::Vector3d m_com; // multibody CoM
332
333
    Eigen::Matrix3d m_S ;
    Eigen::Vector3d m_rpy,m_drpy,m_ddrpy,m_omega,m_domega ;
mnaveau's avatar
mnaveau committed
334

335
336
    // Variables extracted form the urdf used for the analitycal inverse
    // kinematic
337
    bool m_isLegInverseKinematic ;
338
    unsigned int m_modeLegInverseKinematic;
339
340
    bool m_isArmInverseKinematic ;

341
    // length between the waist and the hip
Olivier Stasse's avatar
Olivier Stasse committed
342
    Eigen::Vector3d m_leftDt, m_rightDt ;
343
    double m_femurLength ;
344
345
    double m_tibiaLengthZ ;
    double m_tibiaLengthY ;
346
347


mnaveau's avatar
mnaveau committed
348
349
350
351
    bool m_boolModel     ;
    bool m_boolData      ;
    bool m_boolLeftFoot  ;
    bool m_boolRightFoot ;
mnaveau's avatar
mnaveau committed
352

353
354
355
356
357
358
    /// \brief Size of the free flyer configuration space.
    pinocchio::JointIndex m_PinoFreeFlyerSize;

    /// \brief Size of the free flyer velocity space.
    pinocchio::JointIndex m_PinoFreeFlyerVelSize;

mnaveau's avatar
mnaveau committed
359
360
  }; //PinocchioRobot
}// namespace PatternGeneratorJRL
361
#endif // PinocchioRobot_HH