diff --git a/demos/mcapi_povcdl_estimation.cpp b/demos/mcapi_povcdl_estimation.cpp
index 478c81e853b8160f58e367e525c5647d1553c9ce..a52104160b86739fd85795553c9af4b619146955 100644
--- a/demos/mcapi_povcdl_estimation.cpp
+++ b/demos/mcapi_povcdl_estimation.cpp
@@ -56,36 +56,48 @@ using namespace pinocchio;
using namespace multicontact_api::scenario;
typedef pinocchio::SE3Tpl<double> SE3;
+typedef pinocchio::ForceTpl<double> Force;
const Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
const Eigen::Vector6d ZERO6 = Eigen::Vector6d::Zero();
const Eigen::Vector3d ONES3 = Eigen::Vector3d::Ones();
-const Eigen::Vector3d BIAS_PBC_SMAL = {0.01, 0.02, 0.03};
-
int main (int argc, char **argv) {
- ContactSequence cs;
- cs.loadFromBinary("/home/mfourmy/Documents/Phd_LAAS/tests/centroidalkin/multicontact-api-master-examples/examples/com_motion_above_feet_WB.cs");
- std::cout << "cs.size(): " << cs.size() << std::endl;
-
// retrieve parameters from yaml file
YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/mcapi_povcdl_estimation.yaml");
double dt = config["dt"].as<double>();
double min_t = config["min_t"].as<double>();
double max_t = config["max_t"].as<double>();
- double pbc_noise_std = config["pbc_noise_std"].as<double>();
- double vbc_noise_std = config["vbc_noise_std"].as<double>();
- double pb_rate_stdev = config["pb_rate_stdev"].as<double>();
- double f_noise_std = config["f_noise_std"].as<double>();
- double tau_noise_std = config["tau_noise_std"].as<double>();
- double acc_std = config["acc_std"].as<double>();
- double gyr_std = config["gyr_std"].as<double>();
- double abs_biasp_std = config["abs_biasp_std"].as<double>();
- double rel_pose_std = config["rel_pose_std"].as<double>();
- std::cout << "dt: " << dt << std::endl;
- std::cout << "min_t: " << min_t << std::endl;
- std::cout << "max_t: " << max_t << std::endl;
+ double solve_every_sec = config["solve_every_sec"].as<double>();
+ bool solve_end = config["solve_end"].as<bool>();
+ double std_pbc_noise = config["std_pbc_noise"].as<double>();
+ double std_vbc_noise = config["std_vbc_noise"].as<double>();
+ double std_bp_drift = config["std_bp_drift"].as<double>();
+ double std_f_noise = config["std_f_noise"].as<double>();
+ double std_tau_noise = config["std_tau_noise"].as<double>();
+ double std_acc = config["std_acc"].as<double>();
+ double std_gyr = config["std_gyr"].as<double>();
+ double std_abs_biasp = config["std_abs_biasp"].as<double>();
+ double std_odom3d = config["std_odom3d"].as<double>();
+ 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_prior_c = config["std_prior_c"].as<double>();
+ double std_prior_dc = config["std_prior_dc"].as<double>();
+ double std_prior_L = config["std_prior_L"].as<double>();
+ std::vector<double> bias_pbc_vec = config["bias_pbc"].as<std::vector<double>>();
+ Vector3d bias_pbc;
+ bias_pbc(0) = bias_pbc_vec[0];
+ bias_pbc(1) = bias_pbc_vec[1];
+ bias_pbc(2) = bias_pbc_vec[2];
+
+ std::string contact_sequence_file = config["contact_sequence_file"].as<std::string>();
+
+ ContactSequence cs;
+ std::cout << "Loading file " << contact_sequence_file << std::endl;
+ cs.loadFromBinary(contact_sequence_file);
+ std::cout << "cs.size(): " << cs.size() << std::endl;
auto q_traj = cs.concatenateQtrajectories();
auto dq_traj = cs.concatenateDQtrajectories();
@@ -94,8 +106,8 @@ int main (int argc, char **argv) {
auto dc_traj = cs.concatenateDCtrajectories();
auto L_traj = cs.concatenateLtrajectories();
std::vector<std::string> ee_in_contact = cs.getAllEffectorsInContact();
- std::string left_leg_frame_name = ee_in_contact[0];
- std::string right_leg_frame_name = ee_in_contact[1];
+ std::string left_leg_frame_name = ee_in_contact[0]; // leg_left_sole_fix_joint
+ std::string right_leg_frame_name = ee_in_contact[1]; // leg_right_sole_fix_joint
std::cout << "left_leg_frame_name: " << left_leg_frame_name << std::endl;
std::cout << "right_leg_frame_name: " << right_leg_frame_name << std::endl;
auto ll_cforce_traj = cs.concatenateContactForceTrajectories(left_leg_frame_name);
@@ -108,26 +120,17 @@ int main (int argc, char **argv) {
if (max_t == -1){
max_t = q_traj.max();
}
- // double max_t = 1.75; // only the beginning
// initialize some vectors and fill them with dicretized data
- std::vector<double> t_arr;
- // [p_ob, q_ob, qA]
- std::vector<VectorXd> q_traj_arr;
- // [spvel_b, qA_dot]
- std::vector<VectorXd> dq_traj_arr;
- // [spacc_b, qA_ddot]
- std::vector<VectorXd> ddq_traj_arr;
- // w_p_wc
- std::vector<VectorXd> c_traj_arr;
- // w_v_wc
- std::vector<VectorXd> dc_traj_arr;
- // w_Lc: angular momentum at the Com expressed in CoM (=World) frame
- std::vector<VectorXd> L_traj_arr;
- // [f1x f1y f1z ... f4y f4z] for left leg
- std::vector<VectorXd> ll_cforce_traj_arr;
- // [f1x f1y f1z ... f4y f4z] for right leg
- std::vector<VectorXd> rl_cforce_traj_arr;
+ std::vector<double> t_arr; // timestamps
+ std::vector<VectorXd> q_traj_arr; // [p_ob, q_ob, qA]
+ std::vector<VectorXd> dq_traj_arr; // [spvel_b, qA_dot]
+ std::vector<VectorXd> ddq_traj_arr; // [spacc_b, qA_ddot]
+ std::vector<VectorXd> c_traj_arr; // w_p_wc
+ std::vector<VectorXd> dc_traj_arr; // w_v_wc
+ std::vector<VectorXd> L_traj_arr; // w_Lc: angular momentum at the Com expressed in CoM (=World) frame
+ std::vector<VectorXd> ll_cforce_traj_arr; // [f1x f1y f1z ... f4y f4z] for left leg
+ std::vector<VectorXd> rl_cforce_traj_arr; // [f1x f1y f1z ... f4y f4z] for right leg
for (double t=min_t; t <= max_t; t += dt){
t_arr.push_back(t);
q_traj_arr.push_back(q_traj(t));
@@ -160,11 +163,6 @@ int main (int argc, char **argv) {
std::cout << "right_leg_sole_idx: " << right_leg_sole_idx << std::endl;
std::vector<Vector3d> contacts = contacts_from_footrect_center();
- // CREATE BIASES
- Vector6d bias_imu = ZERO6;
- // Vector3d bias_pbc = BIAS_PBC_SMAL;
- Vector3d bias_pbc = ZERO3;
-
///////////////////////////////////////////////
// Create some vectors to aggregate the groundtruth and measurements
// Groundtruth
@@ -225,13 +223,13 @@ int main (int argc, char **argv) {
// Set ground truth variables
Vector3d p_wb = q.head<3>(); // gtr
Vector4d quat_wb = q.segment<4>(3); // gtr
- SE3 oMb(Quaterniond(quat_wb).toRotationMatrix(), p_wb); // global frame pose
+ SE3 oTb(Quaterniond(quat_wb).toRotationMatrix(), p_wb); // global frame pose
// body vel is expressed in body frame
Vector3d b_vsp_b = dq.head<3>(); // vsp: spatial linear velocity
Vector3d b_w_b = dq.segment<3>(3); // meas
Vector3d b_asp_b = ddq.head<3>(); // body spatial acceleration in body plucker frame
- Vector3d w_v_wb = oMb.rotation() * b_vsp_b; // gtr
+ Vector3d w_v_wb = oTb.rotation() * b_vsp_b; // gtr
////////////////////////////////////
////////////////////////////////////
@@ -241,34 +239,49 @@ int main (int argc, char **argv) {
// 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 = b_asp_b + b_w_b.cross(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
+ Vector3d b_proper_acc = b_acc - oTb.inverse().rotation() * gravity(); // meas
// update and retrieve kinematics
forwardKinematics(model,data,q);
- // 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 frame_rl_sole_idx = model.frames[right_leg_sole_idx];
- auto oMll = data.oMi[frame_ll_sole_idx.parent].act(frame_ll_sole_idx.placement);
- 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;
+ // auto oTll = data.oTf[left_leg_sole_idx];
+ // auto oTrl = data.oTf[right_leg_sole_idx];
+ std::string left_force_sensor_joint_name = "leg_left_6_joint";
+ std::string right_force_sensor_joint_name = "leg_right_6_joint";
+ int left_force_sensor_joint_id = model.getJointId(left_force_sensor_joint_name);
+ int right_force_sensor_joint_id = model.getJointId(right_force_sensor_joint_name);
+ auto oTll = data.oMi[left_force_sensor_joint_id];
+ auto oTrl = data.oMi[right_force_sensor_joint_id];
+ // std::cout << "oTll.translation(): " << oTll.translation().transpose() << std::endl;
+ // std::cout << "oTrl.translation(): " << oTrl.translation().transpose() << std::endl;
+ auto bMll = oTb.inverse() * oTll;
+ auto bMrl = oTb.inverse() * oTrl;
Vector3d b_p_ll = bMll.translation(); // meas
Vector4d b_qvec_ll = Quaterniond(bMll.rotation()).coeffs(); // meas
Vector3d b_p_rl = bMrl.translation(); // meas
Vector4d b_qvec_rl = Quaterniond(bMrl.rotation()).coeffs(); // meas
// get local feet wrenchs
- Vector6d l_wrench_ll = contact_force_to_wrench(contacts, cforce_ll, oMll.rotation()); // meas
- Vector6d l_wrench_rl = contact_force_to_wrench(contacts, cforce_rl, oMrl.rotation()); // meas
- l_wrench_ll.tail<3>() = Vector3d::Zero();
- l_wrench_rl.tail<3>() = Vector3d::Zero();
+ Vector6d l_wrench_ll = contact_force_to_wrench(contacts, cforce_ll); // meas
+ Vector6d l_wrench_rl = contact_force_to_wrench(contacts, cforce_rl); // meas
+
+ // turn these wrench to centroidal wrenches
+ Force l_spf_ll(l_wrench_ll.head<3>(), l_wrench_ll.tail<3>());
+ Force l_spf_rl(l_wrench_rl.head<3>(), l_wrench_rl.tail<3>());
+ SE3 oTc(Matrix3d::Identity(), c); // global frame pose
+ SE3 cTll = oTc.inverse() * oTll;
+ SE3 cTrl = oTc.inverse() * oTrl;
+ Force c_spf_ll = cTll.act(l_spf_ll);
+ Force c_spf_rl = cTrl.act(l_spf_rl);
+ std::cout << "\n\n\ncTll\n" << cTll << std::endl;
+ std::cout << "cTrl\n" << cTrl << std::endl;
+ std::cout << "c_spf_ll\n" << c_spf_ll << std::endl;
+ std::cout << "c_spf_rl\n" << c_spf_rl << std::endl;
+ std::cout << "c_spf_ll+c_spf_rl\n" << c_spf_ll + c_spf_rl << std::endl;
//////////////////////////////////////////////
// COM relative position and velocity measures
- Vector3d b_p_bc = oMb.inverse().act(c); // meas
+ // c =def w_p_wc
+ Vector3d b_p_bc = oTb.inverse().act(c); // meas
VectorXd q_static = q;
VectorXd dq_static = dq;
@@ -283,7 +296,7 @@ int main (int argc, char **argv) {
// 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);
+ SE3 bTc (oTb.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
@@ -329,8 +342,8 @@ int main (int argc, char **argv) {
//===================================================== INITIALIZATION
// SENSOR + PROCESSOR INERTIAL KINEMATICS
ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
- intr_ik->pbc_noise_std = pbc_noise_std;
- intr_ik->vbc_noise_std = vbc_noise_std;
+ intr_ik->std_pbc_noise = std_pbc_noise;
+ intr_ik->std_vbc_noise = std_vbc_noise;
VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
SensorBasePtr sen_ikin_base = problem->installSensor("SensorInertialKinematics", "SenIK", extr, intr_ik);
// SensorBasePtr sen_ikin_base = problem->installSensor("SensorInertialKinematics", "SenIK", extr, bodydynamics_root_dir + "/demos/sensor_inertial_kinematics.yaml"); // TODO: does not work!
@@ -338,7 +351,7 @@ int main (int argc, char **argv) {
ProcessorParamsInertialKinematicsPtr params_ik = std::make_shared<ProcessorParamsInertialKinematics>();
params_ik->sensor_angvel_name = "SenImu";
- params_ik->pb_rate_stdev = pb_rate_stdev;
+ params_ik->std_bp_drift = std_bp_drift;
ProcessorBasePtr proc_ikin_base = problem->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", sen_ikin, params_ik);
// ProcessorBasePtr proc_ikin_base = problem->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/demos/processor_inertial_kinematics.yaml");
ProcessorInertialKinematicsPtr proc_inkin = std::static_pointer_cast<ProcessorInertialKinematics>(proc_ikin_base);
@@ -351,9 +364,10 @@ int main (int argc, char **argv) {
// SENSOR + PROCESSOR FT PREINT
ParamsSensorForceTorquePtr intr_ft = std::make_shared<ParamsSensorForceTorque>();
- intr_ft->f_noise_std = f_noise_std;
- intr_ft->tau_noise_std = tau_noise_std;
+ intr_ft->std_f_noise = std_f_noise;
+ intr_ft->std_tau_noise = std_tau_noise;
intr_ft->mass = data.mass[0];
+ std::cout << "\n\nTALOS MASS: " << intr_ft->mass << std::endl;
SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), intr_ft);
// SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), bodydynamics_root_dir + "/demos/sensor_ft.yaml");
@@ -380,8 +394,14 @@ 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_gtr_v[0], q_ob_gtr_v[0], v_ob_gtr_v[0], c_traj_arr[0], dc_traj_arr[0], L_traj_arr[0]; // TODO: origin vel shoulde not be zero but v_wb_gtr_v[0]
- MatrixXd P_origin = pow(1e-3, 2) * Eigen::Matrix9d::Identity();
+ x_origin << p_ob_gtr_v[0], q_ob_gtr_v[0], v_ob_gtr_v[0], c_traj_arr[0], dc_traj_arr[0], L_traj_arr[0];
+ MatrixXd P_origin(18,18); P_origin.setIdentity();
+ P_origin.block<3,3>(0,0) = pow(std_prior_p, 2) * Eigen::Matrix3d::Identity();
+ P_origin.block<3,3>(3,3) = pow(std_prior_o, 2) * Eigen::Matrix3d::Identity();
+ P_origin.block<3,3>(6,6) = pow(std_prior_v, 2) * Eigen::Matrix3d::Identity();
+ P_origin.block<3,3>(9,9) = pow(std_prior_c, 2) * Eigen::Matrix3d::Identity();
+ P_origin.block<3,3>(12,12) = pow(std_prior_dc,2) * Eigen::Matrix3d::Identity();
+ P_origin.block<3,3>(15,15) = pow(std_prior_L, 2) * Eigen::Matrix3d::Identity();
FrameBasePtr KF1 = problem->emplaceFrame(KEY, x_origin, t0);
// Prior pose factor
CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1, t0, nullptr, x_origin.head(7), P_origin.topLeftCorner(6, 6));
@@ -409,11 +429,29 @@ int main (int argc, char **argv) {
proc_ftpreint->setOrigin(KF1);
problem->setPriorIsSet(true); // We handle the whole initialization manually here
+ ////////////////////////////////////////////
+ // BIAS FACTORS
+ // Add absolute factor on Imu biases to simulate a fix()
+ Eigen::Matrix6d Q_bi = 1e-8 * Eigen::Matrix6d::Identity();
+ CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
+ FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", ZERO6, Q_bi);
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic());
+
+ // Add loose absolute factor on initial bp bias since dynamical trajectories
+ // able to excite the system enough to make COM and bias_p observable hard to generate in a simple gtest
+ Eigen::Matrix3d Q_bp = pow(std_abs_biasp, 2)* Eigen::Matrix3d::Identity();
+ CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1, "bp0", t0);
+ FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", ZERO3, Q_bp);
+ FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, KF1->getCaptureOf(sen_ikin)->getSensorIntrinsic());
+
+
///////////////////////////////////////////
// process measurements in sequential order
int traj_size = problem->getTrajectory()->getFrameList().size();
- SE3 oMb_prev(Quaterniond(q_ob_gtr_v[0]).toRotationMatrix(), p_ob_gtr_v[0]);
+ SE3 oTb_prev(Quaterniond(q_ob_gtr_v[0]).toRotationMatrix(), p_ob_gtr_v[0]);
FrameBasePtr KF_prev = KF1;
+ double ts_since_last_kf = 0;
+
for (int i=1; i < t_arr.size(); i++){
TimeStamp ts(t_arr[i]);
@@ -421,8 +459,8 @@ int main (int argc, char **argv) {
Vector6d acc_gyr_meas; acc_gyr_meas << acc_b_meas_v[i], w_b_meas_v[i];
Matrix6d acc_gyr_cov = Matrix6d::Identity();
- acc_gyr_cov.topLeftCorner<3,3>() = pow(acc_std, 2) * Matrix3d::Identity();
- acc_gyr_cov.bottomRightCorner<3,3>() = pow(gyr_std, 2) * Matrix3d::Identity();
+ acc_gyr_cov.topLeftCorner<3,3>() = pow(std_acc, 2) * Matrix3d::Identity();
+ acc_gyr_cov.bottomRightCorner<3,3>() = pow(std_gyr, 2) * Matrix3d::Identity();
// Inertial kinematics
meas_ikin << p_bc_meas_v[i], v_bc_meas_v[i], w_b_meas_v[i];
@@ -468,51 +506,40 @@ 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_ob_gtr_v[i]).toRotationMatrix(), p_ob_gtr_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 = pow(rel_pose_std, 2) * Eigen::Matrix6d::Identity();
+ SE3 oTb_now(Quaterniond(q_ob_gtr_v[i]).toRotationMatrix(), p_ob_gtr_v[i]);
+ SE3 bprev_T_bnow = oTb_prev.inverse() * oTb_now;
+ Vector7d prev_pose_now; prev_pose_now << bprev_T_bnow.translation(), Quaterniond(bprev_T_bnow.rotation()).coeffs();
+ Eigen::Matrix6d rel_pose_cov = pow(std_odom3d, 2) * Eigen::Matrix6d::Identity();
CaptureBasePtr cap_pose_base = CaptureBase::emplace<CapturePose>(KF_now, KF_now->getTimeStamp(), nullptr, prev_pose_now, rel_pose_cov);
FeatureBasePtr ftr_odom3d_base = FeatureBase::emplace<FeaturePose>(cap_pose_base, prev_pose_now, rel_pose_cov);
FactorBase::emplace<FactorOdom3d>(ftr_odom3d_base, ftr_odom3d_base, KF_prev, nullptr, false);
- oMb_prev = oMb_now;
+ oTb_prev = oTb_now;
KF_prev = KF_now;
traj_size = problem->getTrajectory()->getFrameList().size();
}
- if (i%20 == 0){
- // std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
- // std::cout << report << std::endl;
+ ts_since_last_kf += dt;
+ if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){
+ std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::cout << report << std::endl;
+ ts_since_last_kf = 0;
}
}
- ////////////////////////////////////////////
- // BIAS FACTORS
- // Add absolute factor on Imu biases to simulate a fix()
- Eigen::Matrix6d Q_bi = 1e-8 * Eigen::Matrix6d::Identity();
- CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
- FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu, Q_bi);
- FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic());
-
- // Add loose absolute factor on initial bp bias since dynamical trajectories
- // able to excite the system enough to make COM and bias_p observable hard to generate in a simple gtest
- Eigen::Matrix3d Q_bp = pow(abs_biasp_std, 2)* 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);
- FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, KF1->getCaptureOf(sen_ikin)->getSensorIntrinsic());
///////////////////////////////////////////
//////////////// SOLVING //////////////////
///////////////////////////////////////////
- // std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
- // problem->print(4,true,true,true);
- // std::cout << report << std::endl;
-
-
+ problem->print(4,true,true,true);
+ if (solve_end){
+ std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
+ problem->print(4,true,true,true);
+ std::cout << report << std::endl;
+ }
//////////////////////////////////////
//////////////////////////////////////
@@ -532,7 +559,7 @@ int main (int argc, char **argv) {
file_est << header;
std::string header_kfs = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,cx,cy,cz,cdx,cdy,cdz,Lx,Ly,Lz,bpx,bpy,bpz\n";
file_kfs << header_kfs;
- std::string header_wre = "t,pllx,plly,pllz,prlx,prly,prlz,fllx,flly,fllz,frlx,frly,frlz,taullx,taully,taullz,taurlx,taurly,taurlz\n";
+ std::string header_wre = "t,plfx,plfy,plfz,prfx,prfy,prfz,f_lfx,f_lfy,f_lfz,f_rfx,f_rfy,f_rfz,tau_lfx,tau_lfy,tau_lfz,tau_rfx,tau_rfy,tau_rfz\n";
file_wre << header_wre;
VectorXd state_est;
@@ -607,11 +634,11 @@ int main (int argc, char **argv) {
}
VectorXd kf_state;
- Vector3d bp_bias;
+ Vector3d bp_bias_est;
for (auto kf: problem->getTrajectory()->getFrameList()){
if (kf->isKey()){
kf_state = kf->getState();
- bp_bias = kf->getCaptureOf(sen_ikin)->getState();
+ bp_bias_est = kf->getCaptureOf(sen_ikin)->getState();
// std::cout << kf->getTimeStamp().getSeconds() << std::endl;
@@ -635,9 +662,9 @@ int main (int argc, char **argv) {
<< kf_state(16) << ","
<< kf_state(17) << ","
<< kf_state(18) << ","
- << bp_bias(0) << ","
- << bp_bias(1) << ","
- << bp_bias(2)
+ << bp_bias_est(0) << ","
+ << bp_bias_est(1) << ","
+ << bp_bias_est(2)
<< "\n";
}
}
diff --git a/demos/mcapi_povcdl_estimation.yaml b/demos/mcapi_povcdl_estimation.yaml
index e4b0e1b034ac3f9ef5effac37720a620c2f43002..986e653a0b0bb668d1d72c7ddbeb9f96081feb6f 100644
--- a/demos/mcapi_povcdl_estimation.yaml
+++ b/demos/mcapi_povcdl_estimation.yaml
@@ -1,12 +1,28 @@
-dt: 0.0005
-min_t: -1.0
-max_t: -1.0
-pbc_noise_std: 0.001
-vbc_noise_std: 0.001
-pb_rate_stdev: 0.0001
-f_noise_std: 0.01
-tau_noise_std: 0.01
-acc_std: 0.05
-gyr_std: 0.01
-abs_biasp_std: 0.0001
-rel_pose_std: 0.01
\ No newline at end of file
+dt: 0.001
+min_t: -1.0 # -1 means from the beginning of the trajectory
+max_t: -1.0 # -1 means until the end of the trajectory
+solve_every_sec: -0.1 # < 0 strict --> no solve
+solve_end: false
+
+std_pbc_noise: 0.001
+std_vbc_noise: 0.001
+std_bp_drift: 0.5
+std_f_noise: 0.01
+std_tau_noise: 0.01
+std_acc: 0.005
+std_gyr: 0.001
+std_abs_biasp: 0.3
+std_odom3d: 0.01
+std_prior_p: 0.001
+std_prior_o: 0.001
+std_prior_v: 0.001
+std_prior_c: 0.001
+std_prior_dc: 0.001
+std_prior_L: 0.001
+
+bias_pbc: [0,0,0]
+
+#Â files
+# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/tests/centroidalkin/data/multicontact-api-master-examples/examples/com_motion_above_feet_WB.cs"
+contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/tests/centroidalkin/data/tsid_gen/com_traj_zero_motion.cs"
+
diff --git a/demos/mcapi_utils.cpp b/demos/mcapi_utils.cpp
index bdb0dce95f4ab752fc84c04b52ce466eab3afe58..50a31e622a9de029d316dc45fe81e2763253cc77 100644
--- a/demos/mcapi_utils.cpp
+++ b/demos/mcapi_utils.cpp
@@ -10,28 +10,27 @@ Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vec
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.65;
+ double ly = 0.065;
+ double lz = 0.107;
std::vector<Vector3d> contacts; contacts.resize(4);
- contacts[0] = {-lx, -ly, 0};
- contacts[1] = {-lx, ly, 0};
- contacts[2] = { lx, -ly, 0};
- contacts[3] = { lx, ly, 0};
+ 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>& w_cforces,
- const Matrix3d& wRl)
+ const Matrix<double, 12, 1>& cf12)
{
- 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 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;
+ 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;
}
diff --git a/demos/mcapi_utils.h b/demos/mcapi_utils.h
index a5f88635beece170d8665a0191ccf5633f160595..3d1bcf64c0e65c430f3898cccd15fb6e78083114 100644
--- a/demos/mcapi_utils.h
+++ b/demos/mcapi_utils.h
@@ -35,5 +35,4 @@ std::vector<Vector3d> contacts_from_footrect_center();
* \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>& w_cforces,
- const Matrix3d& wRl);
+ const Matrix<double, 12, 1>& cf12);
diff --git a/demos/processor_imu_mcapi_demo.yaml b/demos/processor_imu_mcapi_demo.yaml
index 0eeb5bd4ee7a3425655eade1bf986a9febdc8be3..1c2c67f31e8b5b6cd0266ab9b05ac2ae879d163a 100644
--- a/demos/processor_imu_mcapi_demo.yaml
+++ b/demos/processor_imu_mcapi_demo.yaml
@@ -3,7 +3,7 @@ name: "Main imu" # This is ignored. The name provided to the SensorF
time_tolerance: 0.0025 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.000001
keyframe_vote:
- max_time_span: 100 # seconds
+ max_time_span: 2 # seconds
max_buff_length: 100000000000 # motion deltas
dist_traveled: 20000.0 # meters
angle_turned: 1000 # radians (1 rad approx 57 deg, approx 60 deg)
diff --git a/demos/processor_inertial_kinematics.yaml b/demos/processor_inertial_kinematics.yaml
index de71c755ac6f0282b1f7c01053b14839dabf236b..64c0d60f2f1b2e7f1af0b9e570bbcec1473c0207 100644
--- a/demos/processor_inertial_kinematics.yaml
+++ b/demos/processor_inertial_kinematics.yaml
@@ -2,4 +2,4 @@ type: "ProcessorInertialKinematics" # This must match the KEY used i
name: "PInertialKinematics" # This is ignored. The name provided to the SensorFactory prevails
sensor_angvel_name: "SenImu"
-pb_rate_stdev: 0.01
\ No newline at end of file
+std_bp_drift: 0.01
\ No newline at end of file
diff --git a/demos/sensor_ft.yaml b/demos/sensor_ft.yaml
index f2dc1c6455014cffdde5eb286cd2813c563b8533..ce91f47725a24996e3fed53b6077ff96344e6db8 100644
--- a/demos/sensor_ft.yaml
+++ b/demos/sensor_ft.yaml
@@ -1,5 +1,5 @@
type: "SensorForceTorque" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
name: "SenFT" # This is ignored. The name provided to the SensorFactory prevails
mass: 10
-f_noise_std: 0.001
-tau_noise_std: 0.001
\ No newline at end of file
+std_f_noise: 0.001
+std_tau_noise: 0.001
\ No newline at end of file
diff --git a/demos/sensor_inertial_kinematics.yaml b/demos/sensor_inertial_kinematics.yaml
index dbe8f86825ed66bab6d46ef6c3dec3423b66829d..471411901d3a035891aee3e55d52f7199a8dcb89 100644
--- a/demos/sensor_inertial_kinematics.yaml
+++ b/demos/sensor_inertial_kinematics.yaml
@@ -1,5 +1,5 @@
type: "SensorInertialKinematics" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
name: "SInertialKinematics" # This is ignored. The name provided to the SensorFactory prevails
-pbc_noise_std: 0.01
-vbc_noise_std: 0.01
\ No newline at end of file
+std_pbc_noise: 0.01
+std_vbc_noise: 0.01
\ No newline at end of file
diff --git a/include/bodydynamics/math/force_torque_delta_tools.h b/include/bodydynamics/math/force_torque_delta_tools.h
index 0a7704565baa3da3640a30c6d78e5c4e03bf20dd..6a0f97b12e183997e9dc041692e6e8fb95e148dd 100644
--- a/include/bodydynamics/math/force_torque_delta_tools.h
+++ b/include/bodydynamics/math/force_torque_delta_tools.h
@@ -619,7 +619,8 @@ inline void body2delta(const MatrixBase<D1>& f1,
dc = 0.5 * (qbl1*f1 + qbl2*f2) * dt * dt / mass;
dcd = (qbl1*f1 + qbl2*f2) * dt / mass;
- dLc = (qbl1*(tau1 + (pbl1-pbc).cross(f1)) + qbl2*(tau2 + (pbl2-pbc).cross(f2))) * dt;
+ dLc = (qbl1*tau1 + (pbl1-pbc).cross(qbl1*f1)
+ + qbl2*tau2 + (pbl2-pbc).cross(qbl2*f2)) * dt;
dq = exp_q(wb * dt);
// std::cout << "\ndc: " << dc.transpose() << std::endl;
diff --git a/include/bodydynamics/processor/processor_inertial_kinematics.h b/include/bodydynamics/processor/processor_inertial_kinematics.h
index 0b8f352b39e5ca92d12f90de896caa8f2d6f69cf..bd50122690d8f3ad6bf3097f6c5e0c1866e581a5 100644
--- a/include/bodydynamics/processor/processor_inertial_kinematics.h
+++ b/include/bodydynamics/processor/processor_inertial_kinematics.h
@@ -13,21 +13,21 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsInertialKinematics);
struct ProcessorParamsInertialKinematics : public ProcessorParamsBase
{
std::string sensor_angvel_name;
- double pb_rate_stdev;
+ double std_bp_drift;
ProcessorParamsInertialKinematics() = default;
ProcessorParamsInertialKinematics(std::string _unique_name, const ParamsServer& _server):
ProcessorParamsBase(_unique_name, _server)
{
sensor_angvel_name = _server.getParam<std::string>(_unique_name + "/sensor_angvel_name");
- pb_rate_stdev = _server.getParam<double>(_unique_name + "/pb_rate_stdev");
+ std_bp_drift = _server.getParam<double>(_unique_name + "/std_bp_drift");
}
virtual ~ProcessorParamsInertialKinematics() = default;
std::string print() const
{
return "\n" + ProcessorParamsBase::print() + "\n"
+ "sensor_angvel_name: " + sensor_angvel_name + "\n"
- + "pb_rate_stdev: " + std::to_string(pb_rate_stdev) + "\n";
+ + "std_bp_drift: " + std::to_string(std_bp_drift) + "\n";
}
};
diff --git a/include/bodydynamics/sensor/sensor_force_torque.h b/include/bodydynamics/sensor/sensor_force_torque.h
index fd4c4ef26c820be0e735468d658c64ed22e5fea8..82152ef685b0abb95cb6f568317b7b89e3f94e65 100644
--- a/include/bodydynamics/sensor/sensor_force_torque.h
+++ b/include/bodydynamics/sensor/sensor_force_torque.h
@@ -15,24 +15,24 @@ struct ParamsSensorForceTorque : public ParamsSensorBase
{
//noise std dev
double mass = 10; // total mass of the robot (kg)
- double f_noise_std = 0.001; // standard deviation of the force sensors (N)
- double tau_noise_std = 0.001; // standard deviation of the torque sensors (N.m)
+ double std_f_noise = 0.001; // standard deviation of the force sensors (N)
+ double std_tau_noise = 0.001; // standard deviation of the torque sensors (N.m)
ParamsSensorForceTorque() = default;
ParamsSensorForceTorque(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
mass = _server.getParam<double>(_unique_name + "/mass");
- f_noise_std = _server.getParam<double>(_unique_name + "/f_noise_std");
- tau_noise_std = _server.getParam<double>(_unique_name + "/tau_noise_std");
+ std_f_noise = _server.getParam<double>(_unique_name + "/std_f_noise");
+ std_tau_noise = _server.getParam<double>(_unique_name + "/std_tau_noise");
}
virtual ~ParamsSensorForceTorque() = default;
std::string print() const
{
return "\n" + ParamsSensorBase::print() + "\n"
+ "mass: " + std::to_string(mass) + "\n"
- + "f_noise_std: " + std::to_string(f_noise_std) + "\n"
- + "tau_noise_std: " + std::to_string(tau_noise_std) + "\n";
+ + "std_f_noise: " + std::to_string(std_f_noise) + "\n"
+ + "std_tau_noise: " + std::to_string(std_tau_noise) + "\n";
}
};
@@ -44,8 +44,8 @@ class SensorForceTorque : public SensorBase
protected:
//noise std dev
double mass_; // total mass of the robot (kg)
- double f_noise_std_; // standard deviation of the force sensors (N)
- double tau_noise_std_; // standard deviation of the torque sensors (N.m)
+ double std_f_noise_; // standard deviation of the force sensors (N)
+ double std_tau_noise_; // standard deviation of the torque sensors (N.m)
public:
@@ -66,12 +66,12 @@ inline double SensorForceTorque::getMass() const
inline double SensorForceTorque::getForceNoiseStd() const
{
- return f_noise_std_;
+ return std_f_noise_;
}
inline double SensorForceTorque::getTorqueNoiseStd() const
{
- return tau_noise_std_;
+ return std_tau_noise_;
}
} // namespace wolf
diff --git a/include/bodydynamics/sensor/sensor_inertial_kinematics.h b/include/bodydynamics/sensor/sensor_inertial_kinematics.h
index e0b39d1db12ddbb307de868f8b9c63981e0029af..58a67654fcf78333fb5f9f1cde716485affdf24b 100644
--- a/include/bodydynamics/sensor/sensor_inertial_kinematics.h
+++ b/include/bodydynamics/sensor/sensor_inertial_kinematics.h
@@ -14,22 +14,22 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorInertialKinematics);
struct ParamsSensorInertialKinematics : public ParamsSensorBase
{
//noise std dev
- double pbc_noise_std; // standard deviation of the com position measurement relative to the base frame (m)
- double vbc_noise_std; // standard deviation of the com velocity measurement relative to the base frame (m/s)
+ double std_pbc_noise; // standard deviation of the com position measurement relative to the base frame (m)
+ double std_vbc_noise; // standard deviation of the com velocity measurement relative to the base frame (m/s)
ParamsSensorInertialKinematics() = default;
ParamsSensorInertialKinematics(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
- pbc_noise_std = _server.getParam<double>(_unique_name + "/pbc_noise_std");
- vbc_noise_std = _server.getParam<double>(_unique_name + "/vbc_noise_std");
+ std_pbc_noise = _server.getParam<double>(_unique_name + "/std_pbc_noise");
+ std_vbc_noise = _server.getParam<double>(_unique_name + "/std_vbc_noise");
}
virtual ~ParamsSensorInertialKinematics() = default;
std::string print() const
{
return "\n" + ParamsSensorBase::print() + "\n"
- + "pbc_noise_std: " + std::to_string(pbc_noise_std) + "\n"
- + "vbc_noise_std: " + std::to_string(vbc_noise_std) + "\n";
+ + "std_pbc_noise: " + std::to_string(std_pbc_noise) + "\n"
+ + "std_vbc_noise: " + std::to_string(std_vbc_noise) + "\n";
}
};
@@ -54,12 +54,12 @@ class SensorInertialKinematics : public SensorBase
inline double SensorInertialKinematics::getPbcNoiseStd() const
{
- return intr_ikin_->pbc_noise_std;
+ return intr_ikin_->std_pbc_noise;
}
inline double SensorInertialKinematics::getVbcNoiseStd() const
{
- return intr_ikin_->vbc_noise_std;
+ return intr_ikin_->std_vbc_noise;
}
} // namespace wolf
diff --git a/src/processor/processor_inertial_kinematics.cpp b/src/processor/processor_inertial_kinematics.cpp
index 5c9fe32d0abcd50e89d645a5bfda09aa38ce77f7..7fb2ab2d343c58001adaee8921670882bdf4a0eb 100644
--- a/src/processor/processor_inertial_kinematics.cpp
+++ b/src/processor/processor_inertial_kinematics.cpp
@@ -129,7 +129,7 @@ inline bool ProcessorInertialKinematics::createInertialKinematicsFactor(CaptureI
// 5. Create bias drift factor if an origin capture exists -> zero difference + covariance as integration of a Brownian noise
if (_cap_ikin_origin){
double dt_drift = _cap_ikin->getTimeStamp() - _cap_ikin_origin->getTimeStamp();
- Eigen::Matrix3d cov_drift = Eigen::Matrix3d::Identity() * pow(params_ikin_->pb_rate_stdev, 2) * sqrt(dt_drift);
+ Eigen::Matrix3d cov_drift = Eigen::Matrix3d::Identity() * pow(params_ikin_->std_bp_drift, 2) * sqrt(dt_drift);
// CaptureBasePtr cap_diff = CaptureBase::emplace<CaptureBase>(feat_ikin->getFrame(), "ComBiasDrift", feat_ikin->getFrame()->getTimeStamp());
FeatureBasePtr feat_diff = FeatureBase::emplace<FeatureBase>(_cap_ikin, "ComBiasDrift", Eigen::Vector3d::Zero(), cov_drift, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
diff --git a/src/sensor/sensor_force_torque.cpp b/src/sensor/sensor_force_torque.cpp
index 37bcb20a11208bcc446cba182a64b43baff47697..0de96e015b76226e7f549d11b2cf67e491b900aa 100644
--- a/src/sensor/sensor_force_torque.cpp
+++ b/src/sensor/sensor_force_torque.cpp
@@ -10,16 +10,16 @@ SensorForceTorque::SensorForceTorque(const Eigen::VectorXd& _extrinsics, ParamsS
nullptr,
nullptr,
nullptr,
- (Eigen::Matrix<double, 12, 1>() << _params->f_noise_std,_params->f_noise_std,_params->f_noise_std,
- _params->f_noise_std,_params->f_noise_std,_params->f_noise_std,
- _params->tau_noise_std,_params->tau_noise_std,_params->tau_noise_std,
- _params->tau_noise_std,_params->tau_noise_std,_params->tau_noise_std).finished(),
+ (Eigen::Matrix<double, 12, 1>() << _params->std_f_noise,_params->std_f_noise,_params->std_f_noise,
+ _params->std_f_noise,_params->std_f_noise,_params->std_f_noise,
+ _params->std_tau_noise,_params->std_tau_noise,_params->std_tau_noise,
+ _params->std_tau_noise,_params->std_tau_noise,_params->std_tau_noise).finished(),
false,
false,
true),
mass_(_params->mass),
- f_noise_std_(_params->f_noise_std),
- tau_noise_std_(_params->tau_noise_std)
+ std_f_noise_(_params->std_f_noise),
+ std_tau_noise_(_params->std_tau_noise)
{
assert(_extrinsics.size() == 0 && "Wrong extrinsics vector size! Should be 0");
}
diff --git a/src/sensor/sensor_inertial_kinematics.cpp b/src/sensor/sensor_inertial_kinematics.cpp
index f27c0e7b2a2c9b9bb43017e2a9bdb4fd2c0cf398..b6ed72c83159524ab2d3b326f86030022858ed50 100644
--- a/src/sensor/sensor_inertial_kinematics.cpp
+++ b/src/sensor/sensor_inertial_kinematics.cpp
@@ -10,8 +10,8 @@ SensorInertialKinematics::SensorInertialKinematics(const Eigen::VectorXd& _extri
nullptr, // no position
nullptr, // no orientation
std::make_shared<StateBlock>(Eigen::Vector3d(0,0,0), false), // bias; false = unfixed
- (Eigen::Vector6d() << _params->pbc_noise_std,_params->pbc_noise_std,_params->pbc_noise_std,
- _params->vbc_noise_std,_params->vbc_noise_std,_params->vbc_noise_std).finished(),
+ (Eigen::Vector6d() << _params->std_pbc_noise,_params->std_pbc_noise,_params->std_pbc_noise,
+ _params->std_vbc_noise,_params->std_vbc_noise,_params->std_vbc_noise).finished(),
false, false, true),
intr_ikin_(_params)
{
diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
index de081e23f24f8e4eabbc0819116647b05e651c15..75138edee7ebe94ffa68b4ca9a5c737d26fafbe4 100644
--- a/test/gtest_factor_force_torque.cpp
+++ b/test/gtest_factor_force_torque.cpp
@@ -196,8 +196,8 @@ class FactorInertialKinematics_2KF : public testing::Test
VectorXd extr_ik(0);
ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
- intr_ik->pbc_noise_std = 0.1;
- intr_ik->vbc_noise_std = 0.1;
+ intr_ik->std_pbc_noise = 0.1;
+ intr_ik->std_vbc_noise = 0.1;
auto senik = problem_->installSensor("SensorInertialKinematics", "senIK", extr_ik, intr_ik);
SIK_ = std::static_pointer_cast<SensorInertialKinematics>(senik);
diff --git a/test/gtest_processor_force_torque_preint.cpp b/test/gtest_processor_force_torque_preint.cpp
index 5b7ce5ed0b124a0e648b4c8e7e88b0b44a669584..a11711ae72de99f02354ea01b92a64d58eae71de 100644
--- a/test/gtest_processor_force_torque_preint.cpp
+++ b/test/gtest_processor_force_torque_preint.cpp
@@ -90,8 +90,8 @@ public:
//===================================================== INITIALIZATION
// SENSOR + PROCESSOR INERTIAL KINEMATICS
ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
- intr_ik->pbc_noise_std = 0.1;
- intr_ik->vbc_noise_std = 0.1;
+ intr_ik->std_pbc_noise = 0.1;
+ intr_ik->std_vbc_noise = 0.1;
VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr, intr_ik);
// SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr, bodydynamics_root_dir + "/demos/sensor_inertial_kinematics.yaml"); // TODO: does not work!
@@ -99,7 +99,7 @@ public:
ProcessorParamsInertialKinematicsPtr params_ik = std::make_shared<ProcessorParamsInertialKinematics>();
params_ik->sensor_angvel_name = "SenImu";
- params_ik->pb_rate_stdev = 1e-2;
+ params_ik->std_bp_drift = 1e-2;
ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", sen_ikin_, params_ik);
// ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/demos/processor_inertial_kinematics.yaml");
proc_inkin_ = std::static_pointer_cast<ProcessorInertialKinematics>(proc_ikin_base);
@@ -112,8 +112,8 @@ public:
// SENSOR + PROCESSOR FT PREINT
ParamsSensorForceTorquePtr intr_ft = std::make_shared<ParamsSensorForceTorque>();
- intr_ft->f_noise_std = 0.001;
- intr_ft->tau_noise_std = 0.001;
+ intr_ft->std_f_noise = 0.001;
+ intr_ft->std_tau_noise = 0.001;
intr_ft->mass = MASS;
SensorBasePtr sen_ft_base = problem_->installSensor("SensorForceTorque", "SenFT", VectorXd(0), intr_ft);
// SensorBasePtr sen_ft_base = problem_->installSensor("SensorForceTorque", "SenFT", VectorXd(0), bodydynamics_root_dir + "/demos/sensor_ft.yaml");
diff --git a/test/gtest_processor_inertial_kinematics.cpp b/test/gtest_processor_inertial_kinematics.cpp
index 6992157b53f1a1dad1884cd2078280b4516534ef..df19af4628a4411200324bea8a942f87345966bb 100644
--- a/test/gtest_processor_inertial_kinematics.cpp
+++ b/test/gtest_processor_inertial_kinematics.cpp
@@ -74,8 +74,8 @@ class FactorInertialKinematics_2KF : public testing::Test
// SENSOR + PROCESSOR INERTIAL KINEMATICS
ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
- intr_ik->pbc_noise_std = 0.1;
- intr_ik->vbc_noise_std = 0.1;
+ intr_ik->std_pbc_noise = 0.1;
+ intr_ik->std_vbc_noise = 0.1;
VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr, intr_ik);
// SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr, bodydynamics_root_dir + "/demos/sensor_inertial_kinematics.yaml"); // TODO: does not work!
@@ -92,7 +92,7 @@ class FactorInertialKinematics_2KF : public testing::Test
ProcessorParamsInertialKinematicsPtr params_ik = std::make_shared<ProcessorParamsInertialKinematics>();
params_ik->sensor_angvel_name = "SenImu";
- params_ik->pb_rate_stdev = 0.01;
+ params_ik->std_bp_drift = 0.01;
ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", sen_ikin_, params_ik);
// ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/demos/processor_inertial_kinematics.yaml");
// auto proc_inkin = std::static_pointer_cast<ProcessorInertialKinematics>(proc_ikin_base);
diff --git a/test/gtest_sensor_force_torque.cpp b/test/gtest_sensor_force_torque.cpp
index f6b94c6b8c8a35a35e1851acfd8d8ea0ad316f0d..aa55d2a80e52532383550ba17e550e6a67670d0f 100644
--- a/test/gtest_sensor_force_torque.cpp
+++ b/test/gtest_sensor_force_torque.cpp
@@ -19,8 +19,8 @@ using namespace Eigen;
TEST(SensorForceTorque, constructor_and_getters)
{
ParamsSensorForceTorquePtr intr = std::make_shared<ParamsSensorForceTorque>();
- intr->f_noise_std = 1;
- intr->tau_noise_std = 2;
+ intr->std_f_noise = 1;
+ intr->std_tau_noise = 2;
VectorXd extr(0);
SensorForceTorque S(extr, intr);
diff --git a/test/gtest_sensor_inertial_kinematics.cpp b/test/gtest_sensor_inertial_kinematics.cpp
index a18c2a276e4a43ed2f31bb1dd4439be2bc173a71..d5cdb01a24d41c0ba5e5ddd959e649da2ff346ba 100644
--- a/test/gtest_sensor_inertial_kinematics.cpp
+++ b/test/gtest_sensor_inertial_kinematics.cpp
@@ -19,8 +19,8 @@ using namespace Eigen;
TEST(SensorInertialKinematics, constructor_and_getters)
{
ParamsSensorInertialKinematicsPtr intr = std::make_shared<ParamsSensorInertialKinematics>();
- intr->pbc_noise_std = 1;
- intr->vbc_noise_std = 2;
+ intr->std_pbc_noise = 1;
+ intr->std_vbc_noise = 2;
VectorXd extr(0);
SensorInertialKinematics S(extr, intr);