biped_ig.hpp
Go to the documentation of this file.
1 
10 #ifndef AIG_BIPED_IG
11 #define AIG_BIPED_IG
12 
13 // clang-format off
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"
19 #include "aig/arm_ig.hpp"
20 #include "aig/leg_ig.hpp"
21 // clang-format on
22 
23 namespace aig {
29  public:
30  std::string left_hip_joint_name = "";
31  std::string left_knee_joint_name = "";
32  std::string left_ankle_joint_name = "";
33  std::string left_foot_frame_name = "";
34  std::string right_hip_joint_name = "";
35  std::string right_knee_joint_name = "";
36  std::string right_ankle_joint_name = "";
37  std::string right_foot_frame_name = "";
42  std::string urdf = "";
47  std::string srdf = "";
48 
49  friend std::ostream &operator<<(std::ostream &out,
50  const BipedIGSettings &obj) {
51  out << "BipedIGSettings:\n";
52  out << " left_hip_joint_name: " << obj.left_hip_joint_name << "\n";
53  out << " left_knee_joint_name: " << obj.left_knee_joint_name << "\n";
54  out << " left_ankle_joint_name: " << obj.left_ankle_joint_name << "\n";
55  out << " left_foot_frame_name: " << obj.left_foot_frame_name << "\n";
56  out << " right_hip_joint_name: " << obj.right_hip_joint_name << "\n";
57  out << " right_knee_joint_name: " << obj.right_knee_joint_name << "\n";
58  out << " right_ankle_joint_name: " << obj.right_ankle_joint_name << "\n";
59  out << " right_foot_frame_name: " << obj.right_foot_frame_name << "\n";
60  out << " urdf: " << obj.urdf << "\n";
61  out << " srdf: " << obj.srdf << std::endl;
62  return out;
63  }
64 
65  friend bool operator==(const BipedIGSettings &lhs,
66  const BipedIGSettings &rhs) {
67  bool test = true;
68  test &= lhs.left_hip_joint_name == rhs.left_hip_joint_name;
69  test &= lhs.left_knee_joint_name == rhs.left_knee_joint_name;
71  test &= lhs.left_foot_frame_name == rhs.left_foot_frame_name;
72  test &= lhs.right_hip_joint_name == rhs.right_hip_joint_name;
76  test &= lhs.urdf == rhs.urdf;
77  test &= lhs.srdf == rhs.srdf;
78  return test;
79  }
80 };
81 
82 BipedIGSettings makeSettingsFor(const std::string &path_to_robots,
83  const std::string &robot_name);
84 
89 class BipedIG {
90  // Private attributes.
91  private:
92  pinocchio::Model model_;
93  pinocchio::Data data_;
94  BipedIGSettings settings_;
95  LegIG left_leg_, right_leg_;
96  // ArmIG left_arm_, right_arm_;
97  Eigen::VectorXd q0_; // q0_ is a reference configuration used to take all not
98  // computed joints (such as head and arms)
99  Eigen::Vector3d com_from_waist_;
100  int lleg_idx_qs_; // Indexes in the configuration vector.
101  int rleg_idx_qs_; // Indexes in the configuration vector.
102  double mass_;
103  Eigen::Matrix2d S_;
104  Eigen::Vector3d gravity_;
105  // Eigen::Vector3d com_;
106  // Eigen::Vector3d vcom_;
107  Eigen::Vector3d acom_;
108  Eigen::Vector2d cop_;
109  Eigen::Vector3d dL_;
110  Eigen::Vector3d L_;
111  Eigen::Vector2d n_;
112 
113  // variables used in the waist-com vector correction:
114  Eigen::Vector3d error_, com_temp_;
115  Eigen::VectorXd posture_temp_;
116  Eigen::Matrix3d baseRotation_temp_;
117 
118  // Private methods.
119  private:
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);
123 
124  pinocchio::SE3 computeBase(const Eigen::Vector3d &com,
125  const pinocchio::SE3 &leftFoot,
126  const pinocchio::SE3 &rightFoot);
127 
128  pinocchio::SE3 computeBase(const Eigen::Vector3d &com,
129  const Eigen::Matrix3d &baseRotation);
130 
131  void configureLegs();
132 
133  // Internal computation variables
134  // on computeDynamics
135  Eigen::Vector3d groundForce_, groundCoMTorque_, nonCoPTorque_, weight_;
136 
137  // Public methods.
138  public:
139  BipedIG();
140 
141  BipedIG(const BipedIGSettings &settings);
142 
143  void initialize(const BipedIGSettings &settings);
144 
145  const BipedIGSettings &get_settings() { return settings_; };
147  return left_leg_.get_settings();
148  };
150  return right_leg_.get_settings();
151  };
152 
153  const Eigen::VectorXd &getQ0() { return q0_; }
154  void setQ0(const Eigen::VectorXd q0) { q0_ = q0; }
155 
158  const Eigen::Vector3d &getAMVariation() { return dL_; }
159 
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_; }
167 
168  void checkCompatibility(); // TODO
169 
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);
174 
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);
179 
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);
184 
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);
190 
191  void solve(const pinocchio::SE3 &base, const pinocchio::SE3 &leftFoot,
192  const pinocchio::SE3 &rightFoot, const Eigen::VectorXd &q0,
193  Eigen::VectorXd &posture);
194 
195  void solve(const Eigen::Isometry3d &base, const Eigen::Isometry3d &leftFoot,
196  const Eigen::Isometry3d &rightFoot, const Eigen::VectorXd &q0,
197  Eigen::VectorXd &posture);
198 
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);
206 
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);
214 
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);
223 
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);
232 
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,
238  const double &dt);
239 
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,
245  const double &dt);
246 
247  void set_com_from_waist(const Eigen::Vector3d &com_from_waist);
248 
249  void set_com_from_waist(const Eigen::VectorXd &q);
250 
251  void correctCoMfromWaist(const Eigen::Vector3d &com,
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);
257 
258  void correctCoMfromWaist(const Eigen::Vector3d &com,
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);
264 
265  void computeDynamics(const Eigen::VectorXd &posture,
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);
271 
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);
278 
279  void computeNL(const double &w);
280 
281  pinocchio::Model &get_model() { return model_; }
282 };
283 } // namespace aig
284 #endif // AIG_BIPED_IG
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