diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
index 5e6cb76dd09078cf651a5b468366628be6e419ba..2ecf9765f0da4c23dba93f674946e0833d250cca 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2008, 2009, 2010,
+ * Copyright 2008, 2009, 2010,
*
* Francois Keith
* Olivier Stasse
@@ -19,7 +19,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/* This object generate all the values for the foot trajectories, */
@@ -45,8 +45,9 @@ FootTrajectoryGenerationStandard::FootTrajectoryGenerationStandard(SimplePluginM
m_PolynomeOmega = 0;
m_PolynomeOmega2 = 0;
m_PolynomeTheta = 0;
+ m_isStepStairOn = 1;
- /* Computes information on foot dimension
+ /* Computes information on foot dimension
from humanoid specific informations. */
double lWidth,lHeight,lDepth;
if (m_Foot!=0)
@@ -74,7 +75,7 @@ FootTrajectoryGenerationStandard::FootTrajectoryGenerationStandard(SimplePluginM
m_AnklePositionRight[0] = -lDepth*0.5 + AnklePosition[0];
m_AnklePositionRight[1] = lWidth*0.5 - AnklePosition[1];
m_AnklePositionRight[2] = AnklePosition[2];
-
+
/* Compute Left foot coordinates */
if (m_Foot!=0)
{
@@ -97,7 +98,7 @@ FootTrajectoryGenerationStandard::~FootTrajectoryGenerationStandard()
{
if (m_PolynomeX!=0)
delete m_PolynomeX;
-
+
if (m_PolynomeY!=0)
delete m_PolynomeY;
@@ -120,14 +121,14 @@ FootTrajectoryGenerationStandard::~FootTrajectoryGenerationStandard()
void FootTrajectoryGenerationStandard::InitializeInternalDataStructures()
{
FreeInternalDataStructures();
-
+
m_PolynomeX = new Polynome5(0,0);
m_PolynomeY = new Polynome5(0,0);
m_PolynomeZ = new Polynome4(0,0);
m_BsplinesZ = new ZBsplines(0,0,0,0);
m_PolynomeOmega = new Polynome3(0,0);
m_PolynomeOmega2 = new Polynome3(0,0);
- m_PolynomeTheta = new Polynome3(0,0);
+ m_PolynomeTheta = new Polynome3(0,0);
}
void FootTrajectoryGenerationStandard::FreeInternalDataStructures()
@@ -161,18 +162,24 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex,
{
switch (PolynomeIndex)
{
-
+
case X_AXIS:
m_PolynomeX->SetParameters(TimeInterval,Position);
break;
-
+
case Y_AXIS:
m_PolynomeY->SetParameters(TimeInterval,Position);
break;
case Z_AXIS:
- m_PolynomeZ->SetParameters(TimeInterval,Position);
- m_BsplinesZ->SetParameters(TimeInterval,Position/1.5,TimeInterval/3.0,Position);
+ if (m_isStepStairOn == 0)
+ {
+ m_PolynomeZ->SetParameters(TimeInterval,Position);
+ }
+ else
+ {
+ m_BsplinesZ->SetParameters(TimeInterval,Position/1.5,TimeInterval/3.0,Position);
+ }
break;
case THETA_AXIS:
@@ -202,19 +209,26 @@ int FootTrajectoryGenerationStandard::SetParametersWithInitPosInitSpeed(int Poly
{
switch (PolynomeIndex)
{
-
+
case X_AXIS:
ODEBUG2("Initspeed: " << InitSpeed << " ");
m_PolynomeX->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,0.0);
break;
-
+
case Y_AXIS:
m_PolynomeY->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,0.0);
break;
case Z_AXIS:
- m_PolynomeZ->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
- m_BsplinesZ->SetParameters(TimeInterval,FinalPosition/1.5,TimeInterval/3.0,FinalPosition);
+ if (m_isStepStairOn == 0)
+ {
+ m_PolynomeZ->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ }
+ else
+ {
+ m_BsplinesZ->SetParameters(TimeInterval,FinalPosition/1.5,TimeInterval/3.0,FinalPosition);
+ }
+
break;
case THETA_AXIS:
@@ -251,8 +265,14 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex, double Ti
break;
case Z_AXIS:
- m_PolynomeZ->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
- m_BsplinesZ->SetParameters(TimeInterval,FinalPosition/1.5,TimeInterval/3.0,FinalPosition);
+ if (m_isStepStairOn == 0)
+ {
+ m_PolynomeZ->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ }
+ else
+ {
+ m_BsplinesZ->SetParameters(TimeInterval,FinalPosition/1.5,TimeInterval/3.0,FinalPosition);
+ }
break;
case THETA_AXIS:
@@ -282,19 +302,25 @@ int FootTrajectoryGenerationStandard::GetParametersWithInitPosInitSpeed(int Poly
{
switch (PolynomeIndex)
{
-
+
case X_AXIS:
ODEBUG2("Initspeed: " << InitSpeed << " ");
m_PolynomeX->GetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
break;
-
+
case Y_AXIS:
m_PolynomeY->GetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
break;
case Z_AXIS:
+ if (m_isStepStairOn == 0)
+ {
m_PolynomeZ->GetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ }
+ else
+ {
m_BsplinesZ->GetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ }
break;
case THETA_AXIS:
@@ -322,18 +348,22 @@ double FootTrajectoryGenerationStandard::ComputeAll(FootAbsolutePosition & aFoot
aFootAbsolutePosition.x = m_PolynomeX->Compute(Time);
aFootAbsolutePosition.dx = m_PolynomeX->ComputeDerivative(Time);
// aFootAbsolutePosition.ddx = m_PolynomeX->ComputeSecDerivative(Time);
- ODEBUG2("t: " << Time << " : " << aFootAbsolutePosition.x);
+ ODEBUG2("t: " << Time << " : " << aFootAbsolutePosition.x);
aFootAbsolutePosition.y = m_PolynomeY->Compute(Time);
aFootAbsolutePosition.dy = m_PolynomeY->ComputeDerivative(Time);
// aFootAbsolutePosition.ddy = m_PolynomeY->ComputeSecDerivative(Time);
- //aFootAbsolutePosition.z = m_PolynomeZ->Compute(Time);
- //aFootAbsolutePosition.dz = m_PolynomeZ->ComputeDerivative(Time);
- // aFootAbsolutePosition.ddz = m_PolynomeZ->ComputeSecDerivative(Time);
-
+ if (m_isStepStairOn == 0)
+ {
+ aFootAbsolutePosition.z = m_PolynomeZ->Compute(Time);
+ aFootAbsolutePosition.dz = m_PolynomeZ->ComputeDerivative(Time);
+ }// aFootAbsolutePosition.ddz = m_PolynomeZ->ComputeSecDerivative(Time);
+ else
+ {
aFootAbsolutePosition.z = m_BsplinesZ->ZComputePosition(Time);
aFootAbsolutePosition.dz = m_BsplinesZ->ZComputeVelocity(Time);
+ }
aFootAbsolutePosition.theta = m_PolynomeTheta->Compute(Time);
aFootAbsolutePosition.dtheta = m_PolynomeTheta->ComputeDerivative(Time);
@@ -353,18 +383,24 @@ double FootTrajectoryGenerationStandard::Compute(unsigned int PolynomeIndex, dou
switch (PolynomeIndex)
{
-
+
case X_AXIS:
r=m_PolynomeX->Compute(Time);
break;
-
+
case Y_AXIS:
r=m_PolynomeY->Compute(Time);
break;
case Z_AXIS:
- //r=m_PolynomeZ->Compute(Time);
+ if (m_isStepStairOn == 0)
+ {
+ r=m_PolynomeZ->Compute(Time);
+ }
+ else
+ {
r=m_BsplinesZ->ZComputePosition(Time);
+ }
break;
case THETA_AXIS:
@@ -402,8 +438,14 @@ double FootTrajectoryGenerationStandard::ComputeSecDerivative(unsigned int Polyn
break;
case Z_AXIS:
- //r=m_PolynomeZ->ComputeSecDerivative(Time);
+ if (m_isStepStairOn == 0)
+ {
+ r=m_PolynomeZ->ComputeSecDerivative(Time);
+ }
+ else
+ {
r=m_BsplinesZ->ZComputeVelocity(Time);
+ }
break;
case THETA_AXIS:
@@ -427,10 +469,10 @@ double FootTrajectoryGenerationStandard::ComputeSecDerivative(unsigned int Polyn
void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
- int CurrentAbsoluteIndex,
- int IndexInitial,
+ int CurrentAbsoluteIndex,
+ int IndexInitial,
double ModulatedSingleSupportTime,
- int StepType,
+ int StepType,
int /* LeftOrRight */)
{
unsigned int k = CurrentAbsoluteIndex - IndexInitial;
@@ -440,7 +482,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
double StartLanding = EndOfLiftOff + ModulatedSingleSupportTime;
// The foot support does not move.
- SupportFootAbsolutePositions[CurrentAbsoluteIndex] =
+ SupportFootAbsolutePositions[CurrentAbsoluteIndex] =
SupportFootAbsolutePositions[CurrentAbsoluteIndex-1];
SupportFootAbsolutePositions[CurrentAbsoluteIndex].stepType = (-1)*StepType;
@@ -449,7 +491,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
const FootAbsolutePosition & init_NSFAP = NoneSupportFootAbsolutePositions[IndexInitial];
curr_NSFAP.stepType = StepType;
-
+
if (LocalTime < EndOfLiftOff)
{
// Do not modify x, y and theta while liftoff.
@@ -464,7 +506,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
curr_NSFAP.y = init_NSFAP.y + m_PolynomeY->Compute(LocalTime - EndOfLiftOff);
curr_NSFAP.theta = init_NSFAP.theta + m_PolynomeTheta->Compute(LocalTime - EndOfLiftOff);
}
- else
+ else
{
// Do not modify x, y and theta while landing.
curr_NSFAP.x = init_NSFAP.x + m_PolynomeX->Compute(ModulatedSingleSupportTime);
@@ -472,12 +514,18 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
curr_NSFAP.theta = init_NSFAP.theta + m_PolynomeTheta->Compute(ModulatedSingleSupportTime);
}
- //curr_NSFAP.z = init_NSFAP.z + m_PolynomeZ->Compute(LocalTime);
+ if (m_isStepStairOn == 0)
+ {
+ curr_NSFAP.z = init_NSFAP.z + m_PolynomeZ->Compute(LocalTime);
+ }
+ else
+ {
curr_NSFAP.z = init_NSFAP.z + m_BsplinesZ->ZComputePosition(LocalTime);
+ }
ODEBUG2("x:" << curr_NSFAP.x << " LocalTime - EndOfLiftOff" << LocalTime - EndOfLiftOff
<< " " << m_PolynomeX->Compute(LocalTime - EndOfLiftOff));
// m_PolynomeX->print();
-
+
bool ProtectionNeeded=false;
// Treat Omega with the following strategy:
@@ -494,7 +542,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
m_Omega - m_PolynomeOmega2->Compute(LocalTime-EndOfLiftOff);
}
// Realize the landing.
- else
+ else
{
curr_NSFAP.omega =
m_PolynomeOmega->Compute(LocalTime - StartLanding) - m_Omega;
@@ -512,7 +560,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
{
// Make sure the foot is not going inside the floor.
double dX=0,Z1=0,Z2=0,X1=0,X2=0;
- double B=m_FootB,H=m_FootH,F=m_FootF;
+ double B=m_FootB,H=m_FootH,F=m_FootF;
if (lOmega<0)
{
@@ -548,7 +596,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
co = cos(lOmega);
so = sin(lOmega);
-
+
// COM Orientation
MAL_S3x3_MATRIX(Foot_R,double);
@@ -573,9 +621,9 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
curr_NSFAP.y += dFY ;
curr_NSFAP.z += dFZ ;
#endif
-
- ODEBUG4( "Foot Step:" << StepType << "Foot Shift: "<< Foot_Shift
- << " ( " << dFX<< " , " << dFY<< " , " << " , " << dFZ << " )"
+
+ ODEBUG4( "Foot Step:" << StepType << "Foot Shift: "<< Foot_Shift
+ << " ( " << dFX<< " , " << dFY<< " , " << " , " << dFZ << " )"
<< curr_NSFAP.x << " "
<< curr_NSFAP.y << " "
<< curr_NSFAP.z << " "
@@ -600,7 +648,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
double StartLanding = EndOfLiftOff + ModulatedSingleSupportTime;
// The foot support does not move.
- SupportFootAbsolutePositions[CurrentAbsoluteIndex] =
+ SupportFootAbsolutePositions[CurrentAbsoluteIndex] =
SupportFootAbsolutePositions[StartIndex-1];
SupportFootAbsolutePositions[CurrentAbsoluteIndex].stepType = (-1)*StepType;
@@ -611,13 +659,13 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
{
// Do not modify x, y and theta while liftoff.
// cout<<"no change"<<endl;
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].x;
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].y;
-
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
+
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].theta;
}
else if (LocalInterpolationStartTime < EndOfLiftOff && LocalInterpolationStartTime +InterpolationTime > EndOfLiftOff)
@@ -625,91 +673,105 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
// cout<<"rest changes"<<endl;
// DO MODIFY x, y and theta the remaining time.
// x, dx
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
m_PolynomeX->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
// NoneSupportFootAbsolutePositions[StartIndex-1].x;
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dx =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dx =
m_PolynomeX->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
// NoneSupportFootAbsolutePositions[StartIndex-1].dx;
//y, dy
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
- m_PolynomeY->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
+ m_PolynomeY->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
// NoneSupportFootAbsolutePositions[StartIndex-1].y;
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dy =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dy =
m_PolynomeY->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff); // +
// NoneSupportFootAbsolutePositions[StartIndex-1].dy;
//theta, dtheta
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
- m_PolynomeTheta->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
+ m_PolynomeTheta->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
//NoneSupportFootAbsolutePositions[StartIndex].theta;
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dtheta =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dtheta =
m_PolynomeTheta->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
// +NoneSupportFootAbsolutePositions[StartIndex].dtheta;
}
- else
+ else
{
// cout<<"all changes";
// DO MODIFY x, y and theta all the time.
// x, dx
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
m_PolynomeX->Compute(InterpolationTime);
//+NoneSupportFootAbsolutePositions[StartIndex-1].x;
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dx =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dx =
m_PolynomeX->ComputeDerivative(InterpolationTime);
//+NoneSupportFootAbsolutePositions[StartIndex-1].dx;
//y, dy
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
m_PolynomeY->Compute(InterpolationTime);
//+NoneSupportFootAbsolutePositions[StartIndex].y;
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dy =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dy =
m_PolynomeY->ComputeDerivative(InterpolationTime);
//+NoneSupportFootAbsolutePositions[StartIndex].dy;
//theta, dtheta
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
m_PolynomeTheta->Compute( InterpolationTime );
// +NoneSupportFootAbsolutePositions[StartIndex].theta;
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dtheta =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dtheta =
m_PolynomeTheta->ComputeDerivative(InterpolationTime);
// + NoneSupportFootAbsolutePositions[StartIndex].dtheta;
}
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z =
+ if (m_isStepStairOn == 1)
+ {
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z =
//m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime);
//m_AnklePositionRight[2];
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
//m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
- m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
+ m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
+ //m_AnklePositionRight[2];
+ }
+ else
+ {
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z =
+ m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
+
//m_AnklePositionRight[2];
-
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
+ m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
+
+ //m_AnklePositionRight[2];
+ }
+
bool ProtectionNeeded=false;
// Treat Omega with the following strategy:
// First treat the lift-off.
if (LocalInterpolationStartTime+InterpolationTime<EndOfLiftOff)
{
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
- m_PolynomeOmega->Compute(InterpolationTime); // +
- // NoneSupportFootAbsolutePositions[StartIndex-1].omega;
-
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].domega =
- m_PolynomeOmega->Compute(InterpolationTime);// +
- // NoneSupportFootAbsolutePositions[StartIndex-1].domega;
-
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
+ m_PolynomeOmega->Compute(InterpolationTime); // +
+ // NoneSupportFootAbsolutePositions[StartIndex-1].omega;
+
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].domega =
+ m_PolynomeOmega->Compute(InterpolationTime);// +
+ // NoneSupportFootAbsolutePositions[StartIndex-1].domega;
+
ProtectionNeeded=true;
}
// Prepare for the landing.
else if (LocalInterpolationStartTime+InterpolationTime<StartLanding)
{
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
m_Omega - m_PolynomeOmega2->Compute(LocalInterpolationStartTime+InterpolationTime-EndOfLiftOff)-
NoneSupportFootAbsolutePositions[StartIndex-1].omega2;
}
// Realize the landing.
- else
+ else
{
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
- m_PolynomeOmega->Compute(LocalInterpolationStartTime+InterpolationTime - StartLanding) +
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
+ m_PolynomeOmega->Compute(LocalInterpolationStartTime+InterpolationTime - StartLanding) +
NoneSupportFootAbsolutePositions[StartIndex-1].omega - m_Omega;
ProtectionNeeded=true;
}
@@ -725,7 +787,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
{
// Make sure the foot is not going inside the floor.
double dX=0,Z1=0,Z2=0,X1=0,X2=0;
- double B=m_FootB,H=m_FootH,F=m_FootF;
+ double B=m_FootB,H=m_FootH,F=m_FootF;
if (lOmega<0)
{
@@ -734,7 +796,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
Z1 = H*cos(-lOmega);
Z2 = B*sin(-lOmega);
dX = -(B - X1 + X2);
- dFZ = Z1 + Z2 - H;
+ dFZ = Z1 + Z2 - H;
}
else
{
@@ -743,7 +805,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
Z1 = H*cos(lOmega);
Z2 = F*sin(lOmega);
dX = (F - X1 + X2);
- dFZ = Z1 + Z2 - H;
+ dFZ = Z1 + Z2 - H;
}
dFX = c*dX;
dFY = s*dX;
@@ -761,7 +823,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
co = cos(lOmega);
so = sin(lOmega);
-
+
// COM Orientation
MAL_S3x3_MATRIX(Foot_R,double);
@@ -786,9 +848,9 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y += dFY ;
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z += dFZ ;
#endif
-
- ODEBUG4( "Foot Step:" << StepType << "Foot Shift: "<< Foot_Shift
- << " ( " << dFX<< " , " << dFY<< " , " << " , " << dFZ << " )"
+
+ ODEBUG4( "Foot Step:" << StepType << "Foot Shift: "<< Foot_Shift
+ << " ( " << dFX<< " , " << dFY<< " , " << " , " << dFZ << " )"
<< NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x << " "
<< NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y << " "
<< NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z << " "
@@ -799,7 +861,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
void FootTrajectoryGenerationStandard::ComputingAbsFootPosFromQueueOfRelPos(deque<RelativeFootPosition> &RelativeFootPositions,
deque<FootAbsolutePosition> &AbsoluteFootPositions )
{
-
+
if (AbsoluteFootPositions.size()==0)
AbsoluteFootPositions.resize(RelativeFootPositions.size());
@@ -820,29 +882,29 @@ void FootTrajectoryGenerationStandard::ComputingAbsFootPosFromQueueOfRelPos(dequ
s = sin(RelativeFootPositions[i].theta*M_PI/180.0);
MM(0,0) = c; MM(0,1) = -s;
MM(1,0) = s; MM(1,1) = c;
-
+
/*! Update the orientation */
CurrentAbsTheta+= RelativeFootPositions[i].theta;
CurrentAbsTheta = fmod(CurrentAbsTheta,180.0);
-
+
/*! Extract the current absolute orientation matrix. */
for(int k=0;k<2;k++)
for(int l=0;l<2;l++)
Orientation(k,l) = CurrentSupportFootPosition(k,l);
-
+
/*! Put in a vector form the translation of the relative foot. */
v(0,0) = RelativeFootPositions[i].sx;
v(1,0) = RelativeFootPositions[i].sy;
-
+
/*! Compute the new orientation of the foot vector. */
Orientation = MAL_RET_A_by_B(MM , Orientation);
v2 = MAL_RET_A_by_B(Orientation, v);
-
+
/*! Update the world coordinates of the support foot. */
for(int k=0;k<2;k++)
for(int l=0;l<2;l++)
CurrentSupportFootPosition(k,l) = Orientation(k,l);
-
+
for(int k=0;k<2;k++)
CurrentSupportFootPosition(k,2) += v2(k,0);
@@ -866,6 +928,6 @@ void FootTrajectoryGenerationStandard::print()
m_PolynomeOmega2->print();
std::cout << "Polynome Yaw:" <<std::endl;
m_PolynomeTheta->print();
-
+
}
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
index 3891071140e1675e39357585876d7b7b72017370..e94cfac4621374424625c65a83f12884a3d00b59 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
@@ -1,7 +1,7 @@
/*
- * Copyright 2008, 2009, 2010,
+ * Copyright 2008, 2009, 2010,
+ *
*
- *
* Olivier Stasse
*
* JRL, CNRS/AIST
@@ -19,7 +19,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/*! \file FootTrajectoryGenerationStandard.h
@@ -40,7 +40,7 @@ namespace PatternGeneratorJRL
/** @ingroup foottrajectorygeneration
This class generates a trajectory for the swinging foot during single support phase.
- It uses a classical approach relying in polynome of 3rd orders for the position in the
+ It uses a classical approach relying in polynome of 3rd orders for the position in the
orthogonal plan as well as the direction.For the height modification a 4th order polynome
is used. Finally a landing and take off phase using an angular value (\f$\omega\f$).
*/
@@ -48,9 +48,9 @@ namespace PatternGeneratorJRL
{
public:
- /*!\name Constants related to the direction for the generation of the polynomes.
+ /*!\name Constants related to the direction for the generation of the polynomes.
@{ */
-
+
/*! \brief along the frontal direction */
const static unsigned int X_AXIS = 0;
/*! \brief along the left of the robot */
@@ -75,16 +75,16 @@ namespace PatternGeneratorJRL
/*! This method computes the position of the swinging foot during single support phase,
and maintain a constant position for the support foot.
- It uses polynomial of 3rd order for the X-axis, Y-axis,
+ It uses polynomial of 3rd order for the X-axis, Y-axis,
orientation in the X-Z axis, and orientation in the X-Y axis,
and finally it uses a 4th order polynome for the Z-axis.
-
+
@param SupportFootAbsolutePositions: Queue of absolute position for the support foot.
This method will set the foot position at index CurrentAbsoluteIndex of the queue.
This position is supposed to be constant.
@param NoneSupportFootAbsolutePositions: Queue of absolute position for the swinging
foot. This method will set the foot position at index NoneSupportFootAbsolutePositions
- of the queue.
+ of the queue.
@param CurrentAbsoluteIndex: Index in the queues of the foot position to be set.
@param IndexInitial: Index in the queues which correspond to the starting point
of the current single support phase.
@@ -94,11 +94,11 @@ namespace PatternGeneratorJRL
*/
virtual void UpdateFootPosition(deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
- int CurrentAbsoluteIndex,
- int IndexInitial,
+ int CurrentAbsoluteIndex,
+ int IndexInitial,
double ModulatedSingleSupportTime,
int StepType,int LeftOrRight);
-
+
virtual void UpdateFootPosition(deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
int StartIndex, int k,
@@ -111,17 +111,17 @@ namespace PatternGeneratorJRL
In this specific case, it is in charge of creating the polynomial structures.
*/
virtual void InitializeInternalDataStructures();
-
+
/*! Free internal data structures.
In this specific case, it is in charge of freeing the polynomial data structures.
*/
virtual void FreeInternalDataStructures();
-
+
/*! This method specifies the parameters for each of the polynome used by this
object. In this case, as it is used for the 3rd order polynome. The polynome to
- which those parameters are set is specified with PolynomeIndex.
- It assumes an initial position and an initial speed set to zero.
- @param[in] AxisReference: Set to which axis the parameters will be applied.
+ which those parameters are set is specified with PolynomeIndex.
+ It assumes an initial position and an initial speed set to zero.
+ @param[in] AxisReference: Set to which axis the parameters will be applied.
@param[in] TimeInterval: Set the time base of the polynome.
@param[in] Position: Set the final position of the polynome at TimeInterval.
*/
@@ -131,8 +131,8 @@ namespace PatternGeneratorJRL
/*! This method specifies the parameters for each of the polynome used by this
object. In this case, as it is used for the 3rd order polynome. The polynome to
- which those parameters are set is specified with PolynomeIndex.
- @param[in] AxisReference: Set to which axis the parameters will be applied.
+ which those parameters are set is specified with PolynomeIndex.
+ @param[in] AxisReference: Set to which axis the parameters will be applied.
@param[in] TimeInterval: Set the time base of the polynome.
@param[in] FinalPosition: Set the final position of the polynome at TimeInterval.
@param[in] InitPosition: Initial position when computing the polynome at t=0.0.
@@ -146,8 +146,8 @@ namespace PatternGeneratorJRL
/*! This method get the parameters for each of the polynome used by this
object. In this case, as it is used for the 3rd order polynome. The polynome to
- which those parameters are set is specified with PolynomeIndex.
- @param[in] AxisReference: Set to which axis the parameters will be applied.
+ which those parameters are set is specified with PolynomeIndex.
+ @param[in] AxisReference: Set to which axis the parameters will be applied.
@param[in] TimeInterval: Set the time base of the polynome.
@param[in] FinalPosition: Set the final position of the polynome at TimeInterval.
@param[in] InitPosition: Initial position when computing the polynome at t=0.0.
@@ -179,26 +179,26 @@ namespace PatternGeneratorJRL
/*! Compute the value for a given polynome's second derivative. */
double ComputeSecDerivative(unsigned int PolynomeIndex, double Time);
- /*! Compute the absolute foot position from the queue of relative positions.
+ /*! Compute the absolute foot position from the queue of relative positions.
There is not direct dependency with time.
*/
void ComputingAbsFootPosFromQueueOfRelPos(deque<RelativeFootPosition> &RelativeFootPositions,
deque<FootAbsolutePosition> &AbsoluteFootPositions);
/*! Methods to compute a set of positions for the feet according to the discrete time given in parameters
- and the phase of walking.
+ and the phase of walking.
@{
*/
-
+
/*! @} */
-
+
void print();
protected:
-
+
/*! \brief Polynomes for X and Y axis positions*/
Polynome5 *m_PolynomeX,*m_PolynomeY;
-
+
/*! \brief Polynome for X-Y orientation */
Polynome3 *m_PolynomeTheta;
@@ -211,6 +211,8 @@ namespace PatternGeneratorJRL
/*! \brief Bsplines for Z axis position. */
ZBsplines *m_BsplinesZ;
+ int m_isStepStairOn;
+
/*! \brief Foot dimension. */
double m_FootB, m_FootH, m_FootF;
@@ -221,8 +223,8 @@ namespace PatternGeneratorJRL
MAL_S3_VECTOR(m_AnklePositionRight,double);
};
-
-
+
+
}
#endif /* _FOOT_TRAJECTORY_GENERATION_ABSTRACT_H_ */
diff --git a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
index 6b28881be612eb9eb21a44550fd7f29cbad34c3e..2397ef5abcf6da21a6e14d29a96eb7bbe3034e07 100644
--- a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
+++ b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
@@ -20,7 +20,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/* This object generate all the values for the foot trajectories, */
@@ -65,7 +65,7 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
const FootAbsolutePosition & prev_NSFAP = NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1];
// The foot support does not move.
- SupportFootAbsolutePositions[CurrentAbsoluteIndex] =
+ SupportFootAbsolutePositions[CurrentAbsoluteIndex] =
SupportFootAbsolutePositions[StartIndex-1];
SupportFootAbsolutePositions[CurrentAbsoluteIndex].stepType = (-1)*StepType;
@@ -99,7 +99,7 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
if(m_PolynomeTheta->Degree() > 4)
curr_NSFAP.ddtheta = m_PolynomeTheta->ComputeSecDerivative(remainingTime);
}
- else
+ else
{
// DO MODIFY x, y and theta all the time.
// x, dx
@@ -119,13 +119,23 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
curr_NSFAP.ddtheta = m_PolynomeTheta->ComputeSecDerivative(InterpolationTime);
}
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z =
- m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime);
+ if (m_isStepStairOn == 1)
+ {
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z =
+ m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime);
//m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
// m_PolynomeZ->ComputeDerivative(LocalInterpolationStartTime+InterpolationTime);
+ }
+ else{
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z =
+ m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
+ m_PolynomeZ->ComputeDerivative(LocalInterpolationStartTime+InterpolationTime);
+
+ }
bool ProtectionNeeded=false;
// Treat Omega with the following strategy:
@@ -148,7 +158,7 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
NoneSupportFootAbsolutePositions[StartIndex-1].omega2;
}
// Realize the landing.
- else
+ else
{
curr_NSFAP.omega =
m_PolynomeOmega->Compute(LocalInterpolationStartTime+InterpolationTime - StartLanding) +
@@ -167,7 +177,7 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
{
// Make sure the foot is not going inside the floor.
double dX=0,Z1=0,Z2=0,X1=0,X2=0;
- double B=m_FootB,H=m_FootH,F=m_FootF;
+ double B=m_FootB,H=m_FootH,F=m_FootF;
if (lOmega<0)
{
diff --git a/tests/TestBsplines.cpp b/tests/TestBsplines.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7b3a6f6cedf9a09774d5a5b0f4bc6a18565ebecb
--- /dev/null
+++ b/tests/TestBsplines.cpp
@@ -0,0 +1,79 @@
+/*
+ * Copyright 2009, 2010, 2014
+ *
+ *
+ * Olivier Stasse, Huynh Ngoc Duc
+ *
+ * JRL, CNRS/AIST
+ *
+ * This file is part of walkGenJrl.
+ * walkGenJrl is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * walkGenJrl is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Lesser Public License for more details.
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Research carried out within the scope of the
+ * Joint Japanese-French Robotics Laboratory (JRL)
+ */
+/*! \file TestBsplines.cpp
+ \brief This Example shows you how to use Bsplines to create a foot trajectory on Z . */
+#include <stdlib.h>
+#include <iostream>
+#include <fstream>
+#include "Mathematics/Bsplines.hh"
+
+using namespace std;
+
+int main()
+{
+ PatternGeneratorJRL::ZBsplines *Z;
+ double t=0.0;
+ int m_degree;
+ int i , j;
+ ofstream myfile;
+ myfile.open("TestBsplines.txt");
+
+ //Create the parameters of foot trajectory on Z
+ double m_FT = 0.7;
+ double m_FP = 0.2;
+ double m_MP = 0.3;
+ double m_ToMP = m_FT/3.0;
+
+ //Create an object for test
+ Z = new PatternGeneratorJRL::ZBsplines(m_FT, m_FP, m_ToMP, m_MP);
+
+ Z->PrintDegree();
+ Z->PrintKnotVector();
+ Z->PrintControlPoints();
+
+ for (int k=1; k<1000;k++)
+ {
+
+ t=double(k)*Z->GetKnotVector().back()/1000.0;
+ cout << k << endl;
+ myfile << t << " " << Z->ZComputePosition(t) << " " << Z->ZComputeVelocity(t)<< " " << Z->ZComputeAcc(t)<< endl;
+ // time - Position - Velocity - Acceleration
+ cout << t << " " << Z->ZComputePosition(t)<<" "<< Z->ZComputeVelocity(t)<< " "<< Z->ZComputeAcc(t)<< endl;
+ }
+ myfile.close();
+ delete Z;
+
+ //draw a foot trajectory with the data given from bsplines
+ myfile.open("DrawTestBsplines.gnu");
+ myfile << "set term wxt 0" << endl;
+ myfile << "plot 'control_point.txt' with points, 'TestBsplines.txt' using 1:2 with lines title 'Pos'"<< endl;
+ myfile << "set term wxt 1" << endl;
+ myfile << "plot 'TestBsplines.txt' using 1:2 with lines title 'Pos', 'TestBsplines.txt' using 1:3 with lines title 'Speed','TestBsplines.txt' using 1:4 with lines title 'Acc'"<< endl;
+ myfile.close();
+
+ return 0;
+}
+
+