diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
index 973613c8e8f85362db221c9f8e75b53ce658af5e..009ab141dc9b4f1e24f7833bfd3a85ae36287f1a 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
@@ -123,7 +123,7 @@ void FootTrajectoryGenerationStandard::InitializeInternalDataStructures()
m_PolynomeX = new Polynome5(0,0);
m_PolynomeY = new Polynome5(0,0);
- m_PolynomeZ = new Polynome4(0,0);
+ m_PolynomeZ = new Polynome6(0,0);
m_BsplinesZ = new ZBsplines(0,0,0,0);
m_PolynomeOmega = new Polynome3(0,0);
m_PolynomeOmega2 = new Polynome3(0,0);
@@ -214,7 +214,8 @@ int FootTrajectoryGenerationStandard::SetParametersWithInitPosInitSpeed(int Poly
case Z_AXIS:
- m_PolynomeZ->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition+m_StepHeight,InitPosition,InitSpeed,FinalPosition);
+ m_PolynomeZ->SetParametersWithMiddlePos(TimeInterval, FinalPosition+m_StepHeight,
+ InitPosition, InitSpeed, 0.0, FinalPosition);
if ((FinalPosition - InitPosition) == m_StepHeight)
m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,InitPosition,0.5*TimeInterval,InitPosition+m_StepHeight);
@@ -263,7 +264,8 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex, double Ti
case Z_AXIS:
// if (m_isStepStairOn == 0)
//{
- m_PolynomeZ->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ m_PolynomeZ->SetParametersWithMiddlePos(TimeInterval,FinalPosition+m_StepHeight,InitPosition,InitSpeed,
+ InitAcc,FinalPosition);
//}
//else
//{
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
index d11b6a022a30ff1e2391411c3e1346699d75779d..05ea1911d6ea6596c9f8100c3d401f8ddde18e9c 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
@@ -167,7 +167,7 @@ namespace PatternGeneratorJRL
/// \param[in] InitSpeed
/// \param[in] InitAcc
int SetParameters(int PolynomeIndex, double TimeInterval,
- double FinalPosition, double InitPosition, double InitSpeed, double InitAcc, double InitJerk=0.0);
+ double FinalPosition, double InitPosition, double InitSpeed, double InitAcc=0.0, double InitJerk=0.0);
/*! Fill an absolute foot position structure for a given time. */
double ComputeAll(FootAbsolutePosition & aFootAbsolutePosition,
@@ -208,7 +208,7 @@ namespace PatternGeneratorJRL
Polynome3 *m_PolynomeOmega, *m_PolynomeOmega2;
/*! \brief Polynome for Z axis position. */
- Polynome4 *m_PolynomeZ;
+ Polynome6 *m_PolynomeZ;
/*! \brief Bsplines for Z axis position. */
ZBsplines *m_BsplinesZ;
diff --git a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
index e229a0e34997a1e1b7b9af83e5ceb0c7421fbf9f..5f5854d76b1c4171863b3e90f68306d192cf0502 100644
--- a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
+++ b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
@@ -130,6 +130,7 @@ void
{
curr_NSFAP.z = m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);
curr_NSFAP.dz = m_PolynomeZ->ComputeDerivative(LocalInterpolationStartTime+InterpolationTime);
+ curr_NSFAP.ddz = m_PolynomeZ->ComputeSecDerivative(LocalInterpolationStartTime+InterpolationTime);
}
else
{
@@ -303,19 +304,21 @@ void
//Set parameters for current polynomial
double TimeInterval = UnlockedSwingPeriod-SwingTimePassed;
SetParameters(
- FootTrajectoryGenerationStandard::X_AXIS,
- TimeInterval,FPx,
- LastSFP->x, LastSFP->dx, LastSFP->ddx, LastSFP->dddx
- );
+ FootTrajectoryGenerationStandard::X_AXIS,
+ TimeInterval,FPx,
+ LastSFP->x, LastSFP->dx, LastSFP->ddx, LastSFP->dddx
+ );
SetParameters(
- FootTrajectoryGenerationStandard::Y_AXIS,
- TimeInterval,FPy,
- LastSFP->y, LastSFP->dy, LastSFP->ddy, LastSFP->dddy
- );
+ FootTrajectoryGenerationStandard::Y_AXIS,
+ TimeInterval,FPy,
+ LastSFP->y, LastSFP->dy, LastSFP->ddy, LastSFP->dddy
+ );
if(CurrentSupport.StateChanged==true)
{
- SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::Z_AXIS,
- m_TSingle,m_AnklePositionLeft[2],LastSFP->z, LastSFP->dz);
+ SetParameters(FootTrajectoryGenerationStandard::Z_AXIS,
+ m_TSingle,m_AnklePositionLeft[2],
+ LastSFP->z, LastSFP->dz, LastSFP->ddz
+ );
}
SetParameters(
diff --git a/src/Mathematics/PolynomeFoot.cpp b/src/Mathematics/PolynomeFoot.cpp
index e49210be1f4b35f186a2d9ff5f48cbb1ec445b86..527ed9e6f9ba219c33bdd2f6b1eaae78a5e162c0 100644
--- a/src/Mathematics/PolynomeFoot.cpp
+++ b/src/Mathematics/PolynomeFoot.cpp
@@ -230,28 +230,31 @@ void Polynome5::SetParameters(double FT, double FP,
}
}
-Polynome6::Polynome6(double FT, double MP) :PolynomeFoot(6,FT)
+Polynome6::Polynome6(double FT, double MP, double FP) :PolynomeFoot(6,FT)
{
- SetParameters(FT,MP);
+ SetParameters(FT,MP,FP);
}
-void Polynome6::SetParameters(double FT, double MP)
+void Polynome6::SetParameters(double FT, double MP, double FP)
{
- SetParameters(FT,MP,
+ SetParametersWithMiddlePos(FT,MP,
/*InitPos=*/0.0,
/*InitSpeed=*/0.0,
- /*InitAcc=*/0.0);
+ /*InitAcc=*/0.0,
+ FP);
}
-void Polynome6::SetParameters(
+void Polynome6::SetParametersWithMiddlePos(
double FT, double MP,
- double InitPos, double InitSpeed, double InitAcc)
+ double InitPos, double InitSpeed, double InitAcc,
+ double FP)
{
FT_=FT;
MP_=MP;
+ FP_=FP;
InitPos_=InitPos;
InitSpeed_=InitSpeed;
InitAcc_=InitAcc;
@@ -265,14 +268,27 @@ void Polynome6::SetParameters(
m_Coefficients[5] = 0.0;
m_Coefficients[6] = 0.0;
}else{
- m_Coefficients[3] = -0.5*(5*FT*FT*InitAcc + 32*InitSpeed*FT + 84*InitPos - 128*MP)/(FT*FT*FT);
- m_Coefficients[4] = 0.5*(76*InitSpeed*FT + 222*InitPos - 384*MP + 9*FT*FT*InitAcc)/(FT*FT*FT*FT);
- m_Coefficients[5] = -0.5*(204*InitPos + 66*InitSpeed*FT - 384*MP + 7*FT*FT*InitAcc)/(FT*FT*FT*FT*FT);
- m_Coefficients[6] = (-64*MP+32*InitPos + 10*InitSpeed*FT + FT*FT*InitAcc)/(FT*FT*FT*FT*FT*FT);
+ m_Coefficients[3] = -0.5*( 5*FT*FT*InitAcc+32*FT*InitSpeed-128*MP+ 84*InitPos+ 44*FP )/(FT*FT*FT);
+ m_Coefficients[4] = 0.5*( 9*FT*FT*InitAcc+76*FT*InitSpeed-384*MP+222*InitPos+162*FP )/(FT*FT*FT*FT);
+ m_Coefficients[5] = -0.5*( 7*FT*FT*InitAcc+66*FT*InitSpeed-384*MP+204*InitPos+180*FP )/(FT*FT*FT*FT*FT);
+ m_Coefficients[6] = ( FT*FT*InitAcc+10*FT*InitSpeed- 64*MP+ 32*InitPos+ 32*FP )/(FT*FT*FT*FT*FT*FT);
}
}
+void Polynome6::GetParametersWithInitPosInitSpeed(double &TimeInterval,
+ double &MiddlePosition,
+ double &FinalPosition,
+ double &InitPosition,
+ double &InitSpeed)
+{
+ TimeInterval = FT_ ;
+ MiddlePosition = MP_ ;
+ FinalPosition = FP_ ;
+ InitPosition = InitPos_ ;
+ InitSpeed = InitSpeed_ ;
+}
+
Polynome6::~Polynome6()
{
}
diff --git a/src/Mathematics/PolynomeFoot.hh b/src/Mathematics/PolynomeFoot.hh
index ea796fabacc14e0aa61313544b43b34d585f6b2f..41a5e842d6399596649ad7edee2c027fe62136f1 100644
--- a/src/Mathematics/PolynomeFoot.hh
+++ b/src/Mathematics/PolynomeFoot.hh
@@ -194,20 +194,24 @@ namespace PatternGeneratorJRL
class Polynome6 : public PolynomeFoot
{
private:
- double MP_, InitPos_, InitSpeed_,InitAcc_;
+ double MP_, FP_, InitPos_, InitSpeed_,InitAcc_;
public:
/// Constructor:
/// FT: Final time
/// MP: Middle position
- Polynome6(double FT, double MP);
+ Polynome6(double FT, double MP, double FP=0.0);
/// Set the parameters
// Initial acceleration, velocity and position by default 0
// Final acceleration, velocity and position are 0
- void SetParameters(double FT, double MP);
- void SetParameters(double FT, double MP,
- double InitPos, double InitSpeed, double InitAcc);
-
+ void SetParameters(double FT, double MP, double FP = 0.0);
+ void SetParametersWithMiddlePos(double FT, double MP,
+ double InitPos, double InitSpeed, double InitAcc=0.0, double FP = 0.0);
+ void GetParametersWithInitPosInitSpeed(double &TimeInterval,
+ double &MiddlePosition,
+ double &FinalPosition,
+ double &InitPosition,
+ double &InitSpeed);
/// Destructor.
~Polynome6();
};
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
index c75ba473ce2737f272678132cb985d1fe95b4924..2993820759394210f610fd5dcb95aa0a6bb03c7d 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
@@ -48,7 +48,7 @@ ComAndFootRealizationByGeometry::
m_ZARM = -1.0;
m_LeftShoulder = 0;
m_RightShoulder = 0;
-
+ ShiftFoot_ = false ;
RegisterMethods();
for(unsigned int i=0;i<3;i++)
@@ -716,7 +716,10 @@ bool ComAndFootRealizationByGeometry::
Ankle = getHumanoidDynamicRobot()->leftAnkle();
}
- Foot_P = Foot_P + Foot_Shift;
+ if(ShiftFoot_)
+ {
+ Foot_P = Foot_P + Foot_Shift;
+ }
// Foot_P(2)-=(aCoMPosition(2) + ToTheHip(2));
ODEBUG("Body_P:" << endl << Body_P);
ODEBUG("Body_R:" << endl << Body_R);
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.hh b/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
index c186108cf589262047f0c54c7c363e9ca8ca315f..ed142aebcb9b09b9b7b3016690b1aa57a1170a88 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
@@ -290,6 +290,10 @@ namespace PatternGeneratorJRL
inline MAL_VECTOR_TYPE(double) & GetPreviousVelocityStage1()
{ return m_prev_Velocity1 ;};
+ inline bool ShiftFoot()
+ {return ShiftFoot_ ;}
+ inline void ShiftFoot(bool ShiftFoot)
+ {ShiftFoot_=ShiftFoot ;}
/*! \brief Get the COG of the ankles at the starting position. */
virtual MAL_S3_VECTOR_TYPE(double) GetCOGInitialAnkles();
@@ -451,6 +455,8 @@ namespace PatternGeneratorJRL
/*! Store the position of the left and right shoulders. */
CjrlJoint *m_LeftShoulder, *m_RightShoulder;
+
+ bool ShiftFoot_ ;
};
ostream & operator <<(ostream &os,const ComAndFootRealization &obj);
diff --git a/src/PatternGeneratorInterfacePrivate.cpp b/src/PatternGeneratorInterfacePrivate.cpp
index 1f8e7efcbbc0128d1912bb76b600d5421d5357f9..53b6ceafd4e82338811a1d15ac09d6bf510408fa 100644
--- a/src/PatternGeneratorInterfacePrivate.cpp
+++ b/src/PatternGeneratorInterfacePrivate.cpp
@@ -528,8 +528,7 @@ namespace PatternGeneratorJRL {
COMState lStartingCOMState;
memset(&lStartingCOMState,0,sizeof(COMState));
- MAL_S3_VECTOR_TYPE(double) lStartingZMPPosition;
- MAL_VECTOR( BodyAnglesIni,double);
+ MAL_S3_VECTOR_TYPE(double) lStartingZMPPosition;;
FootAbsolutePosition InitLeftFootAbsPos, InitRightFootAbsPos;
memset(&InitLeftFootAbsPos,0,sizeof(InitLeftFootAbsPos));
@@ -1288,7 +1287,6 @@ namespace PatternGeneratorJRL {
FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition )
{
-
m_InternalClock+=m_SamplingPeriod;
if ((!m_ShouldBeRunning) ||
@@ -1360,8 +1358,6 @@ namespace PatternGeneratorJRL {
m_RightFootPositions);
m_Running = m_ZMPVRQP->Running();
}
-
-
m_GlobalStrategyManager->OneGlobalStepOfControl(LeftFootPosition,
RightFootPosition,
ZMPTarget,
@@ -1369,7 +1365,6 @@ namespace PatternGeneratorJRL {
CurrentConfiguration,
CurrentVelocity,
CurrentAcceleration);
-
ODEBUG("finalCOMState: " <<
finalCOMState.x[0] << " " <<
finalCOMState.x[1] << " " <<
@@ -1758,9 +1753,9 @@ namespace PatternGeneratorJRL {
// Carefull : Extremly specific to the pattern generator.
- double cx,cy,cz, sx,sy,sz;
- cx = 0; cy = 0; cz = cos(0.5*m_AbsTheta);
- sx = 0; sy = 0; sz = sin(0.5*m_AbsTheta);
+ double /*cx,cy,*/ cz, /*sx,sy,*/ sz;
+ /*cx = 0; cy = 0;*/ cz = cos(0.5*m_AbsTheta);
+ /*sx = 0; sy = 0;*/ sz = sin(0.5*m_AbsTheta);
aTQ[3] = 0;
aTQ[4] = 0;
aTQ[5] = sz;
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index 7fabffb58b2d28478191c600934b36bf99432b17..61b53b7bba434d492d1ea8db79ef5584f0b6f61d 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -234,15 +234,15 @@ int DynamicFilter::OnLinefilter(
const deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
deque<COMState> & outputDeltaCOMTraj_deq_)
{
- static int currentIteration = 0 ;
+ int currentIteration = 20 ;
vector< MAL_VECTOR_TYPE(double) > q_vec ;
vector< MAL_VECTOR_TYPE(double) > dq_vec ;
vector< MAL_VECTOR_TYPE(double) > ddq_vec ;
- for(unsigned int i = 0 ; i <= NCtrl_; ++i)
+ for(unsigned int i = 0 ; i < NbI_; ++i)
{
- InverseKinematics( ctrlCoMState[i], ctrlLeftFoot[i],
- ctrlRightFoot[i], ZMPMBConfiguration_, ZMPMBVelocity_,
- ZMPMBAcceleration_, controlPeriod_, stage0_, currentIteration+i) ;
+ InverseKinematics( inputCOMTraj_deq_[i], inputLeftFootTraj_deq_[i],
+ inputRightFootTraj_deq_[i], ZMPMBConfiguration_, ZMPMBVelocity_,
+ ZMPMBAcceleration_, interpolationPeriod_, stage0_, currentIteration) ;
}
unsigned int N = PG_N_ * NbI_ ;
@@ -255,7 +255,7 @@ int DynamicFilter::OnLinefilter(
inputRightFootTraj_deq_[i],
ZMPMB_vec_[i],
stage1_,
- currentIteration + i);
+ currentIteration);
q_vec.push_back(ZMPMBConfiguration_);
dq_vec.push_back(ZMPMBVelocity_);
ddq_vec.push_back(ZMPMBAcceleration_);
@@ -274,7 +274,6 @@ int DynamicFilter::OnLinefilter(
}
OptimalControl(deltaZMP_deq_,outputDeltaCOMTraj_deq_) ;
- currentIteration += NbI_ ;
//#############################################################################
deque<COMState> CoM_tmp = ctrlCoMState ;
for (unsigned int i = 0 ; i < NCtrl_ ; ++i)
@@ -296,7 +295,7 @@ int DynamicFilter::OnLinefilter(
ctrlRightFoot[i],
zmpmb_corr[i],
stage2,
- currentIteration + i);
+ 20);
}
ofstream aof;
@@ -420,7 +419,7 @@ void DynamicFilter::InverseKinematics(
int iteration)
{
- // lower body
+ // lower body !!!!! the angular quantities are set in degree !!!!!!
aCoMState_(0) = inputCoMState.x[0]; aCoMSpeed_(0) = inputCoMState.x[1];
aCoMState_(1) = inputCoMState.y[0]; aCoMSpeed_(1) = inputCoMState.y[1];
aCoMState_(2) = inputCoMState.z[0]; aCoMSpeed_(2) = inputCoMState.z[1];
diff --git a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp
index bb1f2c4ca6c8052b9ccd216ea5f4868dd93a9775..2800966f21b39de032cb144e6ba12244c492f7ee 100644
--- a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp
+++ b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp
@@ -68,6 +68,9 @@ OrientationsPreview::OrientationsPreview(CjrlHumanoidDynamicRobot *aHS)
uaLimitHipYaw_ = 0.1;
//Maximal cross angle between the feet
uLimitFeet_ = 5.0/180.0*M_PI;
+
+ // Polynome to interpolate the trunk orientation
+ TrunkStateTheta_ = new Polynome5(0.0,0.0);
}
OrientationsPreview::~OrientationsPreview() {
diff --git a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh
index d690ff0757652b3dd0ccd5683859420d9536cff5..c1e5f96d65e78d030f63139a0c26c7ab11e8fe42 100644
--- a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh
+++ b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh
@@ -37,6 +37,7 @@
#include <privatepgtypes.hh>
#include <jrl/walkgen/pgtypes.hh>
+#include <Mathematics/PolynomeFoot.hh>
#include <abstract-robot-dynamics/humanoid-dynamic-robot.hh>
namespace PatternGeneratorJRL
@@ -222,6 +223,8 @@ namespace PatternGeneratorJRL
/// \brief State of the trunk at the first previewed sampling
COMState TrunkStateT_;
+ Polynome5 * TrunkStateTheta_ ;
+
};
}
#endif /* ORIENTATIONSPREVIEW_H_ */
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index a7326e1652a93cc1f2e7148e8f01a7fd0641a45e..b117cead058943f9d00a5c49cd95c9ce23b72259 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -78,7 +78,7 @@ Solution_(),OFTG_DF_(0),OFTG_control_(0),dynamicFilter_(0)
QP_T_ = 0.1 ;
QP_N_ = 16 ;
m_SamplingPeriod = 0.005 ;
- InterpolationPeriod_ = QP_T_/2;
+ InterpolationPeriod_ = QP_T_/10;
NbSampleControl_ = (int)round(QP_T_/m_SamplingPeriod) ;
NbSampleInterpolation_ = (int)round(QP_T_/InterpolationPeriod_) ;
StepPeriod_ = 0.8 ;
@@ -93,7 +93,6 @@ Solution_(),OFTG_DF_(0),OFTG_control_(0),dynamicFilter_(0)
Solution_.useWarmStart=false ;
CurrentIndex_ = 0 ;
- CurrentIndex_DF_ = 0 ;
// Create and initialize online interpolation of feet trajectories
RFI_ = new RelativeFeetInequalities( SPM,aHS );
@@ -132,7 +131,7 @@ Solution_(),OFTG_DF_(0),OFTG_control_(0),dynamicFilter_(0)
// Initialize the 2D LIPM
LIPM_subsampled_.SetSimulationControlPeriod( QP_T_ );
- LIPM_subsampled_.SetRobotControlPeriod( InterpolationPeriod_ );
+ LIPM_subsampled_.SetRobotControlPeriod( m_SamplingPeriod );
LIPM_subsampled_.InitializeSystem();
// Initialize the 2D LIPM
@@ -170,7 +169,7 @@ Solution_(),OFTG_DF_(0),OFTG_control_(0),dynamicFilter_(0)
OFTG_DF_->InitializeInternalDataStructures();
OFTG_DF_->SetSingleSupportTime( SSPeriod_ );
OFTG_DF_->SetDoubleSupportTime( DSPeriod_ );
- OFTG_DF_->SetSamplingPeriod( InterpolationPeriod_ );
+ OFTG_DF_->SetSamplingPeriod( m_SamplingPeriod );
OFTG_DF_->QPSamplingPeriod( QP_T_ );
OFTG_DF_->NbSamplingsPreviewed( QP_N_ );
OFTG_DF_->FeetDistance( FeetDistance_ );
@@ -209,8 +208,13 @@ Solution_(),OFTG_DF_(0),OFTG_control_(0),dynamicFilter_(0)
// init of the buffer for the kajita's dynamic filter
// size = numberOfIterationOfThePreviewControl * NumberOfSample + Margin
- ZMPTraj_deq_.resize( QP_N_ * NbSampleInterpolation_+20);
- COMTraj_deq_.resize( QP_N_ * NbSampleInterpolation_+20);
+ ZMPTraj_deq_.resize( QP_N_ * NbSampleInterpolation_+10);
+ COMTraj_deq_.resize( QP_N_ * NbSampleInterpolation_+10);
+ LeftFootTraj_deq_.resize( QP_N_ * NbSampleInterpolation_+10) ;
+ RightFootTraj_deq_.resize( QP_N_ * NbSampleInterpolation_+10) ;
+
+ ZMPTraj_deq_ctrl_.resize( QP_N_ * NbSampleControl_+10) ;
+ COMTraj_deq_ctrl_.resize( QP_N_ * NbSampleControl_+10) ;
}
@@ -268,8 +272,7 @@ ZMPVelocityReferencedQP::~ZMPVelocityReferencedQP()
}
-void
- ZMPVelocityReferencedQP::setCoMPerturbationForce(istringstream &strm)
+void ZMPVelocityReferencedQP::setCoMPerturbationForce(istringstream &strm)
{
MAL_VECTOR_RESIZE(PerturbationAcceleration_,6);
@@ -281,8 +284,7 @@ void
PerturbationOccured_ = true;
}
-void
- ZMPVelocityReferencedQP::setCoMPerturbationForce(double x, double y)
+void ZMPVelocityReferencedQP::setCoMPerturbationForce(double x, double y)
{
MAL_VECTOR_RESIZE(PerturbationAcceleration_,6);
@@ -462,7 +464,7 @@ int ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
FinalCurrentStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
dynamicFilter_->init(m_SamplingPeriod,InterpolationPeriod_,
- QP_T_, QP_N_*QP_T_ - QP_T_/m_SamplingPeriod * InterpolationPeriod_ ,CoMHeight_,InitLeftFootAbsolutePosition,lStartingCOMState);
+ QP_T_, QP_N_*QP_T_/2 - QP_T_/m_SamplingPeriod * InterpolationPeriod_ ,CoMHeight_,InitLeftFootAbsolutePosition,lStartingCOMState);
return 0;
}
@@ -475,6 +477,7 @@ void ZMPVelocityReferencedQP::OnLine(double time,
deque<FootAbsolutePosition> & FinalRightFootTraj_deq)
{
+ cout << "time = " << time << endl ;
// If on-line mode not activated we go out.
if (!m_OnLineMode)
{
@@ -546,63 +549,80 @@ void ZMPVelocityReferencedQP::OnLine(double time,
Problem_.dump( time );
}
VRQPGenerator_->LastFootSol(Solution_);
+ //OrientPrw_->
// INITIALIZE INTERPOLATION:
// ------------------------
CurrentIndex_ = FinalCOMTraj_deq.size();
- CurrentIndex_DF_ = (int)round(CurrentIndex_ * m_SamplingPeriod / InterpolationPeriod_) ;
- LeftFootTraj_deq_.resize(CurrentIndex_DF_) ;
- RightFootTraj_deq_.resize(CurrentIndex_DF_) ;
-
- cout << "CurrentIndex_ = " << CurrentIndex_ << " CurrentIndex_DF_ = " << CurrentIndex_DF_ << endl << endl;
-
-
- int inc = (int)round(InterpolationPeriod_ / m_SamplingPeriod) ;
- for (unsigned int i = 0 , j = 0 ; j < CurrentIndex_DF_ ; i = i + inc , ++j )
+ for (unsigned int i = 0 ; i < CurrentIndex_ ; ++i )
{
- ZMPTraj_deq_[j] = FinalZMPTraj_deq[i] ;
- COMTraj_deq_[j] = FinalCOMTraj_deq[i] ;
- LeftFootTraj_deq_[j] = FinalLeftFootTraj_deq[i] ;
- RightFootTraj_deq_[j] = FinalRightFootTraj_deq[i] ;
+ ZMPTraj_deq_ctrl_[i] = FinalZMPTraj_deq[i] ;
+ COMTraj_deq_ctrl_[i] = FinalCOMTraj_deq[i] ;
}
- FinalZMPTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
- FinalCOMTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
+ LeftFootTraj_deq_ctrl_ = FinalLeftFootTraj_deq ;
+ RightFootTraj_deq_ctrl_ = FinalRightFootTraj_deq ;
- // INTERPRET THE SOLUTION VECTOR :
- // -------------------------------
solution_ = Solution_ ;
InterpretSolutionVector();
// INTERPOLATION
+ FinalZMPTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
+ FinalCOMTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
ControlInterpolation( FinalCOMTraj_deq, FinalZMPTraj_deq, FinalLeftFootTraj_deq,
FinalRightFootTraj_deq, time) ;
DynamicFilterInterpolation(time);
- deque<COMState> outputDeltaCOMTraj_deq ;
- dynamicFilter_->OnLinefilter(time,FinalCOMTraj_deq,
- FinalLeftFootTraj_deq,
- FinalRightFootTraj_deq,
- COMTraj_deq_,ZMPTraj_deq_,
- LeftFootTraj_deq_,
- RightFootTraj_deq_,
- outputDeltaCOMTraj_deq);
- // Correct the CoM.
- for (unsigned int i = 0 ; i < NbSampleControl_ ; ++i)
+ unsigned int IndexMax = (int)round(QP_N_*QP_T_*0.5 / InterpolationPeriod_);
+ ZMPTraj_deq_.resize(IndexMax);
+ COMTraj_deq_.resize(IndexMax);
+ LeftFootTraj_deq_.resize(IndexMax);
+ RightFootTraj_deq_.resize(IndexMax);
+ int inc = (int)round(InterpolationPeriod_ / m_SamplingPeriod) ;
+ for (unsigned int i = 0 , j = 0 ; j < IndexMax ; i = i + inc , ++j )
{
- for(int j=0;j<3;j++)
- {
- FinalCOMTraj_deq[i].x[j] += outputDeltaCOMTraj_deq[i].x[j] ;
- FinalCOMTraj_deq[i].y[j] += outputDeltaCOMTraj_deq[i].y[j] ;
- }
+ ZMPTraj_deq_[j] = ZMPTraj_deq_ctrl_[i] ;
+ COMTraj_deq_[j] = COMTraj_deq_ctrl_[i] ;
+ LeftFootTraj_deq_[j] = LeftFootTraj_deq_ctrl_[i] ;
+ RightFootTraj_deq_[j] = RightFootTraj_deq_ctrl_[i] ;
}
- // Specify that we are in the ending phase.
- if (EndingPhase_ == false)
+ bool filterOn_ = true ;
+ if(filterOn_)
{
- TimeToStopOnLineMode_ = UpperTimeLimitToUpdate_ + QP_T_ * QP_N_;
+ dynamicFilter_->OnLinefilter(time,FinalCOMTraj_deq,
+ FinalLeftFootTraj_deq,
+ FinalRightFootTraj_deq,
+ COMTraj_deq_,ZMPTraj_deq_,
+ LeftFootTraj_deq_,
+ RightFootTraj_deq_,
+ deltaCOMTraj_deq_);
+ // Correct the CoM.
+ for (unsigned int i = 0 ; i < NbSampleControl_ ; ++i)
+ {
+ for(int j=0;j<3;j++)
+ {
+ FinalCOMTraj_deq[i].x[j] += deltaCOMTraj_deq_[i].x[j] ;
+ FinalCOMTraj_deq[i].y[j] += deltaCOMTraj_deq_[i].y[j] ;
+ }
+ }
}
- UpperTimeLimitToUpdate_ = UpperTimeLimitToUpdate_ + QP_T_;
+ // Specify that we are in the ending phase.
+ if (time <= m_SamplingPeriod )
+ {
+ if (EndingPhase_ == false)
+ {
+ TimeToStopOnLineMode_ = UpperTimeLimitToUpdate_ + QP_T_ * QP_N_ + m_SamplingPeriod;
+ }
+ UpperTimeLimitToUpdate_ = UpperTimeLimitToUpdate_ + QP_T_ + m_SamplingPeriod ;
+ }else{
+ if (EndingPhase_ == false)
+ {
+ TimeToStopOnLineMode_ = UpperTimeLimitToUpdate_ + QP_T_ * QP_N_;
+ }
+ UpperTimeLimitToUpdate_ = UpperTimeLimitToUpdate_ + QP_T_;
+ }
+
}
//-----------------------------------
@@ -640,6 +660,13 @@ void ZMPVelocityReferencedQP::ControlInterpolation(
Solution_.SupportStates_deq, Solution_,
Solution_.SupportOrientations_deq,
FinalLeftFootTraj_deq, FinalRightFootTraj_deq);
+
+ for(unsigned int i = 0 ; i<FinalCOMTraj_deq.size() ; ++i)
+ {
+ FinalCOMTraj_deq[i].yaw[0] = 0.5*(FinalRightFootTraj_deq[i].theta +FinalLeftFootTraj_deq[i].theta );
+ FinalCOMTraj_deq[i].yaw[1] = 0.5*(FinalRightFootTraj_deq[i].dtheta +FinalLeftFootTraj_deq[i].dtheta );
+ FinalCOMTraj_deq[i].yaw[2] = 0.5*(FinalRightFootTraj_deq[i].ddtheta+FinalLeftFootTraj_deq[i].ddtheta );
+ }
return ;
}
@@ -650,16 +677,16 @@ void ZMPVelocityReferencedQP::DynamicFilterInterpolation(double time)
OrientPrw_DF_->CurrentTrunkState(InitStateOrientPrw_) ;
for ( int i = 0 ; i < QP_N_ ; i++ )
{
- CoMZMPInterpolation(ZMPTraj_deq_, COMTraj_deq_,
- LeftFootTraj_deq_, RightFootTraj_deq_,
+ CoMZMPInterpolation(ZMPTraj_deq_ctrl_, COMTraj_deq_ctrl_,
+ LeftFootTraj_deq_ctrl_, RightFootTraj_deq_ctrl_,
&Solution_, &LIPM_subsampled_,
- NbSampleInterpolation_, i, CurrentIndex_DF_);
+ NbSampleControl_, i, CurrentIndex_);
}
InterpretSolutionVector();
// Copy the solution for the orientation interpolation function
- OFTG_DF_->SetSamplingPeriod( InterpolationPeriod_ );
+ OFTG_DF_->SetSamplingPeriod( m_SamplingPeriod );
solution_t aSolution = Solution_ ;
//solution_.SupportStates_deq.pop_front();
@@ -668,12 +695,12 @@ void ZMPVelocityReferencedQP::DynamicFilterInterpolation(double time)
aSolution.SupportOrientations_deq.clear();
OrientPrw_DF_->preview_orientations( time + i * QP_T_, VelRef_,
SupportFSM_->StepPeriod(),
- LeftFootTraj_deq_, RightFootTraj_deq_,
+ LeftFootTraj_deq_ctrl_, RightFootTraj_deq_ctrl_,
aSolution);
OrientPrw_DF_->interpolate_trunk_orientation( time + i * QP_T_,
- CurrentIndex_DF_ + i * NbSampleInterpolation_, InterpolationPeriod_,
- solution_.SupportStates_deq, COMTraj_deq_ );
+ CurrentIndex_ + i * NbSampleControl_, m_SamplingPeriod,
+ solution_.SupportStates_deq, COMTraj_deq_ctrl_ );
// Modify a copy of the solution to allow "OFTG_DF_->interpolate_feet_positions"
// to use the correcte feet step previewed
@@ -681,7 +708,7 @@ void ZMPVelocityReferencedQP::DynamicFilterInterpolation(double time)
OFTG_DF_->interpolate_feet_positions( time + i * QP_T_,
solution_.SupportStates_deq, solution_,
aSolution.SupportOrientations_deq,
- LeftFootTraj_deq_, RightFootTraj_deq_);
+ LeftFootTraj_deq_ctrl_, RightFootTraj_deq_ctrl_);
solution_.SupportStates_deq.pop_front();
}
@@ -689,6 +716,13 @@ void ZMPVelocityReferencedQP::DynamicFilterInterpolation(double time)
OrientPrw_DF_->CurrentTrunkState(FinalPreviewStateOrientPrw_);
OFTG_DF_->copyPolynomesFromFTGS(OFTG_control_);
+
+ for(unsigned int i = 0 ; i<QP_N_*NbSampleControl_ ; ++i)
+ {
+ COMTraj_deq_ctrl_[i].yaw[0] = 0.5*(LeftFootTraj_deq_ctrl_[i].theta +RightFootTraj_deq_ctrl_[i].theta );
+ COMTraj_deq_ctrl_[i].yaw[1] = 0.5*(LeftFootTraj_deq_ctrl_[i].dtheta +RightFootTraj_deq_ctrl_[i].dtheta );
+ COMTraj_deq_ctrl_[i].yaw[2] = 0.5*(LeftFootTraj_deq_ctrl_[i].ddtheta+RightFootTraj_deq_ctrl_[i].ddtheta );
+ }
return ;
}
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index ef1300ceb86d1d0c4d46dd2c5b5cea611346883a..bfb8484dd5c6464978a174d500090329a8aa0d99 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -218,18 +218,22 @@ namespace PatternGeneratorJRL
CjrlHumanoidDynamicRobot * HDR_ ;
/// \brief Buffers for the Kajita's dynamic filter
- deque<ZMPPosition> ZMPTraj_deq_ ;
- deque< vector<double> >ZMPMBTraj_deq_ ;
- deque<COMState> COMTraj_deq_ ;
deque<COMState> deltaCOMTraj_deq_ ;
+
+ deque<ZMPPosition> ZMPTraj_deq_ ;
+ deque<COMState> COMTraj_deq_ ;
deque<FootAbsolutePosition> LeftFootTraj_deq_ ;
deque<FootAbsolutePosition> RightFootTraj_deq_ ;
+ deque<ZMPPosition> ZMPTraj_deq_ctrl_ ;
+ deque<COMState> COMTraj_deq_ctrl_ ;
+ deque<FootAbsolutePosition> LeftFootTraj_deq_ctrl_ ;
+ deque<FootAbsolutePosition> RightFootTraj_deq_ctrl_ ;
+
vector< vector<double> > FootPrw_vec ;
/// \brief Index where to begin the interpolation
unsigned CurrentIndex_ ;
- unsigned CurrentIndex_DF_ ;
/// \brief Interpolation Period for the dynamic filter
double InterpolationPeriod_ ;
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index 5539db8cea06a87345270256ee8fcd76a5dd386e..5203b73fb389b17582bbb9f59d4b34af038acd2e 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -181,8 +181,6 @@ public:
m_clock.fillInStatistics();
/*! Fill the debug files with appropriate information. */
- ComparingZMPs();
- ComputeAndDisplayAverageError(false);
fillInDebugFiles();
}
else
@@ -199,7 +197,6 @@ public:
m_clock.writeBuffer(lProfileOutput);
m_clock.displayStatistics(os,m_OneStep);
// Compare debugging files
- ComputeAndDisplayAverageError(true);
return compareDebugFiles();
}
@@ -281,14 +278,21 @@ protected:
ComAndFootRealization_->Initialization();
}
- void fillInDebugFiles( )
+ void fillInDebugFiles()
{
+ TestObject::fillInDebugFiles();
if (m_DebugFGPI)
{
+ static int inc = 0 ;
ofstream aof;
string aFileName;
aFileName = m_TestName;
- aFileName += "TestFGPI.dat";
+ aFileName += "TestFGPIFull.dat";
+ if (inc==0)
+ {
+ aof.open(aFileName.c_str(),ofstream::out);
+ }
+ inc = 1;
aof.open(aFileName.c_str(),ofstream::app);
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
@@ -296,7 +300,7 @@ protected:
<< filterprecision(m_OneStep.finalCOMPosition.x[0] ) << " " // 2
<< filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " " // 3
<< filterprecision(m_OneStep.finalCOMPosition.z[0] ) << " " // 4
- << filterprecision(m_OneStep.finalCOMPosition.yaw[0] ) << " " // 5
+ << filterprecision(m_OneStep.finalCOMPosition.yaw[0]*M_PI/180 ) << " " // 5
<< filterprecision(m_OneStep.finalCOMPosition.x[1] ) << " " // 6
<< filterprecision(m_OneStep.finalCOMPosition.y[1] ) << " " // 7
<< filterprecision(m_OneStep.finalCOMPosition.z[1] ) << " " // 8
@@ -335,90 +339,9 @@ protected:
aof.close();
}
-
- /// \brief Debug Purpose
- /// --------------------
- ofstream aof;
- string aFileName;
- ostringstream oss(std::ostringstream::ate);
-
- if ( iteration == 0 ){
- oss.str("/tmp/walk_mnaveau.pos");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- }
- ///----
- oss.str("/tmp/walk_mnaveau.pos");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- aof << filterprecision( iteration * 0.1 ) << " " ; // 1
- for(unsigned int i = 6 ; i < m_CurrentConfiguration.size() ; i++){
- aof << filterprecision( m_CurrentConfiguration(i) ) << " " ; // 1
- }
- for(unsigned int i = 0 ; i < 10 ; i++){
- aof << 0.0 << " " ;
- }
- aof << endl ;
- aof.close();
-
- if ( iteration == 0 ){
- oss.str("/tmp/walk_mnaveau.hip");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- }
- oss.str("/tmp/walk_mnaveau.hip");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- for(unsigned int j = 0 ; j < 20 ; j++){
- aof << filterprecision( iteration * 0.1 ) << " " ; // 1
- aof << filterprecision( 0.0 ) << " " ; // 1
- aof << filterprecision( 0.0 ) << " " ; // 1
- aof << filterprecision( m_OneStep.finalCOMPosition.yaw[0] ) << " " ; // 1
- aof << endl ;
- }
- aof.close();
-
- iteration++;
- }
-
- void SpecializedRobotConstructor( CjrlHumanoidDynamicRobot *& aHDR, CjrlHumanoidDynamicRobot *& aDebugHDR )
- {
- dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
- Chrp2OptHumanoidDynamicRobot *aHRP2HDR = new Chrp2OptHumanoidDynamicRobot( &aRobotDynamicsObjectConstructor );
- aHDR = aHRP2HDR;
- aDebugHDR = new Chrp2OptHumanoidDynamicRobot(&aRobotDynamicsObjectConstructor);
- }
-
- double filterprecision(double adb)
- {
- if (fabs(adb)<1e-7)
- return 0.0;
-
- double ladb2 = adb * 1e7;
- double lintadb2 = trunc(ladb2);
- return lintadb2/1e7;
- }
-
- void ComparingZMPs()
- {
- const int stage0 = 0 ;
/// \brief calculate, from the CoM of computed by the preview control,
/// the corresponding articular position, velocity and acceleration
/// ------------------------------------------------------------------
- MAL_VECTOR(CurrentConfiguration,double);
- MAL_VECTOR(CurrentVelocity,double);
- MAL_VECTOR(CurrentAcceleration,double);
-
- MAL_VECTOR_RESIZE(CurrentConfiguration, m_HDR->numberDof());
- MAL_VECTOR_RESIZE(CurrentVelocity, m_HDR->numberDof());
- MAL_VECTOR_RESIZE(CurrentAcceleration, m_HDR->numberDof());
-
MAL_VECTOR_DIM(aCOMState,double,6);
MAL_VECTOR_DIM(aCOMSpeed,double,6);
MAL_VECTOR_DIM(aCOMAcc,double,6);
@@ -428,9 +351,9 @@ protected:
aCOMState(0) = m_OneStep.finalCOMPosition.x[0]; aCOMSpeed(0) = m_OneStep.finalCOMPosition.x[1]; aCOMAcc(0) = m_OneStep.finalCOMPosition.x[2];
aCOMState(1) = m_OneStep.finalCOMPosition.y[0]; aCOMSpeed(1) = m_OneStep.finalCOMPosition.y[1]; aCOMAcc(1) = m_OneStep.finalCOMPosition.y[2];
aCOMState(2) = m_OneStep.finalCOMPosition.z[0]; aCOMSpeed(2) = m_OneStep.finalCOMPosition.z[1]; aCOMAcc(2) = m_OneStep.finalCOMPosition.z[2];
- aCOMState(3) = m_OneStep.finalCOMPosition.roll[0]; aCOMSpeed(3) = m_OneStep.finalCOMPosition.roll[1]; aCOMAcc(3) = m_OneStep.finalCOMPosition.roll[2];
- aCOMState(4) = m_OneStep.finalCOMPosition.pitch[0]; aCOMSpeed(4) = m_OneStep.finalCOMPosition.pitch[1]; aCOMAcc(4) = m_OneStep.finalCOMPosition.pitch[2];
- aCOMState(5) = m_OneStep.finalCOMPosition.yaw[0]; aCOMSpeed(5) = m_OneStep.finalCOMPosition.yaw[1]; aCOMAcc(5) = m_OneStep.finalCOMPosition.yaw[2];
+ aCOMState(3) = m_OneStep.finalCOMPosition.roll[0]; aCOMSpeed(3) = m_OneStep.finalCOMPosition.roll[1]; aCOMAcc(3) = m_OneStep.finalCOMPosition.roll[2];
+ aCOMState(4) = m_OneStep.finalCOMPosition.pitch[0]; aCOMSpeed(4) = m_OneStep.finalCOMPosition.pitch[1]; aCOMAcc(4) = m_OneStep.finalCOMPosition.pitch[2];
+ aCOMState(5) = m_OneStep.finalCOMPosition.yaw[0]; aCOMSpeed(5) = m_OneStep.finalCOMPosition.yaw[1]; aCOMAcc(5) = m_OneStep.finalCOMPosition.yaw[2];
aLeftFootPosition(0) = m_OneStep.LeftFootPosition.x; aRightFootPosition(0) = m_OneStep.RightFootPosition.x;
aLeftFootPosition(1) = m_OneStep.LeftFootPosition.y; aRightFootPosition(1) = m_OneStep.RightFootPosition.y;
@@ -441,151 +364,95 @@ protected:
ComAndFootRealization_->ComputePostureForGivenCoMAndFeetPosture(aCOMState, aCOMSpeed, aCOMAcc,
aLeftFootPosition,
aRightFootPosition,
- CurrentConfiguration,
- CurrentVelocity,
- CurrentAcceleration,
- m_OneStep.NbOfIt,
- stage0);
+ m_CurrentConfiguration,
+ m_CurrentVelocity,
+ m_CurrentAcceleration,
+ 20,
+ 0);
- /// \brief Debug Purpose
+ /// \brief Create file .hip .pos .zmp
/// --------------------
- ofstream aof;
- string aFileName;
- ostringstream oss(std::ostringstream::ate);
- oss.str("TestHerdt2010DynamicART2.dat");
- aFileName = oss.str();
- if ( iteration_zmp == 0 )
+ ofstream aof ;
+ string root = "/opt/grx/HRP2LAAS/etc/mnaveau/" ;
+ string aFileName = root + m_TestName + ".pos" ;
+ if ( iteration == 0 )
{
aof.open(aFileName.c_str(),ofstream::out);
aof.close();
}
- ///----
aof.open(aFileName.c_str(),ofstream::app);
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
- for (unsigned int j = 0 ; j < CurrentConfiguration.size() ; ++j)
- {
- aof << filterprecision(CurrentConfiguration(j)) << " " ;
- }
- for (unsigned int j = 0 ; j < CurrentVelocity.size() ; ++j)
- {
- aof << filterprecision(CurrentVelocity(j)) << " " ;
- }
- for (unsigned int j = 0 ; j < CurrentAcceleration.size() ; ++j)
- {
- aof << filterprecision(CurrentAcceleration(j)) << " " ;
- }
- aof << endl ;
-
-
- /// \brief rnea, calculation of the multi body ZMP
- /// ----------------------------------------------
- Robot_Model2::confVector q, dq, ddq;
- for(unsigned int j = 0 ; j < CurrentConfiguration.size() ; j++ )
- {
- q(j,0) = CurrentConfiguration[j] ;
- dq(j,0) = CurrentVelocity[j] ;
- ddq(j,0) = CurrentAcceleration[j] ;
+ aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+ for(unsigned int i = 6 ; i < m_CurrentConfiguration.size() ; i++){
+ aof << filterprecision( m_CurrentConfiguration(i) ) << " " ; // 2
}
- metapod::rnea< Robot_Model2, true >::run(robot_, q, dq, ddq);
-
- Node2 anode = boost::fusion::at_c<Robot_Model2::BODY>(robot_.nodes);
- Force2 aforce = anode.body.iX0.applyInv (anode.joint.f) ;
-
- double ZMPMB[2];
-
- ZMPMB[0] = - aforce.n()[1] / aforce.f()[2] ;
- ZMPMB[1] = aforce.n()[0] / aforce.f()[2] ;
-
-
- if (m_OneStep.NbOfIt<=10){
- dInitX = m_OneStep.ZMPTarget(0) - ZMPMB[0] ;
- dInitY = m_OneStep.ZMPTarget(1) - ZMPMB[1] ;
- {
- vector<double> tmp_zmp(2) ;
- tmp_zmp[0] =0.0 ;
- tmp_zmp[1] =0.0 ;
- errZMP_.push_back(tmp_zmp);
- }
+ for(unsigned int i = 0 ; i < 9 ; i++){
+ aof << 0.0 << " " ;
}
+ aof << 0.0 << endl ;
+ aof.close();
- if (m_OneStep.NbOfIt >= 10)
- {
- double errx = sqrt( ( m_OneStep.ZMPTarget(0) - ZMPMB[0] - dInitX )*( m_OneStep.ZMPTarget(0) - ZMPMB[0] - dInitX ) ) ;
- double erry = sqrt( ( m_OneStep.ZMPTarget(1) - ZMPMB[1] - dInitY )*( m_OneStep.ZMPTarget(1) - ZMPMB[1] - dInitY ) ) ;
- {
- vector<double> tmp_zmp(2) ;
- tmp_zmp[0] = errx ;
- tmp_zmp[1] = erry ;
- errZMP_.push_back(tmp_zmp);
- }
+ aFileName = root + m_TestName + ".hip" ;
+ if ( iteration == 0 ){
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
}
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+ aof << filterprecision( m_OneStep.finalCOMPosition.roll[0]) << " " ; // 2
+ aof << filterprecision( m_OneStep.finalCOMPosition.pitch[0]) << " " ;// 3
+ aof << filterprecision( m_OneStep.finalCOMPosition.yaw[0]) ; // 4
+ aof << endl ;
+ aof.close();
-
- static double ecartmaxX = 0 ;
- static double ecartmaxY = 0 ;
- if ( abs(errZMP_.back()[0]) > ecartmaxX )
- ecartmaxX = abs(errZMP_.back()[0]) ;
- if ( abs(errZMP_.back()[1]) > ecartmaxY )
- ecartmaxY = abs(errZMP_.back()[1]) ;
-
- // cout << "ecartmaxX :" << ecartmaxX << endl ;
- // cout << "ecartmaxY :" << ecartmaxY << endl ;
-
- // Writing of the two zmps and the error.
- if (once)
- {
- aof.open("TestHerdt2010ErrorZMP.dat",ofstream::out);
+ aFileName = root + m_TestName + ".waist" ;
+ if ( iteration == 0 ){
+ aof.open(aFileName.c_str(),ofstream::out);
aof.close();
- once = false ;
}
- aof.open("TestHerdt2010ErrorZMP.dat",ofstream::app);
+ aof.open(aFileName.c_str(),ofstream::app);
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
- aof << filterprecision( iteration_zmp ) << " " // 1
- << filterprecision( ZMPMB[0] + dInitX ) << " " // 2
- << filterprecision( ZMPMB[1] + dInitY ) << " " // 3
- << filterprecision(m_OneStep.ZMPTarget(0) ) << " " // 4
- << filterprecision(m_OneStep.ZMPTarget(1) ) << " " // 5
- << endl ;
+ aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+ aof << filterprecision( m_OneStep.finalCOMPosition.roll[0]) << " " ; // 2
+ aof << filterprecision( m_OneStep.finalCOMPosition.pitch[0]) << " " ;// 3
+ aof << filterprecision( m_OneStep.finalCOMPosition.yaw[0]) ; // 4
+ aof << endl ;
aof.close();
- iteration_zmp++;
- return ;
- }
-
- void ComputeAndDisplayAverageError(bool display){
- static int plot_it = 0 ;
- double moyErrX = 0 ;
- double moyErrY = 0 ;
- for (unsigned int i = 0 ; i < errZMP_.size(); ++i)
- {
- moyErrX += errZMP_[i][0] ;
- moyErrY += errZMP_[i][1] ;
- }
- moyErrX = moyErrX / errZMP_.size() ;
- moyErrY = moyErrY / errZMP_.size() ;
- if ( display )
- {
- cout << "moyErrX = " << moyErrX << endl
- << "moyErrY = " << moyErrY << endl ;
- }
- ofstream aof;
- string aFileName;
- aFileName = m_TestName;
- aFileName += "MoyZMP.dat";
- if(plot_it==0){
+ aFileName = root + m_TestName + ".zmp" ;
+ if ( iteration == 0 ){
aof.open(aFileName.c_str(),ofstream::out);
aof.close();
}
+ FootAbsolutePosition aSupportState;
+ if (m_OneStep.LeftFootPosition.stepType < 0 )
+ aSupportState = m_OneStep.LeftFootPosition ;
+ else
+ aSupportState = m_OneStep.RightFootPosition ;
+
aof.open(aFileName.c_str(),ofstream::app);
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
- aof << filterprecision(moyErrX ) << " " // 1
- << filterprecision(moyErrY ) << " " // 2
- << endl ;
+ aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+ aof << filterprecision( m_OneStep.ZMPTarget(0) - m_CurrentConfiguration(0)) << " " ; // 2
+ aof << filterprecision( m_OneStep.ZMPTarget(1) - m_CurrentConfiguration(1) ) << " " ;// 3
+ aof << filterprecision( aSupportState.z - m_CurrentConfiguration(2)) ; // 4
+ aof << endl ;
aof.close();
- plot_it++;
+
+ iteration++;
+ }
+
+ void SpecializedRobotConstructor( CjrlHumanoidDynamicRobot *& aHDR, CjrlHumanoidDynamicRobot *& aDebugHDR )
+ {
+ dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
+ Chrp2OptHumanoidDynamicRobot *aHRP2HDR = new Chrp2OptHumanoidDynamicRobot( &aRobotDynamicsObjectConstructor );
+ aHDR = aHRP2HDR;
+ aDebugHDR = new Chrp2OptHumanoidDynamicRobot(&aRobotDynamicsObjectConstructor);
}
void startOnLineWalking(PatternGeneratorInterface &aPGI)
@@ -755,62 +622,54 @@ protected:
void generateEventOnLineWalking()
{
-
struct localEvent
{
unsigned time;
localeventHandler_t Handler ;
};
-
-#define localNbOfEvents 4
+#define localNbOfEvents 12
struct localEvent events [localNbOfEvents] =
- {
- { 1*200,&TestHerdt2010::walkForward},
- { 6*200,&TestHerdt2010::walkSidewards},
- {11*200,&TestHerdt2010::stop},
- {20*200,&TestHerdt2010::stopOnLineWalking}
-// { 1*200,&TestHerdt2010::walkForward},
-// { 5*200,&TestHerdt2010::walkSidewards},
-// {10*200,&TestHerdt2010::startTurningRightOnSpot},
-// {15*200,&TestHerdt2010::walkForward},
-// {20*200,&TestHerdt2010::startTurningLeftOnSpot},
-// {25*200,&TestHerdt2010::walkForward},
-// {30*200,&TestHerdt2010::startTurningRightOnSpot},
-// {35*200,&TestHerdt2010::walkForward},
-// {40*200,&TestHerdt2010::startTurningLeft},
-// {45*200,&TestHerdt2010::startTurningRight},
-// {50*200,&TestHerdt2010::stop},
-// {55*200,&TestHerdt2010::stopOnLineWalking}
- };
-
+ { { 5*200,&TestHerdt2010::walkForward},
+ {10*200,&TestHerdt2010::startTurningRightOnSpot},
+/* {25*200,&TestHerdt2010::startTurningRightOnSpot},
+ {35*200,&TestHerdt2010::walkForward},
+ {45*200,&TestHerdt2010::startTurningLeftOnSpot},*//*
+ {55*200,&TestHerdt2010::walkForward},
+ {65*200,&TestHerdt2010::startTurningRightOnSpot},
+ {75*200,&TestHerdt2010::walkForward},
+ {85*200,&TestHerdt2010::startTurningLeft},
+ {95*200,&TestHerdt2010::startTurningRight},*/
+// {105*200,&TestHerdt2010::stop},
+// {110*200,&TestHerdt2010::stopOnLineWalking}};
+ {15*200,&TestHerdt2010::stop},
+ {20*200,&TestHerdt2010::stopOnLineWalking}};
// Test when triggering event.
for(unsigned int i=0;i<localNbOfEvents;i++)
- {
- if ( m_OneStep.NbOfIt==events[i].time){
- ODEBUG3("********* GENERATE EVENT ONLINE WALKING ***********");
- (this->*(events[i].Handler))(*m_PGI);
+ {
+ if ( m_OneStep.NbOfIt==events[i].time)
+ {
+ ODEBUG3("********* GENERATE EVENT OLW ***********");
+ (this->*(events[i].Handler))(*m_PGI);
+ }
}
- }
}
-
void generateEventEmergencyStop()
{
-
-#define localNbOfEventsEMS 3
+#define localNbOfEventsEMS 4
struct localEvent events [localNbOfEventsEMS] =
- {
- { 5*200,&TestHerdt2010::walkSidewards},
- {15*200,&TestHerdt2010::stop},
- {20*200,&TestHerdt2010::stopOnLineWalking}
- };
-
+ { {5*200,&TestHerdt2010::startTurningLeft2},
+ {10*200,&TestHerdt2010::startTurningRight2},
+ {15.2*200,&TestHerdt2010::stop},
+ {20.8*200,&TestHerdt2010::stopOnLineWalking}};
// Test when triggering event.
- for(unsigned int i=0;i<localNbOfEventsEMS;i++){
- if ( m_OneStep.NbOfIt==events[i].time){
- ODEBUG3("********* GENERATE EVENT EMERGENCY STOP ***********");
- (this->*(events[i].Handler))(*m_PGI);
+ for(unsigned int i=0;i<localNbOfEventsEMS;i++)
+ {
+ if ( m_OneStep.NbOfIt==events[i].time)
+ {
+ ODEBUG3("********* GENERATE EVENT EMS ***********");
+ (this->*(events[i].Handler))(*m_PGI);
+ }
}
- }
}
void generateEvent()
diff --git a/tests/TestKajita2003.cpp b/tests/TestKajita2003.cpp
index 6081a90b02c63ea6e7e9940d6523a74b5357782d..a23e912d85f650d3ad5fb5dba6b2ade616640d38 100644
--- a/tests/TestKajita2003.cpp
+++ b/tests/TestKajita2003.cpp
@@ -41,19 +41,6 @@ enum Profiles_t {
PROFIL_PB_FLORENT_SEQ2 // 4
};
-double filterprecision(double adb)
-{
- if (fabs(adb)<1e-7)
- return 0.0;
-
- if (fabs(adb)>1e7)
- return 1e7 ;
-
- double ladb2 = adb * 1e7;
- double lintadb2 = trunc(ladb2);
- return lintadb2/1e7;
-}
-
class TestKajita2003: public TestObject
{
@@ -63,7 +50,7 @@ public:
TestObject(argc,argv,aString)
{
m_TestProfile = TestProfile;
- };
+ }
protected:
diff --git a/tests/TestObject.cpp b/tests/TestObject.cpp
index 5a2ce25bc0809d38f9dbbb99a84e96e5dfdffd14..40026fb232220e4a473b2d35e5c9359624935399 100644
--- a/tests/TestObject.cpp
+++ b/tests/TestObject.cpp
@@ -343,7 +343,7 @@ namespace PatternGeneratorJRL
<< filterprecision(m_OneStep.finalCOMPosition.x[0] ) << " " // 2
<< filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " " // 3
<< filterprecision(m_OneStep.finalCOMPosition.z[0] ) << " " // 4
- << filterprecision(m_OneStep.finalCOMPosition.yaw[0] ) << " " // 5
+ << filterprecision(m_OneStep.finalCOMPosition.yaw[0] ) << " " // 5
<< filterprecision(m_OneStep.finalCOMPosition.x[1] ) << " " // 6
<< filterprecision(m_OneStep.finalCOMPosition.y[1] ) << " " // 7
<< filterprecision(m_OneStep.finalCOMPosition.z[1] ) << " " // 8
@@ -370,23 +370,23 @@ namespace PatternGeneratorJRL
<< filterprecision(m_OneStep.RightFootPosition.ddx ) << " " // 29
<< filterprecision(m_OneStep.RightFootPosition.ddy ) << " " // 30
<< filterprecision(m_OneStep.RightFootPosition.ddz ) << " " // 31
- << filterprecision(m_OneStep.RightFootPosition.theta*M_PI/180 ) << " " // 32
+ << filterprecision(m_OneStep.RightFootPosition.theta*M_PI/180 ) << " " // 32
<< filterprecision(m_OneStep.RightFootPosition.omega ) << " " // 33
<< filterprecision(m_OneStep.RightFootPosition.omega2 ) << " " // 34
<< filterprecision(m_OneStep.ZMPTarget(0)*cos(m_CurrentConfiguration(5)) -
m_OneStep.ZMPTarget(1)*sin(m_CurrentConfiguration(5))
- +m_CurrentConfiguration(0) ) << " " // 35
+ +m_CurrentConfiguration(0) ) << " " // 35
<< filterprecision(m_OneStep.ZMPTarget(0)*sin(m_CurrentConfiguration(5)) +
m_OneStep.ZMPTarget(1)*cos(m_CurrentConfiguration(5))
- +m_CurrentConfiguration(1) ) << " " // 36
+ +m_CurrentConfiguration(1) ) << " " // 36
<< filterprecision(m_CurrentConfiguration(0) ) << " " // 37
- << filterprecision(m_CurrentConfiguration(1) ) << " " // 38
- << filterprecision(m_OneStep.finalCOMPosition.x[2] ) << " " // 39
- << filterprecision(m_OneStep.finalCOMPosition.y[2] ) << " " // 40
- << filterprecision(m_OneStep.finalCOMPosition.z[2] ) << " " ; // 41
+ << filterprecision(m_CurrentConfiguration(1) ) << " " // 38
+ << filterprecision(m_OneStep.finalCOMPosition.x[2] ) << " " // 39
+ << filterprecision(m_OneStep.finalCOMPosition.y[2] ) << " " // 40
+ << filterprecision(m_OneStep.finalCOMPosition.z[2] ) << " " ; // 41
for (unsigned int i = 0 ; i < m_HDR->currentConfiguration().size() ; i++)
{
- aof << filterprecision(m_HDR->currentConfiguration()(i)) << " " ; // 39 - 74
+ aof << filterprecision(m_HDR->currentConfiguration()(i)) << " " ; // 39 - 74
}
aof << endl;
aof.close();
diff --git a/tests/TestObject.hh b/tests/TestObject.hh
index 4c71b975d798e422d5dcd8e36c14790d387fe35e..74cc8d87836f3c18b94675fa8d27f2b87431f404 100644
--- a/tests/TestObject.hh
+++ b/tests/TestObject.hh
@@ -55,6 +55,7 @@ namespace PatternGeneratorJRL
{
namespace TestSuite
{
+ double filterprecision(double adb);
/*! \brief Class running one test per algorithm */
class TestObject