Commit 20217445 authored by andreadelprete's avatar andreadelprete
Browse files

[balance-ctrl] Set reference forces in force regularization task. Set min and...

[balance-ctrl] Set reference forces in force regularization task. Set min and max contact forces at each time step.
parent 38f7bfc7
......@@ -42,7 +42,9 @@ SOT_TORQUE_INSTALL_DIR = '/home/adelpret/devel/openrobots/src/sot-torque-control
robot.com_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/com_spline.curve", 1)
robot.lf_force_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/lf_force.curve", 1)
robot.rf_force_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/rf_force.curve", 1)
#robot.traj_sync.turnOn()
robot.com_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/com_spline.curve", -1)
robot.lf_force_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/lf_force.curve", -1)
robot.rf_force_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/rf_force.curve", -1)
robot.traj_sync.out.value = 1
sleep(5.0)
......
......@@ -423,6 +423,12 @@ namespace dynamicgraph
m_contactLF->Kd(kd_contact);
m_invDyn->addRigidContact(*m_contactLF);
if(m_f_ref_left_footSIN.isPlugged() && m_f_ref_right_footSIN.isPlugged())
{
m_contactLF->setRegularizationTaskWeightVector(Vector6::Ones());
m_contactRF->setRegularizationTaskWeightVector(Vector6::Ones());
}
m_taskCom = new TaskComEquality("task-com", *m_robot);
m_taskCom->Kp(kp_com);
m_taskCom->Kd(kd_com);
......@@ -529,6 +535,9 @@ namespace dynamicgraph
{
const Vector6 & f_ref_left_foot = m_f_ref_left_footSIN(iter);
const Vector6 & f_ref_right_foot = m_f_ref_right_footSIN(iter);
m_contactLF->setForceReference(f_ref_left_foot);
m_contactRF->setForceReference(f_ref_right_foot);
if(m_contactState == DOUBLE_SUPPORT)
{
if(f_ref_left_foot.norm() < ZERO_FORCE_THRESHOLD)
......@@ -594,9 +603,6 @@ namespace dynamicgraph
const double & w_posture = m_w_postureSIN(iter);
const double & w_forces = m_w_forcesSIN(iter);
const double & fMaxRF = m_f_max_right_footSIN(iter);
const double & fMaxLF = m_f_max_left_footSIN(iter);
if(m_contactState == LEFT_SUPPORT || m_contactState == LEFT_SUPPORT_TRANSITION)
{
const Eigen::Matrix<double,12,1>& x_rf_ref = m_rf_ref_posSIN(iter);
......@@ -658,6 +664,13 @@ namespace dynamicgraph
m_invDyn->updateTaskWeight(m_taskPosture->name(), w_posture);
}
const double & fMin = m_f_minSIN(0);
const double & fMaxRF = m_f_max_right_footSIN(iter);
const double & fMaxLF = m_f_max_left_footSIN(iter);
m_contactLF->setMinNormalForce(fMin);
m_contactRF->setMinNormalForce(fMin);
m_contactLF->setMaxNormalForce(fMaxLF);
m_contactRF->setMaxNormalForce(fMaxRF);
m_contactLF->Kp(kp_contact);
m_contactLF->Kd(kd_contact);
m_contactLF->setRegularizationTaskWeight(w_forces);
......
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