From abbfa8742f5f68ea2305ee7f66ea80a2305b9b0b Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Thu, 8 May 2014 19:44:03 +0200
Subject: [PATCH] Adding debugging tools
---
src/MotionGeneration/ComAndFootRealization.hh | 51 +-
.../ComAndFootRealizationByGeometry.cpp | 86 +--
.../ComAndFootRealizationByGeometry.hh | 14 +
.../LinearizedInvertedPendulum2D.cpp | 50 +-
.../ZMPVelocityReferencedQP.cpp | 585 +++++++++---------
.../ZMPVelocityReferencedQP.hh | 27 +-
tests/TestHerdt2010DynamicFilter.cpp | 10 +-
7 files changed, 404 insertions(+), 419 deletions(-)
diff --git a/src/MotionGeneration/ComAndFootRealization.hh b/src/MotionGeneration/ComAndFootRealization.hh
index a375d728..181757b8 100644
--- a/src/MotionGeneration/ComAndFootRealization.hh
+++ b/src/MotionGeneration/ComAndFootRealization.hh
@@ -1,5 +1,5 @@
/*
- * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
+ * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
*
* Florent Lamiraux
* 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)
*/
/* Abstract class to generate Humanoid Body motion
@@ -58,12 +58,12 @@ namespace PatternGeneratorJRL
/*! \brief Store the dynamic robot. */
CjrlHumanoidDynamicRobot * m_HumanoidDynamicRobot;
-
+
/*! Store the height of the CoM */
double m_HeightOfCoM;
/*! Sampling Period. */
- double m_SamplingPeriod;
+ double m_SamplingPeriod;
/*! Object which handles a Stack of steps */
StepStackHandler * m_StepStackHandler;
@@ -72,7 +72,7 @@ namespace PatternGeneratorJRL
/* \name Constructor and destructor.*/
- /*! \brief Constructor
+ /*! \brief Constructor
*/
inline ComAndFootRealization(PatternGeneratorInterfacePrivate * aPatternGeneratorInterface):
@@ -83,22 +83,22 @@ namespace PatternGeneratorJRL
,m_StepStackHandler(0)
{
};
-
+
/*! \brief virtual destructor */
inline virtual ~ComAndFootRealization() {};
-
+
/*! \brief Initialization phase */
virtual void Initialization() = 0;
/** @} */
- /*! \name Methods related to the computation to be asked by
+ /*! \name Methods related to the computation to be asked by
\a ZMPPreviewControlWithMultiBodyZMP. */
/*! Compute the robot state for a given CoM and feet posture.
Each posture is given by a 3D position and two Euler angles \f$ (\theta, \omega) \f$.
Very important: This method is assume to set correctly the body angles of
- its \a HumanoidDynamicRobot and a subsequent call to the ZMP position
+ its \a HumanoidDynamicRobot and a subsequent call to the ZMP position
will return the associated ZMP vector.
@param CoMPosition: a 5 dimensional vector with the first dimension for position,
and the last two for the orientation (Euler angle).
@@ -125,11 +125,11 @@ namespace PatternGeneratorJRL
int IterationNumber,
int Stage) =0;
- /*! Returns the waist position associate to the current
+ /*! Returns the waist position associate to the current
@} */
/*! \name Setter and getter for the jrlHumanoidDynamicRobot object. */
-
+
/*! @param aHumanoidDynamicRobot: an object able to compute dynamic parameters
of the robot. */
inline virtual bool setHumanoidDynamicRobot(CjrlHumanoidDynamicRobot *aHumanoidDynamicRobot)
@@ -139,7 +139,7 @@ namespace PatternGeneratorJRL
/*! Returns the object able to compute dynamic parametersof the robot. */
inline CjrlHumanoidDynamicRobot * getHumanoidDynamicRobot() const
{ return m_HumanoidDynamicRobot;}
-
+
/** @} */
@@ -147,15 +147,15 @@ namespace PatternGeneratorJRL
1/ we take the current state of the robot
to compute the current CoM value.
2/ We deduce the difference between the CoM and the waist,
- which is suppose to be constant for the all duration of the motion.
+ which is suppose to be constant for the all duration of the motion.
IMPORTANT: The jrlHumanoidDynamicRobot must have been properly set up.
-
+
*/
virtual bool InitializationCoM(MAL_VECTOR_TYPE(double) &BodyAnglesIni,
MAL_S3_VECTOR_TYPE(double) & lStartingCOMPosition,
MAL_VECTOR_TYPE(double) & lStartingWaistPose,
- FootAbsolutePosition & InitLeftFootAbsPos,
+ FootAbsolutePosition & InitLeftFootAbsPos,
FootAbsolutePosition & InitRightFootAbsPos)=0;
/*! This initialization phase, make sure that the needed buffers
@@ -169,27 +169,27 @@ namespace PatternGeneratorJRL
/*! Set the algorithm used for ZMP and CoM trajectory. */
void SetAlgorithmForZMPAndCoMTrajectoryGeneration(int anAlgo);
-
+
/*! Get the algorithm used for ZMP and CoM trajectory. */
int GetAlgorithmForZMPAndCoMTrajectoryGeneration() ;
- /*! \name Setter and getter for the height of the CoM.
+ /*! \name Setter and getter for the height of the CoM.
@{
*/
-
- void SetHeightOfTheCoM(double theHeightOfTheCoM)
+
+ void SetHeightOfTheCoM(double theHeightOfTheCoM)
{ m_HeightOfCoM = theHeightOfTheCoM; }
-
+
const double & GetHeightOfTheCoM() const
{return m_HeightOfCoM;}
/*! @} */
- /*! \name Setter and getter for the sampling period.
+ /*! \name Setter and getter for the sampling period.
@{
*/
/*! Setter for the sampling period. */
inline void setSamplingPeriod(double aSamplingPeriod)
- { m_SamplingPeriod = aSamplingPeriod; }
+ { m_SamplingPeriod = aSamplingPeriod; }
/*! Getter for the sampling period. */
inline const double & getSamplingPeriod() const
{ return m_SamplingPeriod;}
@@ -197,17 +197,14 @@ namespace PatternGeneratorJRL
/*! \name Getter and setter for the handler of step stack @{ */
-
void SetStepStackHandler(StepStackHandler * lStepStackHandler)
{ m_StepStackHandler = lStepStackHandler;}
StepStackHandler * GetStepStackHandler() const
{ return m_StepStackHandler; }
-
/*! @} */
-
- /*! Get the current position of the waist in the COM reference frame
+ /*! Get the current position of the waist in the COM reference frame
@return a 4x4 matrix which contains the pose and the position of the waist
in the CoM reference frame.
*/
@@ -215,7 +212,7 @@ namespace PatternGeneratorJRL
/*! \brief Get the COG of the ankles at the starting position. */
virtual MAL_S3_VECTOR_TYPE(double) GetCOGInitialAnkles() = 0;
-
+
};
}
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
index 196db758..15b6da83 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
@@ -1039,29 +1039,29 @@ ComputePostureForGivenCoMAndFeetPosture(MAL_VECTOR_TYPE(double) & aCoMPosition,
if (Stage==0)
{
if (IterationNumber>0)
- {
- /* Compute the speed */
- for(unsigned int i=6;i<MAL_VECTOR_SIZE(m_prev_Configuration);i++)
- {
- CurrentVelocity[i] = (CurrentConfiguration[i] - m_prev_Configuration[i])/ ldt;
- /* Keep the new value for the legs. */
- }
-
- if (IterationNumber>1)
- {
- for(unsigned int i=6;i<MAL_VECTOR_SIZE(m_prev_Velocity);i++)
- CurrentAcceleration[i] = (CurrentVelocity[i] - m_prev_Velocity[i])/ ldt;
- }
- }
+ {
+ /* Compute the speed */
+ for(unsigned int i=6;i<MAL_VECTOR_SIZE(m_prev_Configuration);i++)
+ {
+ CurrentVelocity[i] = (CurrentConfiguration[i] - m_prev_Configuration[i])/ ldt;
+ /* Keep the new value for the legs. */
+ }
+
+ if (IterationNumber>1)
+ {
+ for(unsigned int i=6;i<MAL_VECTOR_SIZE(m_prev_Velocity);i++)
+ CurrentAcceleration[i] = (CurrentVelocity[i] - m_prev_Velocity[i])/ ldt;
+ }
+ }
else
- {
- /* Compute the speed */
- for(unsigned int i=0;i<MAL_VECTOR_SIZE(CurrentVelocity);i++)
- {
- CurrentVelocity[i] = 0.0;
- /* Keep the new value for the legs. */
- }
- }
+ {
+ /* Compute the speed */
+ for(unsigned int i=0;i<MAL_VECTOR_SIZE(CurrentVelocity);i++)
+ {
+ CurrentVelocity[i] = 0.0;
+ /* Keep the new value for the legs. */
+ }
+ }
ODEBUG4(CurrentVelocity, "DebugDataVelocity0.dat");
m_prev_Configuration = CurrentConfiguration;
@@ -1071,28 +1071,28 @@ ComputePostureForGivenCoMAndFeetPosture(MAL_VECTOR_TYPE(double) & aCoMPosition,
{
ODEBUG("lql: "<<lql<< " lqr: " <<lqr);
if (IterationNumber>0)
- {
- /* Compute the speed */
- for(unsigned int i=6;i<MAL_VECTOR_SIZE(m_prev_Configuration1);i++)
- {
- CurrentVelocity[i] = (CurrentConfiguration[i] - m_prev_Configuration1[i])/ getSamplingPeriod();
- /* Keep the new value for the legs. */
- }
- if (IterationNumber>1)
- {
- for(unsigned int i=6;i<MAL_VECTOR_SIZE(m_prev_Velocity1);i++)
- CurrentAcceleration[i] = (CurrentVelocity[i] - m_prev_Velocity1[i])/ ldt;
- }
- }
+ {
+ /* Compute the speed */
+ for(unsigned int i=6;i<MAL_VECTOR_SIZE(m_prev_Configuration1);i++)
+ {
+ CurrentVelocity[i] = (CurrentConfiguration[i] - m_prev_Configuration1[i])/ getSamplingPeriod();
+ /* Keep the new value for the legs. */
+ }
+ if (IterationNumber>1)
+ {
+ for(unsigned int i=6;i<MAL_VECTOR_SIZE(m_prev_Velocity1);i++)
+ CurrentAcceleration[i] = (CurrentVelocity[i] - m_prev_Velocity1[i])/ ldt;
+ }
+ }
else
- {
- /* Compute the speed */
- for(unsigned int i=0;i<MAL_VECTOR_SIZE(m_prev_Configuration1);i++)
- {
- CurrentVelocity[i] = 0.0;
- /* Keep the new value for the legs. */
- }
- }
+ {
+ /* Compute the speed */
+ for(unsigned int i=0;i<MAL_VECTOR_SIZE(m_prev_Configuration1);i++)
+ {
+ CurrentVelocity[i] = 0.0;
+ /* Keep the new value for the legs. */
+ }
+ }
ODEBUG4(CurrentVelocity, "DebugDataVelocity1.dat");
m_prev_Configuration1 = CurrentConfiguration;
m_prev_Velocity1 = CurrentVelocity;
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.hh b/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
index 8b0f94de..09e960d3 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
@@ -245,6 +245,20 @@ namespace PatternGeneratorJRL
*/
MAL_S4x4_MATRIX_TYPE(double) GetCurrentPositionofWaistInCOMFrame();
+ /*! \brief Getter and setter for the previous configurations and velocities */
+ inline void SetPreviousConfigurationStage0(MAL_VECTOR_TYPE(double) & prev_Configuration)
+ { m_prev_Configuration = prev_Configuration ;};
+
+ inline void SetPreviousConfigurationStage1(MAL_VECTOR_TYPE(double) & prev_Configuration1)
+ { m_prev_Configuration1 = prev_Configuration1 ;};
+
+ inline void SetPreviousVelocityStage0(MAL_VECTOR_TYPE(double) & prev_Velocity)
+ { m_prev_Velocity = prev_Velocity ;};
+
+ inline void SetPreviousVelocityStage1(MAL_VECTOR_TYPE(double) & prev_Velocity1)
+ { m_prev_Velocity1 = prev_Velocity1 ;};
+
+
/*! \brief Get the COG of the ankles at the starting position. */
virtual MAL_S3_VECTOR_TYPE(double) GetCOGInitialAnkles();
diff --git a/src/PreviewControl/LinearizedInvertedPendulum2D.cpp b/src/PreviewControl/LinearizedInvertedPendulum2D.cpp
index 9b560760..990a24ec 100644
--- a/src/PreviewControl/LinearizedInvertedPendulum2D.cpp
+++ b/src/PreviewControl/LinearizedInvertedPendulum2D.cpp
@@ -188,46 +188,52 @@ int LinearizedInvertedPendulum2D::Interpolation(deque<COMState> &COMStates,
{
int lCurrentPosition = CurrentPosition;
// Fill the queues with the interpolated CoM values.
-
+ cout << "m_CoM = \n"
+ << m_CoM.x[0] << " " << m_CoM.y[0] << " " << m_CoM.z[0] << endl
+ << m_CoM.x[1] << " " << m_CoM.y[1] << " " << m_CoM.z[1] << endl
+ << m_CoM.x[2] << " " << m_CoM.y[2] << " " << m_CoM.z[2] << endl
+ << " Jerk = " << CX << " " << CY << endl ;
//TODO: with TestHerdt, it is mandatory to use COMStates.size()-1, or it will crash.
// Is it the same for the other PG ? Please check.
- int loopEnd = std::min<int>( m_InterpolationInterval, ((int)COMStates.size())-1-CurrentPosition);
+ // TODO: with TestHerdt, it is mandatory to use m_InterpolationInterval-1 to interpolate correctly
+ // along the whole preview window will it be still fine with the reste of the PG?
+ int loopEnd = std::min<int>( m_InterpolationInterval-1, ((int)COMStates.size())-1-CurrentPosition);
for(int lk=0;lk<=loopEnd;lk++,lCurrentPosition++)
{
ODEBUG("lCurrentPosition: "<< lCurrentPosition);
COMState & aCOMPos = COMStates[lCurrentPosition];
double lkSP;
lkSP = (lk+1) * m_SamplingPeriod;
-
+ cout << "lkSP=" << lkSP << " " ;
aCOMPos.x[0] =
- m_CoM.x[0] + // Position
- lkSP * m_CoM.x[1] + // Speed
- 0.5 * lkSP*lkSP * m_CoM.x[2] +// Acceleration
- lkSP * lkSP * lkSP * CX /6.0; // Jerk
+ m_CoM.x[0] + // Position
+ lkSP * m_CoM.x[1] + // Speed
+ 0.5 * lkSP*lkSP * m_CoM.x[2] +// Acceleration
+ lkSP * lkSP * lkSP * CX /6.0; // Jerk
aCOMPos.x[1] =
- m_CoM.x[1] + // Speed
- lkSP * m_CoM.x[2] + // Acceleration
- 0.5 * lkSP * lkSP * CX; // Jerk
+ m_CoM.x[1] + // Speed
+ lkSP * m_CoM.x[2] + // Acceleration
+ 0.5 * lkSP * lkSP * CX; // Jerk
aCOMPos.x[2] =
- m_CoM.x[2] + // Acceleration
- lkSP * CX; // Jerk
+ m_CoM.x[2] + // Acceleration
+ lkSP * CX; // Jerk
aCOMPos.y[0] =
- m_CoM.y[0] + // Position
- lkSP * m_CoM.y[1] + // Speed
- 0.5 * lkSP*lkSP * m_CoM.y[2] + // Acceleration
- lkSP * lkSP * lkSP * CY /6.0; // Jerk
+ m_CoM.y[0] + // Position
+ lkSP * m_CoM.y[1] + // Speed
+ 0.5 * lkSP*lkSP * m_CoM.y[2] + // Acceleration
+ lkSP * lkSP * lkSP * CY /6.0; // Jerk
aCOMPos.y[1] =
- m_CoM.y[1] + // Speed
- lkSP * m_CoM.y[2] + // Acceleration
- 0.5 * lkSP * lkSP * CY; // Jerk
+ m_CoM.y[1] + // Speed
+ lkSP * m_CoM.y[2] + // Acceleration
+ 0.5 * lkSP * lkSP * CY; // Jerk
aCOMPos.y[2] =
- m_CoM.y[2] + // Acceleration
- lkSP * CY; // Jerk
+ m_CoM.y[2] + // Acceleration
+ lkSP * CY; // Jerk
aCOMPos.yaw[0] = ZMPRefPositions[lCurrentPosition].theta;
@@ -250,6 +256,7 @@ int LinearizedInvertedPendulum2D::Interpolation(deque<COMState> &COMStates,
CX << " " << CY << " " <<
lkSP << " " << m_T , "DebugInterpol.dat");
}
+ cout << endl ;
return 0;
}
@@ -272,6 +279,7 @@ com_t LinearizedInvertedPendulum2D::OneIteration(double ux, double uy)
m_CoM.x = m_CoM.x + Bux;
m_CoM.y = MAL_RET_A_by_B(m_A,m_CoM.y);
m_CoM.y = m_CoM.y + Buy;
+
// Modif. from Dimitar: Initially a mistake regarding the ordering.
//MAL_C_eq_A_by_B(m_zk,m_C,m_xk);
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index e222c22d..b2b5b27e 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -86,7 +86,7 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
QP_T_ = 0.1 ;
QP_N_ = 16 ;
m_SamplingPeriod = 0.005 ;
- InterpolationPeriod_ = QP_T_/20;
+ InterpolationPeriod_ = QP_T_/2;
StepPeriod_ = 0.8 ;
SSPeriod_ = 0.7 ;
DSPeriod_ = 0.1 ;
@@ -186,11 +186,11 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
// Create and initialize the class that compute the simplify inverse kinematics :
// ------------------------------------------------------------------------------
- ComAndFootRealization_ = new ComAndFootRealizationByGeometry( (PatternGeneratorInterfacePrivate*) SPM );
- ComAndFootRealization_->setHumanoidDynamicRobot(aHS);
- ComAndFootRealization_->SetHeightOfTheCoM(CoMHeight_);// seems weird...
- ComAndFootRealization_->setSamplingPeriod(InterpolationPeriod_);
- ComAndFootRealization_->Initialization();
+ ComAndFootRealizationByGeometry_ = new ComAndFootRealizationByGeometry( (PatternGeneratorInterfacePrivate*) SPM );
+ ComAndFootRealizationByGeometry_->setHumanoidDynamicRobot(aHS);
+ ComAndFootRealizationByGeometry_->SetHeightOfTheCoM(CoMHeight_);// seems weird...
+ ComAndFootRealizationByGeometry_->setSamplingPeriod(InterpolationPeriod_);
+ ComAndFootRealizationByGeometry_->Initialization();
// Register method to handle
const unsigned int NbMethods = 3;
@@ -239,6 +239,9 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
ComStateBuffer_.resize(NbSampleControl_);
+
+ m_Jacobian_LF = Jacobian_LF::Jacobian::Zero();
+ m_Jacobian_RF = Jacobian_RF::Jacobian::Zero();
Once_ = true ;
DInitX_ = 0 ;
DInitY_ = 0 ;
@@ -290,9 +293,9 @@ ZMPVelocityReferencedQP::~ZMPVelocityReferencedQP()
IntermedData_ = 0 ;
}
- if (ComAndFootRealization_!=0){
- delete ComAndFootRealization_;
- ComAndFootRealization_ = 0 ;
+ if (ComAndFootRealizationByGeometry_!=0){
+ delete ComAndFootRealizationByGeometry_;
+ ComAndFootRealizationByGeometry_ = 0 ;
}
}
@@ -503,7 +506,7 @@ ZMPVelocityReferencedQP::OnLine(double time,
VelRef_=NewVelRef_;
SupportFSM_->update_vel_reference(VelRef_, IntermedData_->SupportState());
IntermedData_->Reference( VelRef_ );
- IntermedData_->CoM( LIPM_() );
+ IntermedData_->CoM( LIPM_.getState() );
// PREVIEW SUPPORT STATES FOR THE WHOLE PREVIEW WINDOW:
// ----------------------------------------------------
@@ -565,8 +568,12 @@ ZMPVelocityReferencedQP::OnLine(double time,
FinalZMPTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
FinalCOMTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
+ // INTERPRET THE SOLUTION VECTOR :
+ // -------------------------------
+ InterpretSolutionVector();
+
static int iteration = 0;
- if (iteration == 11){
+ if (iteration >= 11){
iteration = 11;
}
@@ -575,6 +582,16 @@ ZMPVelocityReferencedQP::OnLine(double time,
FinalRightFootTraj_deq, time) ;
DynamicFilterInterpolation( time) ;
+ cout << "FinalCOMTraj_deq(0.05) = \n"
+ << FinalCOMTraj_deq[CurrentIndex_+9].x[0] << " " << FinalCOMTraj_deq[CurrentIndex_+9].y[0] << " " << FinalCOMTraj_deq[CurrentIndex_+9].z[0] << endl
+ << FinalCOMTraj_deq[CurrentIndex_+9].x[1] << " " << FinalCOMTraj_deq[CurrentIndex_+9].y[1] << " " << FinalCOMTraj_deq[CurrentIndex_+9].z[1] << endl
+ << FinalCOMTraj_deq[CurrentIndex_+9].x[2] << " " << FinalCOMTraj_deq[CurrentIndex_+9].y[2] << " " << FinalCOMTraj_deq[CurrentIndex_+9].z[2] << endl;
+
+ cout << "COMTraj_deq_(0.05) = \n"
+ << COMTraj_deq_[CurrentIndex_].x[0] << " " << COMTraj_deq_[CurrentIndex_].y[0] << " " << COMTraj_deq_[CurrentIndex_].z[0] << endl
+ << COMTraj_deq_[CurrentIndex_].x[1] << " " << COMTraj_deq_[CurrentIndex_].y[1] << " " << COMTraj_deq_[CurrentIndex_].z[1] << endl
+ << COMTraj_deq_[CurrentIndex_].x[2] << " " << COMTraj_deq_[CurrentIndex_].y[2] << " " << COMTraj_deq_[CurrentIndex_].z[2] << endl;
+
// DYNAMIC FILTER
// --------------
DynamicFilter( time, FinalCOMTraj_deq );
@@ -604,7 +621,7 @@ ZMPVelocityReferencedQP::OnLine(double time,
aof.setf(ios::scientific, ios::floatfield);
for (unsigned int i = 0 ; i < NbSampleInterpolation_ * QP_N_ ; ++i )
{
- aof << filterprecision( COMTraj_deq_[/*CurrentIndex_+*/i].x[0] ) << " " // 1
+ aof << filterprecision( COMTraj_deq_[CurrentIndex_+i].x[0] ) << " " // 1
<< filterprecision( COMTraj_deq_[CurrentIndex_+i].y[0] ) << " " // 2
<< filterprecision( COMTraj_deq_[CurrentIndex_+i].z[0] ) << " " // 3
<< filterprecision( COMTraj_deq_[CurrentIndex_+i].x[1] ) << " " // 4
@@ -760,27 +777,31 @@ void ZMPVelocityReferencedQP::ControlInterpolation(
double time) // INPUT
{
InitStateLIPM_ = LIPM_.GetState() ;
+ cout << "InitStateLIPM_ = \n"
+ << InitStateLIPM_.x[0] << " " << InitStateLIPM_.y[0] << " " << InitStateLIPM_.z[0] << endl
+ << InitStateLIPM_.x[1] << " " << InitStateLIPM_.y[1] << " " << InitStateLIPM_.z[1] << endl
+ << InitStateLIPM_.x[2] << " " << InitStateLIPM_.y[2] << " " << InitStateLIPM_.z[2] << endl;
InitStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
// INTERPOLATE CoM AND ZMP TRAJECTORIES:
// -------------------------------------
CoMZMPInterpolation(FinalZMPTraj_deq, FinalCOMTraj_deq,
FinalLeftFootTraj_deq, FinalRightFootTraj_deq,
- &solution_, &LIPM_, NbSampleControl_, 0);
+ &Solution_, &LIPM_, NbSampleControl_, 0);
// INTERPOLATE TRUNK ORIENTATION:
// ------------------------------
InitStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
OrientPrw_->interpolate_trunk_orientation( time, CurrentIndex_,
- m_SamplingPeriod, solution_.SupportStates_deq,
+ m_SamplingPeriod, Solution_.SupportStates_deq,
FinalCOMTraj_deq );
FinalCurrentStateOrientPrw_ = OrientPrw_->CurrentTrunkState();
FinalPreviewStateOrientPrw_ = OrientPrw_->PreviewTrunkState();
// INTERPOLATE THE COMPUTED FOOT POSITIONS:
// ----------------------------------------
OFTG_control_->interpolate_feet_positions( time,
- solution_.SupportStates_deq, solution_,
- solution_.SupportOrientations_deq,
+ Solution_.SupportStates_deq, Solution_,
+ Solution_.SupportOrientations_deq,
FinalLeftFootTraj_deq, FinalRightFootTraj_deq);
return ;
}
@@ -791,15 +812,15 @@ void ZMPVelocityReferencedQP::DynamicFilterInterpolation(double time)
vector<double> solFoot;
LIPM_subsampled_.setState(InitStateLIPM_) ;
OrientPrw_DF_->CurrentTrunkState(InitStateOrientPrw_) ;
- InterpretSolution(solFoot);
for ( int i = 0 ; i < QP_N_ ; i++ )
{
CoMZMPInterpolation(ZMPTraj_deq_, COMTraj_deq_,
LeftFootTraj_deq_, RightFootTraj_deq_,
- &solution_, &LIPM_subsampled_,
+ &Solution_, &LIPM_subsampled_,
NbSampleInterpolation_, i);
}
+ InterpretSolution(solFoot);
// Copy the solution for the orientation interpolation function
OFTG_DF_->SetSamplingPeriod( InterpolationPeriod_ );
solution_t aSolution = solution_ ;
@@ -837,75 +858,63 @@ void ZMPVelocityReferencedQP::DynamicFilterInterpolation(double time)
void ZMPVelocityReferencedQP::InterpretSolution(vector<double> &solFoot)
{
- double PosFootX=0; double Vx=0; double cosTheta(0); double Dx=0;
- double PosFootY=0; double Vy=0; double sinTheta(0); double Dy=0;
- int nbSteps=0; int Sign=0;
-
- support_state_t & LastSupport = solution_.SupportStates_deq.back() ;
- int index = 0 ;
+ double PosFootX=0;
+ double PosFootY=0;
+ support_state_t & CurrentSupport = solution_.SupportStates_deq.front() ;
+ support_state_t & LastSupport = solution_.SupportStates_deq.back() ;
+ int nbSteps = LastSupport.StepNumber ;
+ const double & LastPrwSupState_X = solution_.Solution_vec[2*QP_N_ + nbSteps -1] ;
+ const double & LastPrwSupState_Y = solution_.Solution_vec[2*QP_N_ + 2*nbSteps -1] ;
+ vector<int> index ;
for (unsigned int i = 0 ; i < solution_.SupportStates_deq.size() ; ++i)
{
if ( solution_.SupportStates_deq[i].StateChanged )
+ index.push_back(i-1) ;
+ if ( !(solution_.SupportStates_deq[i].NbStepsLeft > 0 && nbSteps > 0) )
{
- index = i ;
- break ;
+ solution_.SupportStates_deq[i].X = CurrentSupport.X ;
+ solution_.SupportStates_deq[i].Y = CurrentSupport.Y ;
}
}
- support_state_t & BeforeLastSupport = solution_.SupportStates_deq[index] ;
- support_state_t & FirstSupport = solution_.SupportStates_deq.front() ;
- nbSteps = LastSupport.StepNumber ;
- Vx = VelRef_.Local.X ;
- Vy = VelRef_.Local.Y ;
- cosTheta = cos(LastSupport.Yaw) ;
- sinTheta = sin(LastSupport.Yaw) ;
- cout << "velref X=" << VelRef_.Local.X << " Y=" << VelRef_.Local.Y<< " Yaw=" << VelRef_.Local.Yaw << " Lastfoot.yaw="<< LastSupport.Yaw <<endl ;
- static int tour (0);
- if ( tour == 58 )
- {
- tour = 58 ;
- }
- tour++;
- if (LastSupport.Phase == DS)
+ static int patate_flambee = 0 ;
+ if ( patate_flambee == 26 )
+ cout << "etape 26\n" ;
+ patate_flambee++;
+
+ solFoot.resize ((nbSteps+1)*2,0.0);
+ if ( (CurrentSupport.Phase == DS && LastSupport.Phase == DS)
+ || (CurrentSupport.Phase == SS && LastSupport.Phase == DS) )
{
PosFootX = LastSupport.X ;
PosFootY = LastSupport.Y ;
cout << "PosFootX = "<< PosFootX << " " << "PosFootY = "<< PosFootY ;
cout << endl ;
+ solFoot[nbSteps] = PosFootX ;
+ solFoot[solFoot.size()-1] = PosFootY ;
+ return ;
}
- else
- {
- if(FirstSupport.Foot == LEFT )
- Sign = 1.0 ;
- else
- Sign = -1.0 ;
- if(LastSupport.Phase == SS && FirstSupport.Phase == DS )
- Sign = -Sign ;
- if( (Vx*Vx + Vy*Vy) < 0.001)
- {
- PosFootX = solution_.Solution_vec[2*QP_N_ + nbSteps -1] - BeforeLastSupport.X - Sign * sin(LastSupport.Yaw) * FeetDistance_ ;
- PosFootY = solution_.Solution_vec[2*QP_N_ + 2*nbSteps -1] - BeforeLastSupport.Y + Sign * cos(LastSupport.Yaw) * FeetDistance_ ;
- }
- else
- {
- PosFootX = (Vx * StepPeriod_ + Sign * sin(LastSupport.Yaw) * FeetDistance_ + solution_.Solution_vec[2*QP_N_ + nbSteps -1]) ;
- PosFootY = (Vy * StepPeriod_ - Sign * cos(LastSupport.Yaw) * FeetDistance_ + solution_.Solution_vec[2*QP_N_ + 2*nbSteps -1]) ;
- }
-
- // compute the next foot step
-
- cout << "PosFootX = "<< PosFootX << " " << "PosFootY = "<< PosFootY ;
- cout << endl ;
- //ProjectionOnConstraints(PosFootX,PosFootY);
- }
-
- solFoot.resize ((nbSteps+1)*2,0.0);
for (int i = 0 ; i < nbSteps ; ++i )
{
solFoot[i] = solution_.Solution_vec[2*QP_N_+i] ;
solFoot[nbSteps+1+i] = solution_.Solution_vec[2*QP_N_+nbSteps+i] ;
}
+
+ double Vx = VelRef_.Local.X ;
+ double Vy = VelRef_.Local.Y ;
+ int lastCoMDSIndex = CurrentIndex_+ index.back()*NbSampleInterpolation_ + 2 ;
+ if ( lastCoMDSIndex > (COMTraj_deq_.size()-1) )
+ lastCoMDSIndex = COMTraj_deq_.size()-1 ;
+
+ const COMState & aCOMPos = COMTraj_deq_[lastCoMDSIndex];
+ PosFootX = LastPrwSupState_X + 2*( (aCOMPos.x[0]+Vx*StepPeriod_) - LastPrwSupState_X );
+ PosFootY = LastPrwSupState_Y + 2*( (aCOMPos.y[0]+Vy*StepPeriod_) - LastPrwSupState_Y );
+
+ cout << "PosFootX = "<< PosFootX << " " << "PosFootY = "<< PosFootY ;
+ cout << endl ;
+ //ProjectionOnConstraints(PosFootX,PosFootY);
+
solFoot[nbSteps] = PosFootX ;
solFoot[solFoot.size()-1] = PosFootY ;
@@ -918,133 +927,6 @@ void ZMPVelocityReferencedQP::InterpretSolution(vector<double> &solFoot)
return ;
}
-void ZMPVelocityReferencedQP::ProjectionOnConstraints(double & X, double & Y)
-{
- // intialization
- vector<int> active_set ;
- boost_ublas::compressed_matrix<double, boost_ublas::row_major> DX ;
- linear_inequality_t IneqFeet = IntermedData_->Inequalities( INEQ_FEET );
- int nbSteps(0),Sign(1);
- double dx(0),dy(0);
-
- // definition of a fake buffer of support foot to get an appropriate convex hull
- std::deque<support_state_t> SupportStatesDeq = Solution_.SupportStates_deq ;
- support_state_t LastSupport = SupportStatesDeq.back() ;
- support_state_t FirstSupport = SupportStatesDeq.front() ;
- nbSteps = LastSupport.StepNumber ;
- if(FirstSupport.Foot == LEFT )
- Sign = 1.0 ;
- else
- Sign = -1.0 ;
- if(LastSupport.Phase == SS && FirstSupport.Phase == DS )
- Sign = -Sign ;
-
- if ( Sign == 1 )
- LastSupport.Foot = RIGHT ;
- else
- LastSupport.Foot = LEFT ;
- LastSupport.StateChanged = true ;
- SupportStatesDeq.pop_front();
- for (unsigned i = 0 ; i < SupportStatesDeq.size() ; ++i)
- {
- if ( SupportStatesDeq[i].StateChanged )
- SupportStatesDeq[i].StateChanged = false ;
- SupportStatesDeq[i].StepNumber = 1 ;
- }
- SupportStatesDeq.push_back(LastSupport);
-
- // collecting the linear inequalities
- VRQPGenerator_->build_inequalities_feet(IneqFeet,SupportStatesDeq);
-
- // transalating the linear inequalities at the appropriate position
- dx = Sign * sin(LastSupport.Yaw) * FeetDistance_ * 0.5 + solution_.Solution_vec[2*QP_N_ + nbSteps -1] ;
- dy = Sign * cos(LastSupport.Yaw) * FeetDistance_ * 0.5 + solution_.Solution_vec[2*QP_N_ + 2*nbSteps -1] ;
-
- // Translation
- for (unsigned i = 0 ; i < IneqFeet.Dc_vec.size() ; ++ i)
- {
- IneqFeet.Dc_vec(i) = IneqFeet.Dc_vec(i) + IneqFeet.D.X_mat(i,0) * dx + IneqFeet.D.Y_mat(i,0) * dy ;
- }
-
- cout << " dx dy : " ;
- cout << dx << " " << dy << endl ;
-
- // Computing the active set
- DX = (IneqFeet.D.X_mat * X + IneqFeet.D.Y_mat * Y) ;
- for (unsigned i = 0 ; i < DX.size1() ; ++ i)
- {
- if( (DX(i,0) - IneqFeet.Dc_vec(i)) < 0 )
- {
- active_set.push_back(i);
- }
- }
- cout << "active_set.size() = " << active_set.size() << endl ;
-
- // Projection on the active set if it is not empty
- if ( active_set.size() == 1 )
- {
- double a = IneqFeet.D.X_mat(active_set[0],0);
- double b = IneqFeet.D.Y_mat(active_set[0],0);
- double c = IneqFeet.Dc_vec(active_set[0]);
- double Bx(1),By(1) ;
- double BH(0),normV(0);
- if ( a == 0 && b != 0 )
- {
- By = c/b ;
- }else if ( a != 0 && b == 0 )
- {
- Bx = c/a ;
- }else
- {
- By = (c-a*Bx) / b ;
- }
- normV = sqrt( a*a + b*b );
- BH = ( (X - Bx)*(-b) + (Y - By)*a ) / normV ;
-
- X = Bx + BH / normV * (-b) ;
- Y = By + BH / normV * a ;
- }else if ( active_set.size() == 2 )
- {
- double a1 = IneqFeet.D.X_mat(active_set[0],0);
- double b1 = IneqFeet.D.Y_mat(active_set[0],0);
- double c1 = IneqFeet.Dc_vec(active_set[0]);
- double a2 = IneqFeet.D.X_mat(active_set[1],0);
- double b2 = IneqFeet.D.Y_mat(active_set[1],0);
- double c2 = IneqFeet.Dc_vec(active_set[1]);
-
- if (a1!=0 && b1!=0 && a2==0 && b2!=0)
- {
- Y = c2/b2 ;
- X = (c1-b1*Y)/a1 ;
- }else if (a1==0 && b1!=0 && a2!=0 && b2!=0)
- {
- Y = c1/b1 ;
- X = (c2-b2*Y)/a2 ;
- }else if (a1!=0 && b1==0 && a2!=0 && b2!=0)
- {
- X = c1/a1 ;
- Y = (c2-a2*X)/b2 ;
- }else if (a1!=0 && b1!=0 && a2!=0 && b2==0)
- {
- X = c2/a2 ;
- Y = (c1-a1*X)/b1 ;
- }else if (a1!=0 && b1==0 && a2==0 && b2!=0)
- {
- X = c1/a1 ;
- Y = c2/b2 ;
- }else if (a1==0 && b1!=0 && a2!=0 && b2==0)
- {
- X = c2/a2 ;
- Y = c1/b1 ;
- }else if (a1!=0 && b1!=0 && a2!=0 && b2!=0)
- {
- Y = c2/(b1-a1/a2*b2) ;
- X = (c2-b2*Y)/a2 ;
- }
- }
- return ;
-}
-
// Modify a copy of the solution to allow "OFTG_DF_->interpolate_feet_positions()"
// to use the correct foot step previewed
void ZMPVelocityReferencedQP::PrepareSolution(int iteration, const vector<double> &solFoot)
@@ -1084,9 +966,12 @@ void ZMPVelocityReferencedQP::PrepareSolution(int iteration, const vector<double
else
cout << "footsupport = RIGHT" ;
if(solution_.Solution_vec.size()!=0 && solution_.SupportStates_deq.front().Phase != DS && nbSteps > 0 ){
- cout << " ; SolX = " << solution_.Solution_vec[2*QP_N_]
- << " ; SolY = " << solution_.Solution_vec[2*QP_N_+nbSteps]
- << " ; nbSteps = " << solution_.SupportStates_deq.front().StepNumber
+ for (int i = 0 ; i < nbSteps ; ++i )
+ {
+ cout << " ; SolX = " << solution_.Solution_vec[2*QP_N_ + i]
+ << " ; SolY = " << solution_.Solution_vec[2*QP_N_ + nbSteps + i ];
+ }
+ cout<< " ; nbSteps = " << solution_.SupportStates_deq.front().StepNumber
<< " ; nbStepChanged = " << nbStepChanged;
}else{}
@@ -1100,27 +985,33 @@ void ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> &
// \brief calculate, from the CoM computed by the preview control,
// the corresponding articular position, velocity and acceleration
// ------------------------------------------------------------------
- for(unsigned int i = 0 ; i < N ; i++ ){
- CallToComAndFootRealization(
- COMTraj_deq_[CurrentIndex_+i],
- LeftFootTraj_deq_ [CurrentIndex_+i],
- RightFootTraj_deq_ [CurrentIndex_+i],
- ConfigurationTraj_[i],
- VelocityTraj_[i],
- AccelerationTraj_[i],
- i);
- }
+ CallToComAndFootRealization();
+
/// \brief Debug Purpose
/// --------------------
- ofstream aof,aof2;
- string aFileName, aFileName2;
+ ofstream aof,aof2,aof3;
+ string aFileName, aFileName2, aFileName3;
ostringstream oss(std::ostringstream::ate);
ostringstream oss2(std::ostringstream::ate);
+ ostringstream oss3(std::ostringstream::ate);
static int iteration = 0;
int iteration100 = (int)iteration/100;
int iteration10 = (int)(iteration - iteration100*100)/10;
int iteration1 = (int)(iteration - iteration100*100 - iteration10*10 );
-
+ static double Px = 0 ;
+ static double Py = 0 ;
+ static double Pz = 0 ;
+ static double Lx = 0 ;
+ static double Ly = 0 ;
+ static double Lz = 0 ;
+ static double PrvFx = 0 ;
+ static double PrvFy = 0 ;
+ static double PrvFz = 0 ;
+ static double PrvNx = 0 ;
+ static double PrvNy = 0 ;
+ static double PrvNz = 0 ;
+ static double ZMPMP_KajitaX = 0 ;
+ static double ZMPMP_KajitaY = 0 ;
/// \brief Debug Purpose
/// --------------------
oss.str("TestHerdt2010DynamicZMPMB.dat");
@@ -1130,6 +1021,14 @@ void ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> &
aof.open(aFileName.c_str(),ofstream::out);
aof.close();
}
+
+ oss3.str("TestHerdt2010DynamicJacobian.dat");
+ aFileName3 = oss.str();
+ if (iteration == 0 )
+ {
+ aof3.open(aFileName3.c_str(),ofstream::out);
+ aof3.close();
+ }
for (unsigned int i = 0 ; i < N ; i++ )
{
// Apply the RNEA to the metapod multibody and print the result in a log file.
@@ -1139,6 +1038,7 @@ void ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> &
m_dq(j,0) = VelocityTraj_[i](j) ;
m_ddq(j,0) = AccelerationTraj_[i](j) ;
}
+
metapod::rnea< Robot_Model, true >::run(m_robot, m_q, m_dq, m_ddq);
Node & node = boost::fusion::at_c<Robot_Model::BODY>(m_robot.nodes);
@@ -1162,10 +1062,49 @@ void ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> &
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
if (i<NbSampleInterpolation_){
+// Px += (m_force.f()[0]+PrvFx)*0.5*InterpolationPeriod_ ;
+// Py += (m_force.f()[1]+PrvFy)*0.5*InterpolationPeriod_ ;
+// Pz += (m_force.f()[2]+PrvFz)*0.5*InterpolationPeriod_ ;
+ Px = RobotMass_*COMTraj_deq_[CurrentIndex_+i].x[1] ;
+ Py = RobotMass_*COMTraj_deq_[CurrentIndex_+i].y[1] ;
+ Pz = RobotMass_*COMTraj_deq_[CurrentIndex_+i].z[1] ;
+
+ Lx += (m_force.n()[0]+PrvNx)*0.5*InterpolationPeriod_ ;
+ Ly += (m_force.n()[1]+PrvNy)*0.5*InterpolationPeriod_ ;
+ Lz += (m_force.n()[2]+PrvNz)*0.5*InterpolationPeriod_ ;
+
+ PrvFx = m_force.f()[0] ;
+ PrvFy = m_force.f()[1] ;
+ PrvFz = m_force.f()[2] ;
+
+ PrvNx = m_force.n()[0] ;
+ PrvNy = m_force.n()[1] ;
+ PrvNz = m_force.n()[2] ;
+
+ ZMPMP_KajitaX = (HDR_->mass()*9.81*COMTraj_deq_[CurrentIndex_+i].x[0] - m_force.n()[0])/(HDR_->mass()*9.81+m_force.f()[2]);
+ ZMPMP_KajitaY = (HDR_->mass()*9.81*COMTraj_deq_[CurrentIndex_+i].y[0] + m_force.f()[1])/(HDR_->mass()*9.81+m_force.f()[2]);
+
aof << filterprecision( ( - m_force.n()[1] / m_force.f()[2] ) + DInitX_ ) << " " // 1
<< filterprecision( ( m_force.n()[0] / m_force.f()[2] ) + DInitY_ ) << " " // 2
- << filterprecision( ZMPTraj_deq_[i].px ) << " " // 3
- << filterprecision( ZMPTraj_deq_[i].py ) << " " // 4
+ << filterprecision( ZMPTraj_deq_[CurrentIndex_+i].px ) << " " // 3
+ << filterprecision( ZMPTraj_deq_[CurrentIndex_+i].py ) << " " // 4
+ << filterprecision( Px ) << " " // 5
+ << filterprecision( Py ) << " " // 6
+ << filterprecision( Pz ) << " " // 7
+ << filterprecision( Lx ) << " " // 8
+ << filterprecision( Ly ) << " " // 9
+ << filterprecision( Lz ) << " " // 10
+ << filterprecision( PrvFx ) << " " // 11
+ << filterprecision( PrvFy ) << " " // 12
+ << filterprecision( PrvFz ) << " " // 13
+ << filterprecision( PrvNx ) << " " // 14
+ << filterprecision( PrvNy ) << " " // 15
+ << filterprecision( PrvNz ) << " " // 16
+ << filterprecision( ZMPMP_KajitaX ) << " " // 17
+ << filterprecision( ZMPMP_KajitaY ) << " " // 18
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].x[0] ) << " " // 19
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].y[0] ) << " " // 20
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].z[0] ) << " " // 21
<< endl ;
}
aof.close();
@@ -1187,10 +1126,8 @@ void ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> &
<< filterprecision( ZMPTraj_deq_[CurrentIndex_+i].py ) << " " // 4
<< endl ;
aof2.close();
-
}
-
static double ecartmaxX = 0 ;
static double ecartmaxY = 0 ;
@@ -1252,27 +1189,27 @@ void ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> &
/// \brief Debug Purpose
/// --------------------
- oss.str("TestHerdt2010DynamicART");
- oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
+ if ( iteration == 0 )
+ {
+ oss.str("TestHerdt2010DynamicART.dat");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ }
///----
+ oss.str("TestHerdt2010DynamicART.dat");
+ aFileName = oss.str();
aof.open(aFileName.c_str(),ofstream::app);
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
- for (unsigned int i = 0 ; i < DeltaZMPMBPositions_.size() ; ++i )
+ for (unsigned int i = 0 ; i < NbSampleInterpolation_ ; ++i )
{
- aof << filterprecision( DeltaZMPMBPositions_[i].px ) << " " // 1
- << filterprecision( DeltaZMPMBPositions_[i].py ) << " "; // 2
+ for (unsigned int j = 0 ; j < ConfigurationTraj_[i].size() ; ++j)
+ aof << filterprecision(ConfigurationTraj_[i](j)) << " " ;
for (unsigned int j = 0 ; j < VelocityTraj_[i].size() ; ++j)
- {
aof << filterprecision(VelocityTraj_[i](j)) << " " ;
- }
for (unsigned int j = 0 ; j < AccelerationTraj_[i].size() ; ++j)
- {
aof << filterprecision(AccelerationTraj_[i](j)) << " " ;
- }
aof << endl ;
}
cout << "iteration = " << iteration << endl;
@@ -1297,98 +1234,123 @@ void ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> &
<< filterprecision( ComStateBuffer_[i].y[2] ) << " " // 2
<< endl ;
}
+ aof.close();
+
+ /// \brief Debug Purpose
+ /// --------------------
+ if ( iteration == 0 ){
+ oss.str("/tmp/walk_herdt.pos");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ }
+ ///----
+ oss.str("/tmp/walk_herdt.pos");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::app);
+ if (!aof.is_open())
+ {cout << "FAIT CH****************************************************\n";}
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for (unsigned int j = 0 ; j < NbSampleInterpolation_ ; ++j )
+ {
+ aof << filterprecision( iteration * InterpolationPeriod_ ) << " " ; // 1
+ for(unsigned int i = 6 ; i < ConfigurationTraj_[j].size() ; i++){
+ aof << filterprecision( ConfigurationTraj_[j](i) ) << " " ; // 1
+ }
+ for(unsigned int i = 0 ; i < 10 ; i++){
+ aof << 0.0 << " " ;
+ }
+ aof << endl ;
+ }
+ aof.close();
+
+
+ if ( iteration == 0 ){
+ oss.str("/tmp/walk_herdt.hip");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ }
+ oss.str("/tmp/walk_herdt.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 < NbSampleInterpolation_ ; j++){
+ aof << filterprecision( iteration * InterpolationPeriod_ ) << " " ; // 1
+ aof << filterprecision( 0.0 ) << " " ; // 1
+ aof << filterprecision( 0.0 ) << " " ; // 1
+ aof << filterprecision( COMTraj_deq_[CurrentIndex_+j].yaw[0] ) << " " ; // 1
+ aof << endl ;
+ }
+ aof.close();
+
iteration++;
return ;
}
-void ZMPVelocityReferencedQP::CallToComAndFootRealization(
- const COMState & acomp,
- const FootAbsolutePosition & aLeftFAP,
- const FootAbsolutePosition & aRightFAP,
- MAL_VECTOR_TYPE(double) & CurrentConfiguration,
- MAL_VECTOR_TYPE(double) & CurrentVelocity,
- MAL_VECTOR_TYPE(double) & CurrentAcceleration,
- const unsigned & IterationNumber)
+void ZMPVelocityReferencedQP::CallToComAndFootRealization()
{
+ const unsigned int N = NbSampleInterpolation_ * QP_N_ ;
+ static int static_i = 0 ;
+ const int stage0 = 0 ;
- // New scheme for WPG v3.0
- // We call the object in charge of generating the whole body
- // motion ( for a given CoM and Feet points) before applying the second filter.
- MAL_VECTOR_DIM(aCOMState,double,6);
- MAL_VECTOR_DIM(aCOMSpeed,double,6);
- MAL_VECTOR_DIM(aCOMAcc,double,6);
-
- aCOMState(0) = acomp.x[0];
- aCOMState(1) = acomp.y[0];
- aCOMState(2) = acomp.z[0];
- aCOMState(3) = acomp.roll[0];
- aCOMState(4) = acomp.pitch[0];
- aCOMState(5) = acomp.yaw[0];
-
- aCOMSpeed(0) = acomp.x[1];
- aCOMSpeed(1) = acomp.y[1];
- aCOMSpeed(2) = acomp.z[1];
- aCOMSpeed(3) = acomp.roll[1];
- aCOMSpeed(4) = acomp.pitch[1];
- aCOMSpeed(5) = acomp.yaw[1];
-
- aCOMAcc(0) = acomp.x[2];
- aCOMAcc(1) = acomp.y[2];
- aCOMAcc(2) = acomp.z[2];
- aCOMAcc(3) = acomp.roll[2];
- aCOMAcc(4) = acomp.pitch[2];
- aCOMAcc(5) = acomp.yaw[2];
-
- MAL_VECTOR_DIM(aLeftFootPosition,double,5);
- MAL_VECTOR_DIM(aRightFootPosition,double,5);
-
- aLeftFootPosition(0) = aLeftFAP.x;
- aLeftFootPosition(1) = aLeftFAP.y;
- aLeftFootPosition(2) = aLeftFAP.z;
- aLeftFootPosition(3) = aLeftFAP.theta;
- aLeftFootPosition(4) = aLeftFAP.omega;
-
- aRightFootPosition(0) = aRightFAP.x;
- aRightFootPosition(1) = aRightFAP.y;
- aRightFootPosition(2) = aRightFAP.z;
- aRightFootPosition(3) = aRightFAP.theta;
- aRightFootPosition(4) = aRightFAP.omega;
-
- if (IterationNumber == 0)
- {
- CurrentConfiguration = HDR_->currentConfiguration();
- CurrentVelocity = HDR_->currentConfiguration();
- CurrentAcceleration = HDR_->currentConfiguration();
- }else
- {
- CurrentConfiguration = PreviousConfiguration_ ;
- CurrentVelocity = PreviousVelocity_ ;
- CurrentAcceleration = PreviousAcceleration_ ;
- }
- ComAndFootRealization_->setSamplingPeriod(InterpolationPeriod_);
- ComAndFootRealization_->ComputePostureForGivenCoMAndFeetPosture(aCOMState, aCOMSpeed, aCOMAcc,
- aLeftFootPosition,
- aRightFootPosition,
- CurrentConfiguration,
- CurrentVelocity,
- CurrentAcceleration,
- IterationNumber,
- 0);
-
- if(IterationNumber == NbSampleInterpolation_-1 )
- {
- HDR_->currentConfiguration(CurrentConfiguration);
- HDR_->currentConfiguration(CurrentVelocity);
- HDR_->currentConfiguration(CurrentAcceleration);
- }else
+ PreviousConfiguration_ = HDR_->currentConfiguration();
+ PreviousVelocity_ = HDR_->currentVelocity();
+ PreviousAcceleration_ = HDR_->currentAcceleration();
+ ComAndFootRealizationByGeometry_->SetPreviousConfigurationStage0(PreviousConfiguration_);
+ ComAndFootRealizationByGeometry_->SetPreviousVelocityStage0(PreviousVelocity_);
+
+ for(unsigned int i = 0 ; i < N ; i++ )
{
- PreviousConfiguration_ = CurrentConfiguration ;
- PreviousVelocity_ = CurrentVelocity ;
- PreviousAcceleration_ = CurrentAcceleration ;
- }
+ const COMState & acomp = COMTraj_deq_[CurrentIndex_+i] ;
+ const FootAbsolutePosition & aLeftFAP = LeftFootTraj_deq_ [CurrentIndex_+i] ;
+ const FootAbsolutePosition & aRightFAP = RightFootTraj_deq_ [CurrentIndex_+i] ;
+
+ MAL_VECTOR_DIM(aCOMState,double,6); MAL_VECTOR_DIM(aLeftFootPosition,double,5);
+ MAL_VECTOR_DIM(aCOMSpeed,double,6); MAL_VECTOR_DIM(aRightFootPosition,double,5);
+ MAL_VECTOR_DIM(aCOMAcc,double,6);
+
+ aCOMState(0) = acomp.x[0]; aCOMSpeed(0) = acomp.x[1]; aCOMAcc(0) = acomp.x[2];
+ aCOMState(1) = acomp.y[0]; aCOMSpeed(1) = acomp.y[1]; aCOMAcc(1) = acomp.y[2];
+ aCOMState(2) = acomp.z[0]; aCOMSpeed(2) = acomp.z[1]; aCOMAcc(2) = acomp.z[2];
+ aCOMState(3) = acomp.roll[0]; aCOMSpeed(3) = acomp.roll[1]; aCOMAcc(3) = acomp.roll[2];
+ aCOMState(4) = acomp.pitch[0]; aCOMSpeed(4) = acomp.pitch[1]; aCOMAcc(4) = acomp.pitch[2];
+ aCOMState(5) = acomp.yaw[0]; aCOMSpeed(5) = acomp.yaw[1]; aCOMAcc(5) = acomp.yaw[2];
+
+ aLeftFootPosition(0) = aLeftFAP.x; aRightFootPosition(0) = aRightFAP.x;
+ aLeftFootPosition(1) = aLeftFAP.y; aRightFootPosition(1) = aRightFAP.y;
+ aLeftFootPosition(2) = aLeftFAP.z; aRightFootPosition(2) = aRightFAP.z;
+ aLeftFootPosition(3) = aLeftFAP.theta; aRightFootPosition(3) = aRightFAP.theta;
+ aLeftFootPosition(4) = aLeftFAP.omega; aRightFootPosition(4) = aRightFAP.omega;
+
+ ConfigurationTraj_[i] = PreviousConfiguration_ ;
+ VelocityTraj_[i] = PreviousVelocity_ ;
+ AccelerationTraj_[i] = PreviousAcceleration_ ;
+
+ ComAndFootRealizationByGeometry_->setSamplingPeriod(InterpolationPeriod_);
+ ComAndFootRealizationByGeometry_->ComputePostureForGivenCoMAndFeetPosture(aCOMState, aCOMSpeed, aCOMAcc,
+ aLeftFootPosition,
+ aRightFootPosition,
+ ConfigurationTraj_[i],
+ VelocityTraj_[i],
+ AccelerationTraj_[i],
+ i+static_i,
+ stage0);
+ PreviousConfiguration_ = ConfigurationTraj_[i] ;
+ PreviousVelocity_ = VelocityTraj_[i] ;
+ PreviousAcceleration_ = AccelerationTraj_[i] ;
+ }
+
+ HDR_->currentConfiguration(ConfigurationTraj_[NbSampleInterpolation_-1]);
+ HDR_->currentVelocity(VelocityTraj_[NbSampleInterpolation_-1]);
+ HDR_->currentAcceleration(AccelerationTraj_[NbSampleInterpolation_-1]);
+ cout << static_i << endl ;
+ static_i += NbSampleInterpolation_ ;
return ;
}
@@ -1424,3 +1386,8 @@ void ZMPVelocityReferencedQP::CoMZMPInterpolation(
return ;
}
+
+void ZMPVelocityReferencedQP::InterpretSolutionVector()
+{
+ return ;
+}
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index 28fd01e4..35604a77 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -46,12 +46,18 @@
// metapod includes
#include <metapod/models/hrp2_14/hrp2_14.hh>
+#include <metapod/algos/jac_point_chain.hh>
+
#ifndef METAPOD_TYPEDEF
#define METAPOD_TYPEDEF
typedef double LocalFloatType;
typedef metapod::Spatial::ForceTpl<LocalFloatType> Force_HRP2_14;
typedef metapod::hrp2_14<LocalFloatType> Robot_Model;
typedef metapod::Nodes< Robot_Model, Robot_Model::BODY >::type Node;
+ typedef metapod::jac_point_chain < Robot_Model,
+ Robot_Model::l_ankle, Robot_Model::BODY,0,true> Jacobian_LF;
+ typedef metapod::jac_point_chain < Robot_Model,
+ Robot_Model::r_ankle, Robot_Model::BODY,0,true> Jacobian_RF;
#endif
namespace PatternGeneratorJRL
@@ -141,10 +147,8 @@ namespace PatternGeneratorJRL
{ return QP_N_; }
/// \brief Setter and getter for the ComAndZMPTrajectoryGeneration.
- inline bool setComAndFootRealization(ComAndFootRealization * aCFR)
- { ComAndFootRealization_ = aCFR; return true;};
inline ComAndFootRealization * getComAndFootRealization()
- { return ComAndFootRealization_;};
+ { return ComAndFootRealizationByGeometry_;};
/// \}
inline double InterpolationPeriod()
@@ -226,7 +230,7 @@ namespace PatternGeneratorJRL
solution_t solution_ ;
/// \brief Store a reference to the object to solve posture resolution.
- ComAndFootRealization * ComAndFootRealization_;
+ ComAndFootRealizationByGeometry * ComAndFootRealizationByGeometry_;
/// \brief HDR allow the computation of the dynamic filter
CjrlHumanoidDynamicRobot * HDR_ ;
@@ -310,6 +314,8 @@ namespace PatternGeneratorJRL
/// \brief Initialize the robot with the autogenerated files by MetapodFromUrdf
Robot_Model m_robot;
+ Jacobian_LF::Jacobian m_Jacobian_LF ;
+ Jacobian_LF::Jacobian m_Jacobian_RF ;
/// \brief Standard polynomial trajectories for the feet.
OnLineFootTrajectoryGeneration * OFTG_DF_ ;
@@ -356,16 +362,8 @@ namespace PatternGeneratorJRL
std::deque<COMState> & FinalCOMTraj_deq
);
- /// \brief Compute the inverse kinamatics with a simplified inverted pendulum model
- void CallToComAndFootRealization(
- const COMState & acomp,
- const FootAbsolutePosition & aLeftFAP,
- const FootAbsolutePosition & aRightFAP,
- MAL_VECTOR_TYPE(double) & CurrentConfiguration,
- MAL_VECTOR_TYPE(double) & CurrentVelocity,
- MAL_VECTOR_TYPE(double) & CurrentAcceleration,
- const unsigned & IterationNumber
- );
+ /// \brief Compute the inverse kinematics with a simplified inverted pendulum model
+ void CallToComAndFootRealization();
/// \brief Interpolation form the com jerk the position of the com and the zmp corresponding to the kart table model
void CoMZMPInterpolation(
@@ -391,6 +389,7 @@ namespace PatternGeneratorJRL
/// \brief Define the position of an additionnal foot step outside the preview to interpolate the position of the feet in 3D
void InterpretSolution(vector<double> &solFoot);
+ void InterpretSolutionVector();
/// \brief Prepare the vecteur containing the solution for the interpolation
void PrepareSolution(int iteration, const vector<double> &solFoot);
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index d6efdb4c..81204657 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -720,17 +720,17 @@ void startOnLineWalking(PatternGeneratorInterface &aPGI)
#define localNbOfEvents 6
struct localEvent events [localNbOfEvents] =
{ {1*200,&TestHerdt2010::walkForward},
- {5*200,&TestHerdt2010::startTurningRightOnSpot},
- {10*200,&TestHerdt2010::walkForward},
+ {2*200,&TestHerdt2010::startTurningRightOnSpot},
+ {5*200,&TestHerdt2010::walkForward},
// {35*200,&TestHerdt2010::walkForward},
- {15*200,&TestHerdt2010::startTurningLeftOnSpot},
+ {7*200,&TestHerdt2010::startTurningLeftOnSpot},
// {55*200,&TestHerdt2010::walkForward},
// {65*200,&TestHerdt2010::startTurningRightOnSpot},
// {75*200,&TestHerdt2010::walkForward},
// {85*200,&TestHerdt2010::startTurningLeft},
// {95*200,&TestHerdt2010::startTurningRight},
- {20*200,&TestHerdt2010::stop},
- {25*200,&TestHerdt2010::stopOnLineWalking}
+ {9*200,&TestHerdt2010::stop},
+ {11*200,&TestHerdt2010::stopOnLineWalking}
};
// Test when triggering event.
--
GitLab