diff --git a/include/qrw/FootstepPlanner.hpp b/include/qrw/FootstepPlanner.hpp
index 35e48a6112d09d1a4126632fd9ebdd5eba7b70ec..e093e3450fd6be39c5498cecb880147c692681fb 100644
--- a/include/qrw/FootstepPlanner.hpp
+++ b/include/qrw/FootstepPlanner.hpp
@@ -12,6 +12,11 @@
#define FOOTSTEPPLANNER_H_INCLUDED
#include "pinocchio/math/rpy.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/data.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/algorithm/compute-all-terms.hpp"
+#include "pinocchio/algorithm/frames.hpp"
#include "qrw/Gait.hpp"
#include "qrw/Params.hpp"
#include "qrw/Types.h"
@@ -56,7 +61,7 @@ public:
///
/// \param[in] refresh true if we move one step further in the gait
/// \param[in] k number of remaining wbc time step for the current mpc time step (wbc frequency is higher so there are inter-steps)
- /// \param[in] q current position vector of the flying base in horizontal frame (linear and angular stacked)
+ /// \param[in] q current position vector of the flying base in horizontal frame (linear and angular stacked) + actuators
/// \param[in] b_v current velocity vector of the flying base in horizontal frame (linear and angular stacked)
/// \param[in] b_vref desired velocity vector of the flying base in horizontal frame (linear and angular stacked)
///
@@ -78,14 +83,16 @@ private:
/// \param[in] b_vref desired velocity vector of the flying base in horizontal frame (linear and angular stacked)
///
////////////////////////////////////////////////////////////////////////////////////////////////
- MatrixN computeTargetFootstep(int k, VectorN const& q, Vector6 const& b_v, Vector6 const& b_vref);
+ MatrixN computeTargetFootstep(int k, Vector7 const& q, Vector6 const& b_v, Vector6 const& b_vref);
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Refresh feet position when entering a new contact phase
///
+ /// \param[in] q current configuration vector
+ ///
////////////////////////////////////////////////////////////////////////////////////////////////
- void updateNewContact();
+ void updateNewContact(Vector19 const& q);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -156,6 +163,13 @@ private:
Vector3 q_dxdy;
Vector3 RPY_;
Eigen::Quaterniond quat_;
+
+ pinocchio::Model model_; // Pinocchio model for forward kinematics
+ pinocchio::Data data_; // Pinocchio datas for forward kinematics
+ int foot_ids_[4] = {0, 0, 0, 0}; // Indexes of feet frames
+ Matrix34 pos_feet_; // Estimated feet positions based on measurements
+ Vector19 q_FK_;
+
};
#endif // FOOTSTEPPLANNER_H_INCLUDED
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 7bf9041a9c4fde0f6d896fc5b9b24d8f5dc98f0a..3495a5d585ddbd252ba3b288ee34981279dcb7d8 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -248,7 +248,7 @@ class Controller:
# Compute target footstep based on current and reference velocities
o_targetFootstep = self.footstepPlanner.updateFootsteps(self.k % self.k_mpc == 0 and self.k != 0,
int(self.k_mpc - self.k % self.k_mpc),
- self.q[0:7, 0:1],
+ self.q[:, 0:1],
self.h_v[0:6, 0:1].copy(),
self.v_ref[0:6, 0])
diff --git a/src/FootstepPlanner.cpp b/src/FootstepPlanner.cpp
index d351c2ee407516b7e226b2eb50dc4093c635f13c..afc939a240a829e1cba5af113e23e314e9e3e198 100644
--- a/src/FootstepPlanner.cpp
+++ b/src/FootstepPlanner.cpp
@@ -14,6 +14,8 @@ FootstepPlanner::FootstepPlanner()
, q_tmp(Vector3::Zero())
, q_dxdy(Vector3::Zero())
, RPY_(Vector3::Zero())
+ , pos_feet_(Matrix34::Zero())
+ , q_FK_(Vector19::Zero())
{
// Empty
}
@@ -40,6 +42,26 @@ void FootstepPlanner::initialize(Params& params, Gait& gaitIn)
footsteps_.push_back(Matrix34::Zero());
}
Rz(2, 2) = 1.0;
+
+ // Path to the robot URDF (TODO: Automatic path)
+ const std::string filename = std::string("/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf");
+
+ // Build model from urdf (base is not free flyer)
+ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_, false);
+
+ // Construct data from model
+ data_ = pinocchio::Data(model_);
+
+ // Update all the quantities of the model
+ VectorN q_tmp = VectorN::Zero(model_.nq);
+ q_tmp(6, 0) = 1.0; // Quaternion (0, 0, 0, 1)
+ pinocchio::computeAllTerms(model_, data_ , q_tmp, VectorN::Zero(model_.nv));
+
+ // Get feet frame IDs
+ foot_ids_[0] = static_cast<int>(model_.getFrameId("FL_FOOT")); // from long uint to int
+ foot_ids_[1] = static_cast<int>(model_.getFrameId("FR_FOOT"));
+ foot_ids_[2] = static_cast<int>(model_.getFrameId("HL_FOOT"));
+ foot_ids_[3] = static_cast<int>(model_.getFrameId("HR_FOOT"));
}
MatrixN FootstepPlanner::updateFootsteps(bool refresh, int k, VectorN const& q, Vector6 const& b_v, Vector6 const& b_vref)
@@ -47,7 +69,7 @@ MatrixN FootstepPlanner::updateFootsteps(bool refresh, int k, VectorN const& q,
// Update location of feet in stance phase (for those which just entered stance phase)
if (refresh && gait_->isNewPhase())
{
- updateNewContact();
+ updateNewContact(q);
}
// Feet in contact with the ground are moving in base frame (they don't move in world frame)
@@ -65,7 +87,7 @@ MatrixN FootstepPlanner::updateFootsteps(bool refresh, int k, VectorN const& q,
}
// Compute location of footsteps
- return computeTargetFootstep(k, q, b_v, b_vref);
+ return computeTargetFootstep(k, q.head(7), b_v, b_vref);
}
void FootstepPlanner::computeFootsteps(int k, Vector6 const& b_v, Vector6 const& b_vref)
@@ -192,7 +214,7 @@ void FootstepPlanner::updateTargetFootsteps()
}
}
-MatrixN FootstepPlanner::computeTargetFootstep(int k, VectorN const& q, Vector6 const& b_v, Vector6 const& b_vref)
+MatrixN FootstepPlanner::computeTargetFootstep(int k, Vector7 const& q, Vector6 const& b_v, Vector6 const& b_vref)
{
// Compute the desired location of footsteps over the prediction horizon
computeFootsteps(k, b_v, b_vref);
@@ -214,8 +236,31 @@ MatrixN FootstepPlanner::computeTargetFootstep(int k, VectorN const& q, Vector6
return o_targetFootstep_;
}
-void FootstepPlanner::updateNewContact()
+void FootstepPlanner::updateNewContact(Vector19 const& q)
{
+ // Remove translation and yaw rotation to get position in local frame
+ q_FK_ = q;
+ q_FK_.head(2) = Vector2::Zero();
+ Vector3 RPY = pinocchio::rpy::matrixToRpy(pinocchio::SE3::Quaternion(q_FK_(6), q_FK_(3), q_FK_(4), q_FK_(5)).toRotationMatrix());
+ q_FK_.block(3, 0, 4, 1) = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(RPY(0, 0), RPY(1, 0), 0.0)).coeffs();
+
+ // Update model and data of the robot
+ pinocchio::forwardKinematics(model_, data_, q_FK_);
+ pinocchio::updateFramePlacements(model_, data_);
+
+ // Get data required by IK with Pinocchio
+ for (int i = 0; i < 4; i++)
+ {
+ pos_feet_.col(i) = data_.oMf[foot_ids_[i]].translation();
+ }
+
+ /*std::cout << "q: " << q.transpose() << std::endl;
+ */
+
+ std::cout << "--- pos_feet_: " << std::endl << pos_feet_ << std::endl;
+ std::cout << "--- footsteps_:" << std::endl << footsteps_[1] << std::endl;
+
+ // Refresh position with estimated position if foot is in stance phase
for (int i = 0; i < 4; i++)
{
if (gait_->getCurrentGaitCoeff(0, i) == 1.0)
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index b2126d35b6fc50085b1b2227b4bdb1387e84a66b..563aeb2461d1fd26115ce71912fbca21cb343b0c 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -583,7 +583,9 @@ void WbcWrapper::initialize(Params& params)
data_ = pinocchio::Data(model_);
// Update all the quantities of the model
- pinocchio::computeAllTerms(model_, data_ , VectorN::Zero(model_.nq), VectorN::Zero(model_.nv));
+ VectorN q_tmp = VectorN::Zero(model_.nq);
+ q_tmp(6, 0) = 1.0; // Quaternion (0, 0, 0, 1)
+ pinocchio::computeAllTerms(model_, data_ , q_tmp, VectorN::Zero(model_.nv));
// Initialize inverse kinematic and box QP solvers
invkin_ = new InvKin();