From 87404a17246d59f61484ec1a8228dffd6fcfd49a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Sat, 11 Apr 2020 01:53:02 +0200
Subject: [PATCH] Solved measurement simulation bugs
---
demos/mcapi_povcdl_estimation.cpp | 69 +++++++++++++++++++++----------
demos/mcapi_utils.cpp | 9 ++--
2 files changed, 51 insertions(+), 27 deletions(-)
diff --git a/demos/mcapi_povcdl_estimation.cpp b/demos/mcapi_povcdl_estimation.cpp
index 4e1d6a8..a02dba2 100644
--- a/demos/mcapi_povcdl_estimation.cpp
+++ b/demos/mcapi_povcdl_estimation.cpp
@@ -91,7 +91,8 @@ int main (int argc, char **argv) {
std::cout << "q_traj.dim(): " << q_traj.dim() << std::endl;
double dt = 1e-2; // chosen data sampling
double min_t = q_traj.min();
- double mac_t = q_traj.max();
+ // double max_t = q_traj.max();
+ double max_t = 0.25; // only the beginning
// initialize some vectors and fill them with dicretized data
std::vector<double> t_arr;
@@ -104,7 +105,7 @@ int main (int argc, char **argv) {
std::vector<VectorXd> L_traj_arr;
std::vector<VectorXd> ll_contactf_traj_arr;
std::vector<VectorXd> rl_contactf_traj_arr;
- for (double t=min_t; t <= mac_t; t += dt){
+ for (double t=min_t; t <= max_t; t += dt){
t_arr.push_back(t);
q_traj_arr.push_back(q_traj(t));
dq_traj_arr.push_back(dq_traj(t));
@@ -131,8 +132,10 @@ int main (int argc, char **argv) {
// std::cout << "q: " << q.transpose() << std::endl;
// // Perform the forward kinematics over the kinematic tree
// forwardKinematics(model,data,q);
- int ll_sole_idx = model.getFrameId("leg_left_sole_fix_joint");
- int rl_sole_idx = model.getFrameId("leg_right_sole_fix_joint");
+ int left_leg_sole_idx = model.getFrameId("leg_left_sole_fix_joint");
+ int right_leg_sole_idx = model.getFrameId("leg_right_sole_fix_joint");
+ std::cout << "left_leg_sole_idx: " << left_leg_sole_idx << std::endl;
+ std::cout << "right_leg_sole_idx: " << right_leg_sole_idx << std::endl;
std::vector<Vector3d> contacts = contacts_from_footrect_center();
@@ -141,7 +144,7 @@ int main (int argc, char **argv) {
// Groundtruth
std::vector<Vector3d> p_ob_gtruth_v;
std::vector<Vector4d> q_ob_gtruth_v;
- // std::vector<Vector3d> v_wb_gtruth_v;
+ std::vector<Vector3d> v_ob_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
// std::vector<Vector3d> Lc_gtruth_v;
@@ -166,13 +169,13 @@ int main (int argc, char **argv) {
* Groundtruth to recover (in world frame, vels/inertial frame):
* b position --> OK
* b orientation (quat) --> OK
- * b velocity: linear classical one -> careful
+ * b velocity: linear classical one --> OK
* CoM posi --> OK
* CoM vel
* Angular momentum
*
* Measures to recover (in local frames):
- * IMU readings --> ALMOST OK
+ * IMU readings --> OK
* Kinematics readings --> OK
* Wrench readings --> OK
* CoM relative position/vel --> OK
@@ -185,7 +188,7 @@ int main (int argc, char **argv) {
VectorXd dq = dq_traj_arr[i];
VectorXd ddq = ddq_traj_arr[i];
Vector3d c = c_traj_arr[i]; // gtruth
- VectorXd dc = dc_traj_arr[i]; // gtruth (??)
+ VectorXd dc = dc_traj_arr[i]; // gtruth
VectorXd cw = cw_traj_arr[i];
VectorXd L = L_traj_arr[i];
VectorXd cf_ll = ll_contactf_traj_arr[i];
@@ -199,7 +202,7 @@ int main (int argc, char **argv) {
// 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
+ 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 ??
@@ -218,8 +221,14 @@ int main (int argc, char **argv) {
// update and retrieve kinematics
forwardKinematics(model,data,q);
- auto oMll = data.oMf[ll_sole_idx];
- auto oMrl = data.oMf[rl_sole_idx];
+ // auto oMll = data.oMf[left_leg_sole_idx];
+ // auto oMrl = data.oMf[right_leg_sole_idx];
+ auto frame_ll_sole_idx = model.frames[left_leg_sole_idx];
+ auto oMll = data.oMi[frame_ll_sole_idx.parent].act(frame_ll_sole_idx.placement);
+ auto frame_rl_sole_idx = model.frames[right_leg_sole_idx];
+ auto oMrl = data.oMi[frame_rl_sole_idx.parent].act(frame_rl_sole_idx.placement);
+ std::cout << "oMll.translation(): " << oMll.translation().transpose() << std::endl;
+ std::cout << "oMrl.translation(): " << oMrl.translation().transpose() << std::endl;
auto bMll = oMb.inverse() * oMll;
auto bMrl = oMb.inverse() * oMrl;
Vector3d b_p_ll = bMll.translation(); // meas
@@ -260,7 +269,7 @@ int main (int argc, char **argv) {
p_ob_gtruth_v.push_back(p_wb);
q_ob_gtruth_v.push_back(quat_wb);
- // v_wb_gtruth_v;
+ v_ob_gtruth_v.push_back(w_v_wb);
// Lc_gtruth_v;
// Measurements
@@ -343,7 +352,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_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]
+ x_origin << p_ob_gtruth_v[0], q_ob_gtruth_v[0], v_ob_gtruth_v[0], 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
@@ -389,7 +398,7 @@ int main (int argc, char **argv) {
// Force Torque
Vector6d f_meas; f_meas << wrench_ll_meas_v[i].head<3>(), wrench_rl_meas_v[i].head<3>();
- Vector6d tau_meas; tau_meas << wrench_ll_meas_v[i].head<3>(), wrench_rl_meas_v[i].head<3>();
+ Vector6d tau_meas; tau_meas << wrench_ll_meas_v[i].tail<3>(), wrench_rl_meas_v[i].tail<3>();
VectorXd kin_meas(14);
kin_meas << p_bll_meas_v[i], p_brl_meas_v[i], q_bll_meas_v[i], q_brl_meas_v[i];
@@ -405,13 +414,24 @@ int main (int argc, char **argv) {
Qftp.block<6, 6>(6, 6) = cov_tau;
Qftp.block<3, 3>(12, 12) = cov_pbc;
-
- CaptureImuPtr CImu2 = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
- CImu2->process();
- auto CIKin2 = std::make_shared<CaptureInertialKinematics>(ts, sen_ikin, meas_ikin, Iq_meas_v[i], Lq_meas_v[i]);
- CIKin2->process();
- auto CFTpreint2 = std::make_shared<CaptureForceTorquePreint>(ts, sen_ft, CIKin2, CImu2, cap_ftp_data, Qftp);
- CFTpreint2->process();
+ std::cout << "\nw_b_meas_v[i]: " << w_b_meas_v[i].transpose() << std::endl;
+ std::cout << "Lq_meas_v[i]: " << Lq_meas_v[i].transpose() << std::endl;
+ std::cout << "Iq_meas_v[i]\n" << Iq_meas_v[i] << std::endl;
+ std::cout << "wrench_ll_meas_v[i]: " << wrench_ll_meas_v[i].transpose() << std::endl;
+ std::cout << "wrench_rl_meas_v[i]: " << wrench_rl_meas_v[i].transpose() << std::endl;
+ std::cout << "p_bc_meas_v[i]: " << p_bc_meas_v[i].transpose() << std::endl;
+ std::cout << "p_bll_meas_v[i]: " << p_bll_meas_v[i].transpose() << std::endl;
+ std::cout << "p_brl_meas_v[i]: " << p_brl_meas_v[i].transpose() << std::endl;
+ std::cout << "q_bll_meas_v[i]: " << q_bll_meas_v[i].transpose() << std::endl;
+ std::cout << "q_brl_meas_v[i]: " << q_brl_meas_v[i].transpose() << std::endl;
+
+
+ CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
+ CImu->process();
+ auto CIKin = std::make_shared<CaptureInertialKinematics>(ts, sen_ikin, meas_ikin, Iq_meas_v[i], Lq_meas_v[i]);
+ CIKin->process();
+ auto CFTpreint = std::make_shared<CaptureForceTorquePreint>(ts, sen_ft, CIKin, CImu, cap_ftp_data, Qftp);
+ CFTpreint->process();
/////////////////////////////////////////////
// Manually create an Odom3d factor when a new KF is detected (will be put in a processorLegOdom3d)
@@ -455,9 +475,14 @@ int main (int argc, char **argv) {
///////////////////////////////////////////
/////////// SOLVING AND PRINTING //////////
///////////////////////////////////////////
- problem->print(4,true,true,true);
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
+ problem->print(4,true,true,true);
std::cout << report << std::endl;
+ std::cout << "Ground truth end: " << std::endl;
+ std::cout << p_ob_gtruth_v[p_ob_gtruth_v.size()-1].transpose() << std::endl;
+ std::cout << q_ob_gtruth_v[q_ob_gtruth_v.size()-1].transpose() << std::endl;
+ std::cout << v_ob_gtruth_v[v_ob_gtruth_v.size()-1].transpose() << std::endl;
+
}
\ No newline at end of file
diff --git a/demos/mcapi_utils.cpp b/demos/mcapi_utils.cpp
index b89e9fb..bdb0dce 100644
--- a/demos/mcapi_utils.cpp
+++ b/demos/mcapi_utils.cpp
@@ -28,11 +28,10 @@ Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contac
Vector3d wf = w_cforces.segment<3>(0) + w_cforces.segment<3>(3) + w_cforces.segment<3>(6) + w_cforces.segment<3>(9);
Vector3d lf = wRl.inverse() * wf;
// torque at point l (center of the foot) in world frame
- Vector3d w_tau_l = contacts[0].cross(w_cforces.segment<3>(0))
- + contacts[1].cross(w_cforces.segment<3>(3))
- + contacts[2].cross(w_cforces.segment<3>(6))
- + contacts[3].cross(w_cforces.segment<3>(9));
- Vector3d l_tau_l = wRl.inverse() * w_tau_l;
+ Vector3d l_tau_l = contacts[0].cross(wRl.inverse() * w_cforces.segment<3>(0))
+ + contacts[1].cross(wRl.inverse() * w_cforces.segment<3>(3))
+ + contacts[2].cross(wRl.inverse() * w_cforces.segment<3>(6))
+ + contacts[3].cross(wRl.inverse() * w_cforces.segment<3>(9));
Matrix<double, 6, 1> l_wrench; l_wrench << lf, l_tau_l;
return l_wrench;
}
--
GitLab