Skip to content
Snippets Groups Projects
Commit 991f46f6 authored by odri's avatar odri
Browse files

Re-add the free flyer in the inverse kinematics

parent 166955d5
No related branches found
No related tags found
No related merge requests found
......@@ -18,6 +18,7 @@
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <cmath>
......@@ -75,16 +76,17 @@ class InvKin {
/// \param[in] pgoals Desired positions of the four feet in base frame
/// \param[in] vgoals Desired velocities of the four feet in base frame
/// \param[in] agoals Desired accelerations of the four feet in base frame
/// \param[in] x_cmd Desired position, orientation and velocity of the base
///
////////////////////////////////////////////////////////////////////////////////////////////////
void run_InvKin(VectorN const& q, VectorN const& dq, MatrixN const& contacts, MatrixN const& pgoals,
MatrixN const& vgoals, MatrixN const& agoals);
MatrixN const& vgoals, MatrixN const& agoals, MatrixN const& x_cmd);
Eigen::MatrixXd get_q_step() { return q_step_; }
Eigen::MatrixXd get_dq_cmd() { return dq_cmd_; }
VectorN get_q_step() { return q_step_; }
VectorN get_dq_cmd() { return dq_cmd_; }
VectorN get_q_cmd() { return q_cmd_; }
VectorN get_ddq_cmd() { return ddq_cmd_; }
Matrix12 get_Jf() { return Jf_; }
Eigen::Matrix<double, 12, 18> get_Jf() { return Jf_; }
int get_foot_id(int i) { return foot_ids_[i]; }
Matrix43 get_posf() { return posf_; }
Matrix43 get_vf() { return vf_; }
......@@ -95,31 +97,72 @@ class InvKin {
// Matrices initialisation
Matrix12 invJ; // Inverse of the feet Jacobian
Matrix112 acc; // Reshaped feet acceleration references to get command accelerations for actuators
Matrix112 x_err; // Reshaped feet position errors to get command position step for actuators
Matrix112 dx_r; // Reshaped feet velocity references to get command velocities for actuators
Matrix118 acc; // Reshaped feet acceleration references to get command accelerations for actuators
Matrix118 x_err; // Reshaped feet position errors to get command position step for actuators
Matrix118 dx_r; // Reshaped feet velocity references to get command velocities for actuators
Matrix43 pfeet_err; // Feet position errors to get command position step for actuators
Matrix43 vfeet_ref; // Feet velocity references to get command velocities for actuators
Matrix43 afeet; // Feet acceleration references to get command accelerations for actuators
int foot_ids_[4] = {0, 0, 0, 0}; // Feet frame IDs
int base_id_ = 0; // Base ID
Matrix43 posf_; // Current feet positions
Matrix43 vf_; // Current feet linear velocities
Matrix43 wf_; // Current feet angular velocities
Matrix43 af_; // Current feet linear accelerations
Matrix12 Jf_; // Current feet Jacobian (only linear part)
Eigen::Matrix<double, 6, 12> Jf_tmp_; // Temporary storage variable to only retrieve the linear part of the Jacobian
Eigen::Matrix<double, 12, 18> Jf_; // Current feet Jacobian (only linear part)
Eigen::Matrix<double, 6, 18> Jf_tmp_; // Temporary storage variable to only retrieve the linear part of the Jacobian
// and discard the angular part
Vector12 ddq_cmd_; // Actuator command accelerations
Vector12 dq_cmd_; // Actuator command velocities
Vector12 q_cmd_; // Actuator command positions
Vector12 q_step_; // Actuator command position steps
Vector3 posb_;
Vector3 posb_ref_;
Vector3 posb_err_;
Matrix3 rotb_;
Matrix3 rotb_ref_;
Vector3 rotb_err_;
Vector3 vb_;
Vector3 vb_ref_;
Vector3 wb_;
Vector3 wb_ref_;
Vector6 ab_;
Vector3 abasis;
Vector3 awbasis;
Eigen::Matrix<double, 6, 18> Jb_;
Eigen::Matrix<double, 18, 18> J_;
Eigen::Matrix<double, 18, 18> invJ_;
Vector3 Kp_base_position;
Vector3 Kd_base_position;
Vector3 Kp_base_orientation;
Vector3 Kd_base_orientation;
Vector18 ddq_cmd_; // Actuator command accelerations
Vector18 dq_cmd_; // Actuator command velocities
Vector19 q_cmd_; // Actuator command positions
Vector18 q_step_; // Actuator command position steps
pinocchio::Model model_; // Pinocchio model for frame computations and inverse kinematics
pinocchio::Data data_; // Pinocchio datas for frame computations and inverse kinematics
};
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Compute the pseudo inverse of a matrix using the Jacobi SVD formula
///
////////////////////////////////////////////////////////////////////////////////////////////////
template <typename _Matrix_Type_>
_Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon = std::numeric_limits<double>::epsilon()) {
Eigen::JacobiSVD<_Matrix_Type_> svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
double tolerance =
epsilon * static_cast<double>(std::max(a.cols(), a.rows())) * svd.singularValues().array().abs()(0);
return svd.matrixV() *
(svd.singularValues().array().abs() > tolerance)
.select(svd.singularValues().array().inverse(), 0)
.matrix()
.asDiagonal() *
svd.matrixU().adjoint();
}
#endif // INVKIN_H_INCLUDED
......@@ -285,10 +285,12 @@ class WbcWrapper {
/// \param[in] vgoals Desired velocities of the four feet in base frame
/// \param[in] agoals Desired accelerations of the four feet in base frame
/// \param[in] q_mpc Estimated configuration vector given to the MPC
/// \param[in] v_mpc Estimated velocity vector given to the MPC
///
////////////////////////////////////////////////////////////////////////////////////////////////
void compute(VectorN const &q, VectorN const &dq, VectorN const &f_cmd, MatrixN const &contacts,
MatrixN const &pgoals, MatrixN const &vgoals, MatrixN const &agoals, VectorN const &q_mpc);
MatrixN const &pgoals, MatrixN const &vgoals, MatrixN const &agoals, VectorN const &q_mpc,
VectorN const &v_mpc);
VectorN get_qdes() { return qdes_; }
VectorN get_vdes() { return vdes_; }
......@@ -343,22 +345,4 @@ class WbcWrapper {
int indexes_[4] = {10, 18, 26, 34}; // Indexes of feet frames in this order: [FL, FR, HL, HR]
};
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Compute the pseudo inverse of a matrix using the Jacobi SVD formula
///
////////////////////////////////////////////////////////////////////////////////////////////////
template <typename _Matrix_Type_>
_Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon = std::numeric_limits<double>::epsilon()) {
Eigen::JacobiSVD<_Matrix_Type_> svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
double tolerance =
epsilon * static_cast<double>(std::max(a.cols(), a.rows())) * svd.singularValues().array().abs()(0);
return svd.matrixV() *
(svd.singularValues().array().abs() > tolerance)
.select(svd.singularValues().array().inverse(), 0)
.matrix()
.asDiagonal() *
svd.matrixU().adjoint();
}
#endif // QPWBC_H_INCLUDED
......@@ -30,6 +30,7 @@ using Matrix12 = Eigen::Matrix<double, 12, 12>;
using Matrix13 = Eigen::Matrix<double, 1, 3>;
using Matrix14 = Eigen::Matrix<double, 1, 4>;
using Matrix112 = Eigen::Matrix<double, 1, 12>;
using Matrix118 = Eigen::Matrix<double, 1, 18>;
using Matrix34 = Eigen::Matrix<double, 3, 4>;
using Matrix43 = Eigen::Matrix<double, 4, 3>;
using Matrix64 = Eigen::Matrix<double, 6, 4>;
......
......@@ -233,7 +233,7 @@ struct InvKinPythonVisitor : public bp::def_visitor<InvKinPythonVisitor<InvKin>>
.def("get_foot_id", &InvKin::get_foot_id, bp::args("i"), "Get food frame id.\n")
// Run InvKin from Python
.def("run_InvKin", &InvKin::run_InvKin, bp::args("contacts", "pgoals", "vgoals", "agoals"),
.def("run_InvKin", &InvKin::run_InvKin, bp::args("contacts", "pgoals", "vgoals", "agoals", "x_cmd"),
"Run InvKin from Python.\n");
}
......@@ -309,7 +309,8 @@ struct WbcWrapperPythonVisitor : public bp::def_visitor<WbcWrapperPythonVisitor<
.def_readonly("feet_acc_target", &WbcWrapper::get_feet_acc_target)
// Run WbcWrapper from Python
.def("compute", &WbcWrapper::compute, bp::args("q", "dq", "f_cmd", "contacts", "pgoals", "vgoals", "agoals"), "Run WbcWrapper from Python.\n");
.def("compute", &WbcWrapper::compute, bp::args("q", "dq", "f_cmd", "contacts", "pgoals", "vgoals",
"agoals", "q_mpc", "v_mpc"), "Run WbcWrapper from Python.\n");
}
static void expose()
......
......@@ -325,7 +325,8 @@ class Controller:
self.feet_p_cmd,
self.feet_v_cmd,
self.feet_a_cmd,
self.q_filt_mpc[:, 0:1])
self.q_filt_mpc[:, 0:1],
self.h_v_filt_mpc[:, 0:1])
# Quantities sent to the control board
self.result.P = np.array(self.Kp_main.tolist() * 4)
......
......@@ -2,9 +2,9 @@
InvKin::InvKin()
: invJ(Matrix12::Zero()),
acc(Matrix112::Zero()),
x_err(Matrix112::Zero()),
dx_r(Matrix112::Zero()),
acc(Matrix118::Zero()),
x_err(Matrix118::Zero()),
dx_r(Matrix118::Zero()),
pfeet_err(Matrix43::Zero()),
vfeet_ref(Matrix43::Zero()),
afeet(Matrix43::Zero()),
......@@ -12,12 +12,28 @@ InvKin::InvKin()
vf_(Matrix43::Zero()),
wf_(Matrix43::Zero()),
af_(Matrix43::Zero()),
Jf_(Matrix12::Zero()),
Jf_tmp_(Eigen::Matrix<double, 6, 12>::Zero()),
ddq_cmd_(Vector12::Zero()),
dq_cmd_(Vector12::Zero()),
q_cmd_(Vector12::Zero()),
q_step_(Vector12::Zero()) {}
Jf_(Eigen::Matrix<double, 12, 18>::Zero()),
Jf_tmp_(Eigen::Matrix<double, 6, 18>::Zero()),
posb_(Vector3::Zero()),
posb_ref_(Vector3::Zero()),
posb_err_(Vector3::Zero()),
rotb_(Matrix3::Identity()),
rotb_ref_(Matrix3::Identity()),
rotb_err_(Vector3::Zero()),
vb_(Vector3::Zero()),
vb_ref_(Vector3::Zero()),
wb_(Vector3::Zero()),
wb_ref_(Vector3::Zero()),
ab_(Vector6::Zero()),
abasis(Vector3::Zero()),
awbasis(Vector3::Zero()),
Jb_(Eigen::Matrix<double, 6, 18>::Zero()),
J_(Eigen::Matrix<double, 18, 18>::Zero()),
invJ_(Eigen::Matrix<double, 18, 18>::Zero()),
ddq_cmd_(Vector18::Zero()),
dq_cmd_(Vector18::Zero()),
q_cmd_(Vector19::Zero()),
q_step_(Vector18::Zero()) {}
void InvKin::initialize(Params& params) {
// Params store parameters
......@@ -28,7 +44,7 @@ void InvKin::initialize(Params& params) {
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, model_, false);
pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_, false);
// Construct data from model
data_ = pinocchio::Data(model_);
......@@ -41,6 +57,16 @@ void InvKin::initialize(Params& params) {
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"));
// Get base ID
base_id_ = static_cast<int>(model_.getFrameId("base_link")); // from long uint to int
// Set task gains
Kp_base_position << 0.0, 0.0, 0.0;
Kd_base_position << 0.0, 0.0, 0.0;
Kp_base_orientation << 0.0, 0.0, 0.0;
Kd_base_orientation << 0.0, 0.0, 0.0;
}
void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals, Matrix43 const& vgoals,
......@@ -57,31 +83,71 @@ void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals,
}*/
afeet.row(i) -= af_.row(i) + (wf_.row(i)).cross(vf_.row(i)); // Drift
}
J_.block(6, 0, 12, 18) = Jf_.block(0, 0, 12, 18);
// Process base position
posb_err_ = posb_ref_ - posb_;
abasis = Kp_base_position.cwiseProduct(posb_err_) - Kd_base_position.cwiseProduct(vb_ - vb_ref_);
abasis -= ab_.head(3) + wb_.cross(vb_);
// Process base orientation
rotb_err_ = -rotb_ref_ * pinocchio::log3(rotb_ref_.transpose() * rotb_);
awbasis = Kp_base_orientation.cwiseProduct(rotb_err_) - Kd_base_orientation.cwiseProduct(wb_ - wb_ref_);
awbasis -= ab_.tail(3);
J_.block(0, 0, 6, 18) = Jb_.block(0, 0, 6, 18); // Position and orientation
acc.block(0, 0, 1, 3) = abasis.transpose();
acc.block(0, 3, 1, 3) = awbasis.transpose();
for (int i = 0; i < 4; i++) {
acc.block(0, 6+3*i, 1, 3) = afeet.row(i);
}
x_err.block(0, 0, 1, 3) = posb_err_.transpose();
x_err.block(0, 3, 1, 3) = rotb_err_.transpose();
for (int i = 0; i < 4; i++) {
x_err.block(0, 6+3*i, 1, 3) = pfeet_err.row(i);
}
dx_r.block(0, 0, 1, 3) = vb_ref_.transpose();
dx_r.block(0, 3, 1, 3) = wb_ref_.transpose();
for (int i = 0; i < 4; i++) {
dx_r.block(0, 6+3*i, 1, 3) = vfeet_ref.row(i);
}
// Jacobian inversion using damped pseudo inverse
invJ_ = pseudoInverse(J_);
// Store data and invert the Jacobian
/*
for (int i = 0; i < 4; i++) {
acc.block(0, 3 * i, 1, 3) = afeet.row(i);
x_err.block(0, 3 * i, 1, 3) = pfeet_err.row(i);
dx_r.block(0, 3 * i, 1, 3) = vfeet_ref.row(i);
invJ.block(3 * i, 3 * i, 3, 3) = Jf_.block(3 * i, 3 * i, 3, 3).inverse();
}
*/
// Once Jacobian has been inverted we can get command accelerations, velocities and positions
ddq_cmd_ = invJ * acc.transpose();
dq_cmd_ = invJ * dx_r.transpose();
q_step_ = invJ * x_err.transpose(); // Not a position but a step in position
ddq_cmd_ = invJ_ * acc.transpose();
dq_cmd_ = invJ_ * dx_r.transpose();
q_step_ = invJ_ * x_err.transpose(); // Not a position but a step in position
/*
std::cout << "J" << std::endl << Jf << std::endl;
std::cout << "invJ" << std::endl << invJ << std::endl;
/*std::cout << "J" << std::endl << J_ << std::endl;
std::cout << "invJ" << std::endl << invJ_ << std::endl;
std::cout << "acc" << std::endl << acc << std::endl;
std::cout << "q_step" << std::endl << q_step << std::endl;
std::cout << "dq_cmd" << std::endl << dq_cmd << std::endl;
*/
std::cout << "dx_r" << std::endl << dx_r << std::endl;
std::cout << "x_err" << std::endl << x_err << std::endl;
std::cout << "ddq_cmd" << std::endl << ddq_cmd_ << std::endl;
std::cout << "dq_cmd" << std::endl << dq_cmd_ << std::endl;
std::cout << "q_step" << std::endl << q_step_ << std::endl;*/
}
void InvKin::run_InvKin(VectorN const& q, VectorN const& dq, MatrixN const& contacts, MatrixN const& pgoals,
MatrixN const& vgoals, MatrixN const& agoals) {
MatrixN const& vgoals, MatrixN const& agoals, MatrixN const& x_cmd) {
// std::cout << "run invkin q: " << q << std::endl;
// Update model and data of the robot
pinocchio::forwardKinematics(model_, data_, q, dq, VectorN::Zero(model_.nv));
pinocchio::computeJointJacobians(model_, data_);
......@@ -98,13 +164,45 @@ void InvKin::run_InvKin(VectorN const& q, VectorN const& dq, MatrixN const& cont
Jf_tmp_.setZero(); // Fill with 0s because getFrameJacobian only acts on the coeffs it changes so the
// other coeffs keep their previous value instead of being set to 0
pinocchio::getFrameJacobian(model_, data_, idx, pinocchio::LOCAL_WORLD_ALIGNED, Jf_tmp_);
Jf_.block(3 * i, 0, 3, 12) = Jf_tmp_.block(0, 0, 3, 12);
Jf_.block(3 * i, 0, 3, 18) = Jf_tmp_.block(0, 0, 3, 18);
}
// Update position and velocity of the base
posb_ = data_.oMf[base_id_].translation(); // Position
rotb_ = data_.oMf[base_id_].rotation(); // Orientation
pinocchio::Motion nu = pinocchio::getFrameVelocity(model_, data_, base_id_, pinocchio::LOCAL_WORLD_ALIGNED);
vb_ = nu.linear(); // Linear velocity
wb_ = nu.angular(); // Angular velocity
pinocchio::Motion acc = pinocchio::getFrameAcceleration(model_, data_, base_id_, pinocchio::LOCAL_WORLD_ALIGNED);
ab_.head(3) = acc.linear(); // Linear acceleration
ab_.tail(3) = acc.angular(); // Angular acceleration
pinocchio::getFrameJacobian(model_, data_, base_id_, pinocchio::LOCAL_WORLD_ALIGNED, Jb_);
// Update reference position and reference velocity of the base
posb_ref_ = x_cmd.block(0, 0, 3, 1); // Ref position
rotb_ref_ = pinocchio::rpy::rpyToMatrix(x_cmd(3, 0), x_cmd(4, 0), x_cmd(5, 0)); // Ref orientation
vb_ref_ = x_cmd.block(6, 0, 3, 1); // Ref linear velocity
wb_ref_ = x_cmd.block(9, 0, 3, 1); // Ref angular velocity
/*std::cout << "----" << std::endl;
std::cout << posf_ << std::endl;
std::cout << Jf_ << std::endl;
std::cout << posb_ << std::endl;
std::cout << rotb_ << std::endl;
std::cout << vb_ << std::endl;
std::cout << wb_ << std::endl;
std::cout << ab_ << std::endl;*/
// IK output for accelerations of actuators (stored in ddq_cmd_)
// IK output for velocities of actuators (stored in dq_cmd_)
refreshAndCompute(contacts, pgoals, vgoals, agoals);
// IK output for positions of actuators
q_cmd_ = q + q_step_;
q_cmd_ = pinocchio::integrate(model_, q, q_step_);
/*std::cout << "q: " << q << std::endl;
std::cout << "q_step_: " << q_step_ << std::endl;
std::cout << " q_cmd_: " << q_cmd_ << std::endl;*/
}
......@@ -516,7 +516,7 @@ void WbcWrapper::initialize(Params &params) {
}
void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_cmd, MatrixN const &contacts,
MatrixN const &pgoals, MatrixN const &vgoals, MatrixN const &agoals, VectorN const &q_mpc) {
MatrixN const &pgoals, MatrixN const &vgoals, MatrixN const &agoals, VectorN const &q_mpc, VectorN const &v_mpc) {
if (f_cmd.rows() != 12) {
throw std::runtime_error("f_cmd should be a vector of size 12");
......@@ -570,8 +570,17 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
// Compute Inverse Kinematics
invkin_->run_InvKin(q.tail(12), dq.tail(12), contacts, pgoals.transpose(), vgoals.transpose(), agoals.transpose());
ddq_cmd_.tail(12) = invkin_->get_ddq_cmd();
Vector19 q_IK = Vector19::Zero();
q_IK.block(3, 0, 4, 1) = q_mpc_.block(3, 0, 4, 1);
q_IK.tail(12) = q.tail(12);
Vector18 dq_IK = Vector18::Zero();
dq_IK.tail(12) = dq.tail(12);
/*std::cout << q.transpose() << std::endl;
std::cout << q_IK.transpose() << std::endl;*/
invkin_->run_InvKin(q_IK, dq_IK, contacts, pgoals.transpose(), vgoals.transpose(), agoals.transpose(), Vector12::Zero());
ddq_cmd_ = invkin_->get_ddq_cmd();
// std::cout << data_fmpc_.M.block(6, 6, 12, 12) * ddq_cmd_.tail(12) << std::endl;
......@@ -592,6 +601,7 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
}
}
/*
int cpt = 0;
double ax = 0.0;
double ay = 0.0;
......@@ -607,6 +617,7 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
ay *= 1 / cpt;
}
ddq_cmd_.head(2) << ax, ay;
*/
// Compute the inverse dynamics, aka the joint torques according to the current state of the system,
// the desired joint accelerations and the external forces, using the Recursive Newton Euler Algorithm.
......@@ -641,13 +652,26 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
std::cout << "ddq del" << std::endl;
std::cout << ddq_with_delta_ << std::endl;
std::cout << "f del" << std::endl;
std::cout << f_with_delta_ << std::endl;*/
std::cout << f_with_delta_ << std::endl;
std::cout << "Jf" << std::endl;
std::cout << invkin_->get_Jf().block(0, 6, 12, 12).transpose() << std::endl;*/
tau_ff_ = data_.tau.tail(12) - invkin_->get_Jf().transpose() * f_with_delta_;
tau_ff_ = data_.tau.tail(12) - invkin_->get_Jf().block(0, 6, 12, 12).transpose() * f_with_delta_;
// Retrieve desired positions and velocities
vdes_ = invkin_->get_dq_cmd();
qdes_ = invkin_->get_q_cmd();
vdes_ = invkin_->get_dq_cmd().tail(12);
// std::cout << "GET:" << invkin_->get_q_cmd() << std::endl;
qdes_ = invkin_->get_q_cmd().tail(12);
/*std::cout << vdes_.transpose() << std::endl;
std::cout << qdes_.transpose() << std::endl;*/
/*std::cout << "----" << std::endl;
std::cout << qdes_.transpose() << std::endl;
std::cout << vdes_.transpose() << std::endl;
std::cout << tau_ff_.transpose() << std::endl;*/
// Increment log counter
k_log_++;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment