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___
Definition: postures.hpp:34
Eigen::VectorXd solve(const pin::SE3 &base, const pin::SE3 &endEffector)
ArmIG()
Definition: postures.cpp:263
Definition: postures.hpp:48
ArmIG rightArm
Definition: postures.hpp:57
void checkCompatibility()
LegIG rightLeg
Definition: postures.hpp:56
void solve(const xyzVector &com, const pin::SE3 &leftFoot, const pin::SE3 &rightFoot, const Eigen::VectorXd &q0, Eigen::VectorXd &posture)
Definition: postures.cpp:113
void configurateLegs()
Definition: postures.cpp:50
LegIG leftLeg
Definition: postures.hpp:56
BipedSettings info
Definition: postures.hpp:55
ArmIG leftArm
Definition: postures.hpp:57
pin::SE3 computeBase(const xyzVector &com, const pin::SE3 &leftFoot, const pin::SE3 &rightFoot)
Definition: postures.cpp:90
BipIK()
Definition: postures.cpp:25
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
Definition: postures.hpp:121
void computeJointDerivatives(const Eigen::VectorXd &q1, const Eigen::VectorXd &q2, const Eigen::VectorXd &q3, const double dt)
Definition: postures.cpp:359
Eigen::VectorXd computePosture(const pin::SE3 &com, const pin::SE3 &leftFoot, const pin::SE3 &rightFoot, Eigen::VectorXd q)
Definition: postures.cpp:349
pin::Model model
Definition: postures.hpp:124
Eigen::VectorXd q
Definition: postures.hpp:125
Eigen::VectorXd q0
Definition: postures.hpp:125
BipedIK()
Definition: postures.cpp:268
legJoints solveLeg(const pin::SE3 &com, const pin::SE3 &foot, const Side &side) const
Definition: postures.cpp:289
void setRootOrientation(pin::SE3 &com, const pin::SE3 &leftFoot, const pin::SE3 &rightFoot)
Definition: postures.cpp:277
Eigen::VectorXd a
Definition: postures.hpp:126
xyzVector comFromWaist
Definition: postures.hpp:123
Eigen::VectorXd v
Definition: postures.hpp:126
Definition: postures.hpp:25
LegSettings info
Definition: postures.hpp:27
LegIG()
Definition: postures.cpp:217
legJoints solve(const pin::SE3 &base, const pin::SE3 &endEffector)
Definition: postures.cpp:219
Definition: postures.hpp:12
Eigen::Matrix< double, 3, 3 > RotMatrix
Definition: postures.hpp:13
Eigen::Matrix< double, 3, 1 > xyzVector
Definition: postures.hpp:14
Eigen::Matrix< double, 6, 1 > Wrench
Definition: postures.hpp:16
Side
Definition: postures.hpp:17
@ RIGHT
Definition: postures.hpp:17
@ LEFT
Definition: postures.hpp:17
Eigen::Matrix< double, 6, 1 > legJoints
Definition: postures.hpp:15
Definition: postures.hpp:40
std::string leftKneeJoint
Definition: postures.hpp:42
std::string leftFootFrame
Definition: postures.hpp:42
std::string rightHipJoint
Definition: postures.hpp:43
std::string rightAnkleJoint
Definition: postures.hpp:43
std::string rightKneeJoint
Definition: postures.hpp:43
std::string rightFootFrame
Definition: postures.hpp:43
xyzVector comFromWaist
Definition: postures.hpp:45
std::string leftHipJoint
Definition: postures.hpp:42
pin::Model model
Definition: postures.hpp:46
std::string leftAnkleJoint
Definition: postures.hpp:42
Definition: postures.hpp:19
xyzVector ankleFromFoot
Definition: postures.hpp:23
Side side
Definition: postures.hpp:21
double femurLength
Definition: postures.hpp:22
double tibiaLenght
Definition: postures.hpp:22
xyzVector hipFromWaist
Definition: postures.hpp:23