pinocchiorobot.hh 9.22 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
33
/*
 * 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"
#include "pinocchio/multibody/model.hpp"
34
#include "pinocchio/multibody/data.hpp"
35
36
37
38
39
40
41
42
43
#include <jrl/mal/matrixabstractlayer.hh>
namespace PatternGeneratorJRL
{
  struct PinocchioRobotFoot_t{
    se3::JointIndex associatedAnkle ;
    double soleDepth ; // z axis
    double soleWidth ; // y axis
    double soleHeight ;// x axis
    vector3d anklePosition ;
44
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45
46
47
48
49
50
  };
  typedef PinocchioRobotFoot_t PRFoot ;

  class PinocchioRobot
  {
  public:
mnaveau's avatar
mnaveau committed
51
52
    // overload the new[] eigen operator
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
53

mnaveau's avatar
mnaveau committed
54
55
    /// Constructor and Destructor
    PinocchioRobot();
56
    virtual ~PinocchioRobot();
57
58
59
60
61
62
63
64
65
66

    /// Functions computing kinematics and dynamics
    void computeInverseDynamics();
    void computeInverseDynamics(MAL_VECTOR_TYPE(double) & q,
                                MAL_VECTOR_TYPE(double) & v,
                                MAL_VECTOR_TYPE(double) & a);

    void computeForwardKinematics();
    void computeForwardKinematics(MAL_VECTOR_TYPE(double) & q);

67
68
69
70
71
72
73
    void RPYToSpatialFreeFlyer(Eigen::Vector3d & rpy,
                               Eigen::Vector3d & drpy,
                               Eigen::Vector3d & ddrpy,
                               Eigen::Quaterniond & quat,
                               Eigen::Vector3d & omega,
                               Eigen::Vector3d & domega);

74
75
    /// \brief ComputeSpecializedInverseKinematics :
    /// compute POSITION (not velocity) of the joints from end effector pose
76
77
78
79
80
81
82
83
84
85
86
    /// This is the implementation of the analitycal inverse kinematic extracted
    /// from the book of Kajita
    /// Authors Shuuji Kajita ; Hirohisa Hirukawa ; Kensuke Harada ; Kazuhito Yokoi
    /// 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,
87
    /// param 6D vector output, filled with zeros if the robot is not compatible
88
    ///
89
    virtual bool ComputeSpecializedInverseKinematics(
90
91
92
93
94
95
        const se3::JointIndex &jointRoot,
        const se3::JointIndex &jointEnd,
        const MAL_S4x4_MATRIX_TYPE(double) & jointRootPosition,
        const MAL_S4x4_MATRIX_TYPE(double) & jointEndPosition,
        MAL_VECTOR_TYPE(double) &q);

96
    ///
97
98
    /// \brief testInverseKinematics :
    /// test if the robot has the good joint
99
    /// configuration to use the analitical inverse geometry
100
    /// to be overloaded if the user wants another inverse kinematics
101
102
    /// \return
    ///
103
104
105
106
107
108
109
110
111
    virtual bool testInverseKinematics();

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

113
114
115
116
117
118
119
  public :
    /// tools :
    std::vector<se3::JointIndex> jointsBetween
    ( se3::JointIndex first, se3::JointIndex second);
    std::vector<se3::JointIndex> fromRootToIt (se3::JointIndex it);

  private :
120
    // needed for the inverse geometry (ComputeSpecializedInverseKinematics)
121
122
123
124
125
126
127
128
129
130
    void getWaistFootKinematics(const matrix4d & jointRootPosition,
                                const matrix4d & jointEndPosition,
                                vectorN &q,
                                vector3d Dt);
    double ComputeXmax(double & Z);
    void getShoulderWristKinematics(const matrix4d & jointRootPosition,
                                    const matrix4d & jointEndPosition,
                                    vectorN &q,
                                    int side);
    void DetectAutomaticallyShoulders();
mnaveau's avatar
mnaveau committed
131
    void DetectAutomaticallyOneShoulder(se3::JointIndex aWrist,
132
133
134
135
136
137
138
139
140
                                        se3::JointIndex & aShoulder);


  public :
    /// Getters
    /// ///////
    inline se3::Data * Data()
    {return m_robotData;}
    inline se3::Data * DataInInitialePose()
mnaveau's avatar
mnaveau committed
141
    {return m_robotDataInInitialePose;}
142
143
144
145
146
147
148
149
    inline se3::Model * Model()
    {return m_robotModel;}

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

mnaveau's avatar
mnaveau committed
150
151
152
153
    inline se3::JointIndex leftWrist()
    {return m_leftWrist;}
    inline se3::JointIndex rightWrist()
    {return m_rightWrist;}
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177

    inline se3::JointIndex chest()
    {return m_chest;}
    inline se3::JointIndex waist()
    {return m_waist;}

    inline double mass()
    {return m_mass;}

    inline se3::JointModelVector & getActuatedJoints()
    {return m_robotModel->joints;}

    inline MAL_VECTOR_TYPE(double) currentConfiguration()
    {return m_qmal;}
    inline MAL_VECTOR_TYPE(double) currentVelocity()
    {return m_vmal;}
    inline MAL_VECTOR_TYPE(double) currentAcceleration()
    {return m_amal;}

    inline unsigned numberDof()
    {return m_robotModel->nv;}

    inline void zeroMomentumPoint(MAL_S3_VECTOR_TYPE(double) & zmp)
    {
178
      m_externalForces = m_robotData->liMi[1].act(m_robotData->f[1]);
179
180
      m_f = m_externalForces.linear() ;
      m_n = m_externalForces.angular() ;
181
182
183
184
185
186
187
188
189
190
191
192
      zmp(0) = -m_n(1)/m_f(2) ;
      zmp(1) =  m_n(0)/m_f(2) ;
      zmp(2) = 0.0 ; // by default
    }

    inline void positionCenterOfMass(MAL_S3_VECTOR_TYPE(double) & com)
    {
      m_com = m_robotData->com[0] ;
      com(0) = m_com(0) ;
      com(1) = m_com(1) ;
      com(2) = m_com(2) ;
    }
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
    inline void CenterOfMass(MAL_S3_VECTOR_TYPE(double) &   com,
                             MAL_S3_VECTOR_TYPE(double) &  dcom,
                             MAL_S3_VECTOR_TYPE(double) & ddcom)
    {
      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) ;
    }
212
213
214
215
216
217
218
219
220
221

    /// SETTERS
    /// ///////
    inline void currentConfiguration(MAL_VECTOR_TYPE(double) conf)
    {m_qmal=conf;}
    inline void currentVelocity(MAL_VECTOR_TYPE(double) vel)
    {m_vmal=vel;}
    inline void currentAcceleration(MAL_VECTOR_TYPE(double) acc)
    {m_amal=acc;}

mnaveau's avatar
mnaveau committed
222
223
224
225
226
227
228
229
230
231
232
233
234
235
    /// Initialization functions
    /// ////////////////////////
    inline bool isInitialized()
    {
      return m_boolModel && m_boolData && m_boolLeftFoot && m_boolRightFoot;
    }
    bool checkModel(se3::Model * robotModel);
    bool initializeRobotModelAndData(se3::Model * robotModel,
                                     se3::Data * robotData);
    bool initializeLeftFoot(PRFoot leftFoot);
    bool initializeRightFoot(PRFoot rightFoot);

    /// Attributes
    /// //////////
mnaveau's avatar
mnaveau committed
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
  private :
    se3::Model * m_robotModel ;
    se3::Data * m_robotDataInInitialePose ; // internal variable
    se3::Data * m_robotData ;
    PRFoot m_leftFoot , m_rightFoot ;
    double m_mass ;
    se3::JointIndex m_chest, m_waist, m_leftShoulder, m_rightShoulder ;
    se3::JointIndex m_leftWrist , m_rightWrist ;

    MAL_VECTOR_TYPE(double) m_qmal ;
    MAL_VECTOR_TYPE(double) m_vmal ;
    MAL_VECTOR_TYPE(double) m_amal ;
    Eigen::VectorXd m_q ;
    Eigen::VectorXd m_v ;
    Eigen::VectorXd m_a ;

    // tmp variables
    Eigen::Quaterniond m_quat ;
254
    Eigen::Matrix3d m_rot ;
255
    se3::Force m_externalForces ; // external forces and torques
256
    Eigen::VectorXd m_tau ; // external forces and torques
mnaveau's avatar
mnaveau committed
257
258
    Eigen::Vector3d m_f,m_n; // external forces and torques
    Eigen::Vector3d m_com; // multibody CoM
259
260
    Eigen::Matrix3d m_S ;
    Eigen::Vector3d m_rpy,m_drpy,m_ddrpy,m_omega,m_domega ;
mnaveau's avatar
mnaveau committed
261

262
263
    // Variables extracted form the urdf used for the analitycal inverse
    // kinematic
264
265
266
    bool m_isLegInverseKinematic ;
    bool m_isArmInverseKinematic ;

267
268
    // length between the waist and the hip
    MAL_S3_VECTOR_TYPE(double) m_leftDt, m_rightDt ;
269
    double m_femurLength ;
270
271
    double m_tibiaLengthZ ;
    double m_tibiaLengthY ;
272
273


mnaveau's avatar
mnaveau committed
274
275
276
277
    bool m_boolModel     ;
    bool m_boolData      ;
    bool m_boolLeftFoot  ;
    bool m_boolRightFoot ;
mnaveau's avatar
mnaveau committed
278
279
280

  }; //PinocchioRobot
}// namespace PatternGeneratorJRL
281
#endif // PinocchioRobot_HH