14 #include <Eigen/Dense>
15 #include "pinocchio/fwd.hpp"
16 #include "pinocchio/multibody/data.hpp"
17 #include "pinocchio/multibody/model.hpp"
18 #include "pinocchio/spatial/se3.hpp"
51 out <<
"BipedIGSettings:\n";
60 out <<
" urdf: " << obj.
urdf <<
"\n";
61 out <<
" srdf: " << obj.
srdf << std::endl;
83 const std::string &robot_name);
92 pinocchio::Model model_;
93 pinocchio::Data data_;
95 LegIG left_leg_, right_leg_;
99 Eigen::Vector3d com_from_waist_;
104 Eigen::Vector3d gravity_;
107 Eigen::Vector3d acom_;
108 Eigen::Vector2d cop_;
114 Eigen::Vector3d error_, com_temp_;
115 Eigen::VectorXd posture_temp_;
116 Eigen::Matrix3d baseRotation_temp_;
120 void derivatives(
const Eigen::VectorXd &q1,
const Eigen::VectorXd &q3,
121 Eigen::VectorXd &posture, Eigen::VectorXd &velocity,
122 Eigen::VectorXd &acceleration,
const double &dt);
124 pinocchio::SE3 computeBase(
const Eigen::Vector3d &com,
125 const pinocchio::SE3 &leftFoot,
126 const pinocchio::SE3 &rightFoot);
128 pinocchio::SE3 computeBase(
const Eigen::Vector3d &com,
129 const Eigen::Matrix3d &baseRotation);
131 void configureLegs();
135 Eigen::Vector3d groundForce_, groundCoMTorque_, nonCoPTorque_, weight_;
153 const Eigen::VectorXd &
getQ0() {
return q0_; }
154 void setQ0(
const Eigen::VectorXd q0) { q0_ = q0; }
161 const Eigen::Vector3d &
getCoM() {
return data_.com[0]; }
162 const Eigen::Vector3d &
getVCoM() {
return data_.vcom[0]; }
163 const Eigen::Vector3d &
getACoM() {
return acom_; }
164 const Eigen::Vector3d &
getAM() {
return L_; }
165 const Eigen::Vector2d &
getCoP() {
return cop_; }
166 const Eigen::Vector2d &
getNL() {
return n_; }
170 void solve(
const Eigen::Vector3d &com,
const pinocchio::SE3 &leftFoot,
171 const pinocchio::SE3 &rightFoot,
const Eigen::VectorXd &q0,
172 Eigen::VectorXd &posture,
const double &tolerance = 1e-10,
173 const int &max_iterations = 0);
175 void solve(
const Eigen::Vector3d &com,
const Eigen::Isometry3d &leftFeet,
176 const Eigen::Isometry3d &rightFeet,
const Eigen::VectorXd &q0,
177 Eigen::VectorXd &posture,
const double &tolerance = 1e-10,
178 const int &max_iterations = 0);
180 void solve(
const Eigen::Vector3d &com,
const Eigen::Matrix3d &baseRotation,
181 const pinocchio::SE3 &leftFoot,
const pinocchio::SE3 &rightFoot,
182 const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
183 const double &tolerance = 1e-10,
const int &max_iterations = 0);
185 void solve(
const Eigen::Vector3d &com,
const Eigen::Matrix3d &baseRotation,
186 const Eigen::Isometry3d &leftFoot,
187 const Eigen::Isometry3d &rightFoot,
const Eigen::VectorXd &q0,
188 Eigen::VectorXd &posture,
const double &tolerance = 1e-10,
189 const int &max_iterations = 0);
191 void solve(
const pinocchio::SE3 &base,
const pinocchio::SE3 &leftFoot,
192 const pinocchio::SE3 &rightFoot,
const Eigen::VectorXd &q0,
193 Eigen::VectorXd &posture);
195 void solve(
const Eigen::Isometry3d &base,
const Eigen::Isometry3d &leftFoot,
196 const Eigen::Isometry3d &rightFoot,
const Eigen::VectorXd &q0,
197 Eigen::VectorXd &posture);
199 void solve(
const std::array<Eigen::Vector3d, 3> &coms,
200 const std::array<pinocchio::SE3, 3> &leftFeet,
201 const std::array<pinocchio::SE3, 3> &rightFeet,
202 const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
203 Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
204 const double &dt,
const double &tolerance = 1e-10,
205 const int &max_iterations = 0);
207 void solve(
const std::array<Eigen::Vector3d, 3> &coms,
208 const std::array<Eigen::Isometry3d, 3> &leftFeet,
209 const std::array<Eigen::Isometry3d, 3> &rightFeet,
210 const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
211 Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
212 const double &dt,
const double &tolerance = 1e-10,
213 const int &max_iterations = 0);
215 void solve(
const std::array<Eigen::Vector3d, 3> &coms,
216 const std::array<Eigen::Matrix3d, 3> &baseRotations,
217 const std::array<pinocchio::SE3, 3> &leftFeet,
218 const std::array<pinocchio::SE3, 3> &rightFeet,
219 const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
220 Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
221 const double &dt,
const double &tolerance = 1e-10,
222 const int &max_iterations = 0);
224 void solve(
const std::array<Eigen::Vector3d, 3> &coms,
225 const std::array<Eigen::Matrix3d, 3> &baseRotations,
226 const std::array<Eigen::Isometry3d, 3> &leftFeet,
227 const std::array<Eigen::Isometry3d, 3> &rightFeet,
228 const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
229 Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
230 const double &dt,
const double &tolerance = 1e-10,
231 const int &max_iterations = 0);
233 void solve(
const std::array<pinocchio::SE3, 3> &bases,
234 const std::array<pinocchio::SE3, 3> &leftFeet,
235 const std::array<pinocchio::SE3, 3> &rightFeet,
236 const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
237 Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
240 void solve(
const std::array<Eigen::Isometry3d, 3> &bases,
241 const std::array<Eigen::Isometry3d, 3> &leftFeet,
242 const std::array<Eigen::Isometry3d, 3> &rightFeet,
243 const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
244 Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
252 const pinocchio::SE3 &leftFoot,
253 const pinocchio::SE3 &rightFoot,
254 const Eigen::VectorXd &q0,
255 const double &tolerance = 1e-10,
256 const int &max_iterations = 20);
259 const Eigen::Isometry3d &leftFoot,
260 const Eigen::Isometry3d &rightFoot,
261 const Eigen::VectorXd &q0,
262 const double &tolerance = 1e-10,
263 const int &max_iterations = 20);
266 const Eigen::VectorXd &velocity,
267 const Eigen::VectorXd &acceleration,
268 const Eigen::Matrix<double, 6, 1> &externalWrench =
269 Eigen::Matrix<double, 6, 1>::Zero(),
270 bool flatHorizontalGround =
true);
272 void computeNL(
const double &w,
const Eigen::VectorXd &posture,
273 const Eigen::VectorXd &velocity,
274 const Eigen::VectorXd &acceleration,
275 const Eigen::Matrix<double, 6, 1> &externalWrench =
276 Eigen::Matrix<double, 6, 1>::Zero(),
277 bool flatHorizontalGround =
true);
284 #endif // AIG_BIPED_IG