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), soleDepth(0.0), soleWidth(0.0), soleHeight(0.0),
48 anklePosition(0.0, 0.0, 0.0) {}
49};
51
52namespace pinocchio_robot {
53const int RPY_SIZE = 6;
54const int QUATERNION_SIZE = 7;
55} // namespace pinocchio_robot
56
58public:
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
136 std::vector<std::string> &leftLegJointNames,
137 std::vector<std::string> &rightLegJointNames);
138
146
147public:
149 std::vector<pinocchio::JointIndex>
150 jointsBetween(pinocchio::JointIndex first, pinocchio::JointIndex second);
151 std::vector<pinocchio::JointIndex> fromRootToIt(pinocchio::JointIndex it);
152
153private:
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
172public:
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;
272private:
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
Definition: pinocchiorobot.hh:57
unsigned numberVelDof()
Definition: pinocchiorobot.hh:206
pinocchio::Data * Data()
Definition: pinocchiorobot.hh:175
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:245
Eigen::VectorXd & currentRPYAcceleration()
Definition: pinocchiorobot.hh:201
unsigned numberDof()
Definition: pinocchiorobot.hh:204
Eigen::VectorXd & currentPinoAcceleration()
Definition: pinocchiorobot.hh:199
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:190
pinocchio::JointIndex getFreeFlyerVelSize() const
Definition: pinocchiorobot.hh:254
void computeForwardKinematics()
Compute the geometry of the robot.
pinocchio::Data * DataInInitialePose()
Definition: pinocchiorobot.hh:176
std::vector< pinocchio::JointIndex > jointsBetween(pinocchio::JointIndex first, pinocchio::JointIndex second)
tools :
void currentRPYVelocity(Eigen::VectorXd &vel)
Definition: pinocchiorobot.hh:247
bool checkModel(pinocchio::Model *robotModel)
void currentPinoAcceleration(Eigen::VectorXd &acc)
Definition: pinocchiorobot.hh:246
pinocchio::JointIndex waist()
Definition: pinocchiorobot.hh:188
virtual bool testArmsInverseKinematics()
testArmsInverseKinematics : test if the robot arms has the good joint configuration to use the analit...
Eigen::VectorXd & currentTau()
Definition: pinocchiorobot.hh:202
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:208
pinocchio::JointModelVector & getActuatedJoints()
Definition: pinocchiorobot.hh:192
const std::string & getName() const
pinocchio::JointIndex chest()
Definition: pinocchiorobot.hh:187
pinocchio::JointIndex rightWrist()
Definition: pinocchiorobot.hh:185
Eigen::VectorXd & currentRPYVelocity()
Definition: pinocchiorobot.hh:200
void positionCenterOfMass(Eigen::Vector3d &com)
Definition: pinocchiorobot.hh:217
void currentRPYAcceleration(Eigen::VectorXd &acc)
Definition: pinocchiorobot.hh:248
pinocchio::JointIndex getFreeFlyerSize() const
Definition: pinocchiorobot.hh:250
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:184
Eigen::VectorXd & currentPinoConfiguration()
Definition: pinocchiorobot.hh:196
void CenterOfMass(Eigen::Vector3d &com, Eigen::Vector3d &dcom, Eigen::Vector3d &ddcom)
Definition: pinocchiorobot.hh:223
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:181
bool isInitialized()
Definition: pinocchiorobot.hh:260
Eigen::VectorXd & currentRPYConfiguration()
Definition: pinocchiorobot.hh:197
pinocchio::Model * Model()
Definition: pinocchiorobot.hh:179
void computeInverseDynamics(Eigen::VectorXd &q, Eigen::VectorXd &v, Eigen::VectorXd &a)
Eigen::VectorXd & currentPinoVelocity()
Definition: pinocchiorobot.hh:198
bool initializeRobotModelAndData(pinocchio::Model *robotModel, pinocchio::Data *robotData)
PRFoot * rightFoot()
Definition: pinocchiorobot.hh:182
const int RPY_SIZE
Definition: pinocchiorobot.hh:53
const int QUATERNION_SIZE
Definition: pinocchiorobot.hh:54
\doc Simulate a rigid body
Definition: patterngeneratorinterface.hh:41
PinocchioRobotFoot_t PRFoot
Definition: pinocchiorobot.hh:50
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