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 : left_hip_joint_name(
""),
52 left_knee_joint_name(
""),
53 left_ankle_joint_name(
""),
54 left_foot_frame_name(
""),
55 right_hip_joint_name(
""),
56 right_knee_joint_name(
""),
57 right_ankle_joint_name(
""),
58 right_foot_frame_name(
""),
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)
71 : left_hip_joint_name(_left_hip_joint_name),
72 left_knee_joint_name(_left_knee_joint_name),
73 left_ankle_joint_name(_left_ankle_joint_name),
74 left_foot_frame_name(_left_foot_frame_name),
75 right_hip_joint_name(_right_hip_joint_name),
76 right_knee_joint_name(_right_knee_joint_name),
77 right_ankle_joint_name(_right_ankle_joint_name),
78 right_foot_frame_name(_right_foot_frame_name),
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; }
194 void checkCompatibility();
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,
273 void set_com_from_waist(
const Eigen::Vector3d &com_from_waist);
275 void set_com_from_waist(
const Eigen::VectorXd &q);
277 void correctCoMfromWaist(
const Eigen::Vector3d &com,
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);
284 void correctCoMfromWaist(
const Eigen::Vector3d &com,
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);
291 void computeDynamics(
const Eigen::VectorXd &posture,
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);
305 void computeNL(
const double &w);
312 #endif // AIG_BIPED_IG const std::string path_to_robots(EXAMPLE_ROBOT_DATA_MODEL_DIR)
const Eigen::Vector3d & getVCoM()
Definition: dyna_com.hpp:163
const LegIGSettings & get_right_leg_settings()
Definition: biped_ig.hpp:170
BipedIGSettings(const std::string &_left_hip_joint_name, const std::string &_left_knee_joint_name, const std::string &_left_ankle_joint_name, const std::string &_left_foot_frame_name, const std::string &_right_hip_joint_name, const std::string &_right_knee_joint_name, const std::string &_right_ankle_joint_name, const std::string &_right_foot_frame_name, const std::string &_urdf, const std::string &_srdf)
Definition: biped_ig.hpp:62
const Eigen::Vector2d & getNL()
Definition: dyna_com.hpp:167
Class to perform inverse geometry on a robot arm.
Eigen::Vector3d & get_com_from_waist()
Definition: biped_ig.hpp:309
pinocchio::Data & get_data()
Definition: biped_ig.hpp:308
Definition: biped_ig.hpp:29
const Eigen::Vector2d & getCoP()
Definition: dyna_com.hpp:166
Definition: dyna_com.hpp:56
BipedIGSettings makeSettingsFor(const std::string &path_to_robots, const std::string &robot_name)
Definition: biped_ig.cpp:21
const Eigen::Vector3d & getAMVariation()
Get the Angular Momentum variation. Please call computeDynamics first. Deprecate it, AIG is not made for dynamics.
Definition: biped_ig.hpp:179
Class to perform inverse geometry on a robot leg.
const Eigen::Vector3d & getCoM()
Get the CoM position. Please call computeDynamics first.
Definition: biped_ig.hpp:182
std::string right_knee_joint_name
Definition: biped_ig.hpp:36
const Eigen::Vector3d & getVCoM()
Get the CoM velocity. Please call computeDynamics first.
Definition: biped_ig.hpp:184
Definition: biped_ig.hpp:122
const Eigen::Vector3d & getAM()
Definition: dyna_com.hpp:165
std::string right_ankle_joint_name
Definition: biped_ig.hpp:37
void setQ0(const Eigen::VectorXd q0)
Definition: biped_ig.hpp:175
const Eigen::Vector2d & getCoP()
Get the CoP Position. Please call computeDynamics first.
Definition: biped_ig.hpp:190
Definition: leg_ig.hpp:27
friend bool operator==(const BipedIGSettings &lhs, const BipedIGSettings &rhs)
Definition: biped_ig.hpp:98
std::string right_foot_frame_name
Definition: biped_ig.hpp:38
BipedIGSettings()
Definition: biped_ig.hpp:50
std::string right_hip_joint_name
Definition: biped_ig.hpp:35
const LegIGSettings & get_settings()
Definition: leg_ig.hpp:89
const Eigen::Vector3d & getACoM()
Definition: dyna_com.hpp:164
std::string urdf
This must contain either a valid path to the urdf file or the content of this file in a string...
Definition: biped_ig.hpp:43
pinocchio::Model & get_model()
Definition: biped_ig.hpp:307
friend std::ostream & operator<<(std::ostream &out, const BipedIGSettings &obj)
Definition: biped_ig.hpp:82
const Eigen::VectorXd & getQ0()
Definition: biped_ig.hpp:174
std::string srdf
This must contain either a valid path to the srdf file or the content of this file in a string...
Definition: biped_ig.hpp:48
const BipedIGSettings & get_settings()
Definition: biped_ig.hpp:166
const Eigen::Vector3d & getCoM()
Definition: dyna_com.hpp:162
const LegIGSettings & get_left_leg_settings()
Definition: biped_ig.hpp:167
const Eigen::Vector2d & getNL()
Get the nonlinear effect. Please call computeDynamics first.
Definition: biped_ig.hpp:192
std::string left_knee_joint_name
Definition: biped_ig.hpp:32
std::string left_ankle_joint_name
Definition: biped_ig.hpp:33
const Eigen::Vector3d & getACoM()
Get the CoM acceleration. Please call computeDynamics first.
Definition: biped_ig.hpp:186
Definition: arm_ig.hpp:18
std::string left_foot_frame_name
Definition: biped_ig.hpp:34
std::string left_hip_joint_name
Definition: biped_ig.hpp:31
const Eigen::Vector3d & getAMVariation()
Please call computeDynamics first.
Definition: dyna_com.hpp:161
Definition: leg_ig.hpp:72
const Eigen::Vector3d & getAM()
Get the angular momentum. Please call computeDynamics first.
Definition: biped_ig.hpp:188
Class to perform inverse geometry on a biped robot.