diff --git a/include/sot/talos_balance/base-estimator.hh b/include/sot/talos_balance/base-estimator.hh
index 2ee3352ae993c616e918aa137bfef6fa173d83d7..5e61607865afdd0772e18a8dde0e22c5a7dba1e7 100644
--- a/include/sot/talos_balance/base-estimator.hh
+++ b/include/sot/talos_balance/base-estimator.hh
@@ -247,7 +247,7 @@ namespace dynamicgraph {
Eigen::VectorXd m_q_sot; /// robot configuration according to SoT convention
Eigen::VectorXd m_v_pin; /// robot velocities according to pinocchio convention
Eigen::VectorXd m_v_sot; /// robot velocities according to SoT convention
- Matrix3 m_oRchest; /// chest orientation in the world from angular fusion
+ Matrix3 m_oRtorso; /// chest orientation in the world from angular fusion
Matrix3 m_oRff; /// base orientation in the world
/* Filter buffers*/
diff --git a/python/sot_talos_balance/test/appli_dcm_estimator.py b/python/sot_talos_balance/test/appli_dcm_estimator.py
index 091e00c83d3779b0a85bab3438101da0cb644e30..4570e97a2ba6a4ce0ef71af1de7b55eb8b45789d 100644
--- a/python/sot_talos_balance/test/appli_dcm_estimator.py
+++ b/python/sot_talos_balance/test/appli_dcm_estimator.py
@@ -67,25 +67,28 @@ create_topic(robot.publisher, robot.device_filters.acc_filter, 'x_filtered', rob
create_topic(robot.publisher, robot.dcm_estimator, 'c', robot = robot, data_type='vector')
create_topic(robot.publisher, robot.dcm_estimator, 'dc', robot = robot, data_type='vector')
create_topic(robot.publisher, robot.base_estimator, 'v', robot = robot, data_type='vector')
+create_topic(robot.publisher, robot.base_estimator, 'v_gyr', robot = robot, data_type='vector')
# --- ROS SUBSCRIBER
robot.subscriber = RosSubscribe("base_subscriber")
-# robot.subscriber.add("vector","position","/sot/base_link/position")
-# robot.subscriber.add("vector","velocity","/sot/base_link/velocity")
-robot.subscriber.add("vector","position","/sot/torso_2_link/position")
+robot.subscriber.add("vector","position","/sot/base_link/position")
+robot.subscriber.add("vector","velocity","/sot/base_link/velocity")
robot.subscriber.add("vector","q_est","/sot/base_estimator/q")
robot.subscriber.add("vector","q_imu","/sot/base_estimator/q_imu")
robot.subscriber.add("vector","gyro_f","/sot/gyro_filter/x_filtered")
robot.subscriber.add("vector","acc_f","/sot/acc_filter/x_filtered")
robot.subscriber.add("vector","com","/sot/dcm_estimator/c")
robot.subscriber.add("vector","vcom","/sot/dcm_estimator/dc")
-robot.subscriber.add("vector","v_torso","/sot/base_estimator/v")
+robot.subscriber.add("vector","v_base","/sot/base_estimator/v")
+robot.subscriber.add("vector","v_gyr","/sot/base_estimator/v_gyr")
+
robot.device.after.addSignal('{0}.position'.format(robot.subscriber.name)) # force recalculation
+robot.device.after.addSignal('{0}.velocity'.format(robot.subscriber.name)) # force recalculation
robot.device.after.addSignal('{0}.com'.format(robot.subscriber.name)) # force recalculation
robot.device.after.addSignal('{0}.vcom'.format(robot.subscriber.name)) # force recalculation
-robot.device.after.addSignal('{0}.v_torso'.format(robot.subscriber.name)) # force recalculation
+robot.device.after.addSignal('{0}.v_base'.format(robot.subscriber.name)) # force recalculation
# robot.device.after.addSignal('{0}.velocity'.format(robot.subscriber.name)) # force recalculation
# robot.device.after.addSignal('{0}.q_est'.format(robot.subscriber.name)) # force recalculation
diff --git a/python/sot_talos_balance/test/test_dcm_estimator.py b/python/sot_talos_balance/test/test_dcm_estimator.py
index f1dc19533f6f36806af80655ddcf1175d356d3f1..fb6650cab5b474106c20a53adeb85753ccb2c4dd 100644
--- a/python/sot_talos_balance/test/test_dcm_estimator.py
+++ b/python/sot_talos_balance/test/test_dcm_estimator.py
@@ -7,7 +7,7 @@ import numpy as np
from IPython import embed
# pub_base = GazeboLinkStatePublisher('base_link',1000)
-pub_torso = GazeboLinkStatePublisher('torso_2_link',1000,local_frame = False)
+pub_torso = GazeboLinkStatePublisher('base_link',1000,local_frame = False)
print("Starting Gazebo link state publisher...")
# pub_base.start()
@@ -18,9 +18,9 @@ raw_input("Wait before running the test")
run_test('appli_dcm_estimator.py')
sleep(2.0)
-runCommandClient('robot.comTrajGen.move(1,-0.025,4.0)')
+runCommandClient('robot.comTrajGen.move(1,-0.025,3.0)')
sleep(5.0)
-runCommandClient('robot.comTrajGen.startSinusoid(1,0.05,8.0)')
+runCommandClient('robot.comTrajGen.startSinusoid(1,0.05,1.0)')
sleep(5.0)
runCommandClient("write_pdf_graph('/tmp/')")
diff --git a/src/base-estimator.cpp b/src/base-estimator.cpp
index 2886f379298e9089cbd85e306bca6c27640cfb7a..afc5a09fd15728465155ccec950d0fb11ccb0d15 100644
--- a/src/base-estimator.cpp
+++ b/src/base-estimator.cpp
@@ -84,14 +84,14 @@ namespace dynamicgraph
void pointRotationByQuaternion(const Eigen::Vector3d & point,const Eigen::Vector4d & quat, Eigen::Vector3d & rotatedPoint)
{
- const Eigen::Vector4d p4(0.0, point(0),point(1),point(2));
- const Eigen::Vector4d quat_conj(quat(0),-quat(1),-quat(2),-quat(3));
- Eigen::Vector4d q_tmp1,q_tmp2;
- quanternionMult(quat,p4,q_tmp1);
- quanternionMult(q_tmp1,quat_conj,q_tmp2);
- rotatedPoint(0) = q_tmp2(1);
- rotatedPoint(1) = q_tmp2(2);
- rotatedPoint(2) = q_tmp2(3);
+ const Eigen::Vector4d p4(0.0, point(0),point(1),point(2));
+ const Eigen::Vector4d quat_conj(quat(0),-quat(1),-quat(2),-quat(3));
+ Eigen::Vector4d q_tmp1,q_tmp2;
+ quanternionMult(quat,p4,q_tmp1);
+ quanternionMult(q_tmp1,quat_conj,q_tmp2);
+ rotatedPoint(0) = q_tmp2(1);
+ rotatedPoint(1) = q_tmp2(2);
+ rotatedPoint(2) = q_tmp2(3);
}
inline
double eulerMean(double a1, double a2)
@@ -104,14 +104,14 @@ namespace dynamicgraph
return mean;
}
- inline
+ inline
double wEulerMean(double a1, double a2, double w1, double w2)
{
double wMean = (a1*w1+ a2*w2)/(w1+w2);
if ( a1-a2 >= EIGEN_PI )
- return (EIGEN_PI*(w1-w2)/(w2+w1) - wMean) < 0 ? -EIGEN_PI + wMean - EIGEN_PI*(w1-w2)/(w2+w1) : EIGEN_PI + wMean - EIGEN_PI*(w1-w2)/(w2+w1);
+ return (EIGEN_PI*(w1-w2)/(w2+w1) - wMean) < 0 ? -EIGEN_PI + wMean - EIGEN_PI*(w1-w2)/(w2+w1) : EIGEN_PI + wMean - EIGEN_PI*(w1-w2)/(w2+w1);
else if ( a1-a2 < -EIGEN_PI )
- return (EIGEN_PI*(w2-w1)/(w1+w2) - wMean) < 0 ? -EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2) : EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2);
+ return (EIGEN_PI*(w2-w1)/(w1+w2) - wMean) < 0 ? -EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2) : EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2);
return wMean;
}
@@ -166,60 +166,60 @@ namespace dynamicgraph
/* ------------------------------------------------------------------- */
BaseEstimator::
BaseEstimator(const std::string& name)
- : Entity(name)
- ,CONSTRUCT_SIGNAL_IN( joint_positions, dynamicgraph::Vector)
- ,CONSTRUCT_SIGNAL_IN( joint_velocities, dynamicgraph::Vector)
- ,CONSTRUCT_SIGNAL_IN( imu_quaternion, dynamicgraph::Vector)
- ,CONSTRUCT_SIGNAL_IN( forceLLEG, dynamicgraph::Vector)
- ,CONSTRUCT_SIGNAL_IN( forceRLEG, dynamicgraph::Vector)
- ,CONSTRUCT_SIGNAL_IN( dforceLLEG, dynamicgraph::Vector)
- ,CONSTRUCT_SIGNAL_IN( dforceRLEG, dynamicgraph::Vector)
- ,CONSTRUCT_SIGNAL_IN( w_lf_in, double)
- ,CONSTRUCT_SIGNAL_IN( w_rf_in, double)
- ,CONSTRUCT_SIGNAL_IN( K_fb_feet_poses, double)
- ,CONSTRUCT_SIGNAL_IN( lf_ref_xyzquat, dynamicgraph::Vector)
- ,CONSTRUCT_SIGNAL_IN( rf_ref_xyzquat, dynamicgraph::Vector)
- ,CONSTRUCT_SIGNAL_IN( accelerometer, dynamicgraph::Vector)
- ,CONSTRUCT_SIGNAL_IN( gyroscope, dynamicgraph::Vector)
- ,CONSTRUCT_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector, joint_positionsSIN
- <<joint_velocitiesSIN)
- ,CONSTRUCT_SIGNAL_OUT(q, dynamicgraph::Vector, m_kinematics_computationsSINNER
- <<joint_positionsSIN
- <<imu_quaternionSIN
- <<forceLLEGSIN
- <<forceRLEGSIN
- <<w_lf_inSIN
- <<w_rf_inSIN
- <<K_fb_feet_posesSIN
- <<w_lf_filteredSOUT
- <<w_rf_filteredSOUT
- <<lf_ref_xyzquatSIN
- <<rf_ref_xyzquatSIN)
- ,CONSTRUCT_SIGNAL_OUT(v, dynamicgraph::Vector, m_kinematics_computationsSINNER << accelerometerSIN << gyroscopeSIN << qSOUT << dforceLLEGSIN << dforceRLEGSIN)
- ,CONSTRUCT_SIGNAL_OUT(v_ac, dynamicgraph::Vector, vSOUT)
- ,CONSTRUCT_SIGNAL_OUT(a_ac, dynamicgraph::Vector, vSOUT)
- ,CONSTRUCT_SIGNAL_OUT(v_flex, dynamicgraph::Vector, vSOUT)
- ,CONSTRUCT_SIGNAL_OUT(v_imu, dynamicgraph::Vector, vSOUT)
- ,CONSTRUCT_SIGNAL_OUT(v_gyr, dynamicgraph::Vector, vSOUT)
- ,CONSTRUCT_SIGNAL_OUT(v_kin, dynamicgraph::Vector, vSOUT)
- ,CONSTRUCT_SIGNAL_OUT(lf_xyzquat, dynamicgraph::Vector, qSOUT)
- ,CONSTRUCT_SIGNAL_OUT(rf_xyzquat, dynamicgraph::Vector, qSOUT)
- ,CONSTRUCT_SIGNAL_OUT(q_lf, dynamicgraph::Vector, qSOUT)
- ,CONSTRUCT_SIGNAL_OUT(q_rf, dynamicgraph::Vector, qSOUT)
- ,CONSTRUCT_SIGNAL_OUT(q_imu, dynamicgraph::Vector, qSOUT<<imu_quaternionSIN)
- ,CONSTRUCT_SIGNAL_OUT(w_lf, double, forceLLEGSIN)
- ,CONSTRUCT_SIGNAL_OUT(w_rf, double, forceRLEGSIN)
- ,CONSTRUCT_SIGNAL_OUT(w_lf_filtered, double, w_lfSOUT)
- ,CONSTRUCT_SIGNAL_OUT(w_rf_filtered, double, w_rfSOUT)
- ,m_initSucceeded(false)
- ,m_reset_foot_pos(true)
- ,m_w_imu(0.0)
- ,m_zmp_std_dev_rf(1.0)
- ,m_zmp_std_dev_lf(1.0)
- ,m_fz_std_dev_rf(1.0)
- ,m_fz_std_dev_lf(1.0)
- ,m_zmp_margin_lf(0.0)
- ,m_zmp_margin_rf(0.0)
+ : Entity(name)
+ ,CONSTRUCT_SIGNAL_IN( joint_positions, dynamicgraph::Vector)
+ ,CONSTRUCT_SIGNAL_IN( joint_velocities, dynamicgraph::Vector)
+ ,CONSTRUCT_SIGNAL_IN( imu_quaternion, dynamicgraph::Vector)
+ ,CONSTRUCT_SIGNAL_IN( forceLLEG, dynamicgraph::Vector)
+ ,CONSTRUCT_SIGNAL_IN( forceRLEG, dynamicgraph::Vector)
+ ,CONSTRUCT_SIGNAL_IN( dforceLLEG, dynamicgraph::Vector)
+ ,CONSTRUCT_SIGNAL_IN( dforceRLEG, dynamicgraph::Vector)
+ ,CONSTRUCT_SIGNAL_IN( w_lf_in, double)
+ ,CONSTRUCT_SIGNAL_IN( w_rf_in, double)
+ ,CONSTRUCT_SIGNAL_IN( K_fb_feet_poses, double)
+ ,CONSTRUCT_SIGNAL_IN( lf_ref_xyzquat, dynamicgraph::Vector)
+ ,CONSTRUCT_SIGNAL_IN( rf_ref_xyzquat, dynamicgraph::Vector)
+ ,CONSTRUCT_SIGNAL_IN( accelerometer, dynamicgraph::Vector)
+ ,CONSTRUCT_SIGNAL_IN( gyroscope, dynamicgraph::Vector)
+ ,CONSTRUCT_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector, joint_positionsSIN
+ <<joint_velocitiesSIN)
+ ,CONSTRUCT_SIGNAL_OUT(q, dynamicgraph::Vector, m_kinematics_computationsSINNER
+ <<joint_positionsSIN
+ <<imu_quaternionSIN
+ <<forceLLEGSIN
+ <<forceRLEGSIN
+ <<w_lf_inSIN
+ <<w_rf_inSIN
+ <<K_fb_feet_posesSIN
+ <<w_lf_filteredSOUT
+ <<w_rf_filteredSOUT
+ <<lf_ref_xyzquatSIN
+ <<rf_ref_xyzquatSIN)
+ ,CONSTRUCT_SIGNAL_OUT(v, dynamicgraph::Vector, m_kinematics_computationsSINNER << accelerometerSIN << gyroscopeSIN << qSOUT << dforceLLEGSIN << dforceRLEGSIN)
+ ,CONSTRUCT_SIGNAL_OUT(v_ac, dynamicgraph::Vector, vSOUT)
+ ,CONSTRUCT_SIGNAL_OUT(a_ac, dynamicgraph::Vector, vSOUT)
+ ,CONSTRUCT_SIGNAL_OUT(v_flex, dynamicgraph::Vector, vSOUT)
+ ,CONSTRUCT_SIGNAL_OUT(v_imu, dynamicgraph::Vector, vSOUT)
+ ,CONSTRUCT_SIGNAL_OUT(v_gyr, dynamicgraph::Vector, vSOUT)
+ ,CONSTRUCT_SIGNAL_OUT(v_kin, dynamicgraph::Vector, vSOUT)
+ ,CONSTRUCT_SIGNAL_OUT(lf_xyzquat, dynamicgraph::Vector, qSOUT)
+ ,CONSTRUCT_SIGNAL_OUT(rf_xyzquat, dynamicgraph::Vector, qSOUT)
+ ,CONSTRUCT_SIGNAL_OUT(q_lf, dynamicgraph::Vector, qSOUT)
+ ,CONSTRUCT_SIGNAL_OUT(q_rf, dynamicgraph::Vector, qSOUT)
+ ,CONSTRUCT_SIGNAL_OUT(q_imu, dynamicgraph::Vector, qSOUT<<imu_quaternionSIN)
+ ,CONSTRUCT_SIGNAL_OUT(w_lf, double, forceLLEGSIN)
+ ,CONSTRUCT_SIGNAL_OUT(w_rf, double, forceRLEGSIN)
+ ,CONSTRUCT_SIGNAL_OUT(w_lf_filtered, double, w_lfSOUT)
+ ,CONSTRUCT_SIGNAL_OUT(w_rf_filtered, double, w_rfSOUT)
+ ,m_initSucceeded(false)
+ ,m_reset_foot_pos(true)
+ ,m_w_imu(0.0)
+ ,m_zmp_std_dev_rf(1.0)
+ ,m_zmp_std_dev_lf(1.0)
+ ,m_fz_std_dev_rf(1.0)
+ ,m_fz_std_dev_lf(1.0)
+ ,m_zmp_margin_lf(0.0)
+ ,m_zmp_margin_rf(0.0)
{
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
@@ -237,9 +237,9 @@ namespace dynamicgraph
addCommand("set_fz_stable_windows_size",
- makeCommandVoid1(*this, &BaseEstimator::set_fz_stable_windows_size,
- docCommandVoid1("Set the windows size used to detect that feet is stable",
- "int")));
+ makeCommandVoid1(*this, &BaseEstimator::set_fz_stable_windows_size,
+ docCommandVoid1("Set the windows size used to detect that feet is stable",
+ "int")));
addCommand("set_alpha_w_filter",
makeCommandVoid1(*this, &BaseEstimator::set_alpha_w_filter,
docCommandVoid1("Set the filter parameter to filter weights",
@@ -342,7 +342,7 @@ namespace dynamicgraph
m_IMU_frame_id = m_model.getFrameId(m_robot_util->m_imu_joint_name);
m_torso_id = m_model.frames[m_IMU_frame_id].parent;
m_torsoMimu = m_model.frames[m_IMU_frame_id].placement;
- std::cerr << "IMU in Torso" << m_torsoMimu << std::endl;
+ std::cerr << "IMU in Torso" << m_torsoMimu << std::endl;
m_q_pin.setZero(m_model.nq);
m_q_pin[6]= 1.; // for quaternion
@@ -382,14 +382,14 @@ namespace dynamicgraph
void BaseEstimator::set_fz_stable_windows_size(const int& ws)
{
- if(ws<0.0)
+ if(ws<0.0)
return SEND_MSG("windows_size should be a positive integer", MSG_TYPE_ERROR);
m_fz_stable_windows_size = ws;
}
void BaseEstimator::set_alpha_w_filter(const double& a)
{
- if(a<0.0 || a>1.0)
+ if(a<0.0 || a>1.0)
return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
m_alpha_w_filter = a;
}
@@ -397,14 +397,14 @@ namespace dynamicgraph
void BaseEstimator::set_alpha_DC_acc(const double& a)
{
- if(a<0.0 || a>1.0)
+ if(a<0.0 || a>1.0)
return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
m_alpha_DC_acc = a;
}
void BaseEstimator::set_alpha_DC_vel(const double& a)
{
- if(a<0.0 || a>1.0)
+ if(a<0.0 || a>1.0)
return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
m_alpha_DC_vel = a;
}
@@ -682,7 +682,7 @@ namespace dynamicgraph
{
SEND_WARNING_STREAM_MSG("The robot is flying!"+
("- forceRLEG: "+toString(ftrf.transpose()))+
- "- forceLLEG: "+toString(ftlf.transpose())+
+ "- forceLLEG: "+toString(ftlf.transpose())+
"- m_right_foot_is_stable: "+toString(m_right_foot_is_stable)+
"- m_left_foot_is_stable: "+toString(m_left_foot_is_stable));
wR = 1e-3;
@@ -701,38 +701,38 @@ namespace dynamicgraph
kinematics_estimation(ftlf, m_K_lf, m_oMlfs, m_left_foot_id, m_oMff_lf, oMlfa, lfsMff);
// get rpy
- const SE3 ffMchest(m_data->oMi[m_torso_id]); // transform between freeflyer and body attached to IMU sensor
- const SE3 chestMff(ffMchest.inverse()); // transform between body attached to IMU sensor and freeflyer
+ const SE3 ffMtorso(m_data->oMi[m_torso_id]); // transform between freeflyer and body attached to IMU sensor (torso)
+ const SE3 torsoMff(ffMtorso.inverse()); // transform between body attached to IMU sensor (torso) and freeflyer
- Vector3 rpy_chest, rpy_chest_lf, rpy_chest_rf, rpy_chest_imu; // orientation of the body which imu is attached to. (fusion, from left kine, from right kine, from imu)
+ Vector3 rpy_torso, rpy_torso_lf, rpy_torso_rf, rpy_torso_imu; // orientation of the body which imu is attached to. (fusion, from left kine, from right kine, from imu)
- matrixToRpy((m_oMff_lf*ffMchest).rotation(), rpy_chest_lf);
- matrixToRpy((m_oMff_rf*ffMchest).rotation(), rpy_chest_rf);
+ matrixToRpy((m_oMff_lf*ffMtorso).rotation(), rpy_torso_lf);
+ matrixToRpy((m_oMff_rf*ffMtorso).rotation(), rpy_torso_rf);
Eigen::Quaterniond quatIMU(quatIMU_vec[0], quatIMU_vec[1], quatIMU_vec[2], quatIMU_vec[3]);
- Eigen::Quaterniond quat_chestMimu(m_torsoMimu.rotation()); // transform between chest and IMU
- Eigen::Quaterniond quat_chest_imu(quatIMU*quat_chestMimu); // quatIMU expressed in chest frame
- //Eigen::Quaterniond quat_chest_imu(quatIMU); // quatIMU expressed in chest frame
- matrixToRpy(quat_chest_imu.toRotationMatrix(), rpy_chest_imu);
+ Eigen::Quaterniond quat_torsoMimu(m_torsoMimu.rotation()); // transform between torso and IMU
+ Eigen::Quaterniond quat_torso_imu(quatIMU*quat_torsoMimu); // quatIMU expressed in torso frame
+ //Eigen::Quaterniond quat_torso_imu(quatIMU); // quatIMU expressed in torso frame
+ matrixToRpy(quat_torso_imu.toRotationMatrix(), rpy_torso_imu);
// average (we do not take into account the IMU yaw)
double wSum = wL + wR + m_w_imu;
- rpy_chest[0] = wEulerMean(rpy_chest_lf[0], rpy_chest_rf[0], wL, wR);
- rpy_chest[0] = wEulerMean(rpy_chest[0], rpy_chest_imu[0], wL+wR, m_w_imu);
- //rpy_chest[0] = eulerMean(rpy_chest_lf[0], rpy_chest_rf[0]);
- //eulerMean(rpy_chest[0], rpy_chest_imu[0], rpy_chest(0))
- rpy_chest[1] = wEulerMean(rpy_chest_lf[1], rpy_chest_rf[1], wL, wR);
- rpy_chest[1] = wEulerMean(rpy_chest[1], rpy_chest_imu[1], wL+wR, m_w_imu);
- //rpy_chest[1] = eulerMean(rpy_chest_lf[1], rpy_chest_rf[1]);
- //eulerMean(rpy_chest[1], rpy_chest_imu[1], rpy_chest(1))
- //rpy_chest(0) = (rpy_chest_lf[0]*wL + rpy_chest_rf[0]*wR + rpy_chest_imu[0]*m_w_imu) / wSum;
- //rpy_chest(1) = (rpy_chest_lf[1]*wL + rpy_chest_rf[1]*wR + rpy_chest_imu[1]*m_w_imu) / wSum;
- rpy_chest[2] = wEulerMean(rpy_chest_lf[2], rpy_chest_rf[2], wL, wR);
- //rpy_chest[2] = wEulerMean(rpy_chest[2], rpy_chest_imu[2], wL+wR, m_w_imu);
- //rpy_chest[2] = eulerMean(rpy_chest_lf[2], rpy_chest_rf[2]);
- //rpy_chest(2) = (rpy_chest_lf[2]*wL + rpy_chest_rf[2]*wR ) / (wL+wR);
-
- rpyToMatrix(rpy_chest, m_oRchest);
- m_oRff = m_oRchest * chestMff.rotation();
+ rpy_torso[0] = wEulerMean(rpy_torso_lf[0], rpy_torso_rf[0], wL, wR);
+ rpy_torso[0] = wEulerMean(rpy_torso[0], rpy_torso_imu[0], wL+wR, m_w_imu);
+ //rpy_torso[0] = eulerMean(rpy_torso_lf[0], rpy_torso_rf[0]);
+ //eulerMean(rpy_torso[0], rpy_torso_imu[0], rpy_torso(0))
+ rpy_torso[1] = wEulerMean(rpy_torso_lf[1], rpy_torso_rf[1], wL, wR);
+ rpy_torso[1] = wEulerMean(rpy_torso[1], rpy_torso_imu[1], wL+wR, m_w_imu);
+ //rpy_torso[1] = eulerMean(rpy_torso_lf[1], rpy_torso_rf[1]);
+ //eulerMean(rpy_torso[1], rpy_torso_imu[1], rpy_torso(1))
+ //rpy_torso(0) = (rpy_torso_lf[0]*wL + rpy_torso_rf[0]*wR + rpy_torso_imu[0]*m_w_imu) / wSum;
+ //rpy_torso(1) = (rpy_torso_lf[1]*wL + rpy_torso_rf[1]*wR + rpy_torso_imu[1]*m_w_imu) / wSum;
+ rpy_torso[2] = wEulerMean(rpy_torso_lf[2], rpy_torso_rf[2], wL, wR);
+ //rpy_torso[2] = wEulerMean(rpy_torso[2], rpy_torso_imu[2], wL+wR, m_w_imu);
+ //rpy_torso[2] = eulerMean(rpy_torso_lf[2], rpy_torso_rf[2]);
+ //rpy_torso(2) = (rpy_torso_lf[2]*wL + rpy_torso_rf[2]*wR ) / (wL+wR);
+
+ rpyToMatrix(rpy_torso, m_oRtorso);
+ m_oRff = m_oRtorso * torsoMff.rotation();
// translation to get foot at the right position
// evaluate Mrl Mlf for q with the good orientation and zero translation for freeflyer
@@ -919,15 +919,16 @@ namespace dynamicgraph
const Eigen::VectorXd & q = qSOUT(iter);
s.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints);
-
+ const SE3 ffMtorso(m_data->oMi[m_torso_id]); // transform between freeflyer and body attached to IMU sensor (torso)
+ const SE3 torsoMff(ffMtorso.inverse()); // transform between body attached to IMU sensor (torso) and freeflyer
const Eigen::Vector4d & quatIMU_vec = imu_quaternionSIN(iter);
//Eigen::Quaternion<double> quatIMU(quatIMU_vec);
Eigen::Quaterniond quatIMU(quatIMU_vec[0], quatIMU_vec[1], quatIMU_vec[2], quatIMU_vec[3]);
- Eigen::Quaterniond quat_torsoMimu(m_torsoMimu.rotation());
+ Eigen::Quaterniond quat_torsoMimu(m_torsoMimu.rotation());
Eigen::Quaterniond quat_torso_imu(quatIMU*quat_torsoMimu);
-
- base_se3_to_sot(q.head<3>(), quat_torso_imu.toRotationMatrix(), s.head<6>());
- //matrixToRpy(quat_chest_imu.toRotationMatrix(), rpy_chest_imu);
+ //m_oRff = quat_torso_imu.toRotationMatrix() * torsoMff.rotation();
+ base_se3_to_sot(q.head<3>(), m_oRff, s.head<6>());
+ //matrixToRpy(quat_torso_imu.toRotationMatrix(), rpy_torso_imu);
return s;
}
@@ -949,17 +950,17 @@ namespace dynamicgraph
double w_fz = compute_force_weight(wrench(2), m_fz_std_dev_lf, m_fz_margin_lf);
//check that foot is sensing a force greater than the margin treshold for more than 'm_fz_stable_windows_size' samples
if (wrench(2) > m_fz_margin_lf)
- m_lf_fz_stable_cpt++;
+ m_lf_fz_stable_cpt++;
else m_lf_fz_stable_cpt = 0;
if (m_lf_fz_stable_cpt >= m_fz_stable_windows_size)
{
- m_lf_fz_stable_cpt = m_fz_stable_windows_size;
- m_left_foot_is_stable = true;
+ m_lf_fz_stable_cpt = m_fz_stable_windows_size;
+ m_left_foot_is_stable = true;
}
else
{
- m_left_foot_is_stable = false;
+ m_left_foot_is_stable = false;
}
s = w_zmp*w_fz;
return s;
@@ -1097,17 +1098,17 @@ namespace dynamicgraph
Vector6 v_flex_l;
Vector6 v_flex_r;
v_flex_l << -dftlf[0]/m_K_lf(0), -dftlf[1]/m_K_lf(1), -dftlf[2]/m_K_lf(2),
- -dftlf[3]/m_K_lf(3), -dftlf[4]/m_K_lf(4), -dftlf[5]/m_K_lf(5);
+ -dftlf[3]/m_K_lf(3), -dftlf[4]/m_K_lf(4), -dftlf[5]/m_K_lf(5);
v_flex_r << -dftrf[0]/m_K_rf(0), -dftrf[1]/m_K_rf(1), -dftrf[2]/m_K_rf(2),
- -dftrf[3]/m_K_rf(3), -dftrf[4]/m_K_rf(4), -dftrf[5]/m_K_rf(5);
+ -dftrf[3]/m_K_rf(3), -dftrf[4]/m_K_rf(4), -dftrf[5]/m_K_rf(5);
const Eigen::Matrix<double, 6, 6> lfAff = ffMlf.inverse().toActionMatrix();
const Eigen::Matrix<double, 6, 6> rfAff = ffMrf.inverse().toActionMatrix();
Eigen::Matrix<double, 12, 6> A;
A << lfAff,
- rfAff;
+ rfAff;
Eigen::Matrix<double, 12, 1> b;
b << v_flex_l,
- v_flex_r;
+ v_flex_r;
//~ m_v_flex.head<6>() = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
m_v_flex.head<6>() = (A.transpose() * A).ldlt().solve(A.transpose() * b);
m_v_flex.head<3>() = m_oRff * m_v_flex.head<3>();
@@ -1116,24 +1117,27 @@ namespace dynamicgraph
/* Get an estimate of linear velocities from gyroscope only*/
// we make the asumtion than we are 'turning around the foot' with pure angular velocity in the ankle measured by the gyro
- const Matrix3 ffRimu = (m_data->oMf[m_torso_id]).rotation();
+ const Matrix3 ffRtorso = (m_data->oMi[m_torso_id]).rotation();
+ const SE3 ffMtorso(m_data->oMi[m_torso_id]); // transform between freeflyer and body attached to IMU sensor
+ const Matrix3 ffRimu = (ffMtorso * m_torsoMimu).rotation();
//std::cerr << m_data->oMf[m_torso_id] << std::endl;
//std::cerr << m_data->liMi[m_torso_id] << std::endl;
//std::cerr << "m_torso_id" << m_torso_id << std::endl;
const Matrix3 lfRimu = ffMlf.rotation().transpose() * ffRimu;
const Matrix3 rfRimu = ffMrf.rotation().transpose() * ffRimu;
- //const SE3 chestMimu(Matrix3::Identity(), Vector3(-0.13, 0.0, 0.118)); ///TODO Read this transform from setable parameter /// TODO chesk the sign of the translation
- const SE3 ffMchest(m_data->oMf[m_torso_id]);
- const SE3 imuMff = (ffMchest * m_torsoMimu).inverse();
+ //const SE3 torsoMimu(Matrix3::Identity(), Vector3(-0.13, 0.0, 0.118)); ///TODO Read this transform from setable parameter /// TODO chesk the sign of the translation
+ //const SE3 ffMtorso(m_data->oMf[m_torso_id]);
+ const SE3 imuMff = (ffMtorso * m_torsoMimu).inverse();
//gVw_a = gVo_g + gHa.act(aVb_a)-gVb_g //angular velocity in the ankle from gyro and d_enc
const Frame & f_imu = m_model.frames[m_torso_id];
- Vector3 gVo_a_l = Vector3(gyr_imu(0),gyr_imu(1),gyr_imu(2)) + (imuMff*ffMlf).act(v_lf_local).angular() - m_data->v[f_imu.parent].angular();
- Vector3 gVo_a_r = Vector3(gyr_imu(0),gyr_imu(1),gyr_imu(2)) + (imuMff*ffMrf).act(v_rf_local).angular() - m_data->v[f_imu.parent].angular();
+ //const Frame & f_imu = m_model.frames[m_IMU_frame_id];
+ Vector3 gVo_a_l = Vector3(gyr_imu(0),gyr_imu(1),gyr_imu(2)); //+ (imuMff*ffMlf).act(v_lf_local).angular() - m_data->v[f_imu.parent].angular();
+ Vector3 gVo_a_r = Vector3(gyr_imu(0),gyr_imu(1),gyr_imu(2)); //+ (imuMff*ffMrf).act(v_rf_local).angular() - m_data->v[f_imu.parent].angular();
Motion v_gyr_ankle_l( Vector3(0.,0.,0.), lfRimu * gVo_a_l);
Motion v_gyr_ankle_r( Vector3(0.,0.,0.), rfRimu * gVo_a_r);
- Vector6 v_gyr_l = -ffMlf.inverse().act(v_gyr_ankle_l).toVector();
- Vector6 v_gyr_r = -ffMrf.inverse().act(v_gyr_ankle_r).toVector();
+ Vector6 v_gyr_l = ffMlf.inverse().actInv(v_gyr_ankle_l).toVector();
+ Vector6 v_gyr_r = ffMrf.inverse().actInv(v_gyr_ankle_r).toVector();
m_v_gyr.head<6>() = (wL*v_gyr_l + wR*v_gyr_r)/(wL+wR);
@@ -1141,7 +1145,7 @@ namespace dynamicgraph
/* rotate acceleration to express it in the world frame */
//~ pointRotationByQuaternion(acc_imu,quatIMU_vec,acc_world); //this is false because yaw from imuquat is drifting
- const Vector3 acc_world = m_oRchest * acc_imu;
+ const Vector3 acc_world = m_oRtorso * acc_imu;
/* now, the acceleration is expressed in the world frame,
* so it can be written as the sum of the proper acceleration and the
@@ -1173,22 +1177,22 @@ namespace dynamicgraph
m_v_ac = ACvel;
/* Express back velocity in the imu frame to get a full 6d velocity with the gyro*/
- const Vector3 imuVimu = m_oRchest.transpose() * ACvel;
+ const Vector3 imuVimu = m_oRtorso.transpose() * ACvel;
/* Here we could remove dc from gyrometer to remove bias*/ ///TODO
const Motion imuWimu(imuVimu,gyr_imu);
//const Frame & f_imu = m_model.frames[m_torso_id];
- const Motion ffWchest = m_data->v[f_imu.parent];
- //const SE3 ffMchest(m_data->oMf[m_torso_id]);
- //const SE3 chestMimu(Matrix3::Identity(), +1.0*Vector3(-0.13, 0.0, 0.118)); ///TODO Read this transform from setable parameter /// TODO chesk the sign of the translation
- const SE3 ffMimu = ffMchest * m_torsoMimu;
- const Motion v= ffMimu.act(imuWimu) ;//- ffWchest;
+ const Motion ffWtorso = m_data->v[f_imu.parent];
+ //const SE3 ffMtorso(m_data->oMf[m_torso_id]);
+ //const SE3 torsoMimu(Matrix3::Identity(), +1.0*Vector3(-0.13, 0.0, 0.118)); ///TODO Read this transform from setable parameter /// TODO chesk the sign of the translation
+ const SE3 ffMimu = ffMtorso * m_torsoMimu;
+ const Motion v= ffMimu.act(imuWimu) ;//- ffWtorso;
m_v_imu.head<6>() = v.toVector();
- m_v_imu.head<3>() = m_oRff * m_v_imu.head<3>();
+ //m_v_imu.head<3>() = m_oRff * m_v_imu.head<3>();
- //~ m_v_sot.head<6>() = m_v_kin.head<6>();
+ m_v_sot.head<6>() = m_v_kin.head<6>();
//~ m_v_sot.head<6>() = m_v_flex.head<6>() + m_v_kin.head<6>();
- m_v_sot.head<6>() = m_v_gyr.head<6>() + m_v_kin.head<6>();
+// m_v_sot.head<6>() = m_v_gyr.head<6>() + m_v_kin.head<6>();
// m_v_sot.head<6>() = m_v_gyr.head<6>();
//~ m_v_sot.head<6>() = m_v_imu.head<6>();