1 #ifndef __PREVIEW_IK_POSTURES___ 2 #define __PREVIEW_IK_POSTURES___ 6 #include "pinocchio/fwd.hpp" 7 #include "pinocchio/algorithm/frames.hpp" 8 #include "pinocchio/parsers/sample-models.hpp" 11 namespace pin = pinocchio;
16 typedef Eigen::Matrix<double, 6, 1>
Wrench;
31 legJoints solve(
const pin::SE3 &base,
const pin::SE3 &endEffector);
37 Eigen::VectorXd solve(
const pin::SE3 &base,
const pin::SE3 &endEffector);
42 std::string leftHipJoint, leftKneeJoint, leftAnkleJoint, leftFootFrame,
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);
63 void checkCompatibility();
65 void configurateLegs();
67 pin::SE3 computeBase(
const xyzVector &com,
const pin::SE3 &leftFoot,
68 const pin::SE3 &rightFoot);
70 pin::SE3 computeBase(
const xyzVector &com,
const RotMatrix &baseRotation);
72 void solve(
const xyzVector &com,
const pin::SE3 &leftFoot,
73 const pin::SE3 &rightFoot,
const Eigen::VectorXd &q0,
74 Eigen::VectorXd &posture);
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);
80 void solve(
const pin::SE3 &base,
const pin::SE3 &leftFoot,
81 const pin::SE3 &rightFoot,
const Eigen::VectorXd &q0,
82 Eigen::VectorXd &posture);
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,
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,
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,
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);
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);
125 Eigen::VectorXd
q0, q;
126 Eigen::VectorXd
v, a;
128 legJoints solveLeg(
const pin::SE3 &com,
const pin::SE3 &foot,
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);
141 #endif //__PREVIEW_IK_POSTURES___