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);
Class to perform inverse geometry on a robot arm.
Definition: biped_ig.hpp:89
const Eigen::Vector3d & getACoM()
Definition: biped_ig.hpp:163
const LegIGSettings & get_left_leg_settings()
Definition: biped_ig.hpp:146
const Eigen::Vector3d & getCoM()
Get the Angular Momentum. Please call computeDynamics first.
Definition: biped_ig.hpp:161
const LegIGSettings & get_right_leg_settings()
Definition: biped_ig.hpp:149
const BipedIGSettings & get_settings()
Definition: biped_ig.hpp:145
const Eigen::Vector3d & getAMVariation()
Get the Angular Momentum variation. Please call computeDynamics first.
Definition: biped_ig.hpp:158
const Eigen::VectorXd & getQ0()
Definition: biped_ig.hpp:153
pinocchio::Model & get_model()
Definition: biped_ig.hpp:281
void computeNL(const double &w, const Eigen::VectorXd &posture, const Eigen::VectorXd &velocity, const Eigen::VectorXd &acceleration, const Eigen::Matrix< double, 6, 1 > &externalWrench=Eigen::Matrix< double, 6, 1 >::Zero(), bool flatHorizontalGround=true)
Definition: biped_ig.cpp:423
void correctCoMfromWaist(const Eigen::Vector3d &com, const pinocchio::SE3 &leftFoot, const pinocchio::SE3 &rightFoot, const Eigen::VectorXd &q0, const double &tolerance=1e-10, const int &max_iterations=20)
Definition: biped_ig.cpp:360
void computeDynamics(const Eigen::VectorXd &posture, const Eigen::VectorXd &velocity, const Eigen::VectorXd &acceleration, const Eigen::Matrix< double, 6, 1 > &externalWrench=Eigen::Matrix< double, 6, 1 >::Zero(), bool flatHorizontalGround=true)
Definition: biped_ig.cpp:392
const Eigen::Vector2d & getCoP()
Definition: biped_ig.hpp:165
void solve(const Eigen::Vector3d &com, const pinocchio::SE3 &leftFoot, const pinocchio::SE3 &rightFoot, const Eigen::VectorXd &q0, Eigen::VectorXd &posture, const double &tolerance=1e-10, const int &max_iterations=0)
Definition: biped_ig.cpp:198
const Eigen::Vector3d & getAM()
Definition: biped_ig.hpp:164
BipedIG()
Definition: biped_ig.cpp:46
const Eigen::Vector2d & getNL()
Definition: biped_ig.hpp:166
const Eigen::Vector3d & getVCoM()
Definition: biped_ig.hpp:162
void setQ0(const Eigen::VectorXd q0)
Definition: biped_ig.hpp:154
void checkCompatibility()
void initialize(const BipedIGSettings &settings)
Definition: biped_ig.cpp:59
void set_com_from_waist(const Eigen::Vector3d &com_from_waist)
Definition: biped_ig.cpp:348
Definition: leg_ig.hpp:57
const LegIGSettings & get_settings()
Definition: leg_ig.hpp:74
Class to perform inverse geometry on a robot leg.
Definition: arm_ig.hpp:18
BipedIGSettings makeSettingsFor(const std::string &path_to_robots, const std::string &robot_name)
Definition: biped_ig.cpp:19
const std::string path_to_robots(EXAMPLE_ROBOT_DATA_MODEL_DIR)
Definition: biped_ig.hpp:28
std::string left_foot_frame_name
Definition: biped_ig.hpp:33
std::string right_ankle_joint_name
Definition: biped_ig.hpp:36
std::string right_knee_joint_name
Definition: biped_ig.hpp:35
std::string right_hip_joint_name
Definition: biped_ig.hpp:34
std::string right_foot_frame_name
Definition: biped_ig.hpp:37
std::string left_knee_joint_name
Definition: biped_ig.hpp:31
std::string left_hip_joint_name
Definition: biped_ig.hpp:30
std::string left_ankle_joint_name
Definition: biped_ig.hpp:32
friend bool operator==(const BipedIGSettings &lhs, const BipedIGSettings &rhs)
Definition: biped_ig.hpp:65
friend std::ostream & operator<<(std::ostream &out, const BipedIGSettings &obj)
Definition: biped_ig.hpp:49
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:47
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:42
Definition: leg_ig.hpp:27