diff --git a/src/PatternGeneratorInterfacePrivate.cpp b/src/PatternGeneratorInterfacePrivate.cpp
index 56f9b1c6f0cb39ad5c5b781616d7a837834d0809..514a29ec18c951bbcfb671c90867a234e5a0fa78 100644
--- a/src/PatternGeneratorInterfacePrivate.cpp
+++ b/src/PatternGeneratorInterfacePrivate.cpp
@@ -1253,68 +1253,67 @@ namespace PatternGeneratorJRL {
COMState &finalCOMState,
FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition )
-
{
m_InternalClock+=m_SamplingPeriod;
if ((!m_ShouldBeRunning) ||
- (m_GlobalStrategyManager->EndOfMotion()<0))
- {
-
- ODEBUG(" m_ShoulBeRunning " << m_ShouldBeRunning << endl <<
- " m_ZMPPositions " << m_ZMPPositions.size() << endl <<
- " 2*m_NL+1 " << 2*m_NL+1 << endl);
- ODEBUG("m_ShouldBeRunning : "<< m_ShouldBeRunning << endl <<
- "m_GlobalStrategyManager: " << m_GlobalStrategyManager->EndOfMotion());
-
- m_Running = false;
- return m_Running;//Andremize
- }
+ (m_GlobalStrategyManager->EndOfMotion()<0))
+ {
+
+ ODEBUG(" m_ShoulBeRunning " << m_ShouldBeRunning << endl <<
+ " m_ZMPPositions " << m_ZMPPositions.size() << endl <<
+ " 2*m_NL+1 " << 2*m_NL+1 << endl);
+ ODEBUG("m_ShouldBeRunning : "<< m_ShouldBeRunning << endl <<
+ "m_GlobalStrategyManager: " << m_GlobalStrategyManager->EndOfMotion());
+
+ m_Running = false;
+ return m_Running;//Andremize
+ }
ODEBUG("Internal clock:" << m_InternalClock);
m_Running = true;
if (m_StepStackHandler->IsOnLineSteppingOn())
- {
- ODEBUG("On Line Stepping: ON!");
- // ********* WARNING THIS IS THE TIME CONSUMING PART *******************
- if (m_AlgorithmforZMPCOM==ZMPCOM_WIEBER_2006)
- {
- }
- else if (m_AlgorithmforZMPCOM==ZMPCOM_KAJITA_2003)
- {
- m_ZMPD->OnLine(m_InternalClock,
- m_ZMPPositions,
- m_COMBuffer,
- m_LeftFootPositions,
- m_RightFootPositions);
- }
- else if (m_AlgorithmforZMPCOM==ZMPCOM_MORISAWA_2007)
- {
- ODEBUG("InternalClock:" <<m_InternalClock <<
- " SamplingPeriod: "<<m_SamplingPeriod);
-
- m_ZMPM->OnLine(m_InternalClock,
- m_ZMPPositions,
- m_COMBuffer,
- m_LeftFootPositions,
- m_RightFootPositions);
- }
- }
+ {
+ ODEBUG("On Line Stepping: ON!");
+ // ********* WARNING THIS IS THE TIME CONSUMING PART *******************
+ if (m_AlgorithmforZMPCOM==ZMPCOM_WIEBER_2006)
+ {
+ }
+ else if (m_AlgorithmforZMPCOM==ZMPCOM_KAJITA_2003)
+ {
+ m_ZMPD->OnLine(m_InternalClock,
+ m_ZMPPositions,
+ m_COMBuffer,
+ m_LeftFootPositions,
+ m_RightFootPositions);
+ }
+ else if (m_AlgorithmforZMPCOM==ZMPCOM_MORISAWA_2007)
+ {
+ ODEBUG("InternalClock:" <<m_InternalClock <<
+ " SamplingPeriod: "<<m_SamplingPeriod);
+
+ m_ZMPM->OnLine(m_InternalClock,
+ m_ZMPPositions,
+ m_COMBuffer,
+ m_LeftFootPositions,
+ m_RightFootPositions);
+ }
+ }
else
/* Check if we are not in an ending phase generated on-line */
- {
- if ((m_AlgorithmforZMPCOM==ZMPCOM_MORISAWA_2007) &&
- (m_ZMPM->GetOnLineMode()))
- {
- m_ZMPM->OnLine(m_InternalClock,
- m_ZMPPositions,
- m_COMBuffer,
- m_LeftFootPositions,
- m_RightFootPositions);
- }
- }
+ {
+ if ((m_AlgorithmforZMPCOM==ZMPCOM_MORISAWA_2007) &&
+ (m_ZMPM->GetOnLineMode()))
+ {
+ m_ZMPM->OnLine(m_InternalClock,
+ m_ZMPPositions,
+ m_COMBuffer,
+ m_LeftFootPositions,
+ m_RightFootPositions);
+ }
+ }
if (m_AlgorithmforZMPCOM==ZMPCOM_HERDT_2010)
{
@@ -1389,131 +1388,130 @@ namespace PatternGeneratorJRL {
// if ((u=(m_count - (m_ZMPPositions.size()-2*m_NL)))>=0)
if (m_GlobalStrategyManager->EndOfMotion()==GlobalStrategyManager::NEW_STEP_NEEDED)
- {
- ODEBUG("NEW STEP NEEDED" << m_InternalClock/m_SamplingPeriod << " Internal Clock :" << m_InternalClock);
- if (m_StepStackHandler->IsOnLineSteppingOn())
- {
- ODEBUG("Add a step");
- // CAREFULL: we assume that this sequence will create a
- // a new foot steps at the back of the queue handled by the StepStackHandler.
- // Then we have two foot steps: the last one put inside the preview,
- // and the new one.
- RelativeFootPosition lRelativeFootPositions;
- // Add a new step inside the stack.
- if (m_StepStackHandler->ReturnStackSize()<=1)
- {
- m_StepStackHandler->AddStandardOnLineStep(m_NewStep, m_NewStepX, m_NewStepY, m_NewTheta);
- m_NewStep = false;
- }
-
- // Remove the first step of the queue.
- bool EndSequence = m_StepStackHandler->RemoveFirstStepInTheStack();
- ODEBUG("EndSequence:" <<EndSequence);
- // Returns the front foot step in the step stack handler which is not yet
- // in the preview control queue.
- bool EnoughSteps= m_StepStackHandler->ReturnFrontFootPosition(lRelativeFootPositions);
- if ((!EnoughSteps)&& (!EndSequence))
- {
- ODEBUG3("You don't have enough steps in the step stack handler.");
- ODEBUG3("And this is not an end sequence.");
- }
-
-
- ODEBUG(" EnoughSteps: " << EnoughSteps << endl <<
- " EndSequence:" << EndSequence << endl);
-
- if (!EndSequence)
- {
- // ********* WARNING THIS IS THE TIME CONSUMING PART *******************
- if (m_AlgorithmforZMPCOM==ZMPCOM_WIEBER_2006)
- {
- }
- else if (m_AlgorithmforZMPCOM==ZMPCOM_KAJITA_2003)
- {
-
- m_ZMPD->OnLineAddFoot(lRelativeFootPositions,
- m_ZMPPositions,
- m_COMBuffer,
- m_LeftFootPositions,
- m_RightFootPositions,
- EndSequence);
-
- }
- else if (m_AlgorithmforZMPCOM==ZMPCOM_MORISAWA_2007)
- {
- ODEBUG("Putting a new step SX: " <<
- lRelativeFootPositions.sx << " SY: "
- << lRelativeFootPositions.sy );
- m_ZMPM->SetCurrentTime(m_InternalClock);
- m_ZMPM->OnLineAddFoot(lRelativeFootPositions,
- m_ZMPPositions,
- m_COMBuffer,
- m_LeftFootPositions,
- m_RightFootPositions,
- EndSequence);
- ODEBUG("Left and Right foot positions queues: "
- << m_LeftFootPositions.size() << " "
- << m_RightFootPositions.size() );
- }
- }
- else if (EndSequence)
- {
- ODEBUG("End Sequence");
- if (m_AlgorithmforZMPCOM==ZMPCOM_WIEBER_2006)
- {
- }
- else if (m_AlgorithmforZMPCOM==ZMPCOM_KAJITA_2003)
- {
- m_ZMPD->EndPhaseOfTheWalking(m_ZMPPositions,
- m_COMBuffer,
- m_LeftFootPositions,
- m_RightFootPositions);
- }
- else if (m_AlgorithmforZMPCOM==ZMPCOM_MORISAWA_2007)
- {
- ODEBUG("Putting a new step SX: " <<
- lRelativeFootPositions.sx << " SY: "
- << lRelativeFootPositions.sy );
- m_ZMPM->SetCurrentTime(m_InternalClock);
- m_ZMPM->EndPhaseOfTheWalking(m_ZMPPositions,
- m_COMBuffer,
- m_LeftFootPositions,
- m_RightFootPositions);
- ODEBUG("("<<m_InternalClock << ")");
- ODEBUG("Left and Right foot positions queues: "
- << m_LeftFootPositions.size() << " "
- << m_RightFootPositions.size() );
- }
- }
- // ************* THIS HAS TO FIT INSIDE THE control step time ***********
-
- }
- else
- {
-
- // cout << "Sorry not enough information" << endl;
- m_ShouldBeRunning = false;
- UpdateAbsMotionOrNot = true;
-
- if ((m_AlgorithmforZMPCOM==ZMPCOM_MORISAWA_2007) &&
- (m_ZMPM->GetOnLineMode()))
- {
- m_ShouldBeRunning = true;
- }
-
- ODEBUG("Finished the walking pattern generator ("<<m_InternalClock << ")");
- }
-
- ODEBUG4("*** TAG *** " , "DebugDataIK.dat");
-
- }
-
- // Update the absolute position of the robot.
- // to be done only when the robot has finish a motion.
- UpdateAbsolutePosition(UpdateAbsMotionOrNot);
- ODEBUG("Return true");
- return m_Running;
- }
+ {
+ ODEBUG("NEW STEP NEEDED" << m_InternalClock/m_SamplingPeriod << " Internal Clock :" << m_InternalClock);
+ if (m_StepStackHandler->IsOnLineSteppingOn())
+ {
+ ODEBUG("Add a step");
+ // CAREFULL: we assume that this sequence will create a
+ // a new foot steps at the back of the queue handled by the StepStackHandler.
+ // Then we have two foot steps: the last one put inside the preview,
+ // and the new one.
+ RelativeFootPosition lRelativeFootPositions;
+ // Add a new step inside the stack.
+ if (m_StepStackHandler->ReturnStackSize()<=1)
+ {
+ m_StepStackHandler->AddStandardOnLineStep(m_NewStep, m_NewStepX, m_NewStepY, m_NewTheta);
+ m_NewStep = false;
+ }
+
+ // Remove the first step of the queue.
+ bool EndSequence = m_StepStackHandler->RemoveFirstStepInTheStack();
+ ODEBUG("EndSequence:" <<EndSequence);
+ // Returns the front foot step in the step stack handler which is not yet
+ // in the preview control queue.
+ bool EnoughSteps= m_StepStackHandler->ReturnFrontFootPosition(lRelativeFootPositions);
+ if ((!EnoughSteps)&& (!EndSequence))
+ {
+ ODEBUG3("You don't have enough steps in the step stack handler.");
+ ODEBUG3("And this is not an end sequence.");
+ }
+
+
+ ODEBUG(" EnoughSteps: " << EnoughSteps << endl <<
+ " EndSequence:" << EndSequence << endl);
+
+ if (!EndSequence)
+ {
+ // ********* WARNING THIS IS THE TIME CONSUMING PART *******************
+ if (m_AlgorithmforZMPCOM==ZMPCOM_WIEBER_2006)
+ {
+ }
+ else if (m_AlgorithmforZMPCOM==ZMPCOM_KAJITA_2003)
+ {
+
+ m_ZMPD->OnLineAddFoot(lRelativeFootPositions,
+ m_ZMPPositions,
+ m_COMBuffer,
+ m_LeftFootPositions,
+ m_RightFootPositions,
+ EndSequence);
+
+ }
+ else if (m_AlgorithmforZMPCOM==ZMPCOM_MORISAWA_2007)
+ {
+ ODEBUG("Putting a new step SX: " <<
+ lRelativeFootPositions.sx << " SY: "
+ << lRelativeFootPositions.sy );
+ m_ZMPM->SetCurrentTime(m_InternalClock);
+ m_ZMPM->OnLineAddFoot(lRelativeFootPositions,
+ m_ZMPPositions,
+ m_COMBuffer,
+ m_LeftFootPositions,
+ m_RightFootPositions,
+ EndSequence);
+ ODEBUG("Left and Right foot positions queues: "
+ << m_LeftFootPositions.size() << " "
+ << m_RightFootPositions.size() );
+ }
+ }
+ else if (EndSequence)
+ {
+ ODEBUG("End Sequence");
+ if (m_AlgorithmforZMPCOM==ZMPCOM_WIEBER_2006)
+ {
+ }
+ else if (m_AlgorithmforZMPCOM==ZMPCOM_KAJITA_2003)
+ {
+ m_ZMPD->EndPhaseOfTheWalking(m_ZMPPositions,
+ m_COMBuffer,
+ m_LeftFootPositions,
+ m_RightFootPositions);
+ }
+ else if (m_AlgorithmforZMPCOM==ZMPCOM_MORISAWA_2007)
+ {
+ ODEBUG("Putting a new step SX: " <<
+ lRelativeFootPositions.sx << " SY: "
+ << lRelativeFootPositions.sy );
+ m_ZMPM->SetCurrentTime(m_InternalClock);
+ m_ZMPM->EndPhaseOfTheWalking(m_ZMPPositions,
+ m_COMBuffer,
+ m_LeftFootPositions,
+ m_RightFootPositions);
+ ODEBUG("("<<m_InternalClock << ")");
+ ODEBUG("Left and Right foot positions queues: "
+ << m_LeftFootPositions.size() << " "
+ << m_RightFootPositions.size() );
+ }
+ }
+ // ************* THIS HAS TO FIT INSIDE THE control step time ***********
+
+ }
+ else
+ {
+ // cout << "Sorry not enough information" << endl;
+ m_ShouldBeRunning = false;
+ UpdateAbsMotionOrNot = true;
+
+ if ((m_AlgorithmforZMPCOM==ZMPCOM_MORISAWA_2007) &&
+ (m_ZMPM->GetOnLineMode()))
+ {
+ m_ShouldBeRunning = true;
+ }
+
+ ODEBUG("Finished the walking pattern generator ("<<m_InternalClock << ")");
+ }
+
+ ODEBUG4("*** TAG *** " , "DebugDataIK.dat");
+
+ }
+
+ // Update the absolute position of the robot.
+ // to be done only when the robot has finish a motion.
+ UpdateAbsolutePosition(UpdateAbsMotionOrNot);
+ ODEBUG("Return true");
+ return m_Running;
+ }
diff --git a/src/PreviewControl/SupportFSM.cpp b/src/PreviewControl/SupportFSM.cpp
index a1373701ec22ec22c57a89680c74bf66b935340c..f06c7247e95d99f3a5e51b038d989e70c8bf729a 100644
--- a/src/PreviewControl/SupportFSM.cpp
+++ b/src/PreviewControl/SupportFSM.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2010,
+ * Copyright 2010,
*
* Andrei Herdt
* 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)
*/
@@ -111,43 +111,43 @@ SupportFSM::set_support_state(double time, unsigned int pi,
//FSM
if(time+EPS_+pi*T_ >= Support.TimeLimit)
- {
- //SS->DS
- if(Support.Phase == SS && !ReferenceGiven && Support.NbStepsLeft == 0)
- {
- Support.Phase = DS;
- Support.TimeLimit = time+pi*T_+DSPeriod_-T_/10.0;
- Support.StateChanged = true;
- Support.NbInstants = 0;
- }
- //DS->SS
- else if( ((Support.Phase == DS) && ReferenceGiven)
- || ((Support.Phase == DS) && (Support.NbStepsLeft > 0)))
- {
- Support.Phase = SS;
- Support.TimeLimit = time+pi*T_+StepPeriod_-T_/10.0;
- Support.NbStepsLeft = NbStepsSSDS_;
- Support.StateChanged = true;
- Support.NbInstants = 0;
- }
- //SS->SS
- else if((Support.Phase == SS && Support.NbStepsLeft > 0) ||
- (Support.NbStepsLeft == 0 && ReferenceGiven))
- {
- if(Support.Foot == LEFT)
- Support.Foot = RIGHT;
- else
- Support.Foot = LEFT;
- Support.StateChanged = true;
- Support.NbInstants = 0;
- Support.TimeLimit = time+pi*T_+StepPeriod_-T_/10.0;
- if(pi != 1)//Flying foot is not down
- ++Support.StepNumber;
- if (!ReferenceGiven)
- Support.NbStepsLeft = Support.NbStepsLeft-1;
- if (ReferenceGiven)
- Support.NbStepsLeft = NbStepsSSDS_;
- }
- }
+ {
+ //SS->DS
+ if(Support.Phase == SS && !ReferenceGiven && Support.NbStepsLeft == 0)
+ {
+ Support.Phase = DS;
+ Support.TimeLimit = time+pi*T_+DSPeriod_-T_/10.0;
+ Support.StateChanged = true;
+ Support.NbInstants = 0;
+ }
+ //DS->SS
+ else if( ((Support.Phase == DS) && ReferenceGiven)
+ || ((Support.Phase == DS) && (Support.NbStepsLeft > 0)))
+ {
+ Support.Phase = SS;
+ Support.TimeLimit = time+pi*T_+StepPeriod_-T_/10.0;
+ Support.NbStepsLeft = NbStepsSSDS_;
+ Support.StateChanged = true;
+ Support.NbInstants = 0;
+ }
+ //SS->SS
+ else if((Support.Phase == SS && Support.NbStepsLeft > 0) ||
+ (Support.NbStepsLeft == 0 && ReferenceGiven))
+ {
+ if(Support.Foot == LEFT)
+ Support.Foot = RIGHT;
+ else
+ Support.Foot = LEFT;
+ Support.StateChanged = true;
+ Support.NbInstants = 0;
+ Support.TimeLimit = time+pi*T_+StepPeriod_-T_/10.0;
+ if(pi != 1)//Flying foot is not down
+ ++Support.StepNumber;
+ if (!ReferenceGiven)
+ Support.NbStepsLeft = Support.NbStepsLeft-1;
+ if (ReferenceGiven)
+ Support.NbStepsLeft = NbStepsSSDS_;
+ }
+ }
}
diff --git a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp
index 41bbe3b0f868d550df554a1a76a9f551d9d92e70..fac1a93c0dc7e40d3109d15eb158de50219d8ac0 100644
--- a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp
+++ b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp
@@ -137,6 +137,7 @@ OrientationsPreview::preview_orientations(double Time,
}
else
TrunkStateT_.yaw[0] = TrunkState_.yaw[0] + TrunkState_.yaw[1]*T_;
+
//Compute the trunk angle at the end of the support phase
SupportTimePassed_ = CurrentSupport.TimeLimit-Time;
PreviewedTrunkAngleEnd = TrunkStateT_.yaw[0] + TrunkStateT_.yaw[1]*(SupportTimePassed_-T_);
diff --git a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh
index b6cf8a7fa4495c7824806c5431d2c0af0f737501..a04e5a188618c9e96e01765ee9a6c09a2df8ba94 100644
--- a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh
+++ b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh
@@ -1,5 +1,5 @@
/*
- * Copyright 2010,
+ * Copyright 2010,
*
* Mehdi Benallegue
* Andrei Herdt
@@ -21,7 +21,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)
*/
/*
@@ -94,6 +94,10 @@ namespace PatternGeneratorJRL
{ return TrunkState_; };
inline void CurrentTrunkState(const COMState & TrunkState)
{ TrunkState_ = TrunkState; };
+ inline COMState const & PreviewTrunkState() const
+ { return TrunkStateT_; };
+ inline void PreviewTrunkState(const COMState & TrunkState)
+ { TrunkStateT_ = TrunkState; };
inline double SSLength() const
{ return SSPeriod_; };
inline void SSLength( double SSPeriod)
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 34a4443831d5621b868ae382f117daa4803535f0..2c5d4fb4788d8abd6cd6cb102058acad198c1444 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -30,6 +30,7 @@
Andrei Herdt,
Olivier Stasse,
+ Maximilien Naveau,
*/
#include "portability/gettimeofday.hh"
@@ -76,20 +77,21 @@ double filterprecision(double adb)
ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
string , CjrlHumanoidDynamicRobot *aHS ) :
ZMPRefTrajectoryGeneration(SPM),
- Robot_(0),SupportFSM_(0),OrientPrw_(0),VRQPGenerator_(0),IntermedData_(0),
- RFI_(0),Problem_(),Solution_(),OFTG_DF_(0),OFTG_control_(0)
+ Robot_(0),SupportFSM_(0),OrientPrw_(0),OrientPrw_DF_(0),
+ VRQPGenerator_(0),IntermedData_(0),RFI_(0),Problem_(),
+ Solution_(),OFTG_DF_(0),OFTG_control_(0)
{
Running_ = false ;
TimeBuffer_ = 0.04 ;
QP_T_ = 0.1 ;
QP_N_ = 16 ;
m_SamplingPeriod = 0.005 ;
- InterpolationPeriod_ = QP_T_/2;
+ InterpolationPeriod_ = QP_T_/20;
StepPeriod_ = 0.8 ;
- SSPeriod = 0.7 ;
- DSPeriod = 0.1 ;
- FeetDistance = 0.2 ;
- StepHeight = 0.05 ;
+ SSPeriod_ = 0.7 ;
+ DSPeriod_ = 0.1 ;
+ FeetDistance_ = 0.2 ;
+ StepHeight_ = 0.05 ;
CoMHeight_ = 0.814 ;
PerturbationOccured_ = false ;
UpperTimeLimitToUpdate_ = 0.0 ;
@@ -118,6 +120,14 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
COMState CurrentTrunkState;
OrientPrw_->CurrentTrunkState( CurrentTrunkState );
+ // Create and initialize preview of orientations
+ OrientPrw_DF_ = new OrientationsPreview( aHS->rootJoint() );
+ OrientPrw_DF_->SamplingPeriod( QP_T_ );
+ OrientPrw_DF_->NbSamplingsPreviewed( QP_N_ );
+ OrientPrw_DF_->SSLength( SupportFSM_->StepPeriod() );
+ OrientPrw_DF_->CurrentTrunkState( CurrentTrunkState );
+
+
// Initialize the 2D LIPM
LIPM_.SetSimulationControlPeriod( QP_T_ );
LIPM_.SetRobotControlPeriod( m_SamplingPeriod );
@@ -156,23 +166,23 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
// ----------------------------------------------------------------
OFTG_DF_ = new OnLineFootTrajectoryGeneration(SPM,HDR_->leftFoot());
OFTG_DF_->InitializeInternalDataStructures();
- OFTG_DF_->SetSingleSupportTime( SSPeriod );
- OFTG_DF_->SetDoubleSupportTime( DSPeriod );
+ OFTG_DF_->SetSingleSupportTime( SSPeriod_ );
+ OFTG_DF_->SetDoubleSupportTime( DSPeriod_ );
OFTG_DF_->SetSamplingPeriod( InterpolationPeriod_ );
OFTG_DF_->QPSamplingPeriod( QP_T_ );
OFTG_DF_->NbSamplingsPreviewed( QP_N_ );
- OFTG_DF_->FeetDistance( FeetDistance );
- OFTG_DF_->StepHeight( StepHeight );
+ OFTG_DF_->FeetDistance( FeetDistance_ );
+ OFTG_DF_->StepHeight( StepHeight_ );
OFTG_control_ = new OnLineFootTrajectoryGeneration(SPM,HDR_->leftFoot());
OFTG_control_->InitializeInternalDataStructures();
- OFTG_control_->SetSingleSupportTime( SSPeriod );
- OFTG_control_->SetDoubleSupportTime( DSPeriod );
+ OFTG_control_->SetSingleSupportTime( SSPeriod_ );
+ OFTG_control_->SetDoubleSupportTime( DSPeriod_ );
OFTG_control_->SetSamplingPeriod( m_SamplingPeriod );
OFTG_control_->QPSamplingPeriod( QP_T_ );
OFTG_control_->NbSamplingsPreviewed( QP_N_ );
- OFTG_control_->FeetDistance( FeetDistance );
- OFTG_control_->StepHeight( StepHeight );
+ OFTG_control_->FeetDistance( FeetDistance_ );
+ OFTG_control_->StepHeight( StepHeight_ );
// Create and initialize the class that compute the simplify inverse kinematics :
// ------------------------------------------------------------------------------
@@ -262,6 +272,12 @@ ZMPVelocityReferencedQP::~ZMPVelocityReferencedQP()
OrientPrw_ = 0 ;
}
+ if (OrientPrw_DF_!=0)
+ {
+ delete OrientPrw_DF_;
+ OrientPrw_DF_ = 0 ;
+ }
+
if (Robot_!=0)
{
delete Robot_;
@@ -401,13 +417,13 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
//TODO check init with left foot (divergence)
support_state_t CurrentSupport;
CurrentSupport.Phase = DS;
- CurrentSupport.Foot = RIGHT;
+ CurrentSupport.Foot = LEFT;
CurrentSupport.TimeLimit = 1e9;
CurrentSupport.NbStepsLeft = 1;
CurrentSupport.StateChanged = false;
- CurrentSupport.X = CurrentRightFootAbsPos.x; //0.0 ;
- CurrentSupport.Y = CurrentRightFootAbsPos.y; //0.1 ;
- CurrentSupport.Yaw = CurrentRightFootAbsPos.theta*M_PI/180; //0.0 ;
+ CurrentSupport.X = CurrentLeftFootAbsPos.x; //0.0 ;
+ CurrentSupport.Y = CurrentLeftFootAbsPos.y; //0.1 ;
+ CurrentSupport.Yaw = CurrentLeftFootAbsPos.theta*M_PI/180; //0.0 ;
CurrentSupport.StartTime = 0.0;
IntermedData_->SupportState(CurrentSupport);
@@ -435,7 +451,7 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
// Initialize preview of orientations
OrientPrw_->CurrentTrunkState( lStartingCOMState );
-
+ OrientPrw_DF_->CurrentTrunkState( lStartingCOMState );
// BUILD CONSTANT PART OF THE OBJECTIVE:
// -------------------------------------
Problem_.reset();
@@ -446,7 +462,7 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
// initialize intermed data needed during the interpolation
InitStateLIPM_ = LIPM_.GetState() ;
InitStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
- FinalStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
+ FinalCurrentStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
return 0;
}
@@ -535,7 +551,7 @@ ZMPVelocityReferencedQP::OnLine(double time,
Problem_.dump( time );
}
- // INITIALZE INTERPOLATION:
+ // INITIALIZE INTERPOLATION:
// ------------------------
CurrentIndex_ = FinalCOMTraj_deq.size();
solution_ = Solution_ ;
@@ -588,7 +604,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
@@ -611,10 +627,60 @@ ZMPVelocityReferencedQP::OnLine(double time,
<< filterprecision( RightFootTraj_deq_[CurrentIndex_+i].theta * M_PI / 180 ) << " " //21
<< filterprecision( RightFootTraj_deq_[CurrentIndex_+i].omega * M_PI / 180 ) << " " //22
<< filterprecision( solution_.Solution_vec[i/NbSampleInterpolation_] ) << " " //23
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].roll[0] ) << " " // 24
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].roll[1] ) << " " // 25
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].roll[2] ) << " " // 26
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].pitch[0] ) << " " // 27
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].pitch[1] ) << " " // 28
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].pitch[2] ) << " " // 29
+ << filterprecision( i * InterpolationPeriod_ ) << " " // 30
<< endl ;
}
aof.close() ;
+ oss.str("TestHerdt2010footcom");
+ aFileName = oss.str();
+ 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 i = 0 ; i < (FinalCOMTraj_deq.size()-CurrentIndex_) ; ++i )
+ {
+ aof << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].x[0] ) << " " // 1
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].y[0] ) << " " // 2
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].z[0] ) << " " // 3
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].x[1] ) << " " // 4
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].y[1] ) << " " // 5
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].z[1] ) << " " // 6
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].x[2] ) << " " // 7
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].y[2] ) << " " // 8
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].z[2] ) << " " // 9
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].yaw[0] ) << " " // 10
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].yaw[1] ) << " " // 11
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].yaw[2] ) << " " // 12
+ << filterprecision( FinalLeftFootTraj_deq[CurrentIndex_+i].x ) << " " // 13
+ << filterprecision( FinalLeftFootTraj_deq[CurrentIndex_+i].y ) << " " // 14
+ << filterprecision( FinalLeftFootTraj_deq[CurrentIndex_+i].z ) << " " // 15
+ << filterprecision( FinalLeftFootTraj_deq[CurrentIndex_+i].theta * M_PI / 180 ) << " " // 16
+ << filterprecision( FinalLeftFootTraj_deq[CurrentIndex_+i].omega * M_PI / 180 ) << " " // 17
+ << filterprecision( FinalRightFootTraj_deq[CurrentIndex_+i].x ) << " " //18
+ << filterprecision( FinalRightFootTraj_deq[CurrentIndex_+i].y ) << " " //19
+ << filterprecision( FinalRightFootTraj_deq[CurrentIndex_+i].z ) << " " //20
+ << filterprecision( FinalRightFootTraj_deq[CurrentIndex_+i].theta * M_PI / 180 ) << " " //21
+ << filterprecision( FinalRightFootTraj_deq[CurrentIndex_+i].omega * M_PI / 180 ) << " " //22
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].roll[0] ) << " " // 23
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].roll[1] ) << " " // 24
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].roll[2] ) << " " // 25
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].pitch[0] ) << " " // 26
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].pitch[1] ) << " " // 27
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].pitch[2] ) << " " // 28
+ << endl ;
+ }
+ aof.close() ;
iteration++;
@@ -708,8 +774,8 @@ void ZMPVelocityReferencedQP::ControlInterpolation(
OrientPrw_->interpolate_trunk_orientation( time, CurrentIndex_,
m_SamplingPeriod, solution_.SupportStates_deq,
FinalCOMTraj_deq );
- FinalStateOrientPrw_ = OrientPrw_->CurrentTrunkState();
-
+ FinalCurrentStateOrientPrw_ = OrientPrw_->CurrentTrunkState();
+ FinalPreviewStateOrientPrw_ = OrientPrw_->PreviewTrunkState();
// INTERPOLATE THE COMPUTED FOOT POSITIONS:
// ----------------------------------------
OFTG_control_->interpolate_feet_positions( time,
@@ -721,9 +787,10 @@ void ZMPVelocityReferencedQP::ControlInterpolation(
void ZMPVelocityReferencedQP::DynamicFilterInterpolation(double time)
{
+ vector<double> solFoot;
LIPM_subsampled_.setState(InitStateLIPM_) ;
- OrientPrw_->CurrentTrunkState(InitStateOrientPrw_) ;
-
+ OrientPrw_DF_->CurrentTrunkState(InitStateOrientPrw_) ;
+ InterpretSolution(solFoot);
for ( int i = 0 ; i < QP_N_ ; i++ )
{
CoMZMPInterpolation(ZMPTraj_deq_, COMTraj_deq_,
@@ -732,28 +799,269 @@ void ZMPVelocityReferencedQP::DynamicFilterInterpolation(double time)
NbSampleInterpolation_, i);
}
- OFTG_DF_->SetSamplingPeriod( InterpolationPeriod_ );
+ OFTG_DF_->SetSamplingPeriod( InterpolationPeriod_ );
+ solution_t aSolution = solution_ ;
+
for ( int i = 0 ; i < QP_N_ ; i++ )
{
- if (i > QP_N_*0.5 && solution_.SupportStates_deq.front().StateChanged )
- {
- solution_.SupportOrientations_deq[0] = solution_.SupportOrientations_deq[2] ;
- }
- OrientPrw_->interpolate_trunk_orientation( time + i * QP_T_,
+ OrientPrw_DF_->preview_orientations( time + i * QP_T_, VelRef_,
+ SupportFSM_->StepPeriod(),
+ LeftFootTraj_deq_, RightFootTraj_deq_,
+ aSolution );
+
+ OrientPrw_DF_->interpolate_trunk_orientation( time + i * QP_T_,
CurrentIndex_ + i * NbSampleInterpolation_, InterpolationPeriod_,
solution_.SupportStates_deq, COMTraj_deq_ );
- OFTG_DF_->interpolate_feet_positions( time + i * QP_T_,
+ PrepareSolution(i,solFoot);
+
+ if(solution_.SupportStates_deq.front().Phase != DS){
+ int vjskbsrtlb = 0;
+ }
+
+ OFTG_DF_->interpolate_feet_positions( time + i * QP_T_,
solution_.SupportStates_deq, solution_,
- solution_.SupportOrientations_deq,
+ aSolution.SupportOrientations_deq,
LeftFootTraj_deq_, RightFootTraj_deq_);
solution_.SupportStates_deq.pop_front();
- }
- OrientPrw_->CurrentTrunkState(FinalStateOrientPrw_);
+ }
+
+ static int it_DF = 0 ;
+ it_DF++;
+ if (it_DF == 16)
+ {
+ it_DF = 16;
+ }
+
+ OrientPrw_DF_->CurrentTrunkState(FinalCurrentStateOrientPrw_);
+ OrientPrw_DF_->CurrentTrunkState(FinalPreviewStateOrientPrw_);
+ cout << "----------------------------------------------\n" ;
return ;
}
-int ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> & FinalCOMTraj_deq)
+void ZMPVelocityReferencedQP::InterpretSolution(vector<double> &solFoot)
+{
+ double PosFootX(0),PosFootY(0),Vx(0),Vy(0),cosTheta(0),sinTheta(0),Sign(0);
+ int nbSteps (0);
+
+ support_state_t & LastSupport = solution_.SupportStates_deq.back() ;
+ 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 == 101 )
+ {
+ tour = 101 ;
+ }
+ tour++;
+
+ if ( nbSteps == 0 )
+ {
+ PosFootX = 0 ;
+ PosFootY = 0 ;
+ }
+ else
+ {
+ if(FirstSupport.Foot == LEFT )
+ Sign = -1.0 ;
+ else
+ Sign = 1.0 ;
+ 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]) ;
+ 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] ;
+ }
+ solFoot[nbSteps] = PosFootX ;
+ solFoot[solFoot.size()-1] = PosFootY ;
+
+ for (unsigned int i = 0 ; i < (solFoot.size()) ; ++i )
+ {
+ cout << "solFoot["<<i<<"] = "<< solFoot[i] << " " ;
+ }
+ cout << endl ;
+
+ return ;
+}
+
+void ZMPVelocityReferencedQP::ProjectionOnConstraints(double & X, double & Y)
+{
+ 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);
+
+ 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 ;
+ LastSupport.Foot = LEFT ;
+ }
+ else
+ {
+ Sign = 1.0 ;
+ LastSupport.Foot = RIGHT ;
+ }
+ 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);
+
+ VRQPGenerator_->build_inequalities_feet(IneqFeet,SupportStatesDeq);
+
+ cout << "LastSupport.X = " << LastSupport.X << endl ;
+ 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] ;
+ 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 ;
+
+ 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);
+ }
+ }
+
+ 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 ;
+}
+
+void ZMPVelocityReferencedQP::PrepareSolution(int iteration, const vector<double> &solFoot)
+{
+ static int nbStepChanged = 0 ;
+ int nbSteps (0);
+ nbSteps = solution_.SupportStates_deq.back().StepNumber ;
+
+ if (iteration > 0)
+ {
+ nbStepChanged = nbStepChanged + (int)(solution_.SupportStates_deq.front().StateChanged) ;
+ }
+ if (iteration == 0)
+ {
+ nbStepChanged = 0 ;
+ }
+
+// if ( solution_.SupportStates_deq.front().Phase == DS )
+// {/*do nothing*/}
+/* else*/ if ( nbSteps == 1 && nbStepChanged == 2 )
+ {
+ solution_.Solution_vec[2*QP_N_] = solFoot[nbSteps] ;
+ solution_.Solution_vec[2*QP_N_+nbSteps] = solFoot.back() ;
+ }
+ else if ( nbSteps == 2 && nbStepChanged == 2 )
+ {
+ solution_.Solution_vec[2*QP_N_] = solFoot[nbSteps] ;
+ solution_.Solution_vec[2*QP_N_+nbSteps] = solFoot.back() ;
+ }else if ( nbSteps == 2 && nbStepChanged == 1 ){
+ solution_.Solution_vec[2*QP_N_] = solFoot[nbSteps-1] ;
+ solution_.Solution_vec[2*QP_N_+nbSteps] = solFoot[2*nbSteps] ;
+ }else{/*do nothing*/}
+
+ if (solution_.SupportStates_deq.front().Foot == LEFT)
+ cout << "footsupport = LEFT" ;
+ 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
+ << " ; nbStepChanged = " << nbStepChanged;
+
+ }else{}
+ cout << endl ;
+ return ;
+}
+
+void ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> & FinalCOMTraj_deq)
{
const unsigned int N = NbSampleInterpolation_ * QP_N_ ;
// \brief calculate, from the CoM computed by the preview control,
@@ -771,9 +1079,10 @@ int ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> & F
}
/// \brief Debug Purpose
/// --------------------
- ofstream aof;
- string aFileName;
+ ofstream aof,aof2;
+ string aFileName, aFileName2;
ostringstream oss(std::ostringstream::ate);
+ ostringstream oss2(std::ostringstream::ate);
static int iteration = 0;
int iteration100 = (int)iteration/100;
int iteration10 = (int)(iteration - iteration100*100)/10;
@@ -788,10 +1097,6 @@ int ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> & F
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 i = 0 ; i < N ; i++ )
{
// Apply the RNEA to the metapod multibody and print the result in a log file.
@@ -817,25 +1122,54 @@ int ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> & F
DeltaZMPMBPositions_[i].time = time + i * m_SamplingPeriod ;
DeltaZMPMBPositions_[i].stepType = ZMPTraj_deq_[CurrentIndex_+i].stepType ;
- if (i==0){
+ ///----
+ oss.str("TestHerdt2010DynamicZMPMB.dat");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ if (i<NbSampleInterpolation_){
aof << filterprecision( ( - m_force.n()[1] / m_force.f()[2] ) + DInitX_ ) << " " // 1
- << filterprecision( ( m_force.n()[0] / m_force.f()[2] ) + DInitY_ ) << " " // 1
- << filterprecision( ZMPTraj_deq_[CurrentIndex_].px ) << " " // 1
- << filterprecision( ZMPTraj_deq_[CurrentIndex_].py ) << " " // 1
+ << filterprecision( ( m_force.n()[0] / m_force.f()[2] ) + DInitY_ ) << " " // 2
+ << filterprecision( ZMPTraj_deq_[i].px ) << " " // 3
+ << filterprecision( ZMPTraj_deq_[i].py ) << " " // 4
<< endl ;
}
+ aof.close();
+
+ oss2.str("TestHerdt2010DynamicDZMP");
+ oss2 << "_" << iteration100 << iteration10 << iteration1 << ".dat";
+ aFileName2 = oss2.str();
+ if ( i == 0 ){
+ aof2.open(aFileName2.c_str(),ofstream::out);
+ aof2.close();
+ }
+ ///----
+ aof2.open(aFileName2.c_str(),ofstream::app);
+ aof2.precision(8);
+ aof2.setf(ios::scientific, ios::floatfield);
+ aof2 << 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_[CurrentIndex_+i].px ) << " " // 3
+ << filterprecision( ZMPTraj_deq_[CurrentIndex_+i].py ) << " " // 4
+ << endl ;
+ aof2.close();
+
}
- aof.close();
+
static double ecartmaxX = 0 ;
static double ecartmaxY = 0 ;
- if ( abs(DeltaZMPMBPositions_[0].px) > ecartmaxX )
- ecartmaxX = abs(DeltaZMPMBPositions_[0].px) ;
- if ( abs(DeltaZMPMBPositions_[0].py) > ecartmaxY )
- ecartmaxY = abs(DeltaZMPMBPositions_[0].py) ;
- cout << "ecartmaxX :" << ecartmaxX << endl ;
- cout << "ecartmaxY :" << ecartmaxY << endl ;
+ for (unsigned int i = 0 ; i < N ; i++ )
+ {
+ if ( abs(DeltaZMPMBPositions_[i].px) > ecartmaxX )
+ ecartmaxX = abs(DeltaZMPMBPositions_[i].px) ;
+ if ( abs(DeltaZMPMBPositions_[i].py) > ecartmaxY )
+ ecartmaxY = abs(DeltaZMPMBPositions_[i].py) ;
+ }
+ //cout << "ecartmaxX :" << ecartmaxX << endl ;
+ //cout << "ecartmaxY :" << ecartmaxY << endl ;
/// Preview control on the ZMPMBs computed
/// --------------------------------------
@@ -871,22 +1205,21 @@ int ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> & F
{
for(int j=0;j<3;j++)
{
- if ( ComStateBuffer_[i].x[j] == ComStateBuffer_[i].x[j] ||
- ComStateBuffer_[i].y[j] == ComStateBuffer_[i].y[j] )
- {
- FinalCOMTraj_deq[CurrentIndex_+i].x[j] += ComStateBuffer_[i].x[j] ;
- FinalCOMTraj_deq[CurrentIndex_+i].y[j] += ComStateBuffer_[i].y[j] ;
- }
- else
- {
- cout << "kajita2003 preview control diverged\n" ;
- }
+ if ( ComStateBuffer_[i].x[j] == ComStateBuffer_[i].x[j] ||
+ ComStateBuffer_[i].y[j] == ComStateBuffer_[i].y[j] )
+ {}else{cout << "kajita2003 preview control diverged\n" ;}
}
+
+ for(int j=0;j<3;j++)
+ {
+ FinalCOMTraj_deq[CurrentIndex_+i].x[j] += ComStateBuffer_[i].x[j] ;
+ FinalCOMTraj_deq[CurrentIndex_+i].y[j] += ComStateBuffer_[i].y[j] ;
+ }
}
/// \brief Debug Purpose
/// --------------------
- oss.str("TestHerdt2010DynamicDZMP");
+ oss.str("TestHerdt2010DynamicART");
oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
aFileName = oss.str();
aof.open(aFileName.c_str(),ofstream::out);
@@ -899,17 +1232,17 @@ int ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> & F
{
aof << filterprecision( DeltaZMPMBPositions_[i].px ) << " " // 1
<< filterprecision( DeltaZMPMBPositions_[i].py ) << " "; // 2
- for (int j = 0 ; j < VelocityTraj_[i].size() ; ++j)
+ for (unsigned int j = 0 ; j < VelocityTraj_[i].size() ; ++j)
{
aof << filterprecision(VelocityTraj_[i](j)) << " " ;
}
- for (int j = 0 ; j < AccelerationTraj_[i].size() ; ++j)
+ for (unsigned int j = 0 ; j < AccelerationTraj_[i].size() ; ++j)
{
aof << filterprecision(AccelerationTraj_[i](j)) << " " ;
}
aof << endl ;
}
-
+ cout << "iteration = " << iteration << endl;
/// \brief Debug Purpose
/// --------------------
oss.str("TestHerdt2010DynamicDCOM");
@@ -933,7 +1266,7 @@ int ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> & F
}
iteration++;
- return 0;
+ return ;
}
@@ -944,7 +1277,7 @@ void ZMPVelocityReferencedQP::CallToComAndFootRealization(
MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration,
- const unsigned IterationNumber)
+ const unsigned & IterationNumber)
{
// New scheme for WPG v3.0
@@ -990,11 +1323,13 @@ void ZMPVelocityReferencedQP::CallToComAndFootRealization(
aRightFootPosition(3) = aRightFAP.theta;
aRightFootPosition(4) = aRightFAP.omega;
- if (IterationNumber == 0){
+ if (IterationNumber == 0)
+ {
CurrentConfiguration = HDR_->currentConfiguration();
CurrentVelocity = HDR_->currentConfiguration();
CurrentAcceleration = HDR_->currentConfiguration();
- }else{
+ }else
+ {
CurrentConfiguration = PreviousConfiguration_ ;
CurrentVelocity = PreviousVelocity_ ;
CurrentAcceleration = PreviousAcceleration_ ;
@@ -1008,11 +1343,14 @@ void ZMPVelocityReferencedQP::CallToComAndFootRealization(
CurrentAcceleration,
IterationNumber,
0);
- if(IterationNumber == NbSampleInterpolation_-1 ){
+
+ if(IterationNumber == NbSampleInterpolation_-1 )
+ {
HDR_->currentConfiguration(CurrentConfiguration);
HDR_->currentConfiguration(CurrentVelocity);
HDR_->currentConfiguration(CurrentAcceleration);
- }else{
+ }else
+ {
PreviousConfiguration_ = CurrentConfiguration ;
PreviousVelocity_ = CurrentVelocity ;
PreviousAcceleration_ = CurrentAcceleration ;
@@ -1022,7 +1360,7 @@ void ZMPVelocityReferencedQP::CallToComAndFootRealization(
}
void ZMPVelocityReferencedQP::CoMZMPInterpolation(
- std::deque<ZMPPosition> & ZMPPositions, // OUTPUT
+ std::deque<ZMPPosition> & ZMPPositions, // OUTPUT
std::deque<COMState> & COMTraj_deq , // OUTPUT
const std::deque<FootAbsolutePosition> & LeftFootTraj_deq, // INPUT
const std::deque<FootAbsolutePosition> & RightFootTraj_deq,// INPUT
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
index 377e5baea9bf815231cd2efea96db885a90eee57..7e08b57e1e59aa9072252fc55163561a521a9a04 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
@@ -90,7 +90,7 @@ namespace PatternGeneratorJRL
the queue of ZMP, and foot positions.
*/
int InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
- deque<COMState> & CoMStates,
+ deque<COMState> & FinalCoMPositions_deq,
deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
FootAbsolutePosition & InitLeftFootAbsolutePosition,
@@ -103,7 +103,7 @@ namespace PatternGeneratorJRL
/// \brief Update the stacks on-line
void OnLine(double time,
deque<ZMPPosition> & FinalZMPPositions,
- deque<COMState> & CoMStates,
+ deque<COMState> & FinalCOMTraj_deq,
deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
deque<FootAbsolutePosition> &FinalRightFootTraj_deq);
@@ -194,8 +194,8 @@ namespace PatternGeneratorJRL
int QP_N_;
/// \brief 2D LIPM to simulate the evolution of the robot's CoM.
- LinearizedInvertedPendulum2D LIPM_control_ ;
LinearizedInvertedPendulum2D LIPM_ ;
+ LinearizedInvertedPendulum2D LIPM_subsampled_ ;
/// \brief Simplified robot model
RigidBodySystem * Robot_ ;
@@ -205,6 +205,7 @@ namespace PatternGeneratorJRL
/// \brief Decoupled optimization problem to compute the evolution of feet angles.
OrientationsPreview * OrientPrw_;
+ OrientationsPreview * OrientPrw_DF_;
/// \brief Generator of QP problem
GeneratorVelRef * VRQPGenerator_;
@@ -245,6 +246,8 @@ namespace PatternGeneratorJRL
deque<COMState> tmpCoM_ ;
deque<ZMPPosition> tmpZMP_ ;
+ deque<FootAbsolutePosition> tmpRF_ ;
+ deque<FootAbsolutePosition> tmpLF_ ;
/// \brief Index where to begin the interpolation
unsigned CurrentIndex_ ;
@@ -293,6 +296,10 @@ namespace PatternGeneratorJRL
/// \brief Used to compute once, the initial difference between the ZMP and the ZMPMB
bool Once_ ;
double DInitX_, DInitY_ ;
+ COMState InitStateLIPM_ ;
+ COMState InitStateOrientPrw_ ;
+ COMState FinalCurrentStateOrientPrw_ ;
+ COMState FinalPreviewStateOrientPrw_ ;
/// \brief Buffer comtaimimg the difference between the ZMP computed from the Herdt controler
///and the ZMP Multibody computed from the articular trajectory
@@ -305,7 +312,7 @@ namespace PatternGeneratorJRL
Robot_Model m_robot;
/// \brief Standard polynomial trajectories for the feet.
- OnLineFootTrajectoryGeneration * OFTG_;
+ OnLineFootTrajectoryGeneration * OFTG_DF_ ;
OnLineFootTrajectoryGeneration * OFTG_control_ ;
public:
@@ -344,47 +351,38 @@ namespace PatternGeneratorJRL
int ReturnOptimalTimeToRegenerateAStep();
- int DynamicFilter(std::deque<ZMPPosition> & ZMPPositions,
- std::deque<COMState> & COMTraj_deq,
- std::deque<FootAbsolutePosition> & LeftFootAbsolutePositions,
- std::deque<FootAbsolutePosition> & RightFootAbsolutePositions,
- unsigned currentIndex,
- double time
- );
-
- void CallToComAndFootRealization(COMState & acomp,
- FootAbsolutePosition & aLeftFAP,
- FootAbsolutePosition & aRightFAP,
+ int DynamicFilter(double time,
+ std::deque<COMState> & FinalCOMTraj_deq
+ );
+
+ 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,
- unsigned IterationNumber
+ const unsigned IterationNumber
);
- void Interpolation(std::deque<ZMPPosition> & ZMPPositions,
- std::deque<COMState> & COMTraj_deq ,
- std::deque<FootAbsolutePosition> & LeftFootTraj_deq,
- std::deque<FootAbsolutePosition> & RightFootTraj_deq,
- solution_t * Solution,
- LinearizedInvertedPendulum2D * LIPM,
- OrientationsPreview * OrientPrw,
- OnLineFootTrajectoryGeneration * OFTG,
- unsigned currentIndex,
- double time,
- int IterationNumber,
- unsigned numberOfSample
- );
-
void CoMZMPInterpolation(
- std::deque<ZMPPosition> & ZMPPositions,
- std::deque<COMState> & COMTraj_deq ,
- std::deque<FootAbsolutePosition> & LeftFootTraj_deq,
- std::deque<FootAbsolutePosition> & RightFootTraj_deq,
- solution_t * Solution,
- LinearizedInvertedPendulum2D * LIPM,
- unsigned currentIndex,
- int IterationNumber,
- unsigned numberOfSample);
+ std::deque<ZMPPosition> & ZMPPositions, // OUTPUT
+ std::deque<COMState> & COMTraj_deq , // OUTPUT
+ const std::deque<FootAbsolutePosition> & LeftFootTraj_deq, // INPUT
+ const std::deque<FootAbsolutePosition> & RightFootTraj_deq,// INPUT
+ const solution_t * Solution, // INPUT
+ LinearizedInvertedPendulum2D * LIPM, // INPUT/OUTPUT
+ const unsigned numberOfSample, // INPUT
+ const int IterationNumber); // INPUT
+
+ void ControlInterpolation(
+ std::deque<COMState> & FinalCOMTraj_deq, // OUTPUT
+ std::deque<ZMPPosition> & FinalZMPTraj_deq, // OUTPUT
+ std::deque<FootAbsolutePosition> & FinalLeftFootTraj_deq, // OUTPUT
+ std::deque<FootAbsolutePosition> & FinalRightFootTraj_deq, // OUTPUT
+ double time); // INPUT
+
+ void DynamicFilterInterpolation(double time);
};
}
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index 26c8aa32e0819a985e1dca02a55bc973c0455ed7..0eec465ee0a60723ee45b9327a74fa0a58974577 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -194,10 +194,6 @@ namespace PatternGeneratorJRL
int QP_N_;
/// \brief 2D LIPM to simulate the evolution of the robot's CoM.
-// LinearizedInvertedPendulum2D LIPM_control_ ;
-// LinearizedInvertedPendulum2D LIPM_control_subsampled_ ;
-// LinearizedInvertedPendulum2D LIPM_DF_ ;
-// LinearizedInvertedPendulum2D LIPM_DF_subsampled_ ;
LinearizedInvertedPendulum2D LIPM_ ;
LinearizedInvertedPendulum2D LIPM_subsampled_ ;
@@ -209,6 +205,7 @@ namespace PatternGeneratorJRL
/// \brief Decoupled optimization problem to compute the evolution of feet angles.
OrientationsPreview * OrientPrw_;
+ OrientationsPreview * OrientPrw_DF_;
/// \brief Generator of QP problem
GeneratorVelRef * VRQPGenerator_;
@@ -262,16 +259,16 @@ namespace PatternGeneratorJRL
double StepPeriod_ ;
/// \brief Period where the robot is on ONE feet
- double SSPeriod ;
+ double SSPeriod_ ;
/// \brief Period where the robot is on TWO feet
- double DSPeriod ;
+ double DSPeriod_ ;
/// \brief Maximum distance between the feet
- double FeetDistance ;
+ double FeetDistance_ ;
/// \brief Maximum height of the feet
- double StepHeight ;
+ double StepHeight_ ;
/// \brief Height of the CoM
double CoMHeight_ ;
@@ -301,7 +298,8 @@ namespace PatternGeneratorJRL
double DInitX_, DInitY_ ;
COMState InitStateLIPM_ ;
COMState InitStateOrientPrw_ ;
- COMState FinalStateOrientPrw_ ;
+ COMState FinalCurrentStateOrientPrw_ ;
+ COMState FinalPreviewStateOrientPrw_ ;
/// \brief Buffer comtaimimg the difference between the ZMP computed from the Herdt controler
///and the ZMP Multibody computed from the articular trajectory
@@ -353,7 +351,7 @@ namespace PatternGeneratorJRL
int ReturnOptimalTimeToRegenerateAStep();
- int DynamicFilter(double time,
+ void DynamicFilter(double time,
std::deque<COMState> & FinalCOMTraj_deq
);
@@ -364,27 +362,33 @@ namespace PatternGeneratorJRL
MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration,
- const unsigned IterationNumber
+ const unsigned & IterationNumber
);
void CoMZMPInterpolation(
- std::deque<ZMPPosition> & ZMPPositions, // OUTPUT
- std::deque<COMState> & COMTraj_deq , // OUTPUT
- const std::deque<FootAbsolutePosition> & LeftFootTraj_deq, // INPUT
- const std::deque<FootAbsolutePosition> & RightFootTraj_deq,// INPUT
- const solution_t * Solution, // INPUT
- LinearizedInvertedPendulum2D * LIPM, // INPUT/OUTPUT
- const unsigned numberOfSample, // INPUT
- const int IterationNumber); // INPUT
+ std::deque<ZMPPosition> & ZMPPositions, // OUTPUT
+ std::deque<COMState> & COMTraj_deq , // OUTPUT
+ const std::deque<FootAbsolutePosition> & LeftFootTraj_deq, // INPUT
+ const std::deque<FootAbsolutePosition> & RightFootTraj_deq, // INPUT
+ const solution_t * Solution, // INPUT
+ LinearizedInvertedPendulum2D * LIPM, // INPUT/OUTPUT
+ const unsigned numberOfSample, // INPUT
+ const int IterationNumber); // INPUT
void ControlInterpolation(
std::deque<COMState> & FinalCOMTraj_deq, // OUTPUT
std::deque<ZMPPosition> & FinalZMPTraj_deq, // OUTPUT
std::deque<FootAbsolutePosition> & FinalLeftFootTraj_deq, // OUTPUT
std::deque<FootAbsolutePosition> & FinalRightFootTraj_deq, // OUTPUT
- double time); // INPUT
+ double time); // INPUT
void DynamicFilterInterpolation(double time);
+
+ void InterpretSolution(vector<double> &solFoot);
+
+ void PrepareSolution(int iteration, const vector<double> &solFoot);
+
+ void ProjectionOnConstraints(double & X, double & Y);
};
}
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
old mode 100755
new mode 100644
index db617301d8445c819ba0c744f2c30f117e113d58..c784eaa12dc886e54934f52f2bf9ba916a56993b
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -57,14 +57,14 @@ GeneratorVelRef::~GeneratorVelRef()
//}
-void
+void
GeneratorVelRef::Ponderation( double weight, objective_e type)
{
IntermedQPMat::objective_variant_t & Objective = IntermedData_->Objective( type );
Objective.weight = weight;
-}
+}
void
@@ -74,7 +74,7 @@ GeneratorVelRef::preview_support_states( double time, const SupportFSM * FSM,
deque<support_state_t> & SupportStates_deq )
{
- const FootAbsolutePosition * FAP = 0;
+ const FootAbsolutePosition * FAP = NULL;
// DETERMINE CURRENT SUPPORT STATE:
// --------------------------------
@@ -159,32 +159,32 @@ GeneratorVelRef::generate_selection_matrices( const std::deque<support_state_t>
SS_it = SupportStates_deq.begin();//points at the cur. sup. st.
++SS_it;
for(unsigned i=0;i<N_;i++)
+ {
+ if(SS_it->StepNumber>0)
{
- if(SS_it->StepNumber>0)
- {
- State.V(i,SS_it->StepNumber-1) = State.VT(SS_it->StepNumber-1,i) = 1.0;
- if( SS_it->StepNumber==1 && SS_it->StateChanged && SS_it->Phase == SS )
- {
- --SS_it;
- State.Vc_fX(0) = SS_it->X;
- State.Vc_fY(0) = SS_it->Y;
- ++SS_it;
-
- State.V_f(0,0) = 1.0;
- }
- else if(SS_it->StepNumber>1)
- {
- State.V_f(SS_it->StepNumber-1,SS_it->StepNumber-2) = -1.0;
- State.V_f(SS_it->StepNumber-1,SS_it->StepNumber-1) = 1.0;
- }
- }
- else
- {
- State.VcX(i) = SS_it->X;
- State.VcY(i) = SS_it->Y;
- }
- ++SS_it;
+ State.V(i,SS_it->StepNumber-1) = State.VT(SS_it->StepNumber-1,i) = 1.0;
+ if( SS_it->StepNumber==1 && SS_it->StateChanged && SS_it->Phase == SS )
+ {
+ --SS_it;
+ State.Vc_fX(0) = SS_it->X;
+ State.Vc_fY(0) = SS_it->Y;
+ ++SS_it;
+
+ State.V_f(0,0) = 1.0;
+ }
+ else if(SS_it->StepNumber>1)
+ {
+ State.V_f(SS_it->StepNumber-1,SS_it->StepNumber-2) = -1.0;
+ State.V_f(SS_it->StepNumber-1,SS_it->StepNumber-1) = 1.0;
+ }
+ }
+ else
+ {
+ State.VcX(i) = SS_it->X;
+ State.VcY(i) = SS_it->Y;
}
+ ++SS_it;
+ }
State.VcshiftX.clear();
@@ -195,20 +195,20 @@ GeneratorVelRef::generate_selection_matrices( const std::deque<support_state_t>
State.VcshiftX(0) = SS_it->X;
State.VcshiftY(0) = SS_it->Y;
for(unsigned i=0; i<(N_-1); ++i)
+ {
+ for(unsigned j = 0; j < NbPrwSteps; ++j)
{
- for(unsigned j = 0; j < NbPrwSteps; ++j)
- {
- State.Vshift(i+1,j) = State.V(i,j);
+ State.Vshift(i+1,j) = State.V(i,j);
- }
- State.VcshiftX(i+1) = State.VcX(i);
- State.VcshiftY(i+1) = State.VcY(i);
}
+ State.VcshiftX(i+1) = State.VcX(i);
+ State.VcshiftY(i+1) = State.VcY(i);
+ }
}
-void
+void
GeneratorVelRef::compute_global_reference( const solution_t & Solution )
{
@@ -281,7 +281,7 @@ GeneratorVelRef::initialize_matrices( linear_inequality_t & Inequalities)
}
-void
+void
GeneratorVelRef::build_inequalities_cop(linear_inequality_t & Inequalities,
const std::deque<support_state_t> & SupportStates_deq) const
{
@@ -340,6 +340,14 @@ GeneratorVelRef::build_inequalities_feet( linear_inequality_t & Inequalities,
RFI_->compute_linear_system( FeetHull, *prwSS_it );
+ cout << " FeetHull : " << endl ;
+ for ( int i = 0 ; i < FeetHull.X_vec.size() ; ++i )
+ {
+ cout << FeetHull.X_vec[i] << " ";
+ cout << FeetHull.Y_vec[i] << endl ;
+ }
+ cout << endl ;
+
for( unsigned j = 0; j < nbEdges; j++ )
{
Inequalities.D.X_mat.push_back( (prwSS_it->StepNumber-1)*nbEdges+j, (prwSS_it->StepNumber-1), FeetHull.A_vec[j] );
@@ -457,19 +465,19 @@ GeneratorVelRef::build_constraints_feet(const linear_inequality_t & IneqFeet,
unsigned int NbConstraints = Pb.NbConstraints();
// -D*V_f
- compute_term ( MM_, -1.0, IneqFeet.D.X_mat, State.V_f );
- Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, 2*N_ );
- compute_term ( MM_, -1.0, IneqFeet.D.Y_mat, State.V_f );
- Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, 2*N_+NbStepsPreviewed );
+ compute_term ( MM_, -1.0, IneqFeet.D.X_mat, State.V_f );
+ Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, 2*N_ );
+ compute_term ( MM_, -1.0, IneqFeet.D.Y_mat, State.V_f );
+ Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, 2*N_+NbStepsPreviewed );
// +dc
- Pb.add_term_to( VECTOR_DS, IneqFeet.Dc_vec, NbConstraints );
+ Pb.add_term_to( VECTOR_DS, IneqFeet.Dc_vec, NbConstraints );
// D*Vc_f*FPc
- compute_term ( MV_, 1.0, IneqFeet.D.X_mat, State.Vc_fX );
- Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
- compute_term ( MV_, 1.0, IneqFeet.D.Y_mat, State.Vc_fY );
- Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
+ compute_term ( MV_, 1.0, IneqFeet.D.X_mat, State.Vc_fX );
+ Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
+ compute_term ( MV_, 1.0, IneqFeet.D.Y_mat, State.Vc_fY );
+ Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
}
@@ -584,7 +592,7 @@ GeneratorVelRef::build_constraints( QPProblem & Pb, const solution_t & Solution
}
-void
+void
GeneratorVelRef::build_invariant_part( QPProblem & Pb )
{
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
index 12e727fc255eadd204aff0c7f1e99acf16ac1635..f1500979f5e4551d254f158bb01794bab7126a15 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
@@ -121,6 +121,14 @@ namespace PatternGeneratorJRL
{ IntermedData_->CoM(CoM); };
/// \}
+ /// \brief Generate a queue of inequality constraints on
+ /// the feet positions with respect to previous foot positions
+ ///
+ /// \param[out] Inequalities
+ /// \param[in] SupportStates_deq
+ void build_inequalities_feet(linear_inequality_t & Inequalities,
+ const std::deque<support_state_t> & SupportStates_deq) const;
+
//
// Protected methods
//
@@ -138,14 +146,6 @@ namespace PatternGeneratorJRL
void build_inequalities_cop(linear_inequality_t & Inequalities,
const std::deque<support_state_t> & SupportStates_deq) const;
- /// \brief Generate a queue of inequality constraints on
- /// the feet positions with respect to previous foot positions
- ///
- /// \param[out] Inequalities
- /// \param[in] SupportStates_deq
- void build_inequalities_feet(linear_inequality_t & Inequalities,
- const std::deque<support_state_t> & SupportStates_deq) const;
-
/// \brief Generate a queue of inequality constraints on
/// the feet positions with respect to previous foot positions
///
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index 6e4f5a342df19a8d5a3743cf73511e4258c51214..d6efdb4c3f9e3250f8ad868903634465d4a952cd 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -114,7 +114,6 @@ private:
/*! Open and reset appropriatly the debug files. */
prepareDebugFiles();
- initIK();
for (unsigned int lNbIt=0;lNbIt<m_OuterLoopNbItMax;lNbIt++)
{
os << "<===============================================================>"<<endl;
@@ -235,6 +234,8 @@ private:
ComAndFootRealization_->SetHeightOfTheCoM(0.814);
ComAndFootRealization_->setSamplingPeriod(0.005);
ComAndFootRealization_->Initialization();
+
+ initIK();
}
protected:
@@ -259,6 +260,7 @@ protected:
ComAndFootRealization_->SetHeightOfTheCoM(0.814);
ComAndFootRealization_->setSamplingPeriod(0.005);
ComAndFootRealization_->Initialization();
+
ComAndFootRealization_->InitializationCoM(BodyAngles,lStartingCOMState,
waist,
m_OneStep.LeftFootPosition, m_OneStep.RightFootPosition);
@@ -428,14 +430,15 @@ protected:
aLeftFootPosition(2) = m_OneStep.LeftFootPosition.z; aRightFootPosition(2) = m_OneStep.RightFootPosition.z;
aLeftFootPosition(3) = m_OneStep.LeftFootPosition.theta; aRightFootPosition(3) = m_OneStep.RightFootPosition.theta;
aLeftFootPosition(4) = m_OneStep.LeftFootPosition.omega; aRightFootPosition(4) = m_OneStep.RightFootPosition.omega;
+ ComAndFootRealization_->setSamplingPeriod(0.005);
ComAndFootRealization_->ComputePostureForGivenCoMAndFeetPosture(aCOMState, aCOMSpeed, aCOMAcc,
aLeftFootPosition,
aRightFootPosition,
CurrentConfiguration,
CurrentVelocity,
CurrentAcceleration,
- iteration_zmp,
- 1);
+ m_OneStep.NbOfIt,
+ 0);
/// \brief rnea, calculation of the multi body ZMP
/// ----------------------------------------------
@@ -448,8 +451,8 @@ protected:
}
metapod::rnea< Robot_Model2, true >::run(robot_, q, dq, ddq);
- Node2 node = boost::fusion::at_c<Robot_Model2::BODY>(robot_.nodes);
- Force2 aforce = node.body.iX0.applyInv (node.joint.f) ;
+ Node2 anode = boost::fusion::at_c<Robot_Model2::BODY>(robot_.nodes);
+ Force2 aforce = anode.body.iX0.applyInv (anode.joint.f) ;
double ZMPMB[2];
@@ -474,8 +477,8 @@ protected:
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 ;
+ tmp_zmp[0] = errx ;
+ tmp_zmp[1] = erry ;
errZMP_.push_back(tmp_zmp);
}
}
@@ -717,8 +720,8 @@ void startOnLineWalking(PatternGeneratorInterface &aPGI)
#define localNbOfEvents 6
struct localEvent events [localNbOfEvents] =
{ {1*200,&TestHerdt2010::walkForward},
- {5*200,&TestHerdt2010::walkSidewards},
- {10*200,&TestHerdt2010::startTurningRightOnSpot},
+ {5*200,&TestHerdt2010::startTurningRightOnSpot},
+ {10*200,&TestHerdt2010::walkForward},
// {35*200,&TestHerdt2010::walkForward},
{15*200,&TestHerdt2010::startTurningLeftOnSpot},
// {55*200,&TestHerdt2010::walkForward},
@@ -733,10 +736,10 @@ void startOnLineWalking(PatternGeneratorInterface &aPGI)
// Test when triggering event.
for(unsigned int i=0;i<localNbOfEvents;i++)
{
- if ( m_OneStep.NbOfIt==events[i].time){
- ODEBUG3("********* GENERATE EVENT OLW ***********");
- (this->*(events[i].Handler))(*m_PGI);
- }
+ if ( m_OneStep.NbOfIt==events[i].time){
+ ODEBUG3("********* GENERATE EVENT OLW ***********");
+ (this->*(events[i].Handler))(*m_PGI);
+ }
}
}