Unverified Commit 5e99cb86 authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge pull request #15 from nim65s/devel

Fix orientation pb + Dynamical filter initialization
parents fdd2a0f5 e0df8fb0
...@@ -13,6 +13,7 @@ SET(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}") ...@@ -13,6 +13,7 @@ SET(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
OPTION(USE_LSSOL "Do you want to use the solver lssol?" OFF) OPTION(USE_LSSOL "Do you want to use the solver lssol?" OFF)
OPTION(USE_QUADPROG "Do you want to use the solver eigen-quadprog?" ON) OPTION(USE_QUADPROG "Do you want to use the solver eigen-quadprog?" ON)
OPTION(SUFFIX_SO_VERSION "Suffix library name with its version" ON) OPTION(SUFFIX_SO_VERSION "Suffix library name with its version" ON)
OPTION(FULL_BUILD_TESTING "Complete and long testing" OFF)
# Project configuration # Project configuration
SET(PROJECT_USE_CMAKE_EXPORT TRUE) SET(PROJECT_USE_CMAKE_EXPORT TRUE)
......
...@@ -42,6 +42,10 @@ struct PinocchioRobotFoot_t { ...@@ -42,6 +42,10 @@ struct PinocchioRobotFoot_t {
double soleHeight; // x axis double soleHeight; // x axis
Eigen::Vector3d anklePosition; Eigen::Vector3d anklePosition;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
inline PinocchioRobotFoot_t()
: associatedAnkle(0), soleDepth(0.0), soleWidth(0.0), soleHeight(0.0),
anklePosition(0.0, 0.0, 0.0) {}
}; };
typedef PinocchioRobotFoot_t PRFoot; typedef PinocchioRobotFoot_t PRFoot;
......
...@@ -46,7 +46,8 @@ Clock::Clock() { ...@@ -46,7 +46,8 @@ Clock::Clock() {
struct timeval startingtime; struct timeval startingtime;
gettimeofday(&startingtime, 0); gettimeofday(&startingtime, 0);
m_StartingTime = startingtime.tv_sec + 0.000001 * startingtime.tv_usec; m_StartingTime =
(double)startingtime.tv_sec + 0.000001 * (double)startingtime.tv_usec;
} }
Clock::~Clock() {} Clock::~Clock() {}
...@@ -58,22 +59,24 @@ void Clock::Reset() { ...@@ -58,22 +59,24 @@ void Clock::Reset() {
struct timeval startingtime; struct timeval startingtime;
gettimeofday(&startingtime, 0); gettimeofday(&startingtime, 0);
m_StartingTime = startingtime.tv_sec + 0.000001 * startingtime.tv_usec; m_StartingTime =
(double)startingtime.tv_sec + 0.000001 * (double)startingtime.tv_usec;
} }
void Clock::StartTiming() { gettimeofday(&m_BeginTimeStamp, 0); } void Clock::StartTiming() { gettimeofday(&m_BeginTimeStamp, 0); }
void Clock::StopTiming() { void Clock::StopTiming() {
gettimeofday(&m_EndTimeStamp, 0); gettimeofday(&m_EndTimeStamp, 0);
double ltime = m_EndTimeStamp.tv_sec - m_BeginTimeStamp.tv_sec + double ltime =
0.000001 * (m_EndTimeStamp.tv_usec - m_BeginTimeStamp.tv_usec); (double)m_EndTimeStamp.tv_sec - (double)m_BeginTimeStamp.tv_sec +
0.000001 * (double)(m_EndTimeStamp.tv_usec - m_BeginTimeStamp.tv_usec);
m_MaximumTime = m_MaximumTime < ltime ? ltime : m_MaximumTime; m_MaximumTime = m_MaximumTime < ltime ? ltime : m_MaximumTime;
m_TotalTime += ltime; m_TotalTime += ltime;
m_DataBuffer[(m_NbOfIterations * 2) % 3000000] = m_DataBuffer[(m_NbOfIterations * 2) % 3000000] =
m_BeginTimeStamp.tv_sec + 0.000001 * m_BeginTimeStamp.tv_usec - (double)m_BeginTimeStamp.tv_sec +
m_StartingTime; 0.000001 * (double)m_BeginTimeStamp.tv_usec - m_StartingTime;
m_DataBuffer[(m_NbOfIterations * 2 + 1) % 3000000] = ltime; m_DataBuffer[(m_NbOfIterations * 2 + 1) % 3000000] = ltime;
} }
......
...@@ -52,11 +52,8 @@ int CoMAndFootOnlyStrategy::InitInterObjects( ...@@ -52,11 +52,8 @@ int CoMAndFootOnlyStrategy::InitInterObjects(
int CoMAndFootOnlyStrategy::OneGlobalStepOfControl( int CoMAndFootOnlyStrategy::OneGlobalStepOfControl(
FootAbsolutePosition &LeftFootPosition, FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition, Eigen::VectorXd &ZMPRefPos, FootAbsolutePosition &RightFootPosition, Eigen::VectorXd &ZMPRefPos,
COMState &finalCOMPosition, COMState &finalCOMPosition, Eigen::VectorXd &CurrentConfiguration,
Eigen::VectorXd &, // CurrentConfiguration, Eigen::VectorXd &CurrentVelocity, Eigen::VectorXd &CurrentAcceleration) {
Eigen::VectorXd &, // CurrentVelocity,
Eigen::VectorXd &) // CurrentAcceleration)
{
ODEBUG("Begin OneGlobalStepOfControl " ODEBUG("Begin OneGlobalStepOfControl "
<< m_LeftFootPositions->size() << " " << m_RightFootPositions->size() << m_LeftFootPositions->size() << " " << m_RightFootPositions->size()
<< " " << m_COMBuffer->size() << " " << m_ZMPPositions->size()); << " " << m_COMBuffer->size() << " " << m_ZMPPositions->size());
...@@ -87,6 +84,18 @@ int CoMAndFootOnlyStrategy::OneGlobalStepOfControl( ...@@ -87,6 +84,18 @@ int CoMAndFootOnlyStrategy::OneGlobalStepOfControl(
return -4; 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) { if (m_ZMPPositions->size() > 0) {
ZMPPosition aZMPPosition = (*m_ZMPPositions)[0]; ZMPPosition aZMPPosition = (*m_ZMPPositions)[0];
ZMPRefPos(0) = aZMPPosition.px; ZMPRefPos(0) = aZMPPosition.px;
......
...@@ -339,16 +339,17 @@ int PLDPSolver::BackwardSubstitution() { ...@@ -339,16 +339,17 @@ int PLDPSolver::BackwardSubstitution() {
// LL^t v2 = v1 <-> L y = v1 with L^t v2 = y // LL^t v2 = v1 <-> L y = v1 with L^t v2 = y
// y solved with first phase. // y solved with first phase.
// So now we are looking for v2. // So now we are looking for v2.
unsigned int SizeOfL = m_ActivatedConstraints.size(); std::vector<unsigned int>::size_type SizeOfL = m_ActivatedConstraints.size();
if (SizeOfL == 0) if (SizeOfL == 0)
return 0; return 0;
ODEBUG("BackwardSubstitution " << m_ItNb); ODEBUG("BackwardSubstitution " << m_ItNb);
for (int i = SizeOfL - 1; i >= 0; i--) {
for (std::vector<unsigned int>::size_type i = SizeOfL - 1;; i--) {
double tmp = 0.0; double tmp = 0.0;
m_v2[i] = m_y[i]; m_v2[i] = m_y[i];
for (int k = i + 1; k < (int)SizeOfL; k++) { for (std::vector<unsigned int>::size_type k = i + 1; k < SizeOfL; k++) {
if (k == (int)SizeOfL - 1) if (k == SizeOfL - 1)
tmp = m_v2[i]; tmp = m_v2[i];
m_v2[i] -= m_L[k * m_NbMaxOfConstraints + i] * m_v2[k]; m_v2[i] -= m_L[k * m_NbMaxOfConstraints + i] * m_v2[k];
...@@ -359,6 +360,8 @@ int PLDPSolver::BackwardSubstitution() { ...@@ -359,6 +360,8 @@ int PLDPSolver::BackwardSubstitution() {
ODEBUG("BS: m_L[i*m_NbMaxOfConstraints+i]:" ODEBUG("BS: m_L[i*m_NbMaxOfConstraints+i]:"
<< m_L[i * m_NbMaxOfConstraints + i] << " " << m_y[i]); << m_L[i * m_NbMaxOfConstraints + i] << " " << m_y[i]);
ODEBUG("m_v2[" << i << " ] = " << m_v2[i] << " " << tmp); ODEBUG("m_v2[" << i << " ] = " << m_v2[i] << " " << tmp);
if (i == 0)
break;
} }
return 0; return 0;
} }
...@@ -789,7 +792,7 @@ int PLDPSolver::SolveProblem( ...@@ -789,7 +792,7 @@ int PLDPSolver::SolveProblem(
struct timeval current; struct timeval current;
gettimeofday(&current, 0); gettimeofday(&current, 0);
double r = (double)(current.tv_sec - begin.tv_sec) + double r = (double)(current.tv_sec - begin.tv_sec) +
0.000001 * (current.tv_usec - begin.tv_usec); 0.000001 * (double)(current.tv_usec - begin.tv_usec);
if (r > m_AmountOfLimitedComputationTime) { if (r > m_AmountOfLimitedComputationTime) {
ContinueAlgo = false; ContinueAlgo = false;
} }
......
...@@ -394,18 +394,14 @@ doublereal *eps1; ...@@ -394,18 +394,14 @@ doublereal *eps1;
/* System generated locals */ /* System generated locals */
integer c_dim1, c_offset, a_dim1, a_offset, i__1; integer c_dim1, c_offset, a_dim1, a_offset, i__1;
/* Builtin functions */
/* integer s_wsfe(), do_fio(), e_wsfe(); */
/* Local variables */ /* Local variables */
static doublereal diag; static doublereal diag;
/* extern int ql0002_(); */ /* extern int ql0002_(); */
static integer nact, info; static integer nact, info;
static doublereal zero; static doublereal zero;
static integer i, j, idiag, maxit; static integer i, j, maxit;
static doublereal qpeps; static doublereal qpeps;
static integer in, mn, lw; static integer in, mn, lw;
static doublereal ten;
static logical lql; static logical lql;
static integer inw1, inw2; static integer inw1, inw2;
...@@ -450,7 +446,6 @@ doublereal *eps1; ...@@ -450,7 +446,6 @@ doublereal *eps1;
lql = TRUE_; lql = TRUE_;
} }
zero = 0.; zero = 0.;
ten = 10.;
maxit = (*m + *n) * 40; maxit = (*m + *n) * 40;
qpeps = cmache_1.eps; qpeps = cmache_1.eps;
inw1 = 1; inw1 = 1;
...@@ -496,18 +491,6 @@ L20: ...@@ -496,18 +491,6 @@ L20:
if (info == 2) { if (info == 2) {
goto L90; goto L90;
} }
idiag = 0;
if (diag > zero && diag < 1e3) {
idiag = (integer)diag;
}
/*
if (*iprint > 0 && idiag > 0) {
io___16.ciunit = *iout;
s_wsfe(&io___16);
do_fio(&c__1, (char *)&idiag, (ftnlen)sizeof(integer));
e_wsfe();
}
*/
if (info < 0) { if (info < 0) {
goto L70; goto L70;
} }
...@@ -658,14 +641,14 @@ integer *lw; ...@@ -658,14 +641,14 @@ integer *lw;
static doublereal ga, gb; static doublereal ga, gb;
static integer ia, id; static integer ia, id;
static doublereal fdiffa; static doublereal fdiffa;
static integer ii, il, kk, jl, ip, ir, nm, is, iu, iw, ju, ix, iz, nu, iy; static integer ii, il, kk, jl, ir, nm, is, iu, iw, ju, ix, iz, nu, iy;
static doublereal parinc, parnew; static doublereal parinc, parnew;
static integer ira, irb, iwa; static integer ira, irb, iwa;
static doublereal one; static doublereal one;
static integer iwd, iza; static integer iwd, iza;
static doublereal res; static doublereal res;
static integer ipp, iwr, iws; static integer iwr, iws;
static doublereal sum; static doublereal sum;
static integer iww, iwx, iwy; static integer iww, iwx, iwy;
static doublereal two; static doublereal two;
...@@ -1082,8 +1065,6 @@ L250: ...@@ -1082,8 +1065,6 @@ L250:
L280: L280:
ir = iwr; ir = iwr;
ip = iww + 1;
ipp = iww + *n;
il = iws + 1; il = iws + 1;
iu = iws + *nact; iu = iws + *nact;
i__2 = iu; i__2 = iu;
...@@ -1829,8 +1810,6 @@ L770: ...@@ -1829,8 +1810,6 @@ L770:
/* CALCULATE THE NEXT CONSTRAINT TO DROP. */ /* CALCULATE THE NEXT CONSTRAINT TO DROP. */
L775: L775:
ip = iww + 1;
ipp = iww + *nact;
kdrop = 0; kdrop = 0;
if (*nact == 0) { if (*nact == 0) {
goto L791; goto L791;
......
...@@ -300,11 +300,9 @@ void GenerateMotionFromKineoWorks::ComputeUpperBodyPosition( ...@@ -300,11 +300,9 @@ void GenerateMotionFromKineoWorks::ComputeUpperBodyPosition(
//! Find the closest (X,Y,Z) position in the remaining //! Find the closest (X,Y,Z) position in the remaining
// part of the CoM buffer. // part of the CoM buffer.
for (unsigned int i = count; i < m_COMBuffer.size(); i++) { for (unsigned int i = count; i < m_COMBuffer.size(); i++) {
double ldist = (lX - m_COMBuffer[i].x[0]) * (lX - m_COMBuffer[i].x[0]) double ldist = (lX - m_COMBuffer[i].x[0]) * (lX - m_COMBuffer[i].x[0]) +
/*+ (lY - m_COMBuffer[i].y[0]) * (lY - m_COMBuffer[i].y[0]) +
(lY - m_COMBuffer[i].y[0])*(lY-m_COMBuffer[i].y[0]) + (lZ - m_COMBuffer[i].z[0]) * (lZ - m_COMBuffer[i].z[0]);
(lZ - m_COMBuffer[i].z[0])*(lZ-m_COMBuffer[i].z[0]) */
;
if (ldist < dist) { if (ldist < dist) {
dist = ldist; dist = ldist;
......
...@@ -362,7 +362,8 @@ void StepOverPlanner::DoubleSupportFeasibility() { ...@@ -362,7 +362,8 @@ void StepOverPlanner::DoubleSupportFeasibility() {
Eigen::Vector3d ToTheHip; Eigen::Vector3d ToTheHip;
Eigen::Matrix<double, 6, 1> LeftLegAngles; Eigen::Matrix<double, 6, 1> LeftLegAngles;
Eigen::Matrix<double, 6, 1> RightLegAngles; Eigen::Matrix<double, 6, 1> RightLegAngles;
LeftLegAngles.Zero();
RightLegAngles.Zero();
Eigen::Vector3d AnkleBeforeObst; Eigen::Vector3d AnkleBeforeObst;
Eigen::Vector3d AnkleAfterObst; Eigen::Vector3d AnkleAfterObst;
Eigen::Vector3d TempCOMState; Eigen::Vector3d TempCOMState;
...@@ -778,8 +779,8 @@ void StepOverPlanner::PolyPlannerFirstStep( ...@@ -778,8 +779,8 @@ void StepOverPlanner::PolyPlannerFirstStep(
double StepLenght; double StepLenght;
double Omega1, Omega2, OmegaImpact; double Omega1, Omega2, OmegaImpact;
double xOffset; double xOffset;
double Point1X, Point1Y = 0.0, Point1Z; double Point1X, Point1Z;
double Point2X, Point2Y = 0.0, Point2Z; double Point2X, Point2Z;
double Point3Z; double Point3Z;
StepTime = aStepOverFootBuffer[m_StartDoubleSupp].time - StepTime = aStepOverFootBuffer[m_StartDoubleSupp].time -
...@@ -801,12 +802,10 @@ void StepOverPlanner::PolyPlannerFirstStep( ...@@ -801,12 +802,10 @@ void StepOverPlanner::PolyPlannerFirstStep(
Point1X = StepLenght - m_heelToAnkle - m_ObstacleParameters.d - xOffset - Point1X = StepLenght - m_heelToAnkle - m_ObstacleParameters.d - xOffset -
m_tipToAnkle * cos(Omega1 * M_PI / 180.0); m_tipToAnkle * cos(Omega1 * M_PI / 180.0);
Point1Y = 0.00;
Point1Z = m_ObstacleParameters.h - m_tipToAnkle * sin(Omega1 * M_PI / 180.0); Point1Z = m_ObstacleParameters.h - m_tipToAnkle * sin(Omega1 * M_PI / 180.0);
Point2X = StepLenght - m_heelToAnkle + xOffset + Point2X = StepLenght - m_heelToAnkle + xOffset +
m_heelToAnkle * cos(Omega2 * M_PI / 180.0); m_heelToAnkle * cos(Omega2 * M_PI / 180.0);
Point2Y = 0.00;
Point2Z = m_ObstacleParameters.h - m_tipToAnkle * sin(Omega2 * M_PI / 180.0); Point2Z = m_ObstacleParameters.h - m_tipToAnkle * sin(Omega2 * M_PI / 180.0);
Point3Z = Point1Z + 0.04; Point3Z = Point1Z + 0.04;
...@@ -842,12 +841,10 @@ void StepOverPlanner::PolyPlannerFirstStep( ...@@ -842,12 +841,10 @@ void StepOverPlanner::PolyPlannerFirstStep(
ZfootSpeedBound(0) = 0.0; ZfootSpeedBound(0) = 0.0;
ZfootSpeedBound(1) = 0.0; ZfootSpeedBound(1) = 0.0;
int NumberIntermediate = 0, NumberIntermediate2 = 0, Counter = 0, int NumberIntermediate = 0, Counter = 0, CounterTemp = 0;
CounterTemp = 0;
double IntermediateTimeStep; double IntermediateTimeStep;
NumberIntermediate = 10; NumberIntermediate = 10;
NumberIntermediate2 = 20;
ZfootPos.resize(2 + 3 * NumberIntermediate); ZfootPos.resize(2 + 3 * NumberIntermediate);
TimeIntervalsZ.resize(2 + 3 * NumberIntermediate); TimeIntervalsZ.resize(2 + 3 * NumberIntermediate);
...@@ -1101,8 +1098,8 @@ void StepOverPlanner::PolyPlannerSecondStep( ...@@ -1101,8 +1098,8 @@ void StepOverPlanner::PolyPlannerSecondStep(
double StepLenght; double StepLenght;
double Omega1, Omega2, OmegaImpact; double Omega1, Omega2, OmegaImpact;
double xOffset; double xOffset;
double Point1X, Point1Y, Point1Z; double Point1X, Point1Z;
double Point2X, Point2Y, Point2Z; double Point2X, Point2Z;
double Point3Z; double Point3Z;
StepTime = aStepOverFootBuffer[m_EndStepOver].time - StepTime = aStepOverFootBuffer[m_EndStepOver].time -
...@@ -1118,12 +1115,10 @@ void StepOverPlanner::PolyPlannerSecondStep( ...@@ -1118,12 +1115,10 @@ void StepOverPlanner::PolyPlannerSecondStep(
Point1X = m_StepOverStepLenght - m_heelToAnkle - m_ObstacleParameters.d - Point1X = m_StepOverStepLenght - m_heelToAnkle - m_ObstacleParameters.d -
xOffset - m_tipToAnkle * cos(Omega1 * M_PI / 180.0); xOffset - m_tipToAnkle * cos(Omega1 * M_PI / 180.0);
Point1Y = 0.0;
Point1Z = m_ObstacleParameters.h + m_tipToAnkle * sin(Omega1 * M_PI / 180.0); Point1Z = m_ObstacleParameters.h + m_tipToAnkle * sin(Omega1 * M_PI / 180.0);
Point2X = m_StepOverStepLenght - m_heelToAnkle + xOffset + Point2X = m_StepOverStepLenght - m_heelToAnkle + xOffset +
m_heelToAnkle * cos(Omega2 * M_PI / 180.0); m_heelToAnkle * cos(Omega2 * M_PI / 180.0);
Point2Y = 0.0;
Point2Z = Point1Z; Point2Z = Point1Z;
// m_ObstacleParameters.h+0.04;//-m_tipToAnkle*sin(Omega2*M_PI/180.0); // m_ObstacleParameters.h+0.04;//-m_tipToAnkle*sin(Omega2*M_PI/180.0);
...@@ -1687,8 +1682,6 @@ void StepOverPlanner::CreateBufferFirstPreview( ...@@ -1687,8 +1682,6 @@ void StepOverPlanner::CreateBufferFirstPreview(
void StepOverPlanner::m_SetObstacleParameters(istringstream &strm) { void StepOverPlanner::m_SetObstacleParameters(istringstream &strm) {
bool ReadObstacleParameters = false;
ODEBUG("I am reading the obstacle parameters" ODEBUG("I am reading the obstacle parameters"
<< " "); << " ");
...@@ -1738,7 +1731,6 @@ void StepOverPlanner::m_SetObstacleParameters(istringstream &strm) { ...@@ -1738,7 +1731,6 @@ void StepOverPlanner::m_SetObstacleParameters(istringstream &strm) {
if (!strm.eof()) { if (!strm.eof()) {
bool lObstacleDetected; bool lObstacleDetected;
strm >> lObstacleDetected; strm >> lObstacleDetected;
ReadObstacleParameters = true;
break; break;
} else { } else {
cout << "Not enough inputs for completion of " cout << "Not enough inputs for completion of "
......
...@@ -93,10 +93,8 @@ void UpperBodyMotion::ReadDataFile(string aFileName, ...@@ -93,10 +93,8 @@ void UpperBodyMotion::ReadDataFile(string aFileName,
std::ifstream aif; std::ifstream aif;
unsigned int NumberRows, NumberColumns; Eigen::MatrixXd::Index NumberRows = UpperBodyAngles.rows();
Eigen::MatrixXd::Index NumberColumns = UpperBodyAngles.cols();
NumberRows = UpperBodyAngles.rows();
NumberColumns = UpperBodyAngles.cols();
double r; double r;
...@@ -116,9 +114,8 @@ void UpperBodyMotion::ReadDataFile(string aFileName, ...@@ -116,9 +114,8 @@ void UpperBodyMotion::ReadDataFile(string aFileName,
void UpperBodyMotion::WriteDataFile(string aFileName, void UpperBodyMotion::WriteDataFile(string aFileName,
Eigen::MatrixXd &UpperBodyAngles) { Eigen::MatrixXd &UpperBodyAngles) {
ofstream aof; ofstream aof;
unsigned int NumberRows, NumberColumns; Eigen::MatrixXd::Index NumberRows = UpperBodyAngles.rows();
NumberRows = UpperBodyAngles.rows(); Eigen::MatrixXd::Index NumberColumns = UpperBodyAngles.cols();
NumberColumns = UpperBodyAngles.cols();
aof.open(aFileName.c_str(), ofstream::out); aof.open(aFileName.c_str(), ofstream::out);
if (aof.is_open()) { if (aof.is_open()) {
......
...@@ -201,9 +201,10 @@ int ZMPPreviewControlWithMultiBodyZMP::OneGlobalStepOfControl( ...@@ -201,9 +201,10 @@ int ZMPPreviewControlWithMultiBodyZMP::OneGlobalStepOfControl(
<< aRightFAP.x << " " << aRightFAP.y << " " << aRightFAP.z, << aRightFAP.x << " " << aRightFAP.y << " " << aRightFAP.z,
"1ststage.dat"); "1ststage.dat");
CallToComAndFootRealization(acompos, aLeftFAP, aRightFAP, int StageOfTheAlgorithm = 0;
CurrentConfiguration, CurrentVelocity, CallToComAndFootRealization(
CurrentAcceleration, m_NumberOfIterations, 0); acompos, aLeftFAP, aRightFAP, CurrentConfiguration, CurrentVelocity,
CurrentAcceleration, m_NumberOfIterations, StageOfTheAlgorithm);
if (m_StageStrategy != ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY) if (m_StageStrategy != ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY)
EvaluateMultiBodyZMP(-1); EvaluateMultiBodyZMP(-1);
...@@ -222,9 +223,11 @@ int ZMPPreviewControlWithMultiBodyZMP::OneGlobalStepOfControl( ...@@ -222,9 +223,11 @@ int ZMPPreviewControlWithMultiBodyZMP::OneGlobalStepOfControl(
"2ndStage.dat"); "2ndStage.dat");
if (m_StageStrategy != ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY) { if (m_StageStrategy != ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY) {
int StageOfTheAlgorithm = 1;
CallToComAndFootRealization( CallToComAndFootRealization(
refandfinalCOMState, aLeftFAP, aRightFAP, CurrentConfiguration, refandfinalCOMState, aLeftFAP, aRightFAP, CurrentConfiguration,
CurrentVelocity, CurrentAcceleration, m_NumberOfIterations - m_NL, 1); CurrentVelocity, CurrentAcceleration, m_NumberOfIterations - m_NL,
StageOfTheAlgorithm);
} }
// Here it is assumed that the 4x4 CoM matrix // Here it is assumed that the 4x4 CoM matrix
...@@ -277,7 +280,6 @@ COMState ZMPPreviewControlWithMultiBodyZMP::GetLastCOMFromFirstStage() { ...@@ -277,7 +280,6 @@ COMState ZMPPreviewControlWithMultiBodyZMP::GetLastCOMFromFirstStage() {
int ZMPPreviewControlWithMultiBodyZMP::SecondStageOfControl( int ZMPPreviewControlWithMultiBodyZMP::SecondStageOfControl(
COMState &finalCOMState) { COMState &finalCOMState) {
double Deltazmpx2, Deltazmpy2;
// Inverse Kinematics variables. // Inverse Kinematics variables.
COMState aCOMState = m_FIFOCOMStates[0]; COMState aCOMState = m_FIFOCOMStates[0];
...@@ -286,17 +288,18 @@ int ZMPPreviewControlWithMultiBodyZMP::SecondStageOfControl( ...@@ -286,17 +288,18 @@ int ZMPPreviewControlWithMultiBodyZMP::SecondStageOfControl(
LeftFootPosition = m_FIFOLeftFootPosition[0]; LeftFootPosition = m_FIFOLeftFootPosition[0];
RightFootPosition = m_FIFORightFootPosition[0]; RightFootPosition = m_FIFORightFootPosition[0];
#if 0 double Deltazmpx2, Deltazmpy2;
// Preview control on delta ZMP. // Preview control on delta ZMP.
if ((m_StageStrategy == ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY) || if ((m_StageStrategy == ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY) ||
(m_StageStrategy == ZMPCOM_TRAJECTORY_FULL)) { (m_StageStrategy == ZMPCOM_TRAJECTORY_FULL)) {
ODEBUG2(m_FIFODeltaZMPPositions[0].px << " " ODEBUG2(m_FIFODeltaZMPPositions[0].px << " "
<< m_FIFODeltaZMPPositions[0].py); << m_FIFODeltaZMPPositions[0].py);
ODEBUG("Second Stage Size of FIFODeltaZMPPositions: " ODEBUG("Second Stage Size of FIFODeltaZMPPositions: "
<< m_FIFODeltaZMPPositions.size() << " " << m_Deltax << " " << m_FIFODeltaZMPPositions.size() << " " << m_Deltax << " "
<< m_Deltay << " " << m_sxDeltazmp << " " << m_syDeltazmp << " " << m_Deltay << " " << m_sxDeltazmp << " " << m_syDeltazmp << " "
<< Deltazmpx2 << " " << Deltazmpy2); << Deltazmpx2 << " " << Deltazmpy2);
m_PC->OneIterationOfPreview(m_Deltax, m_Deltay, m_sxDeltazmp, m_syDeltazmp, m_PC->OneIterationOfPreview(m_Deltax, m_Deltay, m_sxDeltazmp, m_syDeltazmp,
m_FIFODeltaZMPPositions, 0, Deltazmpx2, m_FIFODeltaZMPPositions, 0, Deltazmpx2,
...@@ -309,7 +312,6 @@ int ZMPPreviewControlWithMultiBodyZMP::SecondStageOfControl( ...@@ -309,7 +312,6 @@ int ZMPPreviewControlWithMultiBodyZMP::SecondStageOfControl(
aCOMState.y[i] += m_Deltay(i, 0); aCOMState.y[i] += m_Deltay(i, 0);
} }
} }
#endif
ODEBUG2("Delta :" << m_Deltax(0, 0) << " " << m_Deltay(0, 0) << " " ODEBUG2("Delta :" << m_Deltax(0, 0) << " " << m_Deltay(0, 0) << " "
<< aCOMState.x[0] << " " << aCOMState.y[0]); << aCOMState.x[0] << " " << aCOMState.y[0]);
...@@ -452,7 +454,7 @@ int ZMPPreviewControlWithMultiBodyZMP::Setup( ...@@ -452,7 +454,7 @@ int ZMPPreviewControlWithMultiBodyZMP::Setup(