postures.hpp
Go to the documentation of this file.
1 #ifndef __PREVIEW_IK_POSTURES___
2 #define __PREVIEW_IK_POSTURES___
3 
4 // clang-format off
5 #include <Eigen/Dense>
6 #include "pinocchio/fwd.hpp"
7 #include "pinocchio/algorithm/frames.hpp"
8 #include "pinocchio/parsers/sample-models.hpp"
9 // clang-format on
10 
11 namespace pin = pinocchio;
12 namespace IK_tools {
13 typedef Eigen::Matrix<double, 3, 3> RotMatrix;
14 typedef Eigen::Matrix<double, 3, 1> xyzVector;
15 typedef Eigen::Matrix<double, 6, 1> legJoints;
16 typedef Eigen::Matrix<double, 6, 1> Wrench;
17 enum Side { LEFT, RIGHT };
18 
19 struct LegSettings {
20  public:
24 };
25 class LegIG {
26  public:
28 
29  LegIG();
30  LegIG(const LegSettings &configuration);
31  legJoints solve(const pin::SE3 &base, const pin::SE3 &endEffector);
32 };
33 
34 class ArmIG {
35  public:
36  ArmIG();
37  Eigen::VectorXd solve(const pin::SE3 &base, const pin::SE3 &endEffector);
38 };
39 
40 struct BipedSettings {
41  public:
44 
46  pin::Model model;
47 };
48 class BipIK {
49  private:
50  void derivatives(const Eigen::VectorXd &q1, const Eigen::VectorXd &q3,
51  Eigen::VectorXd &posture, Eigen::VectorXd &velocity,
52  Eigen::VectorXd &acceleration, const double &dt);
53 
54  public:
58 
59  BipIK();
60 
61  BipIK(const BipedSettings &configuration);
62 
63  void checkCompatibility(); // TODO
64 
65  void configurateLegs();
66 
67  pin::SE3 computeBase(const xyzVector &com, const pin::SE3 &leftFoot,
68  const pin::SE3 &rightFoot);
69 
70  pin::SE3 computeBase(const xyzVector &com, const RotMatrix &baseRotation);
71 
72  void solve(const xyzVector &com, const pin::SE3 &leftFoot,
73  const pin::SE3 &rightFoot, const Eigen::VectorXd &q0,
74  Eigen::VectorXd &posture);
75 
76  void solve(const xyzVector &com, const RotMatrix &baseRotation,
77  const pin::SE3 &leftFoot, const pin::SE3 &rightFoot,
78  const Eigen::VectorXd &q0, Eigen::VectorXd &posture);
79 
80  void solve(const pin::SE3 &base, const pin::SE3 &leftFoot,
81  const pin::SE3 &rightFoot, const Eigen::VectorXd &q0,
82  Eigen::VectorXd &posture);
83 
84  void solve(const std::array<xyzVector, 3> &coms,
85  const std::array<pin::SE3, 3> &leftFeet,
86  const std::array<pin::SE3, 3> &rightFeet,
87  const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
88  Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
89  const double &dt);
90 
91  void solve(const std::array<xyzVector, 3> &coms,
92  const std::array<RotMatrix, 3> &baseRotations,
93  const std::array<pin::SE3, 3> &leftFeet,
94  const std::array<pin::SE3, 3> &rightFeet,
95  const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
96  Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
97  const double &dt);
98 
99  void solve(const std::array<pin::SE3, 3> &bases,
100  const std::array<pin::SE3, 3> &leftFeet,
101  const std::array<pin::SE3, 3> &rightFeet,
102  const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
103  Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
104  const double &dt);
105 
106  Eigen::Vector2d computeCoP(pin::Data &data, const Eigen::VectorXd &posture,
107  const Eigen::VectorXd &velocity,
108  const Eigen::VectorXd &acceleration,
109  bool flatHorizontalGround = true);
110 
111  Eigen::Vector2d computeCoP(pin::Data &data, const Eigen::VectorXd &posture,
112  const Eigen::VectorXd &velocity,
113  const Eigen::VectorXd &acceleration,
114  const Wrench &externalWrench,
115  bool flatHorizontalGround = true);
116 };
117 
118 // OLD
119 // CODE:////////////////////////////////////////////////////////////////////////
120 
121 class BipedIK {
122  public:
124  pin::Model model;
125  Eigen::VectorXd q0, q;
126  Eigen::VectorXd v, a;
127  BipedIK();
128  legJoints solveLeg(const pin::SE3 &com, const pin::SE3 &foot,
129  const Side &side) const;
130  Eigen::VectorXd computePosture(const pin::SE3 &com, const pin::SE3 &leftFoot,
131  const pin::SE3 &rightFoot, Eigen::VectorXd q);
132  void setRootOrientation(pin::SE3 &com, const pin::SE3 &leftFoot,
133  const pin::SE3 &rightFoot);
134  void computeJointDerivatives(const Eigen::VectorXd &q1,
135  const Eigen::VectorXd &q2,
136  const Eigen::VectorXd &q3, const double dt);
137 };
138 
139 } // namespace IK_tools
140 
141 #endif //__PREVIEW_IK_POSTURES___
IK_tools::BipedSettings::model
pin::Model model
Definition: postures.hpp:46
IK_tools::legJoints
Eigen::Matrix< double, 6, 1 > legJoints
Definition: postures.hpp:15
IK_tools::BipedIK::v
Eigen::VectorXd v
Definition: postures.hpp:126
IK_tools::BipedSettings::leftFootFrame
std::string leftFootFrame
Definition: postures.hpp:42
IK_tools::BipIK::rightArm
ArmIG rightArm
Definition: postures.hpp:57
IK_tools::LegSettings::hipFromWaist
xyzVector hipFromWaist
Definition: postures.hpp:23
IK_tools::xyzVector
Eigen::Matrix< double, 3, 1 > xyzVector
Definition: postures.hpp:14
IK_tools::Wrench
Eigen::Matrix< double, 6, 1 > Wrench
Definition: postures.hpp:16
IK_tools::BipedSettings::rightKneeJoint
std::string rightKneeJoint
Definition: postures.hpp:43
IK_tools::ArmIG
Definition: postures.hpp:34
IK_tools::BipedIK::comFromWaist
xyzVector comFromWaist
Definition: postures.hpp:123
IK_tools::BipIK::leftLeg
LegIG leftLeg
Definition: postures.hpp:56
IK_tools::ArmIG::solve
Eigen::VectorXd solve(const pin::SE3 &base, const pin::SE3 &endEffector)
IK_tools::LegSettings
Definition: postures.hpp:19
IK_tools::RIGHT
@ RIGHT
Definition: postures.hpp:17
IK_tools::BipedIK::a
Eigen::VectorXd a
Definition: postures.hpp:126
IK_tools::LEFT
@ LEFT
Definition: postures.hpp:17
IK_tools::BipedIK::BipedIK
BipedIK()
Definition: postures.cpp:268
IK_tools::BipIK::leftArm
ArmIG leftArm
Definition: postures.hpp:57
IK_tools::LegSettings::side
Side side
Definition: postures.hpp:21
IK_tools::BipedSettings::leftAnkleJoint
std::string leftAnkleJoint
Definition: postures.hpp:42
IK_tools::LegSettings::ankleFromFoot
xyzVector ankleFromFoot
Definition: postures.hpp:23
IK_tools::BipIK
Definition: postures.hpp:48
IK_tools::Side
Side
Definition: postures.hpp:17
IK_tools::BipedIK::computePosture
Eigen::VectorXd computePosture(const pin::SE3 &com, const pin::SE3 &leftFoot, const pin::SE3 &rightFoot, Eigen::VectorXd q)
Definition: postures.cpp:349
IK_tools::LegIG::info
LegSettings info
Definition: postures.hpp:27
IK_tools::BipedIK::computeJointDerivatives
void computeJointDerivatives(const Eigen::VectorXd &q1, const Eigen::VectorXd &q2, const Eigen::VectorXd &q3, const double dt)
Definition: postures.cpp:359
IK_tools::BipedSettings::rightFootFrame
std::string rightFootFrame
Definition: postures.hpp:43
IK_tools::BipedSettings::comFromWaist
xyzVector comFromWaist
Definition: postures.hpp:45
IK_tools::BipedSettings::rightHipJoint
std::string rightHipJoint
Definition: postures.hpp:43
IK_tools::LegIG
Definition: postures.hpp:25
IK_tools::BipedSettings::leftHipJoint
std::string leftHipJoint
Definition: postures.hpp:42
IK_tools::BipIK::solve
void solve(const xyzVector &com, const pin::SE3 &leftFoot, const pin::SE3 &rightFoot, const Eigen::VectorXd &q0, Eigen::VectorXd &posture)
Definition: postures.cpp:113
IK_tools::BipIK::configurateLegs
void configurateLegs()
Definition: postures.cpp:50
IK_tools::BipedIK::q
Eigen::VectorXd q
Definition: postures.hpp:125
IK_tools::BipedSettings::rightAnkleJoint
std::string rightAnkleJoint
Definition: postures.hpp:43
IK_tools::LegSettings::femurLength
double femurLength
Definition: postures.hpp:22
IK_tools::BipIK::checkCompatibility
void checkCompatibility()
IK_tools::BipedIK::solveLeg
legJoints solveLeg(const pin::SE3 &com, const pin::SE3 &foot, const Side &side) const
Definition: postures.cpp:289
IK_tools::BipIK::info
BipedSettings info
Definition: postures.hpp:55
IK_tools::RotMatrix
Eigen::Matrix< double, 3, 3 > RotMatrix
Definition: postures.hpp:13
IK_tools::BipIK::computeBase
pin::SE3 computeBase(const xyzVector &com, const pin::SE3 &leftFoot, const pin::SE3 &rightFoot)
Definition: postures.cpp:90
IK_tools::LegIG::solve
legJoints solve(const pin::SE3 &base, const pin::SE3 &endEffector)
Definition: postures.cpp:219
IK_tools::BipedIK::q0
Eigen::VectorXd q0
Definition: postures.hpp:125
IK_tools::BipedIK::setRootOrientation
void setRootOrientation(pin::SE3 &com, const pin::SE3 &leftFoot, const pin::SE3 &rightFoot)
Definition: postures.cpp:277
IK_tools::BipedIK
Definition: postures.hpp:121
IK_tools::BipedSettings::leftKneeJoint
std::string leftKneeJoint
Definition: postures.hpp:42
IK_tools::BipIK::rightLeg
LegIG rightLeg
Definition: postures.hpp:56
IK_tools::BipedIK::model
pin::Model model
Definition: postures.hpp:124
IK_tools::LegSettings::tibiaLenght
double tibiaLenght
Definition: postures.hpp:22
IK_tools::BipIK::computeCoP
Eigen::Vector2d computeCoP(pin::Data &data, const Eigen::VectorXd &posture, const Eigen::VectorXd &velocity, const Eigen::VectorXd &acceleration, bool flatHorizontalGround=true)
Definition: postures.cpp:207
IK_tools
Definition: postures.hpp:12
IK_tools::ArmIG::ArmIG
ArmIG()
Definition: postures.cpp:263
IK_tools::LegIG::LegIG
LegIG()
Definition: postures.cpp:217
IK_tools::BipIK::BipIK
BipIK()
Definition: postures.cpp:25
IK_tools::BipedSettings
Definition: postures.hpp:40