Loading...
Searching...
No Matches
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
37namespace 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),
48 soleDepth(0.0),
49 soleWidth(0.0),
50 soleHeight(0.0),
51 anklePosition(0.0, 0.0, 0.0) {}
52};
54
55namespace pinocchio_robot {
56const int RPY_SIZE = 6;
57const int QUATERNION_SIZE = 7;
58} // namespace pinocchio_robot
59
61 public:
62 // overload the new[] eigen operator
63 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64
67 virtual ~PinocchioRobot();
68
72
78 void computeInverseDynamics(Eigen::VectorXd &q, Eigen::VectorXd &v,
79 Eigen::VectorXd &a);
80
83
84 void RPYToSpatialFreeFlyer(Eigen::Vector3d &rpy, Eigen::Vector3d &drpy,
85 Eigen::Vector3d &ddrpy, Eigen::Quaterniond &quat,
86 Eigen::Vector3d &omega, Eigen::Vector3d &domega);
87
105 const pinocchio::JointIndex &jointRoot,
106 const pinocchio::JointIndex &jointEnd,
107 const Eigen::Matrix4d &jointRootPosition,
108 const Eigen::Matrix4d &jointEndPosition, Eigen::VectorXd &q);
109
138 std::vector<std::string> &leftLegJointNames,
139 std::vector<std::string> &rightLegJointNames);
140
148
149 public:
151 std::vector<pinocchio::JointIndex> jointsBetween(
152 pinocchio::JointIndex first, pinocchio::JointIndex second);
153 std::vector<pinocchio::JointIndex> fromRootToIt(pinocchio::JointIndex it);
154
155 private:
156 // needed for the inverse geometry (ComputeSpecializedInverseKinematics)
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);
167
172 void ComputeRootSize();
173
174 public:
177 inline pinocchio::Data *Data() { return m_robotData; }
178 inline pinocchio::Data *DataInInitialePose() {
179 return m_robotDataInInitialePose;
180 }
181 inline pinocchio::Model *Model() { return m_robotModel; }
182
183 inline PRFoot *leftFoot() { return &m_leftFoot; }
184 inline PRFoot *rightFoot() { return &m_rightFoot; }
185
186 inline pinocchio::JointIndex leftWrist() { return m_leftWrist; }
187 inline pinocchio::JointIndex rightWrist() { return m_rightWrist; }
188
189 inline pinocchio::JointIndex chest() { return m_chest; }
190 inline pinocchio::JointIndex waist() { return m_waist; }
191
192 inline double mass() { return m_mass; }
193
194 inline pinocchio::JointModelVector &getActuatedJoints() {
195 return m_robotModel->joints;
196 }
197
198 inline Eigen::VectorXd &currentPinoConfiguration() { return m_qpino; }
199 inline Eigen::VectorXd &currentRPYConfiguration() { return m_qrpy; }
200 inline Eigen::VectorXd &currentPinoVelocity() { return m_vpino; }
201 inline Eigen::VectorXd &currentPinoAcceleration() { return m_apino; }
202 inline Eigen::VectorXd &currentRPYVelocity() { return m_vrpy; }
203 inline Eigen::VectorXd &currentRPYAcceleration() { return m_arpy; }
204 inline Eigen::VectorXd &currentTau() { return m_tau; }
205
206 inline unsigned numberDof() { return (unsigned)m_robotModel->nq; }
207
208 inline unsigned numberVelDof() { return (unsigned)m_robotModel->nv; }
209
210 inline void zeroMomentumPoint(Eigen::Vector3d &zmp) {
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);
216 zmp(2) = 0.0; // by default
217 }
218
219 inline void positionCenterOfMass(Eigen::Vector3d &com) {
220 m_com = m_robotData->com[0];
221 com(0) = m_com(0);
222 com(1) = m_com(1);
223 com(2) = m_com(2);
224 }
225 inline void CenterOfMass(Eigen::Vector3d &com, Eigen::Vector3d &dcom,
226 Eigen::Vector3d &ddcom) {
227 m_com = m_robotData->acom[0];
228 ddcom(0) = m_com(0);
229 ddcom(1) = m_com(1);
230 ddcom(2) = m_com(2);
231
232 m_com = m_robotData->vcom[0];
233 dcom(0) = m_com(0);
234 dcom(1) = m_com(1);
235 dcom(2) = m_com(2);
236
237 m_com = m_robotData->com[0];
238 com(0) = m_com(0);
239 com(1) = m_com(1);
240 com(2) = m_com(2);
241 }
242
245 void currentPinoConfiguration(Eigen::VectorXd &conf);
246 void currentRPYConfiguration(Eigen::VectorXd &);
247 inline void currentPinoVelocity(Eigen::VectorXd &vel) { m_vpino = vel; }
248 inline void currentPinoAcceleration(Eigen::VectorXd &acc) { m_apino = acc; }
249 inline void currentRPYVelocity(Eigen::VectorXd &vel) { m_vrpy = vel; }
250 inline void currentRPYAcceleration(Eigen::VectorXd &acc) { m_arpy = acc; }
251
252 inline pinocchio::JointIndex getFreeFlyerSize() const {
253 return m_PinoFreeFlyerSize;
254 }
255
256 inline pinocchio::JointIndex getFreeFlyerVelSize() const {
257 return m_PinoFreeFlyerVelSize;
258 }
259
262 inline bool isInitialized() {
263 return m_boolModel && m_boolData && m_boolLeftFoot && m_boolRightFoot;
264 }
265 bool checkModel(pinocchio::Model *robotModel);
266 bool initializeRobotModelAndData(pinocchio::Model *robotModel,
267 pinocchio::Data *robotData);
270
271 const std::string &getName() const;
274 private:
275 pinocchio::Model *m_robotModel;
276 pinocchio::Data *m_robotDataInInitialePose; // internal variable
277 pinocchio::Data *m_robotData;
278 PRFoot m_leftFoot, m_rightFoot;
279 double m_mass;
280 pinocchio::JointIndex m_chest, m_waist, m_leftShoulder, m_rightShoulder;
281 pinocchio::JointIndex m_leftWrist, m_rightWrist;
282
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;
296 // @}
297
298 // tmp variables
299 Eigen::Quaterniond m_quat;
300 Eigen::Matrix3d m_rot;
301 pinocchio::Force m_externalForces; // external forces and torques
302 Eigen::VectorXd m_tau; // external forces and torques
303 Eigen::Vector3d m_f, m_n; // external forces and torques
304 Eigen::Vector3d m_com; // multibody CoM
305 Eigen::Matrix3d m_S;
306 Eigen::Vector3d m_rpy, m_drpy, m_ddrpy, m_omega, m_domega;
307
308 // Variables extracted form the urdf used for the analitycal inverse
309 // kinematic
310 bool m_isLegInverseKinematic;
311 unsigned int m_modeLegInverseKinematic;
312 bool m_isArmInverseKinematic;
313
314 // length between the waist and the hip
315 Eigen::Vector3d m_leftDt, m_rightDt;
316 double m_femurLength;
317 double m_tibiaLengthZ;
318 double m_tibiaLengthY;
319
320 bool m_boolModel;
321 bool m_boolData;
322 bool m_boolLeftFoot;
323 bool m_boolRightFoot;
324
326 pinocchio::JointIndex m_PinoFreeFlyerSize;
327
329 pinocchio::JointIndex m_PinoFreeFlyerVelSize;
330
331}; // PinocchioRobot
332} // namespace PatternGeneratorJRL
333#endif // PinocchioRobot_HH
Definition: pinocchiorobot.hh:60
unsigned numberVelDof()
Definition: pinocchiorobot.hh:208
pinocchio::Data * Data()
Definition: pinocchiorobot.hh:177
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...
void currentPinoVelocity(Eigen::VectorXd &vel)
Definition: pinocchiorobot.hh:247
Eigen::VectorXd & currentRPYAcceleration()
Definition: pinocchiorobot.hh:203
unsigned numberDof()
Definition: pinocchiorobot.hh:206
Eigen::VectorXd & currentPinoAcceleration()
Definition: pinocchiorobot.hh:201
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...
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PinocchioRobot()
Constructor and Destructor.
double mass()
Definition: pinocchiorobot.hh:192
pinocchio::JointIndex getFreeFlyerVelSize() const
Definition: pinocchiorobot.hh:256
void computeForwardKinematics()
Compute the geometry of the robot.
pinocchio::Data * DataInInitialePose()
Definition: pinocchiorobot.hh:178
std::vector< pinocchio::JointIndex > jointsBetween(pinocchio::JointIndex first, pinocchio::JointIndex second)
tools :
void currentRPYVelocity(Eigen::VectorXd &vel)
Definition: pinocchiorobot.hh:249
bool checkModel(pinocchio::Model *robotModel)
void currentPinoAcceleration(Eigen::VectorXd &acc)
Definition: pinocchiorobot.hh:248
pinocchio::JointIndex waist()
Definition: pinocchiorobot.hh:190
virtual bool testArmsInverseKinematics()
testArmsInverseKinematics : test if the robot arms has the good joint configuration to use the analit...
Eigen::VectorXd & currentTau()
Definition: pinocchiorobot.hh:204
void currentRPYConfiguration(Eigen::VectorXd &)
virtual void initializeLegsInverseKinematics()
initializeInverseKinematics : initialize the internal data for the inverse kinematic e....
void zeroMomentumPoint(Eigen::Vector3d &zmp)
Definition: pinocchiorobot.hh:210
pinocchio::JointModelVector & getActuatedJoints()
Definition: pinocchiorobot.hh:194
const std::string & getName() const
pinocchio::JointIndex chest()
Definition: pinocchiorobot.hh:189
pinocchio::JointIndex rightWrist()
Definition: pinocchiorobot.hh:187
Eigen::VectorXd & currentRPYVelocity()
Definition: pinocchiorobot.hh:202
void positionCenterOfMass(Eigen::Vector3d &com)
Definition: pinocchiorobot.hh:219
void currentRPYAcceleration(Eigen::VectorXd &acc)
Definition: pinocchiorobot.hh:250
pinocchio::JointIndex getFreeFlyerSize() const
Definition: pinocchiorobot.hh:252
void RPYToSpatialFreeFlyer(Eigen::Vector3d &rpy, Eigen::Vector3d &drpy, Eigen::Vector3d &ddrpy, Eigen::Quaterniond &quat, Eigen::Vector3d &omega, Eigen::Vector3d &domega)
void currentPinoConfiguration(Eigen::VectorXd &conf)
bool initializeLeftFoot(PRFoot leftFoot)
pinocchio::JointIndex leftWrist()
Definition: pinocchiorobot.hh:186
Eigen::VectorXd & currentPinoConfiguration()
Definition: pinocchiorobot.hh:198
void CenterOfMass(Eigen::Vector3d &com, Eigen::Vector3d &dcom, Eigen::Vector3d &ddcom)
Definition: pinocchiorobot.hh:225
std::vector< pinocchio::JointIndex > fromRootToIt(pinocchio::JointIndex it)
virtual bool testLegsInverseKinematics()
testLegsInverseKinematics : test if the robot legs has the good joint configuration to use the analit...
bool initializeRightFoot(PRFoot rightFoot)
PRFoot * leftFoot()
Definition: pinocchiorobot.hh:183
bool isInitialized()
Definition: pinocchiorobot.hh:262
Eigen::VectorXd & currentRPYConfiguration()
Definition: pinocchiorobot.hh:199
pinocchio::Model * Model()
Definition: pinocchiorobot.hh:181
void computeInverseDynamics(Eigen::VectorXd &q, Eigen::VectorXd &v, Eigen::VectorXd &a)
Eigen::VectorXd & currentPinoVelocity()
Definition: pinocchiorobot.hh:200
bool initializeRobotModelAndData(pinocchio::Model *robotModel, pinocchio::Data *robotData)
PRFoot * rightFoot()
Definition: pinocchiorobot.hh:184
const int RPY_SIZE
Definition: pinocchiorobot.hh:56
const int QUATERNION_SIZE
Definition: pinocchiorobot.hh:57
\doc Simulate a rigid body
Definition: patterngeneratorinterface.hh:41
PinocchioRobotFoot_t PRFoot
Definition: pinocchiorobot.hh:53
doublereal * a
Definition: qld.cpp:386
Definition: pinocchiorobot.hh:38
double soleWidth
Definition: pinocchiorobot.hh:41
pinocchio::JointIndex associatedAnkle
Definition: pinocchiorobot.hh:39
double soleDepth
Definition: pinocchiorobot.hh:40
Eigen::Vector3d anklePosition
Definition: pinocchiorobot.hh:43
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PinocchioRobotFoot_t()
Definition: pinocchiorobot.hh:46
double soleHeight
Definition: pinocchiorobot.hh:42