From 54be43a8de3b3474c8c1715d4403f41e678d8fe6 Mon Sep 17 00:00:00 2001
From: fbailly <fbailly@laas.fr>
Date: Wed, 23 Jan 2019 11:18:26 +0100
Subject: [PATCH] [basic base estimator] using kinematic fusion of the two legs
with unit weights
---
include/sot/talos_balance/base-estimator.hh | 5 ++-
.../talos/base_estimator_conf.py | 10 +++--
.../test/test_dcm_estimator.py | 38 ++-----------------
src/base-estimator.cpp | 36 +++++++++++++++---
4 files changed, 45 insertions(+), 44 deletions(-)
diff --git a/include/sot/talos_balance/base-estimator.hh b/include/sot/talos_balance/base-estimator.hh
index dccb8fe..ec1a213 100644
--- a/include/sot/talos_balance/base-estimator.hh
+++ b/include/sot/talos_balance/base-estimator.hh
@@ -77,6 +77,9 @@ namespace dynamicgraph {
/** Rotate a point or a vector by a quaternion stored in (w,x,y,z) format */
void pointRotationByQuaternion(const Eigen::Vector3d & point,const Eigen::Vector4d & quat, Eigen::Vector3d & rotatedPoint);
+ /** Avoids singularity while taking the mean of euler angles**/
+ double eulerMean(double a1, double a2);
+
class SOTBASEESTIMATOR_EXPORT BaseEstimator
:public::dynamicgraph::Entity
{
@@ -185,7 +188,6 @@ namespace dynamicgraph {
int m_fz_stable_windows_size; /// size of the windows used to detect that feet did not leave the ground
int m_lf_fz_stable_cpt; ///counter for detecting for how long the feet has been stable
int m_rf_fz_stable_cpt; ///counter for detecting for how long the feet has been stable
-
/* Estimator parameters */
double m_w_imu; /// weight of IMU for sensor fusion
double m_zmp_std_dev_rf; /// standard deviation of ZMP measurement errors for right foot
@@ -219,6 +221,7 @@ namespace dynamicgraph {
se3::Model m_model; /// Pinocchio robot model
se3::Data *m_data; /// Pinocchio robot data
+ //se3::SE3 m_chestMimu /// chest to imu transformation
se3::SE3 m_oMff_lf; /// world-to-base transformation obtained through left foot
se3::SE3 m_oMff_rf; /// world-to-base transformation obtained through right foot
SE3 m_oMlfs; /// transformation from world to left foot sole
diff --git a/python/sot_talos_balance/talos/base_estimator_conf.py b/python/sot_talos_balance/talos/base_estimator_conf.py
index 7152412..24efe1f 100644
--- a/python/sot_talos_balance/talos/base_estimator_conf.py
+++ b/python/sot_talos_balance/talos/base_estimator_conf.py
@@ -1,14 +1,16 @@
''' *********************** USER-PARAMETERS OF BASE ESTIMATOR *********************** '''
-K = (4034, 23770, 239018, 707, 502, 936);
+# K = (4034, 23770, 239018, 707, 502, 936); #HRP2
+# K = (1., 1., 1., 1., 1., 1.);
+K = (1e6, 1e6, 1e6, 1e6, 1e6, 1e6);
std_dev_zmp = 0.02
std_dev_fz = 50.
normal_force_margin = 30.
zmp_margin = 0.002
-w_imu = 0.;
+w_imu = 0.0;
beta = 0.00329
K_fb_feet_poses = 0.0; # gain used for updating foot positions
RIGHT_FOOT_SIZES = (0.130, -0.100, 0.056, -0.075); # pos x, neg x, pos y, neg y size
LEFT_FOOT_SIZES = (0.130, -0.100, 0.075, -0.056); # pos x, neg x, pos y, neg y size
-w_lf_in = 1.0
-w_rf_in = 1.0
+w_lf_in = 1.
+w_rf_in = 1.
#mu = 0.3; # force friction coefficient
diff --git a/python/sot_talos_balance/test/test_dcm_estimator.py b/python/sot_talos_balance/test/test_dcm_estimator.py
index a584712..534a938 100644
--- a/python/sot_talos_balance/test/test_dcm_estimator.py
+++ b/python/sot_talos_balance/test/test_dcm_estimator.py
@@ -11,6 +11,7 @@ from IPython import embed
import os
import numpy as np
import matplotlib.pyplot as plt
+from dynamic_graph.ros import RosPublish
def main(robot):
dt = robot.timeStep;
@@ -55,47 +56,16 @@ def main(robot):
robot.be_filters = create_be_filters(robot, dt)
robot.dcm_estimator = create_dcm_estimator(robot, dt)
- # --- TRACERS
- outputs = ['robotState']
- robot.device_tracer = create_tracer(robot,robot.device, 'device_tracer', outputs)
- outputs = ['q']
- robot.base_estimator_tracer = create_tracer(robot,robot.base_estimator, 'base_estimator_tracer', outputs)
- robot.device.after.addSignal('base_estimator.q')
- outputs = ['c']
- robot.dcm_estimator_tracer = create_tracer(robot,robot.dcm_estimator,'dcm_estimator_tracer', outputs)
- robot.device.after.addSignal('dcm_estimator.c')
-
+
# --- RUN SIMULATION
plug(robot.comTrajGen.x, robot.taskCom.featureDes.errorIN);
sleep(1.0);
os.system("rosservice call \start_dynamic_graph")
sleep(2.0);
- robot.comTrajGen.move(1,-0.025,4.0);
+ robot.comTrajGen.move(1,0.025,4.0);
sleep(5.0);
- robot.comTrajGen.startSinusoid(1,0.05,8.0);
+ robot.comTrajGen.startSinusoid(1,-0.05,8.0);
sleep(0.2);
-
- robot.device_tracer.start();
- robot.base_estimator_tracer.start();
- robot.dcm_estimator_tracer.start();
- sleep(1.0);
- dump_tracer(robot.device_tracer);
- dump_tracer(robot.base_estimator_tracer);
- dump_tracer(robot.dcm_estimator_tracer);
-
- print 'data dumped'
-
-
- # --- DISPLAY
- device_data, device_name = read_tracer_file('/tmp/dg_'+robot.device.name+'-robotState.dat')
- base_estimator_data, base_name = read_tracer_file('/tmp/dg_'+robot.base_estimator.name+'-q.dat')
- dcm_estimator_data, dcm_name = read_tracer_file('/tmp/dg_'+robot.dcm_estimator.name+'-c.dat')
-
- # plot_select_traj(device_data,[10,23,15], device_name)
- # plot_select_traj(base_estimator_data,[np.linspace(1,7,7,dtype=int)], base_name)
- # plot_select_traj(dcm_estimator_data,[0,1,2], dcm_name)
-
- # plt.show()
write_pdf_graph('/tmp/')
diff --git a/src/base-estimator.cpp b/src/base-estimator.cpp
index 628921e..88c013d 100644
--- a/src/base-estimator.cpp
+++ b/src/base-estimator.cpp
@@ -93,6 +93,21 @@ namespace dynamicgraph
rotatedPoint(1) = q_tmp2(2);
rotatedPoint(2) = q_tmp2(3);
}
+ inline
+ double eulerMean(double a1, double a2)
+ {
+ double res = 0;
+ double mean = (a1+a2)/2.;
+ if( (a1- a2) > EIGEN_PI || (a1- a2) < -EIGEN_PI)
+ {
+ res = mean > 0 ? -EIGEN_PI + mean : EIGEN_PI + mean ;
+ }
+ else
+ {
+ res = mean;
+ }
+ return res;
+ }
#define PROFILE_BASE_POSITION_ESTIMATION "base-est position estimation"
#define PROFILE_BASE_VELOCITY_ESTIMATION "base-est velocity estimation"
@@ -176,8 +191,8 @@ namespace dynamicgraph
{
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
- m_K_rf << 4034,23770,239018,707,502,936;
- m_K_lf << 4034,23770,239018,707,502,936;
+ //m_K_rf << 4034,23770,239018,707,502,936;
+ //m_K_lf << 4034,23770,239018,707,502,936;
m_left_foot_sizes << 0.130, -0.100, 0.075, -0.056;
m_right_foot_sizes << 0.130, -0.100, 0.056, -0.075;
@@ -659,13 +674,21 @@ namespace dynamicgraph
matrixToRpy((m_oMff_lf*ffMchest).rotation(), rpy_chest_lf);
matrixToRpy((m_oMff_rf*ffMchest).rotation(), rpy_chest_rf);
Eigen::Quaternion<double> quatIMU(quatIMU_vec[0], quatIMU_vec[1], quatIMU_vec[2], quatIMU_vec[3]);
+ //TODO turn the quaternion to express it in the chest frame
+ // m_imuMbase = SE3::Identity();
+ // m_imuMbase.rotation
matrixToRpy(quatIMU.toRotationMatrix(), rpy_chest_imu);
// average (we do not take into account the IMU yaw)
double wSum = wL + wR + m_w_imu;
- 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) = (rpy_chest_lf[2]*wL + rpy_chest_rf[2]*wR ) / (wL+wR);
+ 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] = 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] = 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();
@@ -1048,6 +1071,9 @@ 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_IMU_body_id]).rotation();
+ //std::cerr << m_data->oMf[m_IMU_body_id] << std::endl;
+ //std::cerr << m_data->liMi[m_IMU_body_id] << std::endl;
+ //std::cerr << "m_IMU_body_id" << m_IMU_body_id << std::endl;
const Matrix3 lfRimu = ffMlf.rotation().transpose() * ffRimu;
const Matrix3 rfRimu = ffMrf.rotation().transpose() * ffRimu;
--
GitLab