Skip to content
GitLab
Commit 59835488 authored by andreadelprete's avatar andreadelprete
Browse files

[base-estimator] Fix a couple of bugs in estimation of base velocity and... 

[base-estimator] Fix a couple of bugs in estimation of base velocity and update of foot position during single-support phases
parent 8a58c67e
Hide whitespace changes
Inline Side-by-side
python/dynamic_graph/sot/torque_control/tests/test_balance_ctrl_openhrp.py
View file @ 59835488
......@@ -67,6 +67,7 @@ def test_balance_ctrl_openhrp(robot, use_real_vel=True, use_real_base_state=Fals
robot.dq.selec(6, NJ+6);
plug(robot.dq.sout, robot.pos_ctrl.jointsVelocities);
plug(robot.dq.sout, robot.base_estimator.joint_velocities);
plug(robot.device.gyrometer, robot.base_estimator.gyroscope);
# BYPASS BASE ESTIMATOR
robot.v = Selec_of_vector("v");
......
scripts/python_commands.txt
View file @ 59835488
......@@ -4,19 +4,19 @@ robot.timeStep = 0.002;
ent = test_balance_ctrl_openhrp(robot, use_real_vel=True, use_real_base_state=True);
robot.ctrl_manager.setStreamPrintPeriod(0.5)
create_topic(robot.ros, robot.filters.estimator_kin.dx, 'dq_est');
create_topic(robot.ros, robot.dq.sout, 'dq');
create_topic(robot.ros, robot.filters.estimator_kin.dx, 'dq_est', robot, robot.filters.estimator_kin);
create_topic(robot.ros, robot.dq.sout, 'dq', robot, robot.dq);
create_topic(robot.ros, robot.inv_dyn.com_ref_pos, 'com_ref_pos');
create_topic(robot.ros, robot.inv_dyn.com_ref_vel, 'com_ref_vel');
create_topic(robot.ros, robot.inv_dyn.com, 'com');
create_topic(robot.ros, robot.inv_dyn.com_vel, 'com_vel');
create_topic(robot.ros, robot.inv_dyn.com_acc, 'com_acc');
create_topic(robot.ros, robot.inv_dyn.com_acc_des, 'com_acc_des');
create_topic(robot.ros, robot.base_estimator.q, 'q_est');
create_topic(robot.ros, robot.base_estimator.q, 'q_est', robot, robot.base_estimator);
create_topic(robot.ros, robot.q.sout, 'q');
create_topic(robot.ros, robot.traj_gen.q, 'q_ref');
create_topic(robot.ros, robot.device.robotVelocity, 'v');
create_topic(robot.ros, robot.base_estimator.v, 'v_est');
create_topic(robot.ros, robot.base_estimator.v, 'v_est', robot, robot.base_estimator);
create_topic(robot.ros, robot.inv_dyn.zmp_des, 'zmp_des');
create_topic(robot.ros, robot.inv_dyn.zmp_ref, 'zmp_ref');
create_topic(robot.ros, robot.inv_dyn.zmp, 'zmp');
......@@ -28,13 +28,13 @@ create_topic(robot.ros, robot.inv_dyn.zmp_left_foot, 'zmp_left_foot');
create_topic(robot.ros, robot.inv_dyn.zmp_right_foot, 'zmp_right_foot');
create_topic(robot.ros, robot.device.forceLLEG, 'forceLLEG');
create_topic(robot.ros, robot.device.forceRLEG, 'forceRLEG');
create_topic(robot.ros, robot.estimator_ft.contactWrenchLeftSole, 'wrenchLeftSole');
#create_topic(robot.ros, robot.estimator_ft.contactWrenchLeftSole, 'wrenchLeftSole');
create_topic(robot.ros, robot.device.gyrometer, 'gyro');
create_topic(robot.ros, robot.device.accelerometer, 'acc');
create_topic(robot.ros, robot.base_estimator.w_lf, 'w_lf', data_type='double');
create_topic(robot.ros, robot.base_estimator.w_rf, 'w_rf', data_type='double');
create_topic(robot.ros, robot.base_estimator.w_lf_filtered, 'w_lf_filtered', data_type='double');
create_topic(robot.ros, robot.base_estimator.w_rf_filtered, 'w_rf_filtered', data_type='double');
create_topic(robot.ros, robot.base_estimator.w_lf, 'w_lf', robot, robot.base_estimator, data_type='double');
create_topic(robot.ros, robot.base_estimator.w_rf, 'w_rf', robot, robot.base_estimator, data_type='double');
create_topic(robot.ros, robot.base_estimator.w_lf_filtered, 'w_lf_filtered', robot, robot.base_estimator, data_type='double');
create_topic(robot.ros, robot.base_estimator.w_rf_filtered, 'w_rf_filtered', robot, robot.base_estimator, data_type='double');
#create_topic(robot.ros, robot.torque_ctrl.dq_motor, 'dq_motor');
#create_topic(robot.ros, robot.torque_ctrl.jointsAccelerationsDesired, 'ddq_des');
create_topic(robot.ros, robot.rf_traj_gen.x, 'rf_pose_ref');
......@@ -50,6 +50,10 @@ create_topic(robot.ros, robot.inv_dyn.left_foot_acc_des, 'lf_acc_des');
#robot.torque_ctrl.reset_ddq_integral();
ent.ctrl_manager.setCtrlMode('all','torque')
robot.base_estimator.set_imu_weight(1.0);
plug(robot.encoders.sout, robot.base_estimator.joint_positions)
#plug(robot.device.gyrometer, robot.base_estimator.gyroscope);
plug(robot.filters.estimator_kin.dx, robot.base_estimator.joint_velocities);
plug(robot.filters.gyro_filter.x_filtered, robot.base_estimator.gyroscope);
SOT_TORQUE_INSTALL_DIR = '/home/adelpret/devel/openrobots/src/sot-torque-control'
......@@ -60,6 +64,9 @@ robot.lf_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/lf_spl
robot.traj_sync.out.value = 1 #turnOn()
plug(robot.base_estimator.q, robot.inv_dyn.q);
plug(robot.base_estimator.v, robot.inv_dyn.v);
sleep(5.0)
robot.com_traj_gen.move(1, 0.05, 1.5)
sleep(2.0*7)
......
src/base-estimator.cpp
View file @ 59835488
......@@ -687,7 +687,7 @@ namespace dynamicgraph
if(m_K_fb_feet_posesSIN.isPlugged())
{
const double K_fb = m_K_fb_feet_posesSIN(iter);
if (K_fb > 0.0)
if (K_fb > 0.0 && m_w_imu > 0.0)
{
assert(m_w_imu > 0.0 && "Update of the feet 6d poses should not be done if wIMU = 0.0");
assert(K_fb < 1.0 && "Feedback gain on foot correction should be less than 1.0 (K_fb_feet_poses>1.0)");
......@@ -705,15 +705,15 @@ namespace dynamicgraph
se3Interp(rightDrift,SE3::Identity(),K_fb*wL,rightDrift_delta);
// if a feet is not stable on the ground (aka flying), fully update his position
if (m_right_foot_is_stable == false)
rightDrift_delta = leftDrift;
rightDrift_delta = rightDrift;
if (m_left_foot_is_stable == false)
leftDrift_delta = rightDrift;
leftDrift_delta = leftDrift;
if (m_right_foot_is_stable == false && m_left_foot_is_stable == false)
{
//robot is jumping, do not update any feet position
rightDrift_delta = SE3::Identity();
leftDrift_delta = SE3::Identity();
}
{
//robot is jumping, do not update any feet position
rightDrift_delta = SE3::Identity();
leftDrift_delta = SE3::Identity();
}
m_oMlfs = m_oMlfs * leftDrift_delta;
m_oMrfs = m_oMrfs * rightDrift_delta;
// dedrift (x, y, z, yaw) using feet pose references
......@@ -910,17 +910,18 @@ namespace dynamicgraph
double w_fz = compute_force_weight(wrench(2), m_fz_std_dev_rf, m_fz_margin_rf);
//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_rf)
m_rf_fz_stable_cpt++;
else m_rf_fz_stable_cpt = 0;
m_rf_fz_stable_cpt++;
else
m_rf_fz_stable_cpt = 0;
if (m_rf_fz_stable_cpt >= m_fz_stable_windows_size)
{
m_rf_fz_stable_cpt = m_fz_stable_windows_size;
m_right_foot_is_stable = true;
m_rf_fz_stable_cpt = m_fz_stable_windows_size;
m_right_foot_is_stable = true;
}
else
{
m_right_foot_is_stable = false;
m_right_foot_is_stable = false;
}
s = w_zmp*w_fz;
return s;
......@@ -999,6 +1000,7 @@ namespace dynamicgraph
// if both weights are zero set them to a small positive value to avoid division by zero
if(wR==0.0 && wL==0.0)
{
SEND_WARNING_STREAM_MSG("The robot is flying!!!");
wR = 1e-3;
wL = 1e-3;
}
......@@ -1007,16 +1009,18 @@ namespace dynamicgraph
const Frame & f_lf = m_model.frames[m_left_foot_id];
const Motion v_lf_local = m_data->v[f_lf.parent];
const SE3 ffMlf = m_data->oMi[f_lf.parent];
Vector6 v_kin_l = -ffMlf.act(v_lf_local).toVector();
v_kin_l.head<3>() = m_oRff * v_kin_l.head<3>();
Vector6 v_kin_l = -ffMlf.act(v_lf_local).toVector(); //this is the velocity of the base in the frame of the base.
v_kin_l.head<3>() = m_oRff * v_kin_l.head<3>();
v_kin_l.segment<3>(3) = m_oRff * v_kin_l.segment<3>(3);
const Frame & f_rf = m_model.frames[m_right_foot_id];
const Motion v_rf_local = m_data->v[f_rf.parent];
const SE3 ffMrf = m_data->oMi[f_rf.parent];
Vector6 v_kin_r = -ffMrf.act(v_rf_local).toVector();
v_kin_r.head<3>() = m_oRff * v_kin_r.head<3>();
Vector6 v_kin_r = -ffMrf.act(v_rf_local).toVector(); //this is the velocity of the base in the frame of the base.
v_kin_r.head<3>() = m_oRff * v_kin_r.head<3>();
v_kin_r.segment<3>(3) = m_oRff * v_kin_r.segment<3>(3);
m_v_kin.head<6>() = (wR*v_kin_r + wL*v_kin_l)/(wL+wR); //this is the velocity of the base in the frame of the base.
m_v_kin.head<6>() = (wR*v_kin_r + wL*v_kin_l)/(wL+wR);
/* Compute velocity induced by the flexibility */
Vector6 v_flex_l;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment