Commit 9f240d3c authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

[balance-ctrl] Add command to update CoM offset based on current CoP measurement

parent 0a83cf31
......@@ -78,6 +78,7 @@ namespace dynamicgraph {
void init(const double& dt,
const std::string& robotRef);
void updateComOffset();
void removeRightFootContact(const double& transitionTime);
void removeLeftFootContact(const double& transitionTime);
void addRightFootContact(const double& transitionTime);
......@@ -240,6 +241,7 @@ namespace dynamicgraph {
tsid::math::Vector m_f; /// desired force coefficients (24d)
tsid::math::Vector6 m_f_RF; /// desired 6d wrench right foot
tsid::math::Vector6 m_f_LF; /// desired 6d wrench left foot
tsid::math::Vector3 m_com_offset; /// 3d CoM offset
tsid::math::Vector3 m_zmp_des_LF; /// 3d desired zmp left foot
tsid::math::Vector3 m_zmp_des_RF; /// 3d desired zmp left foot
tsid::math::Vector3 m_zmp_des_LF_local; /// 3d desired zmp left foot expressed in local frame
......
......@@ -274,6 +274,7 @@ namespace dynamicgraph
m_zmp_RF.setZero();
m_zmp_LF.setZero();
m_zmp.setZero();
m_com_offset.setZero();
/* Commands. */
addCommand("init",
......@@ -282,6 +283,10 @@ namespace dynamicgraph
"Time period in seconds (double)",
"Robot reference (string)")));
addCommand("updateComOffset",
makeCommandVoid0(*this, &InverseDynamicsBalanceController::updateComOffset,
docCommandVoid0("Update the offset on the CoM based on the CoP measurement.")));
addCommand("removeRightFootContact",
makeCommandVoid1(*this, &InverseDynamicsBalanceController::removeRightFootContact,
docCommandVoid1("Remove the contact at the right foot.",
......@@ -295,6 +300,14 @@ namespace dynamicgraph
}
void InverseDynamicsBalanceController::updateComOffset()
{
const Vector3 & com = m_robot->com(m_invDyn->data());
m_com_offset = m_zmp - com;
m_com_offset(2) = 0.0;
SEND_MSG("CoM offset updated: "+toString(m_com_offset), MSG_TYPE_INFO);
}
void InverseDynamicsBalanceController::removeRightFootContact(const double& transitionTime)
{
if(m_contactState == DOUBLE_SUPPORT)
......@@ -369,8 +382,8 @@ namespace dynamicgraph
if(dt<=0.0)
return SEND_MSG("Init failed: Timestep must be positive", MSG_TYPE_ERROR);
/* Retrieve m_robot_util informations */
std::string localName(robotRef);
/* Retrieve m_robot_util informations */
std::string localName(robotRef);
if (isNameInRobotUtil(localName))
m_robot_util = getRobotUtil(localName);
else
......@@ -610,8 +623,8 @@ namespace dynamicgraph
assert(dq_ref.size()==m_robot_util->m_nbJoints);
const VectorN& ddq_ref = m_posture_ref_accSIN(iter);
assert(ddq_ref.size()==m_robot_util->m_nbJoints);
const Vector6& kp_contact = m_kp_constraintsSIN(iter);
const Vector6& kp_contact = m_kp_constraintsSIN(iter);
const Vector6& kd_contact = m_kd_constraintsSIN(iter);
const Vector3& kp_com = m_kp_comSIN(iter);
const Vector3& kd_com = m_kd_comSIN(iter);
......@@ -664,7 +677,7 @@ namespace dynamicgraph
m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf);
m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf);
m_sampleCom.pos = x_com_ref;
m_sampleCom.pos = x_com_ref - m_com_offset;
m_sampleCom.vel = dx_com_ref;
m_sampleCom.acc = ddx_com_ref;
m_taskCom->setReference(m_sampleCom);
......@@ -1123,7 +1136,7 @@ namespace dynamicgraph
if(s.size()!=3)
s.resize(3);
const Vector3 & com = m_robot->com(m_invDyn->data());
s = com;
s = com + m_com_offset;
return s;
}
......
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