diff --git a/demos/mcapi_povcdl_estimation.cpp b/demos/mcapi_povcdl_estimation.cpp
index 799eac9c059c46ff94ed86d17a202d92735c31ae..4e1d6a8eed0e19040a4323c8510f42fc6d8a9335 100644
--- a/demos/mcapi_povcdl_estimation.cpp
+++ b/demos/mcapi_povcdl_estimation.cpp
@@ -11,6 +11,7 @@
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
+#include "pinocchio/algorithm/crba.hpp"
// MCAPI
#include <curves/fwd.h>
@@ -138,8 +139,8 @@ int main (int argc, char **argv) {
///////////////////////////////////////////////
// Create some vectors to aggregate the groundtruth and measurements
// Groundtruth
- std::vector<Vector3d> p_wb_gtruth_v;
- std::vector<Vector4d> q_wb_gtruth_v;
+ std::vector<Vector3d> p_ob_gtruth_v;
+ std::vector<Vector4d> q_ob_gtruth_v;
// std::vector<Vector3d> v_wb_gtruth_v;
// std::vector<VectorXd>& c_gtruth_v = c_traj_arr; // already have it directly
// std::vector<VectorXd>& cd_gtruth_v = dc_traj_arr; // already have it directly
@@ -195,27 +196,25 @@ int main (int argc, char **argv) {
// Set ground truth variables
Vector3d p_wb = q.head<3>(); // gtruth
Vector4d quat_wb = q.segment<4>(3); // gtruth
- // vb ??
- // vc ??
- // Lc ??
-
- ////////////////////////////////////
- ////////////////////////////////////
-
-
// global frame pose
SE3 oMb(Quaterniond(quat_wb).toRotationMatrix(), p_wb);
+ // body vel is expressed in body frame
+ // Vector3d w_v_wb = oMb.rotation() * dq.head<3>(); // NOT USED
+ // com velocity is already expressed in world frame
+ // Vector3d w_v_wc = dc; // TO STORE
+ // Lc ??
////////////////////////////////////
////////////////////////////////////
// Get measurements
// IMU readings
- // Vector3d b_v_b = dq.head<3>();
+ Vector3d b_vsp_b = dq.head<3>(); // vsp: spatial linear velocity
Vector3d b_w_b = dq.segment<3>(3); // meas
- Vector3d w_acc = ddq.head<3>(); // TODO, almost same as classical acceleration
- // Vector3d w_acc = acc_from_spatial(ddq.head<3>(), b_w_b, b_v_b); // NOT THAT -> dr?
- Vector3d b_proper_acc = oMb.inverse().rotation() * (w_acc - gravity()); // meas
+ // We use every quantity (spatial acc, body angvel, dr=lin spatial linvel) in the same body frame -> we get acc in body frame
+ Vector3d b_acc = computeAccFromSpatial(ddq.head<3>(), b_w_b, b_vsp_b);
+ // universe o frame in mcapi/pinocchio is supposed to have same reference point and orientation as WOLF world frame
+ Vector3d b_proper_acc = b_acc - oMb.inverse().rotation() * gravity(); // meas
// update and retrieve kinematics
forwardKinematics(model,data,q);
@@ -234,23 +233,33 @@ int main (int argc, char **argv) {
//////////////////////////////////////////////
// COM relative position and velocity measures
- Vector3d b_p_bc = oMb.inverse().act(c); // meas
+ Vector3d b_p_bc = oMb.inverse().act(c); // meas
VectorXd q_static = q;
VectorXd dq_static = dq;
q_static.head<7>() << 0,0,0, 0,0,0,1;
dq_static.head<6>() << 0,0,0, 0,0,0;
centerOfMass(model, data, q_static, dq_static);
- Vector3d b_v_bc = data.vcom[0]; // meas (???)
+ // velocity due to to gesticulation only in base frame
+ // -> set world frame = base frame and set body frame spatial vel to zero
+ Vector3d b_v_bc = data.vcom[0]; // meas
- // Iq, Lq
- computeCentroidalMap(model,data,q);
+ /////////////////////////
+ // bIomg, Lcm
+ MatrixXd b_Mc = crba(model, data, q); // 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
+ SE3 bTc (oMb.rotation().transpose(), b_p_bc);
+ // Ic = cXb^star * Ib * bXc = bXc^T * Ib * bXc
+ MatrixXd c_I_c = bTc.toActionMatrix().transpose() * b_I_b * bTc.toActionMatrix();
+ Matrix3d b_Iw = bTc.rotation() * c_I_c.bottomRightCorner<3,3>() * bTc.rotation().transpose(); // meas
+ // gesticulation in base frame: just compute centroidal momentum with static q and vq
+ Vector3d b_Lc_gesti = computeCentroidalMomentum(model, data, q_static, dq_static).angular();
/////////////////////////
// Agreggate everything in vectors for easier WOLF consumption
- p_wb_gtruth_v.push_back(p_wb);
- q_wb_gtruth_v.push_back(quat_wb);
+ p_ob_gtruth_v.push_back(p_wb);
+ q_ob_gtruth_v.push_back(quat_wb);
// v_wb_gtruth_v;
// Lc_gtruth_v;
@@ -265,8 +274,8 @@ int main (int argc, char **argv) {
v_bc_meas_v.push_back(b_v_bc);
wrench_ll_meas_v.push_back(l_wrench_ll);
wrench_rl_meas_v.push_back(l_wrench_rl);
- Lq_meas_v.push_back(ZERO3); // TODO
- Iq_meas_v.push_back(Eigen::Matrix3d::Identity()); // TODO
+ Lq_meas_v.push_back(b_Lc_gesti);
+ Iq_meas_v.push_back(b_Iw);
}
/////////////////////////
@@ -315,7 +324,7 @@ int main (int argc, char **argv) {
ProcessorParamsForceTorquePreintPtr params_sen_ft = std::make_shared<ProcessorParamsForceTorquePreint>();
params_sen_ft->sensor_ikin_name = "SenIK";
params_sen_ft->sensor_angvel_name = "SenImu";
- params_sen_ft->time_tolerance = 0.25;
+ params_sen_ft->time_tolerance = 0.025;
params_sen_ft->unmeasured_perturbation_std = 1e-4;
params_sen_ft->max_time_span = 1000;
params_sen_ft->max_buff_length = 500;
@@ -334,7 +343,7 @@ int main (int argc, char **argv) {
// - call setOrigin on processors isMotion since the flag prior_is_set is not true when doing installProcessor (later)
TimeStamp t0(t_arr[0]);
VectorXd x_origin; x_origin.resize(19);
- x_origin << p_wb_gtruth_v[0], q_wb_gtruth_v[0], ZERO3, ZERO3, ZERO3, ZERO3; // TODO: origin vel shoulde not be zero but v_wb_gtruth_v[0]
+ x_origin << p_ob_gtruth_v[0], q_ob_gtruth_v[0], ZERO3, ZERO3, ZERO3, ZERO3; // TODO: origin vel shoulde not be zero but v_wb_gtruth_v[0]
MatrixXd P_origin = pow(1e-3, 2) * Eigen::Matrix9d::Identity();
FrameBasePtr KF1 = problem->emplaceFrame(KEY, x_origin, t0);
// Prior pose factor
@@ -363,12 +372,10 @@ int main (int argc, char **argv) {
proc_ftpreint->setOrigin(KF1);
problem->setPriorIsSet(true); // We handle the whole initialization manually here
- std::cout << "BEFORE WOLF CAPTURES CREATION" << std::endl;
-
///////////////////////////////////////////
// process measurements in sequential order
int traj_size = problem->getTrajectory()->getFrameList().size();
- SE3 oMb_prev(Quaterniond(q_wb_gtruth_v[0]).toRotationMatrix(), p_wb_gtruth_v[0]);
+ SE3 oMb_prev(Quaterniond(q_ob_gtruth_v[0]).toRotationMatrix(), p_ob_gtruth_v[0]);
FrameBasePtr KF_prev = KF1;
for (int i=1; i < t_arr.size(); i++){
TimeStamp ts(t_arr[i]);
@@ -410,7 +417,7 @@ int main (int argc, char **argv) {
// Manually create an Odom3d factor when a new KF is detected (will be put in a processorLegOdom3d)
if (problem->getTrajectory()->getFrameList().size() > traj_size){
FrameBasePtr KF_now = problem->getTrajectory()->getLastKeyFrame();
- SE3 oMb_now(Quaterniond(q_wb_gtruth_v[i]).toRotationMatrix(), p_wb_gtruth_v[i]);
+ SE3 oMb_now(Quaterniond(q_ob_gtruth_v[i]).toRotationMatrix(), p_ob_gtruth_v[i]);
SE3 bprev_M_bnow = oMb_prev.inverse() * oMb_now;
Vector7d prev_pose_now; prev_pose_now << bprev_M_bnow.translation(), Quaterniond(bprev_M_bnow.rotation()).coeffs();
Eigen::Matrix6d rel_pose_cov = 1e-6 * Eigen::Matrix6d::Identity();
diff --git a/demos/mcapi_utils.cpp b/demos/mcapi_utils.cpp
index 5368b1c8ea879b1936bde516e4d79749f3290890..b89e9fb62a527144eb838542e68df2b28288c7c6 100644
--- a/demos/mcapi_utils.cpp
+++ b/demos/mcapi_utils.cpp
@@ -2,7 +2,7 @@
#include "mcapi_utils.h"
-Vector3d acc_from_spatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr)
+Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr)
{
return ddr + w.cross(dr);
}
diff --git a/demos/mcapi_utils.h b/demos/mcapi_utils.h
index dc4dc1c95c046eb0aabccb5593e26de894ca693e..a5f88635beece170d8665a0191ccf5633f160595 100644
--- a/demos/mcapi_utils.h
+++ b/demos/mcapi_utils.h
@@ -10,7 +10,7 @@ using namespace Eigen;
* \param w spatial acc rotational part
* \param dr spatial velocity linear part
**/
-Vector3d acc_from_spatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr);
+Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr);
/**
diff --git a/include/bodydynamics/factor/factor_inertial_kinematics.h b/include/bodydynamics/factor/factor_inertial_kinematics.h
index 54b02618a5ed30a80ad90d46b67c8b98fe9cb024..1253b69b07ce636733aa43441ac8ae580e1aca2a 100644
--- a/include/bodydynamics/factor/factor_inertial_kinematics.h
+++ b/include/bodydynamics/factor/factor_inertial_kinematics.h
@@ -33,12 +33,13 @@ class FactorInertialKinematics : public FactorAutodiff<FactorInertialKinematics,
}
/*
+ Factor state blocks:
_pb: W_p_WB -> base position in world frame
_qb: W_q_B -> base orientation in world frame
_vb: W_v_WB -> base velocity in world frame
_c: W_p_WC -> COM position in world frame
_cd: W_v_WC -> COM velocity in world frame
- _Lc: W_Lc -> angular momentum at the COM in world frame
+ _Lc: W_Lc -> angular momentum at the COM in body frame
_bp: B_b_BC -> bias on relative body->COM position kinematic measurement
_baw: 6d Imu bias containing [acc bias, gyro bias]
*/
@@ -124,8 +125,8 @@ bool FactorInertialKinematics::operator () (
err_c = pBC_m - (qb.inverse()*(c - pb) + bp);
err_cd = vBC_m + (w_m - baw.tail(3)).cross(pBC_m - bp) - qb.inverse()*(cd - vb);
- // err_Lc = feat->getBIq() * (w_m - baw.tail(3)) + feat->getBLcm() - qb.inverse()*Lc;
- err_Lc = feat->getBLcm() - feat->getBIq()*baw.tail(3) - qb.inverse()*Lc; // w_m is actually used to compute BLcm beforehand
+ err_Lc = feat->getBIq() * (w_m - baw.tail(3)) + feat->getBLcm() - qb.inverse()*Lc;
+ // err_Lc = feat->getBLcm() - feat->getBIq()*baw.tail(3) - qb.inverse()*Lc; // w_m is actually used to compute BLcm beforehand
res = getMeasurementSquareRootInformationUpper() * err;
diff --git a/test/gtest_processor_force_torque_preint.cpp b/test/gtest_processor_force_torque_preint.cpp
index 4fb7de25edea27a9bf25e237d8ad3a34fefc63d5..5b7ce5ed0b124a0e648b4c8e7e88b0b44a669584 100644
--- a/test/gtest_processor_force_torque_preint.cpp
+++ b/test/gtest_processor_force_torque_preint.cpp
@@ -311,6 +311,7 @@ public:
Eigen::Matrix3d Q_bp = 1e-1 * Eigen::Matrix3d::Identity();
CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1_, "bp0", t0_);
FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_, Q_bp);
+ // FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_ + (Vector3d()<<0.1,0.1,0.1).finished(), Q_bp);
FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic());
}