Commit 2cdf96f8 authored by Olivier Stasse's avatar Olivier Stasse Committed by Guilhem Saurel
Browse files

[Naveau2015] Fix a problem with the orientation.

parent fdd2a0f5
......@@ -53,9 +53,9 @@ int CoMAndFootOnlyStrategy::OneGlobalStepOfControl(
FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition, Eigen::VectorXd &ZMPRefPos,
COMState &finalCOMPosition,
Eigen::VectorXd &, // CurrentConfiguration,
Eigen::VectorXd &, // CurrentVelocity,
Eigen::VectorXd &) // CurrentAcceleration)
Eigen::VectorXd & CurrentConfiguration,
Eigen::VectorXd & CurrentVelocity,
Eigen::VectorXd & CurrentAcceleration)
{
ODEBUG("Begin OneGlobalStepOfControl "
<< m_LeftFootPositions->size() << " " << m_RightFootPositions->size()
......@@ -87,6 +87,18 @@ int CoMAndFootOnlyStrategy::OneGlobalStepOfControl(
return -4;
}
CurrentConfiguration[3] = finalCOMPosition.roll[0];
CurrentConfiguration[4] = finalCOMPosition.pitch[0];
CurrentConfiguration[5] = finalCOMPosition.yaw[0];
CurrentVelocity[3] = finalCOMPosition.roll[1];
CurrentVelocity[4] = finalCOMPosition.pitch[1];
CurrentVelocity[5] = finalCOMPosition.yaw[1];
CurrentAcceleration[3] = finalCOMPosition.roll[2];
CurrentAcceleration[4] = finalCOMPosition.pitch[2];
CurrentAcceleration[5] = finalCOMPosition.yaw[2];
if (m_ZMPPositions->size() > 0) {
ZMPPosition aZMPPosition = (*m_ZMPPositions)[0];
ZMPRefPos(0) = aZMPPosition.px;
......
Markdown is supported
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