From 913acba1601c2d1d094ca23fd1708faf13d220ca Mon Sep 17 00:00:00 2001
From: Francois Keith <francois.keith@inrialpes.fr>
Date: Wed, 24 Aug 2011 09:52:47 +0200
Subject: [PATCH] Correct some rebase errors
---
.../FootTrajectoryGenerationMultiple.cpp | 2 +-
.../FootTrajectoryGenerationStandard.cpp | 15 ++++++---------
.../FootTrajectoryGenerationStandard.h | 4 ++--
.../OnLineFootTrajectoryGeneration.cpp | 13 ++++++++-----
src/Mathematics/PolynomeFoot.cpp | 3 ---
5 files changed, 17 insertions(+), 20 deletions(-)
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp
index c1db63e2..cb670d35 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp
@@ -252,7 +252,7 @@ int FootTrajectoryGenerationMultiple::SetParametersWithInitPosInitSpeedInitAcc(u
return -1;
- m_SetOfFootTrajectoryGenerationObjects[IntervalIndex]->SetParametersWithInitPosInitSpeedInitAcc(AxisReference,
+ m_SetOfFootTrajectoryGenerationObjects[IntervalIndex]->SetParameters(AxisReference,
TimeInterval,
FinalPosition,
InitPosition,
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
index 5afaf1d0..5d5973dc 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
@@ -208,15 +208,15 @@ int FootTrajectoryGenerationStandard::SetParametersWithInitPosInitSpeed(int Poly
break;
case THETA_AXIS:
- m_PolynomeTheta->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,0.0);
+ m_PolynomeTheta->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
break;
case OMEGA_AXIS:
- m_PolynomeOmega->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,0.0);
+ m_PolynomeOmega->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
break;
case OMEGA2_AXIS:
- m_PolynomeOmega2->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,0.0);
+ m_PolynomeOmega2->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
break;
default:
@@ -268,28 +268,25 @@ double FootTrajectoryGenerationStandard::ComputeAll(FootAbsolutePosition & aFoot
{
aFootAbsolutePosition.x = m_PolynomeX->Compute(Time);
aFootAbsolutePosition.dx = m_PolynomeX->ComputeDerivative(Time);
- aFootAbsolutePosition.ddx = m_PolynomeX->ComputeSecondDerivative(Time);
+// aFootAbsolutePosition.ddx = m_PolynomeX->ComputeSecDerivative(Time);
ODEBUG("t: " << Time << " : " << aFootAbsolutePosition.x);
aFootAbsolutePosition.y = m_PolynomeY->Compute(Time);
aFootAbsolutePosition.dy = m_PolynomeY->ComputeDerivative(Time);
- aFootAbsolutePosition.ddy = m_PolynomeY->ComputeSecondDerivative(Time);
+// aFootAbsolutePosition.ddy = m_PolynomeY->ComputeSecDerivative(Time);
aFootAbsolutePosition.z = m_PolynomeZ->Compute(Time);
aFootAbsolutePosition.dz = m_PolynomeZ->ComputeDerivative(Time);
- aFootAbsolutePosition.ddz = m_PolynomeZ->ComputeSecondDerivative(Time);
+// aFootAbsolutePosition.ddz = m_PolynomeZ->ComputeSecDerivative(Time);
aFootAbsolutePosition.theta = m_PolynomeTheta->Compute(Time);
aFootAbsolutePosition.dtheta = m_PolynomeTheta->ComputeDerivative(Time);
- aFootAbsolutePosition.ddtheta = m_PolynomeTheta->ComputeSecondDerivative(Time);
aFootAbsolutePosition.omega = m_PolynomeOmega->Compute(Time);
aFootAbsolutePosition.domega = m_PolynomeOmega->ComputeDerivative(Time);
- aFootAbsolutePosition.ddomega = m_PolynomeOmega->ComputeSecondDerivative(Time);
aFootAbsolutePosition.omega2 = m_PolynomeOmega2->Compute(Time);
aFootAbsolutePosition.domega2 = m_PolynomeOmega2->ComputeDerivative(Time);
- aFootAbsolutePosition.ddomega2 = m_PolynomeOmega2->ComputeSecondDerivative(Time);
return Time;
}
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h
index 24fb4dca..cfcc820e 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h
@@ -178,10 +178,10 @@ namespace PatternGeneratorJRL
Polynome5 *m_PolynomeX,*m_PolynomeY;
/*! \brief Polynome for X-Y orientation */
- Polynome5 *m_PolynomeTheta;
+ Polynome3 *m_PolynomeTheta;
/*! \brief Polynome for Y-Z orientation */
- Polynome5 *m_PolynomeOmega, *m_PolynomeOmega2;
+ Polynome3 *m_PolynomeOmega, *m_PolynomeOmega2;
/*! \brief Polynome for Z axis position. */
Polynome5 *m_PolynomeZ;
diff --git a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
index 25642a17..ec3ade95 100644
--- a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
+++ b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
@@ -96,7 +96,8 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
//theta, dtheta
curr_NSFAP.theta = m_PolynomeTheta->Compute(remainingTime);
curr_NSFAP.dtheta = m_PolynomeTheta->ComputeDerivative(remainingTime);
- curr_NSFAP.ddtheta = m_PolynomeTheta->ComputeSecondDerivative(remainingTime);
+ if(m_PolynomeTheta->Degree() > 4)
+ curr_NSFAP.ddtheta = m_PolynomeTheta->ComputeSecDerivative(remainingTime);
}
else
{
@@ -114,7 +115,8 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
//theta, dtheta
curr_NSFAP.theta = m_PolynomeTheta->Compute( InterpolationTime );
curr_NSFAP.dtheta = m_PolynomeTheta->ComputeDerivative(InterpolationTime);
- curr_NSFAP.ddtheta = m_PolynomeTheta->ComputeSecondDerivative(InterpolationTime);
+ if(m_PolynomeTheta->Degree() > 4)
+ curr_NSFAP.ddtheta = m_PolynomeTheta->ComputeSecDerivative(InterpolationTime);
}
if(HalfTimePassed_==false && LocalInterpolationStartTime+InterpolationTime >= m_TSingle/2.0 )
@@ -153,7 +155,8 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
curr_NSFAP.omega = m_PolynomeOmega->Compute(InterpolationTime);
//TODO curr_NSFAP.domega = m_PolynomeOmega->Compute(InterpolationTime);
curr_NSFAP.domega = m_PolynomeOmega->ComputeDerivative(InterpolationTime);
- curr_NSFAP.ddomega = m_PolynomeOmega->ComputeSecondDerivative(InterpolationTime);
+ if(m_PolynomeOmega->Degree() > 4)
+ curr_NSFAP.ddomega = m_PolynomeOmega->ComputeSecDerivative(InterpolationTime);
ProtectionNeeded=true;
}
@@ -293,11 +296,11 @@ OnLineFootTrajectoryGeneration::interpolate_feet_positions(double Time,
}
//Set parameters for current polynomial
- double TimeInterval = UnlockedSwingPeriod-InterpolationTimePassed;
+ double TimeInterval = UnlockedSwingPeriod-SwingTimePassed;
SetParameters(
FootTrajectoryGenerationStandard::X_AXIS,
TimeInterval,FPx,
- LastSFP->x, LastSFP->dx, LastSFP->ddx)
+ LastSFP->x, LastSFP->dx, LastSFP->ddx
);
SetParameters(
FootTrajectoryGenerationStandard::Y_AXIS,
diff --git a/src/Mathematics/PolynomeFoot.cpp b/src/Mathematics/PolynomeFoot.cpp
index 561937d8..d213d83d 100644
--- a/src/Mathematics/PolynomeFoot.cpp
+++ b/src/Mathematics/PolynomeFoot.cpp
@@ -153,9 +153,6 @@ void Polynome5::SetParameters(double FT, double FP,
m_Coefficients[5] = ( -1.0/2.0*InitAcc*FT*FT - 3.0*InitSpeed*FT - 6.0*InitPos + 6.0*FP)/tmp;
}
-Polynome5::~Polynome5()
-{}
-
Polynome6::Polynome6(double FT, double MP) :Polynome(6)
{
SetParameters(FT,MP);
--
GitLab