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"
63 const std::string &_left_knee_joint_name,
64 const std::string &_left_ankle_joint_name,
65 const std::string &_left_foot_frame_name,
66 const std::string &_right_hip_joint_name,
67 const std::string &_right_knee_joint_name,
68 const std::string &_right_ankle_joint_name,
69 const std::string &_right_foot_frame_name,
70 const std::string &_urdf,
const std::string &_srdf)
84 out <<
"BipedIGSettings:\n";
93 out <<
" urdf: " << obj.
urdf <<
"\n";
94 out <<
" srdf: " << obj.
srdf << std::endl;
116 const std::string &robot_name);
125 pinocchio::Model model_;
126 pinocchio::Data data_;
128 LegIG left_leg_, right_leg_;
132 Eigen::Vector3d com_from_waist_;
137 Eigen::Vector3d error_, com_temp_;
138 Eigen::VectorXd posture_temp_;
139 Eigen::Matrix3d baseRotation_temp_;
145 void derivatives(
const Eigen::VectorXd &q1,
const Eigen::VectorXd &q3,
146 Eigen::VectorXd &posture, Eigen::VectorXd &velocity,
147 Eigen::VectorXd &acceleration,
const double &dt);
149 pinocchio::SE3 computeBase(
const Eigen::Vector3d &com,
150 const pinocchio::SE3 &leftFoot,
151 const pinocchio::SE3 &rightFoot);
153 pinocchio::SE3 computeBase(
const Eigen::Vector3d &com,
154 const Eigen::Matrix3d &baseRotation);
156 void configureLegs();
174 const Eigen::VectorXd &
getQ0() {
return q0_; }
175 void setQ0(
const Eigen::VectorXd q0) { q0_ = q0; }
196 void solve(
const Eigen::Vector3d &com,
const pinocchio::SE3 &leftFoot,
197 const pinocchio::SE3 &rightFoot,
const Eigen::VectorXd &q0,
198 Eigen::VectorXd &posture,
const double &tolerance = 1e-10,
199 const int &max_iterations = 0);
201 void solve(
const Eigen::Vector3d &com,
const Eigen::Isometry3d &leftFeet,
202 const Eigen::Isometry3d &rightFeet,
const Eigen::VectorXd &q0,
203 Eigen::VectorXd &posture,
const double &tolerance = 1e-10,
204 const int &max_iterations = 0);
206 void solve(
const Eigen::Vector3d &com,
const Eigen::Matrix3d &baseRotation,
207 const pinocchio::SE3 &leftFoot,
const pinocchio::SE3 &rightFoot,
208 const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
209 const double &tolerance = 1e-10,
const int &max_iterations = 0);
211 void solve(
const Eigen::Vector3d &com,
const Eigen::Matrix3d &baseRotation,
212 const Eigen::Isometry3d &leftFoot,
213 const Eigen::Isometry3d &rightFoot,
const Eigen::VectorXd &q0,
214 Eigen::VectorXd &posture,
const double &tolerance = 1e-10,
215 const int &max_iterations = 0);
217 void solve(
const pinocchio::SE3 &base,
const pinocchio::SE3 &leftFoot,
218 const pinocchio::SE3 &rightFoot,
const Eigen::VectorXd &q0,
219 Eigen::VectorXd &posture);
221 void solve(
const Eigen::Isometry3d &base,
const Eigen::Isometry3d &leftFoot,
222 const Eigen::Isometry3d &rightFoot,
const Eigen::VectorXd &q0,
223 Eigen::VectorXd &posture);
225 void solve(
const std::array<Eigen::Vector3d, 3> &coms,
226 const std::array<pinocchio::SE3, 3> &leftFeet,
227 const std::array<pinocchio::SE3, 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<Eigen::Vector3d, 3> &coms,
234 const std::array<Eigen::Isometry3d, 3> &leftFeet,
235 const std::array<Eigen::Isometry3d, 3> &rightFeet,
236 const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
237 Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
238 const double &dt,
const double &tolerance = 1e-10,
239 const int &max_iterations = 0);
241 void solve(
const std::array<Eigen::Vector3d, 3> &coms,
242 const std::array<Eigen::Matrix3d, 3> &baseRotations,
243 const std::array<pinocchio::SE3, 3> &leftFeet,
244 const std::array<pinocchio::SE3, 3> &rightFeet,
245 const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
246 Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
247 const double &dt,
const double &tolerance = 1e-10,
248 const int &max_iterations = 0);
250 void solve(
const std::array<Eigen::Vector3d, 3> &coms,
251 const std::array<Eigen::Matrix3d, 3> &baseRotations,
252 const std::array<Eigen::Isometry3d, 3> &leftFeet,
253 const std::array<Eigen::Isometry3d, 3> &rightFeet,
254 const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
255 Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
256 const double &dt,
const double &tolerance = 1e-10,
257 const int &max_iterations = 0);
259 void solve(
const std::array<pinocchio::SE3, 3> &bases,
260 const std::array<pinocchio::SE3, 3> &leftFeet,
261 const std::array<pinocchio::SE3, 3> &rightFeet,
262 const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
263 Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
266 void solve(
const std::array<Eigen::Isometry3d, 3> &bases,
267 const std::array<Eigen::Isometry3d, 3> &leftFeet,
268 const std::array<Eigen::Isometry3d, 3> &rightFeet,
269 const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
270 Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
278 const pinocchio::SE3 &leftFoot,
279 const pinocchio::SE3 &rightFoot,
280 const Eigen::VectorXd &q0,
281 const double &tolerance = 1e-10,
282 const int &max_iterations = 20);
285 const Eigen::Isometry3d &leftFoot,
286 const Eigen::Isometry3d &rightFoot,
287 const Eigen::VectorXd &q0,
288 const double &tolerance = 1e-10,
289 const int &max_iterations = 20);
292 const Eigen::VectorXd &velocity,
293 const Eigen::VectorXd &acceleration,
294 const Eigen::Matrix<double, 6, 1> &externalWrench =
295 Eigen::Matrix<double, 6, 1>::Zero(),
296 bool flatHorizontalGround =
true);
298 void computeNL(
const double &w,
const Eigen::VectorXd &posture,
299 const Eigen::VectorXd &velocity,
300 const Eigen::VectorXd &acceleration,
301 const Eigen::Matrix<double, 6, 1> &externalWrench =
302 Eigen::Matrix<double, 6, 1>::Zero(),
303 bool flatHorizontalGround =
true);
312 #endif // AIG_BIPED_IG