diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
index 4b0ffd1421c71d6ae56ad8704489d6c3231ec391..78d2b265912a542724c6ccaa3609638dc49259e2 100644
--- a/demos/CMakeLists.txt
+++ b/demos/CMakeLists.txt
@@ -8,6 +8,10 @@ if (pinocchio_FOUND)
include_directories(
SYSTEM ${PINOCCHIO_INCLUDE_DIRS}
)
+
+ add_library(mcapi_utils mcapi_utils.cpp)
+ target_link_libraries(mcapi_utils Eigen3::Eigen)
+
add_executable(solo_real_povcdl_estimation solo_real_povcdl_estimation.cpp)
target_link_libraries(solo_real_povcdl_estimation
wolfcore
@@ -16,6 +20,7 @@ if (pinocchio_FOUND)
${pinocchio_LIBRARIES}
/usr/local/lib/libcnpy.so
z
+ mcapi_utils
)
add_executable(solo_imu_mocap solo_imu_mocap.cpp)
diff --git a/demos/mcapi_utils.cpp b/demos/mcapi_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d893b4669ce2514f3f15b3bf73500f243923e19c
--- /dev/null
+++ b/demos/mcapi_utils.cpp
@@ -0,0 +1,77 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+
+#include "mcapi_utils.h"
+
+Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr)
+{
+ return ddr + w.cross(dr);
+}
+
+
+std::vector<Vector3d> contacts_from_footrect_center()
+{
+ // compute feet corners coordinates like from the leg_X_6_joint
+ double lx = 0.1;
+ double ly = 0.065;
+ double lz = 0.107;
+ std::vector<Vector3d> contacts; contacts.resize(4);
+ contacts[0] = {-lx, -ly, -lz};
+ contacts[1] = {-lx, ly, -lz};
+ contacts[2] = { lx, -ly, -lz};
+ contacts[3] = { lx, ly, -lz};
+ return contacts;
+}
+
+
+Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts,
+ const Matrix<double, 12, 1>& cf12)
+{
+ Vector3d f = cf12.segment<3>(0) + cf12.segment<3>(3) + cf12.segment<3>(6) + cf12.segment<3>(9);
+ Vector3d tau = contacts[0].cross(cf12.segment<3>(0))
+ + contacts[1].cross(cf12.segment<3>(3))
+ + contacts[2].cross(cf12.segment<3>(6))
+ + contacts[3].cross(cf12.segment<3>(9));
+ Matrix<double, 6, 1> wrench; wrench << f, tau;
+ return wrench;
+}
+
+
+Matrix3d compute_Iw(pinocchio::Model& model,
+ pinocchio::Data& data,
+ VectorXd& q_static,
+ Vector3d& b_p_bc)
+{
+ MatrixXd b_Mc = pinocchio::crba(model, data, q_static); // mass matrix at b frame expressed in b frame
+ // MatrixXd b_Mc = crba(model_dist, data_dist, q_static); // mass matrix at b frame expressed in b frame
+ MatrixXd b_I_b = b_Mc.topLeftCorner<6,6>(); // inertia matrix at b frame expressed in b frame
+ pinocchio::SE3 bTc (Eigen::Matrix3d::Identity(), b_p_bc); // "no free flyer robot -> c frame oriented the same as b"
+ pinocchio::SE3 cTb = bTc.inverse();
+ // Ic = cXb^star * Ib * bXc = bXc^T * Ib * bXc
+ // c_I_c block diagonal:
+ // [m*Id3, 03;
+ // 0, Iw]
+ MatrixXd c_I_c = cTb.toDualActionMatrix() * b_I_b * bTc.toActionMatrix();
+ Matrix3d b_Iw = c_I_c.bottomRightCorner<3,3>(); // meas
+ return b_Iw;
+}
diff --git a/demos/mcapi_utils.h b/demos/mcapi_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..c62c4026a1dac41733350f2b96fb554285f30715
--- /dev/null
+++ b/demos/mcapi_utils.h
@@ -0,0 +1,76 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include <vector>
+#include "Eigen/Dense"
+#include "pinocchio/algorithm/crba.hpp"
+
+
+using namespace Eigen;
+
+
+/**
+ * \brief Compute a 3D acceleration from 6D spatial acceleration
+ *
+ * \param ddr spatial acc linear part
+ * \param w spatial acc rotational part
+ * \param dr spatial velocity linear part
+**/
+Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr);
+
+
+/**
+* \brief Compute the relative contact points from foot center of a Talos foot.
+*
+* Order is clockwise, starting from bottom left (looking at a forward pointing foot from above).
+* Expressed in local foot coordinates.
+**/
+std::vector<Vector3d> contacts_from_footrect_center();
+
+
+/**
+* \brief Compute the wrench at the end effector center expressed in world coordinates.
+
+* Each contact force (at a foot for instance) is represented in MAPI as a set of
+* 4 forces at the corners of the contact polygone (foot -> rectangle). These forces,
+* as the other quantities, are expressed in world coordinates. From this 4 3D forces,
+* we can compute the 6D wrench at the center of the origin point of the contacts vectors.
+
+* \param contacts std vector of 3D contact forces
+* \param cforces 12D corner forces vector ordered as [fx1, fy1, fz1, fx2, fy2... fz4], same order as contacts
+* \param wRl rotation matrix, orientation from world frame to foot frame
+**/
+Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts,
+ const Matrix<double, 12, 1>& cf12);
+
+
+/**
+* \brief Compute the centroidal angular inertia used to compute the angular momentum.
+
+* \param model: pinocchio robot model used
+* \param data: pinocchio robot data used
+* \param q_static: configuration of the robot with free flyer pose at origin (local base frame = world frame)
+* \param b_p_bc: measure of the CoM position relative to the base
+**/
+Matrix3d compute_Iw(pinocchio::Model& model,
+ pinocchio::Data& data,
+ VectorXd& q_static,
+ Vector3d& b_p_bc);
diff --git a/demos/solo_imu_kine_mocap.cpp b/demos/solo_imu_kine_mocap.cpp
index 68c837894f73325895b977784ebba7e5a669801b..82d2df1c42da66a66e6894fe49e037b4a04259cd 100644
--- a/demos/solo_imu_kine_mocap.cpp
+++ b/demos/solo_imu_kine_mocap.cpp
@@ -94,35 +94,35 @@ int main (int argc, char **argv) {
// retrieve parameters from yaml file
YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
double dt = config["dt"].as<double>();
- double min_t = config["min_t"].as<double>();
+ // double min_t = config["min_t"].as<double>();
double max_t = config["max_t"].as<double>();
- double solve_every_sec = config["solve_every_sec"].as<double>();
+ // double solve_every_sec = config["solve_every_sec"].as<double>();
bool solve_end = config["solve_end"].as<bool>();
// robot
std::string robot_urdf = config["robot_urdf"].as<std::string>();
- int dimc = config["dimc"].as<int>();
- int nb_feet = config["nb_feet"].as<int>();
+ // int dimc = config["dimc"].as<int>();
+ // int nb_feet = config["nb_feet"].as<int>();
// Ceres setup
int max_num_iterations = config["max_num_iterations"].as<int>();
bool compute_cov = config["compute_cov"].as<bool>();
// estimator sensor noises
- double std_pbc_est = config["std_pbc_est"].as<double>();
- double std_vbc_est = config["std_vbc_est"].as<double>();
- double std_f_est = config["std_f_est"].as<double>();
- double std_tau_est = config["std_tau_est"].as<double>();
+ // double std_pbc_est = config["std_pbc_est"].as<double>();
+ // double std_vbc_est = config["std_vbc_est"].as<double>();
+ // double std_f_est = config["std_f_est"].as<double>();
+ // double std_tau_est = config["std_tau_est"].as<double>();
double std_foot_nomove = config["std_foot_nomove"].as<double>();
// priors
double std_prior_p = config["std_prior_p"].as<double>();
double std_prior_o = config["std_prior_o"].as<double>();
double std_prior_v = config["std_prior_v"].as<double>();
- double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
+ // double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
- double std_bp_drift = config["std_bp_drift"].as<double>();
+ // double std_bp_drift = config["std_bp_drift"].as<double>();
std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
std::vector<double> i_p_im_vec = config["i_p_im"].as<std::vector<double>>();
@@ -137,7 +137,7 @@ int main (int argc, char **argv) {
// contacts
std::vector<double> l_p_lg_vec = config["l_p_lg"].as<std::vector<double>>();
Eigen::Map<Vector3d> l_p_lg(l_p_lg_vec.data());
- double fz_thresh = config["fz_thresh"].as<double>();
+ // double fz_thresh = config["fz_thresh"].as<double>();
std::vector<double> delta_qa_vec = config["delta_qa"].as<std::vector<double>>();
Eigen::Map<Eigen::Matrix<double,12,1>> delta_qa(delta_qa_vec.data());
@@ -159,7 +159,7 @@ int main (int argc, char **argv) {
i_q_m.normalize();
assert(i_q_m.normalized().isApprox(i_q_m));
SE3 i_T_m(i_q_m, i_p_im);
- Matrix3d i_R_m = i_T_m.rotation();
+ // Matrix3d i_R_m = i_T_m.rotation();
// IMU to MOCAP transform
SE3 m_T_i = i_T_m.inverse();
@@ -171,7 +171,7 @@ int main (int argc, char **argv) {
m_q_b.normalize();
assert(m_q_b.normalized().isApprox(m_q_b));
SE3 m_T_b(m_q_b, m_p_mb);
- Matrix3d m_R_b = m_T_b.rotation();
+ // Matrix3d m_R_b = m_T_b.rotation();
// IMU to BASE transform (composition)
SE3 i_T_b = i_T_m*m_T_b;
@@ -320,8 +320,8 @@ int main (int argc, char **argv) {
Quaterniond w_q_i_init = w_q_m_init*m_q_i;
VectorComposite x_origin("POV", {w_p_wi_init, w_q_i_init.coeffs(), Vector3d::Zero()});
VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
- // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005); // needed to anchor the problem
- FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0, 0.0005); // if mocap used
+ // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0); // needed to anchor the problem
+ FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0); // if mocap used
proc_imu->setOrigin(KF1);
@@ -367,7 +367,7 @@ int main (int argc, char **argv) {
VectorComposite curr_state = problem->getState();
Vector4d o_qvec_i_curr = curr_state['O'];
Quaterniond o_q_i_curr(o_qvec_i_curr);
- Vector3d o_v_oi_curr = curr_state['V'];
+ // Vector3d o_v_oi_curr = curr_state['V'];
//////////////
@@ -421,7 +421,6 @@ int main (int argc, char **argv) {
// Or else, retrieve from preprocessed dataset
Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i);
- Vector3d o_f_tot = Vector3d::Zero();
std::vector<Vector3d> l_f_l_vec;
std::vector<Vector3d> o_f_l_vec;
std::vector<Vector3d> i_p_il_vec;
@@ -507,7 +506,6 @@ int main (int argc, char **argv) {
Vector3d Qv_fbk_diag = Qv_fbk.diagonal();
Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
Vector6d Qm_fbk_diag; Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal();
- Vector3d Qmo_fbk_diag = Qmo_fbk.diagonal();
Qp_fbk_v.push_back(Qp_fbk_diag);
Qo_fbk_v.push_back(Qo_fbk_diag);
@@ -659,8 +657,6 @@ int main (int argc, char **argv) {
// std::cout << "Factors " << kf->getTimeStamp().get() << std::endl;
FactorBasePtrList fac_lst;
kf->getFactorList(fac_lst);
- Vector9d imu_err = Vector9d::Zero();
- Vector6d pose_err = Vector6d::Zero();
i++;
}
diff --git a/demos/solo_imu_mocap.cpp b/demos/solo_imu_mocap.cpp
index f7ad2d04ed1bfe843a903b768a81d8683090c254..1ffabd4c847bb9f3f0954f5eb70862cc20eedf00 100644
--- a/demos/solo_imu_mocap.cpp
+++ b/demos/solo_imu_mocap.cpp
@@ -134,7 +134,7 @@ int main (int argc, char **argv) {
m_q_b.normalize();
assert(m_q_b.normalized().isApprox(m_q_b));
SE3 m_T_b(m_q_b, m_p_mb);
- Matrix3d m_R_b = m_T_b.rotation();
+ // Matrix3d m_R_b = m_T_b.rotation();
// IMU to BASE transform (composition)
SE3 i_T_b = i_T_m*m_T_b;
@@ -273,7 +273,7 @@ int main (int argc, char **argv) {
Quaterniond w_q_i_init = w_q_m_init*m_q_i;
VectorComposite x_origin("POV", {w_p_wi_init, w_q_i_init.coeffs(), Vector3d::Zero()});
VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
- // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005);
+ // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0);
FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0); // if mocap used
proc_imu->setOrigin(KF1);
@@ -397,7 +397,6 @@ int main (int argc, char **argv) {
Vector3d Qv_fbk_diag = Qv_fbk.diagonal();
Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
Vector6d Qm_fbk_diag; Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal();
- Vector3d Qmo_fbk_diag = Qmo_fbk.diagonal();
Qp_fbk_v.push_back(Qp_fbk_diag);
Qo_fbk_v.push_back(Qo_fbk_diag);
diff --git a/demos/solo_real_povcdl_estimation.cpp b/demos/solo_real_povcdl_estimation.cpp
index 44819e61223a3461f03cfaab16957676991557b9..9953db41444d0d1b1da732f5460c3bd398cd3a63 100644
--- a/demos/solo_real_povcdl_estimation.cpp
+++ b/demos/solo_real_povcdl_estimation.cpp
@@ -69,6 +69,8 @@
#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
#include "bodydynamics/processor/processor_point_feet_nomove.h"
+#include "mcapi_utils.h"
+
using namespace Eigen;
using namespace wolf;
@@ -85,7 +87,7 @@ int main (int argc, char **argv) {
int dimc = config["dimc"].as<int>();
double mass_offset = config["mass_offset"].as<double>();
double dt = config["dt"].as<double>();
- double min_t = config["min_t"].as<double>();
+ // double min_t = config["min_t"].as<double>();
double max_t = config["max_t"].as<double>();
double solve_every_sec = config["solve_every_sec"].as<double>();
bool solve_end = config["solve_end"].as<bool>();
@@ -574,8 +576,9 @@ int main (int argc, char **argv) {
}
}
- CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
- CPF->process();
+ // TODO: ???? kin_incontact is a map to Vector3d in this implementation, should be Vector7d [p, q]
+ // CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
+ // CPF->process();