diff --git a/include/jrl/walkgen/pgtypes.hh b/include/jrl/walkgen/pgtypes.hh
index 1f10b8a3cd53ab570a438e2d84691e8b48d3a058..8aa6afe71dc535fb4a8542aa9bba8a59c422d667 100644
--- a/include/jrl/walkgen/pgtypes.hh
+++ b/include/jrl/walkgen/pgtypes.hh
@@ -90,8 +90,8 @@ namespace PatternGeneratorJRL
friend std::ostream & operator<<(std::ostream &os, const struct COMState_s & acs);
};
-
-
+
+
typedef struct COMState_s COMState;
@@ -99,7 +99,7 @@ namespace PatternGeneratorJRL
a sequence of relative positions. */
struct RelativeFootPosition_s
{
- double sx,sy,theta;
+ double sx,sy,sz,theta;
double SStime;
double DStime;
int stepType; //1:normal walking 2:one step before obstacle
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
index c7de348265db1fb43a3fa8f14a4b919584abcaf8..2a8969ec24e93b95f84390045ce3508838aedc7b 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2008, 2009, 2010,
+ * Copyright 2008, 2009, 2010,
*
* Francois Keith
* Olivier Stasse
@@ -19,7 +19,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
@@ -34,16 +34,17 @@
using namespace PatternGeneratorJRL;
FootTrajectoryGenerationAbstract::FootTrajectoryGenerationAbstract(SimplePluginManager *lSPM,
- CjrlFoot *aFoot)
+ CjrlFoot *aFoot)
: SimplePlugin(lSPM)
{
m_Omega = 0.0;
- m_Foot= aFoot;
+ m_Foot= aFoot;
m_SamplingPeriod = 0.005;
+ m_isStepStairOn =1;
- std::string aMethodName[5] =
+ std::string aMethodName[5] =
{":omega",
- ":stepheight",
+ ":stepheight",
":singlesupporttime",
":doublesupporttime",
":samplingperiod"};
@@ -84,10 +85,10 @@ void FootTrajectoryGenerationAbstract::CallMethod(std::string &Method,
void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolutePosition> & , //SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> &, //NoneSupportFootAbsolutePositions,
- int , //CurrentAbsoluteIndex,
- int , //IndexInitial,
+ int , //CurrentAbsoluteIndex,
+ int , //IndexInitial,
double , //ModulatedSingleSupportTime,
- int , //StepType,
+ int , //StepType,
int ) //LeftOrRight)
{
LTHROW("FootTrajectoryGenerationAbstract::UpdateFootPosition-1: To be implemented ");
@@ -95,11 +96,11 @@ void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolut
void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolutePosition> & ,//SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> & ,//NoneSupportFootAbsolutePositions,
- int , // StartIndex,
+ int , // StartIndex,
int , //k,
double , //LocalInterpolationStartTime,
double , //ModulatedSingleSupportTime,
- int , //StepType,
+ int , //StepType,
int ) //LeftOrRight)
{
LTHROW("FootTrajectoryGenerationAbstract::UpdateFootPosition-2: To be implemented ");
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh
index ef4ae45c9f50e7054ab798d8a39543b1502faa7c..3f632660f53293e00f5457858386dd1463593034 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh
@@ -1,5 +1,5 @@
/*
- * Copyright 2008, 2009, 2010,
+ * Copyright 2008, 2009, 2010,
*
* Olivier Stasse
*
@@ -18,7 +18,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)
*/
@@ -52,14 +52,14 @@ namespace PatternGeneratorJRL
/** @ingroup foottrajectorygeneration
This class defines the abstract interface to interact with foot generation object.
- Two parameters \f$ T_{DS} \f$ and \f$ T_{SS} \f$ defines respectively
+ Two parameters \f$ T_{DS} \f$ and \f$ T_{SS} \f$ defines respectively
the double support time and the single support time.
\f$ \omega \f$ defines the angle of the feet for landing and taking off.
- The foot rotates around the toe from \f$ 0 \f$ to \f$ \omega \f$
- for taking off. Whereas for the landing the foot rotates around the
+ The foot rotates around the toe from \f$ 0 \f$ to \f$ \omega \f$
+ for taking off. Whereas for the landing the foot rotates around the
heel from \f$ \omega \f$ to \f$ 0 \f$.
-
+
The sampling control time indicates the amount of time between two
iteration of the algorithm. This parameter is used in the method
UpdateFootPosition to compute the time from the iteration number
@@ -68,21 +68,21 @@ namespace PatternGeneratorJRL
An instance of a class derived from FootTrajectoryGenerationAbstract,
should call InitializeInternalDataStructures() once all the internal
parameters of the object are set.
-
+
The virtual function FreeInternalDataStructures() is used when changing some
parameters and by the destructor.
The most important function is UpdateFootPosition() which populates a
- queue of foot absolute positions data structure.
+ queue of foot absolute positions data structure.
+
+ See a derived class such as FootTrajectoryGenerationStandard
+ for more precise information on the usage and sample codes.
- See a derived class such as FootTrajectoryGenerationStandard
- for more precise information on the usage and sample codes.
-
*/
class FootTrajectoryGenerationAbstract :public SimplePlugin
{
public:
-
+
/*! Constructor: In order to compute some appropriate strategies,
this class needs to extract specific details from the humanoid model. */
FootTrajectoryGenerationAbstract(SimplePluginManager *lSPM,
@@ -99,7 +99,7 @@ namespace PatternGeneratorJRL
This position is supposed to be constant.
@param NoneSupportFootAbsolutePositions: Queue of absolute position for the swinging
foot. This method will set the foot position at index NoneSupportFootAbsolutePositions
- of the queue.
+ of the queue.
@param CurrentAbsoluteIndex: Index in the queues of the foot position to be set.
@param IndexInitial: Index in the queues which correspond to the starting point
of the current single support phase.
@@ -109,8 +109,8 @@ namespace PatternGeneratorJRL
*/
virtual void UpdateFootPosition(std::deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
- int CurrentAbsoluteIndex,
- int IndexInitial,
+ int CurrentAbsoluteIndex,
+ int IndexInitial,
double ModulatedSingleSupportTime,
int StepType, int LeftOrRight);
@@ -123,56 +123,56 @@ namespace PatternGeneratorJRL
/*! Initialize internal data structures. */
virtual void InitializeInternalDataStructures()=0;
-
+
/*! Free internal data structures */
virtual void FreeInternalDataStructures()=0;
- /*! \name Setter and getter for parameters
+ /*! \name Setter and getter for parameters
@{
*/
-
- /*! \name Single Support Time
+
+ /*! \name Single Support Time
@{
*/
-
+
/*! \brief Set single support time */
void SetSingleSupportTime(double aTSingle)
{ m_TSingle =aTSingle;};
-
+
/*! \brief Get single support time */
double GetSingleSupportTime() const
{ return m_TSingle;};
/*! @} */
- /*! \name Double Support Time
+ /*! \name Double Support Time
@{
*/
/*! \brief Set double support time */
void SetDoubleSupportTime(double aTDouble)
{ m_TDouble =aTDouble;};
-
+
/*! \brief Get single support time */
double GetDoubleSupportTime() const
{ return m_TDouble;};
/*! @}*/
- /*! \name Sampling control Time
+ /*! \name Sampling control Time
@{
*/
-
+
/*! \brief Set single support time */
void SetSamplingPeriod(double aSamplingPeriod)
{ m_SamplingPeriod = aSamplingPeriod;};
-
+
/*! \brief Get single support time */
double GetSamplingPeriod() const
{ return m_SamplingPeriod;};
/*!@}*/
-
- /*! \name Omega.
+
+ /*! \name Omega.
@{*/
/*! Get Omega */
@@ -182,17 +182,17 @@ namespace PatternGeneratorJRL
/*! Set Omega */
void SetOmega(double anOmega)
{ m_Omega = anOmega; };
-
+
/*! @} */
/*! @} */
- /*! \brief Reimplementation of the call method for the plugin manager.
+ /*! \brief Reimplementation of the call method for the plugin manager.
More explicitly this object will deal with the call which initialize
the feet behaviors (\f$omega\f$, \f$ stepheight \f$) .
*/
virtual void CallMethod(std::string &Method, std::istringstream &strm);
- protected:
+ protected:
/*! Sampling period of the control. */
double m_SamplingPeriod;
@@ -205,12 +205,14 @@ namespace PatternGeneratorJRL
/*! Store a pointer to Foot information. */
CjrlFoot * m_Foot;
-
+
/*! Omega the angle for taking off and landing. */
double m_Omega;
+
+ int m_isStepStairOn;
};
-
-
+
+
}
#endif /* _FOOT_TRAJECTORY_GENERATION_ABSTRACT_H_ */
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
index 2ecf9765f0da4c23dba93f674946e0833d250cca..bd105a7b61bb036b134996fd3a7e1913171518ef 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
@@ -45,7 +45,7 @@ FootTrajectoryGenerationStandard::FootTrajectoryGenerationStandard(SimplePluginM
m_PolynomeOmega = 0;
m_PolynomeOmega2 = 0;
m_PolynomeTheta = 0;
- m_isStepStairOn = 1;
+// m_isStepStairOn = 1;
/* Computes information on foot dimension
from humanoid specific informations. */
@@ -179,6 +179,7 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex,
else
{
m_BsplinesZ->SetParameters(TimeInterval,Position/1.5,TimeInterval/3.0,Position);
+ cout << "XXXXXXXXXXXXXXXXXXXXXXXXXXXX" << endl;
}
break;
@@ -226,9 +227,8 @@ int FootTrajectoryGenerationStandard::SetParametersWithInitPosInitSpeed(int Poly
}
else
{
- m_BsplinesZ->SetParameters(TimeInterval,FinalPosition/1.5,TimeInterval/3.0,FinalPosition);
+ m_BsplinesZ->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition/1.5,TimeInterval/3.0,FinalPosition,InitPosition,InitSpeed);
}
-
break;
case THETA_AXIS:
@@ -271,7 +271,7 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex, double Ti
}
else
{
- m_BsplinesZ->SetParameters(TimeInterval,FinalPosition/1.5,TimeInterval/3.0,FinalPosition);
+ m_BsplinesZ->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition/1.5,TimeInterval/3.0,FinalPosition,InitPosition,InitSpeed);
}
break;
@@ -444,7 +444,7 @@ double FootTrajectoryGenerationStandard::ComputeSecDerivative(unsigned int Polyn
}
else
{
- r=m_BsplinesZ->ZComputeVelocity(Time);
+ r=m_BsplinesZ->ZComputeAcc(Time);
}
break;
@@ -497,6 +497,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
// Do not modify x, y and theta while liftoff.
curr_NSFAP.x = init_NSFAP.x;
curr_NSFAP.y = init_NSFAP.y;
+ //curr_NSFAP.z = init_NSFAP.z;
curr_NSFAP.theta = init_NSFAP.theta;
}
else if (LocalTime < StartLanding)
@@ -504,6 +505,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
// DO MODIFY x, y and theta the remaining time.
curr_NSFAP.x = init_NSFAP.x + m_PolynomeX->Compute(LocalTime - EndOfLiftOff);
curr_NSFAP.y = init_NSFAP.y + m_PolynomeY->Compute(LocalTime - EndOfLiftOff);
+
curr_NSFAP.theta = init_NSFAP.theta + m_PolynomeTheta->Compute(LocalTime - EndOfLiftOff);
}
else
@@ -522,6 +524,8 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
{
curr_NSFAP.z = init_NSFAP.z + m_BsplinesZ->ZComputePosition(LocalTime);
}
+
+
ODEBUG2("x:" << curr_NSFAP.x << " LocalTime - EndOfLiftOff" << LocalTime - EndOfLiftOff
<< " " << m_PolynomeX->Compute(LocalTime - EndOfLiftOff));
// m_PolynomeX->print();
@@ -693,6 +697,8 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dtheta =
m_PolynomeTheta->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
// +NoneSupportFootAbsolutePositions[StartIndex].dtheta;
+
+
}
else
{
@@ -721,29 +727,37 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
// + NoneSupportFootAbsolutePositions[StartIndex].dtheta;
}
- if (m_isStepStairOn == 1)
+ if (m_isStepStairOn == 1)
{
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z =
- //m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
- m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime);
- //m_AnklePositionRight[2];
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
- //m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
- m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
- //m_AnklePositionRight[2];
+ if (m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime)== 0.0)
+ {
+ cout << LocalInterpolationStartTime+InterpolationTime << " " << NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].z << " " << NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z << endl;
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z = NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].z +
+ m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime);
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
+ m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
+ }
+
+ else{
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z = //prev_NSFAP.z +
+ m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime);
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
+ m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
+ }
+
}
else
{
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z =
m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
-
//m_AnklePositionRight[2];
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
- m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
+ m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//
//m_AnklePositionRight[2];
}
+
bool ProtectionNeeded=false;
// Treat Omega with the following strategy:
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
index e94cfac4621374424625c65a83f12884a3d00b59..999d39c93e8539db220caa9cbd3b058dc6fe2338 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
@@ -211,7 +211,7 @@ namespace PatternGeneratorJRL
/*! \brief Bsplines for Z axis position. */
ZBsplines *m_BsplinesZ;
- int m_isStepStairOn;
+ // int m_isStepStairOn;
/*! \brief Foot dimension. */
double m_FootB, m_FootH, m_FootF;
diff --git a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
index 2397ef5abcf6da21a6e14d29a96eb7bbe3034e07..d066e60d4dee5b53284a148bd246157d71e9ba92 100644
--- a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
+++ b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
@@ -77,6 +77,7 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
// Do not modify x, y and theta while liftoff.
curr_NSFAP.x = prev_NSFAP.x;
curr_NSFAP.y = prev_NSFAP.y;
+ //curr_NSFAP.z = prev_NSFAP.z;
curr_NSFAP.theta = prev_NSFAP.theta;
}
else if (LocalInterpolationStartTime < EndOfLiftOff && LocalInterpolationStartTime +InterpolationTime > EndOfLiftOff)
@@ -85,6 +86,7 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
double remainingTime = LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff;
// x, dx
curr_NSFAP.x = m_PolynomeX->Compute(remainingTime);
+ // cout << "remain "<< prev_NSFAP.x << " " << remainingTime << " " << curr_NSFAP.x <<endl;
curr_NSFAP.dx = m_PolynomeX->ComputeDerivative(remainingTime);
if(m_PolynomeX->Degree() > 4)
curr_NSFAP.ddx = m_PolynomeX->ComputeSecDerivative(remainingTime);
@@ -96,6 +98,7 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
//theta, dtheta
curr_NSFAP.theta = m_PolynomeTheta->Compute(remainingTime);
curr_NSFAP.dtheta = m_PolynomeTheta->ComputeDerivative(remainingTime);
+
if(m_PolynomeTheta->Degree() > 4)
curr_NSFAP.ddtheta = m_PolynomeTheta->ComputeSecDerivative(remainingTime);
}
@@ -104,6 +107,7 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
// DO MODIFY x, y and theta all the time.
// x, dx
curr_NSFAP.x = m_PolynomeX->Compute(InterpolationTime);
+ // cout << "Inter "<< prev_NSFAP.x << " " << InterpolationTime << " " << curr_NSFAP.x <<endl;
curr_NSFAP.dx = m_PolynomeX->ComputeDerivative(InterpolationTime);
if(m_PolynomeX->Degree() > 4)
curr_NSFAP.ddx = m_PolynomeX->ComputeSecDerivative(InterpolationTime);
@@ -112,6 +116,7 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
curr_NSFAP.dy = m_PolynomeY->ComputeDerivative(InterpolationTime);
if(m_PolynomeY->Degree() > 4)
curr_NSFAP.ddy = m_PolynomeY->ComputeSecDerivative(InterpolationTime);
+
//theta, dtheta
curr_NSFAP.theta = m_PolynomeTheta->Compute( InterpolationTime );
curr_NSFAP.dtheta = m_PolynomeTheta->ComputeDerivative(InterpolationTime);
@@ -119,14 +124,20 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
curr_NSFAP.ddtheta = m_PolynomeTheta->ComputeSecDerivative(InterpolationTime);
}
- if (m_isStepStairOn == 1)
- {
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z =
- m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime);
- //m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
- m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
- // m_PolynomeZ->ComputeDerivative(LocalInterpolationStartTime+InterpolationTime);
+ if (m_isStepStairOn == 1)
+ {
+ if (m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime) == 0.0)
+ {
+
+ //cout << LocalInterpolationStartTime+InterpolationTime << " " << prev_NSFAP.z << " " << curr_NSFAP.z << endl;
+ curr_NSFAP.z = prev_NSFAP.z;
+ curr_NSFAP.dz = m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
+ }
+
+ else{
+ curr_NSFAP.z = m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime);
+ curr_NSFAP.dz = m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
+ }
}
else{
@@ -134,8 +145,8 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
m_PolynomeZ->ComputeDerivative(LocalInterpolationStartTime+InterpolationTime);
-
}
+
bool ProtectionNeeded=false;
// Treat Omega with the following strategy:
@@ -270,6 +281,7 @@ OnLineFootTrajectoryGeneration::interpolate_feet_positions(double Time,
FinalRightFootTraj_deq.resize((unsigned int)(QP_T_/m_SamplingPeriod)+CurrentIndex+1);
if(CurrentSupport.Phase == SS && Time+3.0/2.0*QP_T_ < CurrentSupport.TimeLimit)
{
+ // m_isStepStairOn = 1;
//determine coefficients of interpolation polynome
double ModulationSupportCoefficient = 0.9;
double UnlockedSwingPeriod = m_TSingle * ModulationSupportCoefficient;
@@ -303,8 +315,14 @@ OnLineFootTrajectoryGeneration::interpolate_feet_positions(double Time,
);
if(CurrentSupport.StateChanged==true)
- SetParameters(FootTrajectoryGenerationStandard::Z_AXIS, m_TSingle, StepHeight_);
+ {
+ if (m_isStepStairOn == 1)
+ SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::Z_AXIS,TimeInterval,StepHeight_,LastSFP->z,LastSFP->dz);
+ else
+ SetParameters(FootTrajectoryGenerationStandard::Z_AXIS, m_TSingle, StepHeight_);
+ // SetParameters(FootTrajectoryGenerationStandard::Z_AXIS, m_TSingle, StepHeight_);
+ }
SetParametersWithInitPosInitSpeed(
FootTrajectoryGenerationStandard::THETA_AXIS,
TimeInterval, PreviewedSupportAngles_deq[0]*180.0/M_PI,
@@ -344,6 +362,7 @@ OnLineFootTrajectoryGeneration::interpolate_feet_positions(double Time,
}
else if (CurrentSupport.Phase == DS || Time+3.0/2.0*QP_T_ > CurrentSupport.TimeLimit)
{
+ //m_isStepStairOn = 0;
for(int k = 0; k<=(int)(QP_T_/m_SamplingPeriod);k++)
{
FinalRightFootTraj_deq[CurrentIndex+k]= FinalRightFootTraj_deq[CurrentIndex+k-1];
diff --git a/src/Mathematics/Bsplines.cpp b/src/Mathematics/Bsplines.cpp
index 62c5273738d1e73e02b5f669057aaad176418e5e..0b100ea4a67cd20e2cd7790bd34814ef2a53283d 100644
--- a/src/Mathematics/Bsplines.cpp
+++ b/src/Mathematics/Bsplines.cpp
@@ -97,7 +97,6 @@ void Bsplines::GenerateKnotVector(std::string method)
cout << "Knot vector cant be created. m_control_points.size()-1>=m_degree "<< endl;
m_knot_vector.clear();
}
-
}
else if (method =="universal")
@@ -296,11 +295,11 @@ void Bsplines::PrintDegree() const
}
-// Class ZBplines heritage of class Bsplines
-// create a foot trajectory of Z on function the time t
+/// Class ZBplines heritage of class Bsplines
+/// create a foot trajectory of Z on function the time t
-ZBsplines::ZBsplines(double FT, double FP, double ToMP, double MP):Bsplines(4)
+ZBsplines::ZBsplines( double FT, double FP, double ToMP, double MP):Bsplines(4)
{
SetParameters(FT, FP, ToMP, MP);
}
@@ -351,16 +350,25 @@ double ZBsplines::ZComputeAcc(double t)
void ZBsplines::SetParameters(double FT, double FP, double ToMP, double MP)
{
ZGenerateKnotVector(FT,ToMP);
- ZGenerateControlPoints(FT, FP, ToMP, MP);
+ ZGenerateControlPoints(0.0,FT, FP, ToMP, MP);
+}
+
+void ZBsplines::SetParametersWithInitPosInitSpeed(double FT, double FP, double ToMP, double MP,
+ double InitPos,
+ double InitSpeed)
+{
+ ZGenerateKnotVector(FT,ToMP);
+ ZGenerateControlPoints(InitPos,FT, FP, ToMP, MP);
}
+
void ZBsplines::GetParametersWithInitPosInitSpeed(double &FT,
- double &FP,
+ double &MP,
double &InitPos,
double &InitSpeed)
{
FT = m_FT;
- FP = m_FP;
+ MP = m_MP;
InitPos = ZComputePosition(0.0);
InitSpeed = ZComputeVelocity(0.0);
}
@@ -382,50 +390,50 @@ void ZBsplines::ZGenerateKnotVector(double FT, double ToMP)
{
knot.push_back(FT);
}
-
SetKnotVector(knot);
}
-void ZBsplines::ZGenerateControlPoints(double FT, double FP, double ToMP, double MP)
+void ZBsplines::ZGenerateControlPoints(double IP,double FT, double FP, double ToMP, double MP)
{
m_FT = FT;
m_FP = FP;
m_ToMP = ToMP;
m_MP = MP;
+
std::vector<Point> control_points;
control_points.clear();
std::ofstream myfile1;
myfile1.open("control_point.txt");
- Point A = {0.0,0.0};
+ Point A = {0.0,IP};
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
- A = {m_FT*0.05,0.0};
+ A = {m_FT*0.05,IP};
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
- A = {m_FT*0.1,0.0};
+ A = {m_FT*0.1,IP};
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
- A = {0.85*m_ToMP,m_MP};
+ A = {0.85*m_ToMP,IP+ m_MP};
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
- A = {1.15*m_ToMP,m_MP};
+ A = {1.15*m_ToMP,IP+m_MP};
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
- A = {0.85*m_FT,m_FP};
+ A = {0.85*m_FT,IP+m_FP};
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
- A = {0.9*m_FT,m_FP};
+ A = {0.9*m_FT,IP+m_FP};
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
- A = {m_FT,m_FP};
+ A = {m_FT,IP+m_FP};
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
@@ -433,5 +441,3 @@ void ZBsplines::ZGenerateControlPoints(double FT, double FP, double ToMP, double
SetControlPoints(control_points);
}
-
-
diff --git a/src/Mathematics/Bsplines.hh b/src/Mathematics/Bsplines.hh
index d8bfca622e3327eece52640011ca49661e058290..d07ab8d1adfd59ad29b129cc3538b828701eb67f 100644
--- a/src/Mathematics/Bsplines.hh
+++ b/src/Mathematics/Bsplines.hh
@@ -109,11 +109,15 @@ namespace PatternGeneratorJRL
*/
void SetParameters(double FT, double FP, double ToMP, double MP);
+ void SetParametersWithInitPosInitSpeed(double FT, double FP, double ToMP, double MP,
+ double InitPos,
+ double InitSpeed);
+
void GetParametersWithInitPosInitSpeed(double &FT,
- double &FP,
+ double &MP,
double &InitPos,
double &InitSpeed);
-
+
/*! Create a vector of Control Points with 8 Points :
{0.0,0.0},
{m_FT*0.05,0.0},
@@ -123,7 +127,7 @@ namespace PatternGeneratorJRL
{0.85*m_FT,m_FP},
{0.9*m_FT,m_FP},
{m_FT,m_FP}*/
- void ZGenerateControlPoints(double FT, double FP, double ToMP, double MP);
+ void ZGenerateControlPoints(double IP,double FT, double FP, double ToMP, double MP);
void ZGenerateKnotVector(double FT, double ToMP);
diff --git a/src/PatternGeneratorInterfacePrivate.cpp b/src/PatternGeneratorInterfacePrivate.cpp
index 8d1d695ab3b0170791f60150545d2284d2fffc59..9cb9d856dae6a0bb498c1e68129192e8f5c856ad 100644
--- a/src/PatternGeneratorInterfacePrivate.cpp
+++ b/src/PatternGeneratorInterfacePrivate.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
+ * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
*
* Andrei Herdt
* Fumio Kanehiro
@@ -9,7 +9,7 @@
* Alireza Nakhaei
* Mathieu Poirier
* Olivier Stasse
- * Eiichi Yoshida
+ * Eiichi Yoshida
*
* JRL, CNRS/AIST
*
@@ -26,7 +26,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)
*/
/* \doc This object is the interface to the walking gait
@@ -183,11 +183,12 @@ namespace PatternGeneratorJRL {
void PatternGeneratorInterfacePrivate::RegisterPluginMethods()
{
- std::string aMethodName[15] =
+ std::string aMethodName[16] =
{":LimitsFeasibility",
":ZMPShiftParameters",
":TimeDistributionParameters",
":stepseq",
+ ":stepstairseq"
":finish",
":StartOnLineStepSequencing",
":StopOnLineStepSequencing",
@@ -200,7 +201,7 @@ namespace PatternGeneratorJRL {
":setVelReference",
":setCoMPerturbationForce"};
- for(int i=0;i<15;i++)
+ for(int i=0;i<16;i++)
{
if (!SimplePlugin::RegisterMethod(aMethodName[i]))
{
@@ -461,13 +462,16 @@ namespace PatternGeneratorJRL {
void PatternGeneratorInterfacePrivate::ReadSequenceOfSteps(istringstream &strm)
{
// Read the data inside strm.
-
-
switch (m_StepStackHandler->GetWalkMode())
{
case 0:
case 4:
case 5:
+ case 6:
+ {
+ m_StepStackHandler->ReadStepSequenceAccordingToWalkMode(strm);
+ break;
+ }
case 3:
case 1:
{
@@ -506,7 +510,7 @@ namespace PatternGeneratorJRL {
// Read the data inside strm.
m_ZMPVRQP->Reference(strm);
}
-
+
void PatternGeneratorInterfacePrivate::setCoMPerturbationForce(istringstream &strm)
{
// Read the data inside strm.
@@ -570,6 +574,17 @@ namespace PatternGeneratorJRL {
ODEBUG("After finish and realize Step Sequence");
}
+ void PatternGeneratorInterfacePrivate::m_StepStairSequence(istringstream &strm)
+ {
+
+ ODEBUG("Step Stair Sequence");
+ ofstream DebugFile;
+ ReadSequenceOfSteps(strm);
+ ODEBUG("After reading Step Sequence");
+ FinishAndRealizeStepSequence();
+ ODEBUG("After finish and realize Step Sequence");
+ }
+
void PatternGeneratorInterfacePrivate::EvaluateStartingCOM(MAL_VECTOR( & Configuration,double),
MAL_S3_VECTOR( & lStartingCOMState,double))
{
@@ -717,6 +732,7 @@ namespace PatternGeneratorJRL {
{
ODEBUG(lRelativeFootPositions[i].sx << " " <<
lRelativeFootPositions[i].sy << " " <<
+ lRelativeFootPositions[i].sz << " " <<
lRelativeFootPositions[i].theta );
}
@@ -743,6 +759,7 @@ namespace PatternGeneratorJRL {
ODEBUG("Push a position in stack of steps:"<<
lRelativeFootPositions[0].sx << " " <<
lRelativeFootPositions[0].sy << " " <<
+ lRelativeFootPositions[0].sz << " " <<
lRelativeFootPositions[0].theta);
}
}
@@ -1047,6 +1064,7 @@ namespace PatternGeneratorJRL {
double ltime = (double)m_ZMPM->GetTSingleSupport();
strm >> aFAP.x;
strm >> aFAP.y;
+ //strm >> aFAP.z;
strm >> aFAP.theta;
ChangeOnLineStep(ltime,aFAP,newtime);
}
@@ -1082,6 +1100,9 @@ namespace PatternGeneratorJRL {
else if (aCmd==":stepseq")
m_StepSequence(strm);
+ else if (aCmd==":stepstairseq")
+ m_StepStairSequence(strm);
+
else if (aCmd==":finish")
m_FinishAndRealizeStepSequence(strm);
@@ -1325,8 +1346,8 @@ namespace PatternGeneratorJRL {
m_RightFootPositions);
m_Running = m_ZMPVRQP->Running();
}
-
-
+
+
m_GlobalStrategyManager->OneGlobalStepOfControl(LeftFootPosition,
RightFootPosition,
ZMPTarget,
@@ -1335,7 +1356,7 @@ namespace PatternGeneratorJRL {
CurrentVelocity,
CurrentAcceleration);
- ODEBUG("finalCOMState: " <<
+ ODEBUG("finalCOMState: " <<
finalCOMState.x[0] << " " <<
finalCOMState.x[1] << " " <<
finalCOMState.x[2] << " " <<
@@ -1714,7 +1735,7 @@ namespace PatternGeneratorJRL {
lWaistAbsPos = m_WaistAbsPos;
}
- //TODO test me
+ //TODO test me
void PatternGeneratorInterfacePrivate::getWaistPositionAndOrientation(double aTQ[7], double &Orientation) const
{
// Position
@@ -1778,7 +1799,7 @@ namespace PatternGeneratorJRL {
dy = m_AbsLinearVelocity(1);
omega = m_AbsAngularVelocity(2);
}
-
+
void PatternGeneratorInterfacePrivate::setVelocityReference(double x,
double y,
double yaw)
diff --git a/src/PreviewControl/rigid-body-system.cpp b/src/PreviewControl/rigid-body-system.cpp
index fafe04dfb1315c53589e4b7f66cd80f98b651e5a..58b4852b655d134fa4d68be7a149af043583b198 100644
--- a/src/PreviewControl/rigid-body-system.cpp
+++ b/src/PreviewControl/rigid-body-system.cpp
@@ -63,7 +63,7 @@ RigidBodySystem::initialize( )
OFTG_->QPSamplingPeriod( T_ );
OFTG_->NbSamplingsPreviewed( N_ );
OFTG_->FeetDistance( 0.2 );
- OFTG_->StepHeight( 0.05 );
+ OFTG_->StepHeight( 0.03 );
// Initialize predetermined trajectories:
// --------------------------------------
diff --git a/src/StepStackHandler.cpp b/src/StepStackHandler.cpp
index 8cb052a09a3871ae7d80bf127f7f37883de6e689..3b6630d46ea8ecaf0ccc1222a7bde8eb104b8660 100644
--- a/src/StepStackHandler.cpp
+++ b/src/StepStackHandler.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
+ * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
*
* Francois Keith
* Olivier Stasse
@@ -19,11 +19,11 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/*! This object handle the step stack of the pattern generator.
- It allows also to create automatically stack of steps according to
+ It allows also to create automatically stack of steps according to
some high level functionnalities.
*/
#include <fstream>
@@ -50,7 +50,7 @@ StepStackHandler::StepStackHandler(SimplePluginManager *lSPM) : SimplePlugin(lSP
m_RelativeFootPositions.clear();
m_TransitionFinishOnLine=false;
- std::string aMethodName[8] =
+ std::string aMethodName[8] =
{":walkmode",
":singlesupporttime",
":doublesupporttime",
@@ -131,12 +131,60 @@ void StepStackHandler::ReadStepSequenceAccordingToWalkMode(istringstream &strm)
switch (m_WalkMode)
{
case 0:
+ case 6:
+ {
+
+ ODEBUG( "Climbing Stair" );
+ RelativeFootPosition aFootPosition;
+
+ while(!strm.eof())
+ {
+ if (!strm.eof())
+ strm >> aFootPosition.sx;
+ else
+ break;
+ if (!strm.eof())
+ strm >> aFootPosition.sy;
+ else
+ break;
+ if (!strm.eof())
+ strm >> aFootPosition.sz;
+ else
+ break;
+ if (!strm.eof())
+ strm >> aFootPosition.theta;
+ else
+ break;
+
+ aFootPosition.DeviationHipHeight = 0;
+ aFootPosition.SStime=m_SingleSupportTime;
+ aFootPosition.DStime=m_DoubleSupportTime;
+ aFootPosition.stepType=6;
+ ODEBUG4(aFootPosition.sx << " " <<
+ aFootPosition.sy << " " <<
+ aFootPosition.sz << " " <<
+ aFootPosition.theta << " " <<
+ aFootPosition.SStime << " " <<
+ aFootPosition.DStime << " " <<
+ aFootPosition.DeviationHipHeight << " " ,
+ "DebugGMFKW.dat");
+
+ m_RelativeFootPositions.push_back(aFootPosition);
+ if (aFootPosition.sy>0)
+ m_KeepLastCorrectSupportFoot=-1;
+ else
+ m_KeepLastCorrectSupportFoot=1;
+ }
+
+ ODEBUG("m_RelativeFootPositions: " << m_RelativeFootPositions.size());
+ break;
+ }
case 4:
- {
-
+ {
+
ODEBUG( "Standard Walk Mode Selected" );
RelativeFootPosition aFootPosition;
-
+
while(!strm.eof())
{
if (!strm.eof())
@@ -144,25 +192,25 @@ void StepStackHandler::ReadStepSequenceAccordingToWalkMode(istringstream &strm)
else break;
if (!strm.eof())
strm >> aFootPosition.sy;
- else
+ else
break;
if (!strm.eof())
strm >> aFootPosition.theta;
- else
+ else
break;
-
+
aFootPosition.DeviationHipHeight = 0;
aFootPosition.SStime=m_SingleSupportTime;
aFootPosition.DStime=m_DoubleSupportTime;
aFootPosition.stepType=1;
ODEBUG4(aFootPosition.sx << " " <<
aFootPosition.sy << " " <<
- aFootPosition.theta << " " <<
- aFootPosition.SStime << " " <<
- aFootPosition.DStime << " " <<
+ aFootPosition.theta << " " <<
+ aFootPosition.SStime << " " <<
+ aFootPosition.DStime << " " <<
aFootPosition.DeviationHipHeight << " " ,
"DebugGMFKW.dat");
-
+
m_RelativeFootPositions.push_back(aFootPosition);
if (aFootPosition.sy>0)
m_KeepLastCorrectSupportFoot=-1;
@@ -175,10 +223,10 @@ void StepStackHandler::ReadStepSequenceAccordingToWalkMode(istringstream &strm)
}
case 3:
case 1:
- {
+ {
ODEBUG4( "Walk Mode with HipHeight Variation Selected","DebugGMFKW.dat" );
RelativeFootPosition aFootPosition;
-
+
ODEBUG4("Inside StepStack Handler","DebugGMFKW.dat");
while(!strm.eof())
{
@@ -199,9 +247,9 @@ void StepStackHandler::ReadStepSequenceAccordingToWalkMode(istringstream &strm)
aFootPosition.stepType=1;
ODEBUG4(aFootPosition.sx << " " <<
aFootPosition.sy << " " <<
- aFootPosition.theta << " " <<
- aFootPosition.SStime << " " <<
- aFootPosition.DStime << " " <<
+ aFootPosition.theta << " " <<
+ aFootPosition.SStime << " " <<
+ aFootPosition.DStime << " " <<
aFootPosition.DeviationHipHeight << " " ,
"DebugFootPrint.dat");
m_RelativeFootPositions.push_back(aFootPosition);
@@ -209,29 +257,29 @@ void StepStackHandler::ReadStepSequenceAccordingToWalkMode(istringstream &strm)
m_KeepLastCorrectSupportFoot=-1;
else
m_KeepLastCorrectSupportFoot=1;
-
+
}
ODEBUG4("Finito for the reading. StepStack Handler","DebugGMFKW.dat");
break;
-
+
}
case 2:
- {
+ {
ODEBUG( "Walk Mode with Obstacle StepOver Selected \
(obstacle parameters have to be set first, \
if not standard dimensions are used)" );
//cout << "I am calculating relative positions to negociate obstacle" << endl;
m_StOvPl->CalculateFootHolds(m_RelativeFootPositions);
-
+
break;
}
// With a varying double support time and a single support time.
case 5:
- {
-
+ {
+
ODEBUG3( "Standard Walk Mode Selected" );
RelativeFootPosition aFootPosition;
-
+
while(!strm.eof())
{
if (!strm.eof())
@@ -239,40 +287,40 @@ void StepStackHandler::ReadStepSequenceAccordingToWalkMode(istringstream &strm)
else break;
if (!strm.eof())
strm >> aFootPosition.sy;
- else
+ else
break;
if (!strm.eof())
strm >> aFootPosition.theta;
- else
+ else
break;
double lSST=0.0, lDST=0.0;
if (!strm.eof())
strm >> aFootPosition.SStime;
- else
+ else
break;
if (!strm.eof())
strm >> aFootPosition.DStime;
- else
+ else
break;
-
+
aFootPosition.DeviationHipHeight = 0;
aFootPosition.stepType=1;
ODEBUG3("FootPositions:" << aFootPosition.sx << " " <<
aFootPosition.sy << " " <<
- aFootPosition.theta << " " <<
- aFootPosition.SStime << " " <<
- aFootPosition.DStime << " " <<
+ aFootPosition.theta << " " <<
+ aFootPosition.SStime << " " <<
+ aFootPosition.DStime << " " <<
aFootPosition.DeviationHipHeight << " " );
-
+
ODEBUG4(aFootPosition.sx << " " <<
aFootPosition.sy << " " <<
- aFootPosition.theta << " " <<
- aFootPosition.SStime << " " <<
- aFootPosition.DStime << " " <<
+ aFootPosition.theta << " " <<
+ aFootPosition.SStime << " " <<
+ aFootPosition.DStime << " " <<
aFootPosition.DeviationHipHeight << " " ,
"DebugGMFKW.dat");
-
+
m_RelativeFootPositions.push_back(aFootPosition);
if (aFootPosition.sy>0)
m_KeepLastCorrectSupportFoot=-1;
@@ -284,7 +332,7 @@ void StepStackHandler::ReadStepSequenceAccordingToWalkMode(istringstream &strm)
break;
}
- default:
+ default:
{
ODEBUG( "PLease select proper walk mode. \
(0 for normal walking ; \
@@ -320,7 +368,7 @@ void StepStackHandler::CreateArcInStepStack( double x,double y, double R,
// Computes the last step value.
LastStep = OmegaTotal*R - NumberOfStep * StepMax;
-
+
// OmegaStep = arc_deg/(double)NumberOfStep;
OmegaStep = StepMax/R;
LastOmegaStep = OmegaTotal - OmegaStep * NumberOfStep;
@@ -344,7 +392,7 @@ void StepStackHandler::CreateArcInStepStack( double x,double y, double R,
ODEBUG4(NumberOfStep << " "
<< LastStep<< " "
<< arc_deg, "DebugFootPrint.dat");
-
+
double Omegakp = 0.0,
Omegak=0.0;
@@ -361,7 +409,7 @@ void StepStackHandler::CreateArcInStepStack( double x,double y, double R,
MAL_MATRIX_SET_IDENTITY(A);
MAL_MATRIX_DIM(Ap,double,2,2);
MAL_MATRIX_SET_IDENTITY(Ap);
-
+
Omegakp = Omegak;
Omegak = Omegak + OmegaStep;
ODEBUG("Omegak:" << Omegak );
@@ -369,7 +417,7 @@ void StepStackHandler::CreateArcInStepStack( double x,double y, double R,
c = cos(Omegak*M_PI/180.0);
s = sin(Omegak*M_PI/180.0);
- // Transpose of the orientation matrix
+ // Transpose of the orientation matrix
// to get the inverse of the orientation matrix.
A(0,0) = c; A(0,1) = s;
A(1,0) = -s; A(1,1) = c;
@@ -377,23 +425,23 @@ void StepStackHandler::CreateArcInStepStack( double x,double y, double R,
double cp,sp;
cp = cos(Omegakp*M_PI/180.0);
sp = sin(Omegakp*M_PI/180.0);
-
+
MAL_VECTOR_DIM(lv,double,2);
MAL_VECTOR_DIM(lv2,double,2);
lv(0) = (R+DirectionRay*SupportFoot*0.095)*s - (R-DirectionRay*SupportFoot*0.095)*sp;
lv(1) = -((R+DirectionRay*SupportFoot*0.095)*c - (R-DirectionRay*SupportFoot*0.095)*cp);
MAL_C_eq_A_by_B(lv2,A,lv);
- ODEBUG(" X: " << (R+DirectionRay*SupportFoot*0.095)*s << " " << (R-DirectionRay*SupportFoot*0.095)*sp
+ ODEBUG(" X: " << (R+DirectionRay*SupportFoot*0.095)*s << " " << (R-DirectionRay*SupportFoot*0.095)*sp
<< " " << StepMax << " " << lv(0) << " " << lv2(0) );
- ODEBUG(" Y: " << (R+DirectionRay*SupportFoot*0.095)*c << " " << (R-DirectionRay*SupportFoot*0.095)*cp
+ ODEBUG(" Y: " << (R+DirectionRay*SupportFoot*0.095)*c << " " << (R-DirectionRay*SupportFoot*0.095)*cp
<< " " << SupportFoot*0.19 << " " << lv(1) << " " << lv2(1));
-
+
aFootPosition.sx = lv2(0);
aFootPosition.sy = lv2(1);
-
+
}
-
+
aFootPosition.SStime = m_SingleSupportTime;
aFootPosition.DStime = m_DoubleSupportTime;
m_RelativeFootPositions.push_back(aFootPosition);
@@ -413,18 +461,18 @@ void StepStackHandler::CreateArcInStepStack( double x,double y, double R,
{
MAL_MATRIX_DIM(A,double,2,2);
MAL_MATRIX_SET_IDENTITY(A);
-
+
Omegakp = Omegak;
Omegak = Omegak + LastOmegaStep;
ODEBUG( "Omegak:" << Omegak );
double c,s;
c = cos(Omegak*M_PI/180.0);
s = sin(Omegak*M_PI/180.0);
-
+
double cp,sp;
cp = cos(Omegakp*M_PI/180.0);
sp = sin(Omegakp*M_PI/180.0);
-
+
A(0,0) = c; A(0,1) =s;
A(1,0) = -s; A(1,1) = c;
MAL_VECTOR_DIM(lv,double,2);
@@ -432,19 +480,19 @@ void StepStackHandler::CreateArcInStepStack( double x,double y, double R,
lv(0) = (R+DirectionRay*SupportFoot*0.095)*s - (R-DirectionRay*SupportFoot*0.095)*sp;
lv(1) = -((R+DirectionRay*SupportFoot*0.095)*c - (R-DirectionRay*SupportFoot*0.095)*cp);
MAL_C_eq_A_by_B(lv2,A,lv);
- ODEBUG(" X: " << (R+DirectionRay*SupportFoot*0.095)*s << " " << (R-DirectionRay*SupportFoot*0.095)*sp
+ ODEBUG(" X: " << (R+DirectionRay*SupportFoot*0.095)*s << " " << (R-DirectionRay*SupportFoot*0.095)*sp
<< " " << lv(0) << " " << lv2(0) );
- ODEBUG(" Y: " << (R+DirectionRay*SupportFoot*0.095)*c << " " << (R-DirectionRay*SupportFoot*0.095)*cp
+ ODEBUG(" Y: " << (R+DirectionRay*SupportFoot*0.095)*c << " " << (R-DirectionRay*SupportFoot*0.095)*cp
<< " " << lv(1) << " " << lv2(1) );
-
+
aFootPosition.sx = lv2(0);
aFootPosition.sy = lv2(1);
-
+
}
aFootPosition.SStime = m_SingleSupportTime;
aFootPosition.DStime = m_DoubleSupportTime;
-
+
m_RelativeFootPositions.push_back(aFootPosition);
ODEBUG4(aFootPosition.sx<< " "
@@ -479,15 +527,15 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
// Computes the last step value.
LastStep = OmegaTotal*R - NumberOfStep * StepMax;
-
+
OmegaStep = StepMax/R;
LastOmegaStep = OmegaTotal - OmegaStep*NumberOfStep;
-
+
#if 0
ofstream DebugFile;
DebugFile.open("/tmp/output.txt",ofstream::out);
DebugFile << NumberOfStep << " "
- << OmegaStep << " "
+ << OmegaStep << " "
<< LastOmegaStep<< " "
<< arc_deg<< " "
<< endl;
@@ -496,8 +544,8 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
cosOmegaStep = cos(OmegaStep);
sinOmegaStep = sin(OmegaStep);
-
- // Make sure that Support Foot is the foot which
+
+ // Make sure that Support Foot is the foot which
// does not lead the motion.
if (SupportFoot*OmegaStep<0.0)
{
@@ -506,17 +554,17 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
aFootPosition.theta = 0;
aFootPosition.SStime = m_SingleSupportTime;
aFootPosition.DStime = m_DoubleSupportTime;
-
+
m_RelativeFootPositions.push_back(aFootPosition);
#if 0
DebugFile.open("/tmp/output.txt",ofstream::app);
DebugFile << aFootPosition.sx<< " "
- << aFootPosition.sy<< " "
+ << aFootPosition.sy<< " "
<< aFootPosition.theta<< " "
<< endl;
DebugFile.close();
#endif
-
+
SupportFoot=-SupportFoot;
}
@@ -546,10 +594,10 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
for(int j=0;j<3;j++)
if (i==j)
{
- MFNSF(i,j) =
- MFSF(i,j) =
- Romega(i,j) =
- Mtmp(i,j) =
+ MFNSF(i,j) =
+ MFSF(i,j) =
+ Romega(i,j) =
+ Mtmp(i,j) =
iRomega(i,j)= 1.0;
}
else
@@ -561,17 +609,17 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
Mtmp2(i,j) =
iRomega(i,j) = 0.0;
}
-
+
MFSF(0,2)=-R;
MFSF(1,2)=-S;
MFNSF(0,2)=-R;
MFNSF(1,2)=S;
- MSupportFoot=MFSF;
+ MSupportFoot=MFSF;
Mtmp(1,2) = 0.19;
#if 0
DebugFile.open("/tmp/outputNL.txt",ofstream::app);
- DebugFile << MSupportFoot(0,2) << " "
+ DebugFile << MSupportFoot(0,2) << " "
<< MSupportFoot(1,2) << endl;
DebugFile.close();
#endif
@@ -583,22 +631,22 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
cosiOmegaStep = cos((i+1)*OmegaStep);
siniOmegaStep = sin((i+1)*OmegaStep);
-
+
Romega(0,0) = cosiOmegaStep;
Romega(0,1) = -siniOmegaStep;
Romega(1,0) = siniOmegaStep;
Romega(1,1) = cosiOmegaStep;
Romega(0,2) = 0;
Romega(1,2) = 0;
-
+
MAL_MATRIX(lTmp,double);
MAL_C_eq_A_by_B(lTmp,MSupportFoot,Romegastep);
MAL_INVERSE(lTmp, RiR,double);
-
+
ODEBUG(" Iteration " << i);
ODEBUG(" Romega " << Romega);
ODEBUG(" RiR " << RiR);
-
+
MAL_C_eq_A_by_B(FPos, Romega, MFNSF);
ODEBUG("FPos: " << FPos);
MAL_C_eq_A_by_B(FPos,RiR,FPos);
@@ -612,7 +660,7 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
m_RelativeFootPositions.push_back(aFootPosition);
MAL_C_eq_A_by_B(MSupportFoot, Romega,MFNSF);
-
+
#if 0
DebugFile.open("/tmp/outputL.txt",ofstream::app);
DebugFile << MSupportFoot(0,2) << " "
@@ -646,14 +694,14 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
for(int li=0;li<2;li++)
for(int lj=0;lj<2;lj++)
Mtmp2[li][lj]=MSupportFoot[li][lj];
-
+
Mtmp2 = Mtmp2*Mtmp;
*/
MSupportFoot = MAL_RET_A_by_B( MSupportFoot, Mtmp);
-
+
#if 0
DebugFile.open("/tmp/outputNL.txt",ofstream::app);
- DebugFile << MSupportFoot(0,2) << " "
+ DebugFile << MSupportFoot(0,2) << " "
<< MSupportFoot(1,2) << endl;
DebugFile.close();
#endif
@@ -663,7 +711,7 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
if (LastStep!=0.0)
{
double cosiOmegaStep,siniOmegaStep;
-
+
cosiOmegaStep = cos(LastOmegaStep+NumberOfStep*OmegaStep);
siniOmegaStep = sin(LastOmegaStep+NumberOfStep*OmegaStep);
for(int i=0;i<3;i++)
@@ -672,14 +720,14 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
Romega(i,j) = iRomega(i,j) = 1.0;
else
Romega(i,j) = iRomega(i,j) = 0.0;
-
+
Romega(0,0) = cosiOmegaStep;
Romega(0,1) = -siniOmegaStep;
Romega(1,0) = siniOmegaStep;
Romega(1,1) = cosiOmegaStep;
Romega(0,2) = 0;
Romega(1,2) = 0;
-
+
double coslOmegaStep,sinlOmegaStep;
coslOmegaStep = cos(LastOmegaStep);
sinlOmegaStep = sin(LastOmegaStep);
@@ -689,7 +737,7 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
iRomega(0,1) = -sinlOmegaStep;
iRomega(1,0) = sinlOmegaStep;
iRomega(1,1) = coslOmegaStep;
-
+
MAL_MATRIX(lTmp,double);
MAL_C_eq_A_by_B(lTmp,MSupportFoot,iRomega);
MAL_INVERSE(lTmp, RiR,double);
@@ -737,10 +785,10 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
DebugFile.close();
#endif
MSupportFoot = MAL_RET_A_by_B( MSupportFoot , Mtmp);
-
+
#if 0
DebugFile.open("/tmp/outputNL.txt",ofstream::app);
- DebugFile << MSupportFoot(0,2) << " "
+ DebugFile << MSupportFoot(0,2) << " "
<< MSupportFoot(1,2) << endl;
DebugFile.close();
#endif
@@ -768,7 +816,7 @@ void StepStackHandler::StopOnLineStep()
// m_OnLineSteps = false;
m_TransitionFinishOnLine=true;
- // Correct the last support foot before cleaning up the
+ // Correct the last support foot before cleaning up the
// stack.
if (m_RelativeFootPositions.size()%2==0)
m_KeepLastCorrectSupportFoot = -m_KeepLastCorrectSupportFoot;
@@ -788,7 +836,7 @@ bool StepStackHandler::IsOnLineSteppingOn()
return m_OnLineSteps;
}
-void StepStackHandler::AddStandardOnLineStep(bool NewStep,
+void StepStackHandler::AddStandardOnLineStep(bool NewStep,
double NewStepX,
double NewStepY,
double NewTheta)
@@ -797,7 +845,7 @@ void StepStackHandler::AddStandardOnLineStep(bool NewStep,
ODEBUG("m_OnLineSteps: "<<m_OnLineSteps);
if (!m_OnLineSteps)
return;
-
+
ODEBUG("m_KeepLastCorrectSupportFoot" << m_KeepLastCorrectSupportFoot);
if (!NewStep)
{
@@ -807,7 +855,7 @@ void StepStackHandler::AddStandardOnLineStep(bool NewStep,
aFootPosition.SStime = m_SingleSupportTime;
aFootPosition.DStime = m_DoubleSupportTime;
aFootPosition.stepType = 0;
-
+
}
else
{
@@ -818,17 +866,17 @@ void StepStackHandler::AddStandardOnLineStep(bool NewStep,
aFootPosition.SStime = m_SingleSupportTime;
aFootPosition.DStime = m_DoubleSupportTime;
aFootPosition.stepType = 0;
- cout << aFootPosition.sx << " "
- << aFootPosition.sy << " "
+ cout << aFootPosition.sx << " "
+ << aFootPosition.sy << " "
<< aFootPosition.theta << endl;
- }
+ }
ODEBUG("m_RelativeFootPositions:" << m_RelativeFootPositions.size());
m_RelativeFootPositions.push_back(aFootPosition);
ODEBUG("m_RelativeFootPositions:" << m_RelativeFootPositions.size());
-
+
m_KeepLastCorrectSupportFoot= - m_KeepLastCorrectSupportFoot;
-
+
}
@@ -857,7 +905,23 @@ void StepStackHandler::AddStepInTheStack(double sx, double sy,
aFootPosition.theta = theta;
aFootPosition.SStime = sstime;
aFootPosition.DStime = dstime;
- aFootPosition.stepType = 0;
+ aFootPosition.stepType = 0;
+
+ m_RelativeFootPositions.push_back(aFootPosition);
+}
+
+void StepStackHandler::AddStepStairInTheStack(double sx, double sy, double sz,
+ double theta, double sstime,
+ double dstime)
+{
+ RelativeFootPosition aFootPosition;
+ aFootPosition.sx = sx;
+ aFootPosition.sy = sy;
+ aFootPosition.sz = sz;
+ aFootPosition.theta = theta;
+ aFootPosition.SStime = sstime;
+ aFootPosition.DStime = dstime;
+ aFootPosition.stepType = 6;
m_RelativeFootPositions.push_back(aFootPosition);
}
@@ -867,7 +931,7 @@ void StepStackHandler::PushFrontAStepInTheStack(RelativeFootPosition &aRFP)
m_RelativeFootPositions.push_front(aRFP);
}
-// Make sure that the previous motion will finish
+// Make sure that the previous motion will finish
// on the last specified correct support foot.
void StepStackHandler::FinishOnTheLastCorrectSupportFoot()
{
@@ -907,7 +971,7 @@ double StepStackHandler::GetDoubleTimeSupport()
void StepStackHandler::m_PartialStepSequence(istringstream &strm)
{
RelativeFootPosition aFootPosition;
-
+
while(!strm.eof())
{
@@ -916,11 +980,11 @@ void StepStackHandler::m_PartialStepSequence(istringstream &strm)
else break;
if (!strm.eof())
strm >> aFootPosition.sy;
- else
+ else
break;
if (!strm.eof())
strm >> aFootPosition.theta;
- else
+ else
break;
aFootPosition.DStime = m_DoubleSupportTime;
@@ -930,6 +994,37 @@ void StepStackHandler::m_PartialStepSequence(istringstream &strm)
}
}
+void StepStackHandler::m_PartialStepStairSequence(istringstream &strm)
+{
+ RelativeFootPosition aFootPosition;
+
+
+ while(!strm.eof())
+ {
+ if (!strm.eof())
+ strm >> aFootPosition.sx;
+ else
+ break;
+ if (!strm.eof())
+ strm >> aFootPosition.sy;
+ else
+ break;
+ if (!strm.eof())
+ strm >> aFootPosition.sz;
+ else
+ break;
+ if (!strm.eof())
+ strm >> aFootPosition.theta;
+ else
+ break;
+ aFootPosition.DStime = m_DoubleSupportTime;
+ aFootPosition.SStime = m_SingleSupportTime;
+ aFootPosition.stepType = 6;
+ m_RelativeFootPositions.push_back(aFootPosition);
+ }
+}
+
+
/* Implementation of the plugin methods. */
void StepStackHandler::CallMethod(std::string &Method, std::istringstream &strm)
{
@@ -961,36 +1056,35 @@ void StepStackHandler::CallMethod(std::string &Method, std::istringstream &strm)
while(!strm.eof())
{
-
if (!strm.eof())
strm >> x;
else break;
-
+
if (!strm.eof())
strm >> y;
else break;
-
+
if (!strm.eof())
strm >> theta;
else break;
-
+
}
AddStandardOnLineStep(true,x,y,theta);
-
+
}
else if (Method==":arc")
{
double x,y,R=0.0,arc_deg;
int SupportFoot=-1;
-
-
+
+
while(!strm.eof())
{
-
+
if (!strm.eof())
strm >> x;
else break;
-
+
if (!strm.eof())
strm >> y;
else break;
@@ -998,14 +1092,14 @@ void StepStackHandler::CallMethod(std::string &Method, std::istringstream &strm)
if (!strm.eof())
strm >> arc_deg;
else break;
-
+
if (!strm.eof())
strm >> SupportFoot;
else break;
}
-
-
+
+
CreateArcInStepStack(x,y,R,arc_deg,SupportFoot);
}
else if (Method==":arccentered")
@@ -1013,10 +1107,10 @@ void StepStackHandler::CallMethod(std::string &Method, std::istringstream &strm)
double R,arc_deg;
int SupportFoot=-1;
ODEBUG4("m_CreateArcCenteredInStepStack 1", "DebugData.txt");
-
+
while(!strm.eof())
{
-
+
if (!strm.eof())
strm >> R;
else break;
@@ -1024,7 +1118,7 @@ void StepStackHandler::CallMethod(std::string &Method, std::istringstream &strm)
if (!strm.eof())
strm >> arc_deg;
else break;
-
+
if (!strm.eof())
strm >> SupportFoot;
else break;
diff --git a/src/StepStackHandler.hh b/src/StepStackHandler.hh
index e9b37bbb6ae31cc283c351455b6dcefac521a8e9..8bdc86c354032df9a8b087c1c6c556a0bd284d05 100644
--- a/src/StepStackHandler.hh
+++ b/src/StepStackHandler.hh
@@ -1,5 +1,5 @@
/*
- * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
+ * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
*
* Francois Keith
* Olivier Stasse
@@ -19,12 +19,12 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/* \file StepStackHandler.h
\brief This object handle the step stack of the pattern generator.
- It allows also to create automatically stack of steps according to
+ It allows also to create automatically stack of steps according to
some high level functionnalities.
*/
@@ -46,10 +46,10 @@ namespace PatternGeneratorJRL
/*! @ingroup pgjrl
This class is in charge of handling the stack of footprints.
There is two modes currently:
- - An off-line mode, where the complete stack is send to
+ - An off-line mode, where the complete stack is send to
the ZMP reference trajectory generator object, and created off-line.
- - An on-line mode, where one step at a time is send to
- the ZMP reference trajectory generator object.
+ - An on-line mode, where one step at a time is send to
+ the ZMP reference trajectory generator object.
*/
class StepStackHandler : public SimplePlugin
{
@@ -67,6 +67,7 @@ namespace PatternGeneratorJRL
2: Stepping over.
3: Read a path.
4: Freeze the upper body.
+ 6: Climbing stairs
*/
void SetWalkMode(int lWalkMode);
@@ -77,29 +78,31 @@ namespace PatternGeneratorJRL
/*! \brief Set the link towards an instance of Step Over planner. */
void SetStepOverPlanner(StepOverPlanner *aSOP);
- /*! \brief Take a serie of string as an input and
+ /*! \brief Take a serie of string as an input and
read the steps according to the chosen walkmode. */
void ReadStepSequenceAccordingToWalkMode(std::istringstream &strm);
/*! \brief Real a partial sequence of steps
- without termination and immediate execution. */
+ without termination and immediate execution. */
void m_PartialStepSequence(std::istringstream &strm);
+ void m_PartialStepStairSequence(std::istringstream &strm);
+
/*! \brief Set the single time support. */
void SetSingleTimeSupport(double aSingleSupportTime);
/*! \brief Get the time for single support. */
double GetSingleTimeSupport();
-
+
/*! \brief Set the time for double support. */
void SetDoubleTimeSupport(double aDoubleSupportTime);
-
+
/*! \brief Get the time for double support. */
double GetDoubleTimeSupport();
/*! \brief Prepare the stack to start for a specific support foot. */
void PrepareForSupportFoot(int SupportFoot);
-
+
/*! \brief To force the last generated support foot. */
void FinishOnTheLastCorrectSupportFoot();
@@ -110,18 +113,18 @@ namespace PatternGeneratorJRL
void CopyRelativeFootPosition(std::deque<RelativeFootPosition> & lRelativeFootPositions,
bool PerformClean);
- /*! \name Method related to online stepping.
+ /*! \name Method related to online stepping.
@{
*/
-
+
/*! \brief Start On Line stepping. */
void StartOnLineStep();
-
+
/*! \brief Stop On Line stepping. */
void StopOnLineStep();
/*! \brief Add a standard step on the stack. */
- void AddStandardOnLineStep(bool NewStep,
+ void AddStandardOnLineStep(bool NewStep,
double NewStepX,
double NewStepY,
double Theta);
@@ -131,11 +134,11 @@ namespace PatternGeneratorJRL
bool IsOnLineSteppingOn();
/*! @} */
- /*! \brief Methods to handle the stack.
+ /*! \brief Methods to handle the stack.
@{
*/
- /*! \brief Remove the first step in the stack.
+ /*! \brief Remove the first step in the stack.
@return Returns true if this is the end of the sequence. */
bool RemoveFirstStepInTheStack();
@@ -144,6 +147,10 @@ namespace PatternGeneratorJRL
double theta, double sstime,
double dstime);
+ void AddStepStairInTheStack(double sx, double sy, double sz,
+ double theta, double sstime,
+ double dstime);
+
/*! \brief Push a step in front of the stack. */
void PushFrontAStepInTheStack(RelativeFootPosition &aRFP);
@@ -158,7 +165,7 @@ namespace PatternGeneratorJRL
/*! @} */
- /*! \name High level methods to create stack of steps for large motion.
+ /*! \name High level methods to create stack of steps for large motion.
@{
*/
/*! \brief Create a sequence of step to realize an arc of rayon R,
@@ -167,7 +174,7 @@ namespace PatternGeneratorJRL
the center of the arc.
*/
void CreateArcCenteredInStepStack( double R,
- double arc_deg,
+ double arc_deg,
int SupportFoot);
/*! \brief Create a sequence of steps to realize an arc of rayon R,
for arc_deg degrees, starting with the support foot defined
@@ -196,17 +203,17 @@ namespace PatternGeneratorJRL
int m_WalkMode;
/*! Link to the step over planner. */
- StepOverPlanner *m_StOvPl;
+ StepOverPlanner *m_StOvPl;
/*! Default value for Single time support and double time support. */
double m_SingleSupportTime, m_DoubleSupportTime;
/*! Variable for delta feasibility limit */
- double m_DeltaFeasibilityLimit;
-
+ double m_DeltaFeasibilityLimit;
+
/*! On line step stack handling. */
bool m_OnLineSteps;
-
+
/*! Transition for finishing on line stepping. */
bool m_TransitionFinishOnLine;
};
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPDiscretization.cpp b/src/ZMPRefTrajectoryGeneration/ZMPDiscretization.cpp
index 5fc6198147a2aac6c4d9747d00e35656d3c244db..09a3755d6ce92703029860d44d57186fc7d7b20f 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPDiscretization.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPDiscretization.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2010,
+ * Copyright 2010,
*
* Andrei Herdt
* Francois Keith
@@ -23,7 +23,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/* This object generate all the values for the foot trajectories,
@@ -54,7 +54,7 @@ using namespace::PatternGeneratorJRL;
OnLineState::OnLineState()
-{
+{
m_CurrentState = IDLE_STATE;
}
@@ -76,7 +76,7 @@ OnLineState & OnLineState::operator=(unsigned int NewState)
ZMPDiscretization::ZMPDiscretization(SimplePluginManager *lSPM,
- string DataFile,
+ string DataFile,
CjrlHumanoidDynamicRobot *aHS)
: ZMPRefTrajectoryGeneration(lSPM)
{
@@ -103,7 +103,7 @@ ZMPDiscretization::ZMPDiscretization(SimplePluginManager *lSPM,
m_ZMPShift.resize(4);
for (unsigned int i=0;i<4;i++)
m_ZMPShift[i]=0.0;
-
+
m_ZMPNeutralPosition[0] = 0.0;
m_ZMPNeutralPosition[1] = 0.0;
@@ -116,7 +116,7 @@ ZMPDiscretization::ZMPDiscretization(SimplePluginManager *lSPM,
m_CurrentSupportFootPosition(i,j)= 1.0;
m_PrevCurrentSupportFootPosition = m_CurrentSupportFootPosition;
-
+
// Prepare size of the matrix used in on-line walking
MAL_MATRIX_RESIZE(m_vdiffsupppre,2,1);
for(unsigned int i=0;i<MAL_MATRIX_NB_ROWS(m_vdiffsupppre);i++)
@@ -134,7 +134,7 @@ ZMPDiscretization::~ZMPDiscretization()
{
if (m_FootTrajectoryGenerationStandard!=0)
delete m_FootTrajectoryGenerationStandard;
-
+
if (m_PolynomeZMPTheta!=0)
delete m_PolynomeZMPTheta;
}
@@ -151,7 +151,7 @@ void ZMPDiscretization::GetZMPDiscretization(deque<ZMPPosition> & FinalZMPPositi
FootAbsolutePosition & InitLeftFootAbsolutePosition,
FootAbsolutePosition & InitRightFootAbsolutePosition)
{
-
+
InitOnLine(FinalZMPPositions,
FinalCOMStates,
LeftFootAbsolutePositions,
@@ -181,18 +181,18 @@ void ZMPDiscretization::DumpFootAbsolutePosition(string aFileName,
{
for(unsigned int i=0;i<aFootAbsolutePositions.size();i++)
{
- aof << aFootAbsolutePositions[i].time << " "
- << aFootAbsolutePositions[i].x << " "
- << aFootAbsolutePositions[i].y << " "
- << aFootAbsolutePositions[i].z << " "
- << aFootAbsolutePositions[i].omega << " "
- << aFootAbsolutePositions[i].theta << " "
- << aFootAbsolutePositions[i].stepType << " "
+ aof << aFootAbsolutePositions[i].time << " "
+ << aFootAbsolutePositions[i].x << " "
+ << aFootAbsolutePositions[i].y << " "
+ << aFootAbsolutePositions[i].z << " "
+ << aFootAbsolutePositions[i].omega << " "
+ << aFootAbsolutePositions[i].theta << " "
+ << aFootAbsolutePositions[i].stepType << " "
<< endl;
}
aof.close();
}
-
+
}
void ZMPDiscretization::ResetADataFile(string &DataFile)
@@ -203,10 +203,10 @@ void ZMPDiscretization::ResetADataFile(string &DataFile)
a_iof.open(DataFile.c_str(),std::ifstream::in);
if (a_iof.is_open())
{
-
+
a_iof.close();
}
-
+
}
}
void ZMPDiscretization::DumpDataFiles(string ZMPFileName, string FootFileName,
@@ -219,7 +219,7 @@ void ZMPDiscretization::DumpDataFiles(string ZMPFileName, string FootFileName,
{
for(unsigned int i=0;i<ZMPPositions.size();i++)
{
- aof << ZMPPositions[i].time << " " << ZMPPositions[i].px << " "
+ aof << ZMPPositions[i].time << " " << ZMPPositions[i].px << " "
<< ZMPPositions[i].py << " " << ZMPPositions[i].stepType << " 0.0" << endl;
}
aof.close();
@@ -230,7 +230,7 @@ void ZMPDiscretization::DumpDataFiles(string ZMPFileName, string FootFileName,
{
for(unsigned int i=0;i<SupportFootAbsolutePositions.size();i++)
{
- aof << SupportFootAbsolutePositions[i].x << " " << SupportFootAbsolutePositions[i].y << " "
+ aof << SupportFootAbsolutePositions[i].x << " " << SupportFootAbsolutePositions[i].y << " "
<< SupportFootAbsolutePositions[i].z << " " << SupportFootAbsolutePositions[i].stepType << " 0.0" << endl;
}
aof.close();
@@ -243,7 +243,7 @@ void ZMPDiscretization::InitializeFilter()
double T=0.05; // Arbritrary from Kajita's San Matlab files.
int n=0;
double sum=0,tmp=0;
-
+
assert(m_SamplingPeriod > 0);
n = (int)floor(T/m_SamplingPeriod);
m_ZMPFilterWindow.resize(n+1);
@@ -295,16 +295,16 @@ void ZMPDiscretization::FilterZMPRef(deque<ZMPPosition> &ZMPPositionsX,
ZMPPositionsY[i].theta = ZMPPositionsX[i].theta;
ZMPPositionsY[i].time = ZMPPositionsX[i].time;
ZMPPositionsY[i].stepType = ZMPPositionsX[i].stepType;
-
+
}
-
+
}
void ZMPDiscretization::SetZMPShift(vector<double> &ZMPShift)
{
-
+
for (unsigned int i=0;i<ZMPShift.size();i++)
{
m_ZMPShift[i] = ZMPShift[i];
@@ -316,7 +316,7 @@ void ZMPDiscretization::SetZMPShift(vector<double> &ZMPShift)
/* Start the online part of ZMP discretization. */
/* Initialiazation of the on-line stacks. */
-int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
+int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
deque<COMState> & FinalCoMStates,
deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
deque<FootAbsolutePosition> &RightFootAbsolutePositions,
@@ -337,24 +337,24 @@ int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
for(unsigned int i=0;i<3;i++)
for(unsigned int j=0;j<3;j++)
if (i==j)
- m_CurrentSupportFootPosition(i,j) = 1.0;
+ m_CurrentSupportFootPosition(i,j) = 1.0;
else
- m_CurrentSupportFootPosition(i,j) = 0.0;
+ m_CurrentSupportFootPosition(i,j) = 0.0;
m_PrevCurrentSupportFootPosition = m_CurrentSupportFootPosition;
ODEBUG4("ZMP::InitOnLine - Step 2 ","ZMDInitOnLine.txt");
- // Initialize position of the feet.
+ // Initialize position of the feet.
CurrentLeftFootAbsPos = InitLeftFootAbsolutePosition;
CurrentLeftFootAbsPos.z = 0.0;
CurrentLeftFootAbsPos.time = 0.0;
-
+
ODEBUG4("CurrentLeftFootAbsPos.y: " << CurrentLeftFootAbsPos.y, "ZMDInitOnLine.txt");
CurrentRightFootAbsPos = InitRightFootAbsolutePosition;
CurrentRightFootAbsPos.z = 0.0;
CurrentRightFootAbsPos.time = 0.0;
- // V pre is the difference between
+ // V pre is the difference between
// the current support position and the precedent.
ODEBUG4("ZMP::InitOnLine - Step 2.5 ","ZMDInitOnLine.txt");
@@ -362,8 +362,8 @@ int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
// between the direction of the left foot and the direction of the right foot.
double CurrentAbsZMPTheta=0;
CurrentAbsZMPTheta = (CurrentRightFootAbsPos.theta + CurrentLeftFootAbsPos.theta)/2.0;
- ODEBUG("CurrentZMPTheta at start: " << " "
- << CurrentRightFootAbsPos.theta << " "
+ ODEBUG("CurrentZMPTheta at start: " << " "
+ << CurrentRightFootAbsPos.theta << " "
<< CurrentLeftFootAbsPos.theta);
// Initialize who is support foot.
@@ -374,16 +374,16 @@ int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
m_AngleDiffToSupportFootTheta = CurrentRightFootAbsPos.theta - CurrentLeftFootAbsPos.theta;
m_AngleDiffFromZMPThetaToSupportFootTheta = CurrentRightFootAbsPos.theta - CurrentAbsZMPTheta;
}
- else
+ else
{
m_vdiffsupppre(0,0) = -CurrentRightFootAbsPos.x + CurrentLeftFootAbsPos.x;
m_vdiffsupppre(1,0) = -CurrentRightFootAbsPos.y + CurrentLeftFootAbsPos.y;
m_AngleDiffToSupportFootTheta = CurrentLeftFootAbsPos.theta - CurrentRightFootAbsPos.theta;
- m_AngleDiffFromZMPThetaToSupportFootTheta = CurrentLeftFootAbsPos.theta - CurrentAbsZMPTheta;
+ m_AngleDiffFromZMPThetaToSupportFootTheta = CurrentLeftFootAbsPos.theta - CurrentAbsZMPTheta;
}
ODEBUG4("ZMP::InitOnLine - Step 3 ","ZMDInitOnLine.txt");
- // Initialization of the ZMP position (stable values during the Preview control time window).
+ // Initialization of the ZMP position (stable values during the Preview control time window).
int AddArraySize;
{
assert(m_SamplingPeriod > 0);
@@ -406,26 +406,26 @@ int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
startingZMPREF[0] = 0.0;
startingZMPREF[1] = 0.0;
startingZMPREF[2] = lStartingZMPPosition(2);
-
+
// Make sure that the robot thinks it is at the position it thinks it is.
//double startingZMPREF[3] = { 0.00949035, 0.00142561, lStartingZMPPosition(2)};
double finalZMPREF[2] = {m_ZMPNeutralPosition[0],m_ZMPNeutralPosition[1]};
- ODEBUG("ZMPNeutralPosition: " << m_ZMPNeutralPosition[0] << " "
+ ODEBUG("ZMPNeutralPosition: " << m_ZMPNeutralPosition[0] << " "
<< m_ZMPNeutralPosition[1] << endl <<
"StartingZMPPosition(toto):" << lStartingZMPPosition(0) << " " << lStartingZMPPosition(1) << " " <<
lStartingZMPPosition(2) << endl
- << "lStartingCOMState: " << lStartingCOMState.x[0] << " "
+ << "lStartingCOMState: " << lStartingCOMState.x[0] << " "
<< lStartingCOMState.y[0] << " "
<< lStartingCOMState.z[0] << endl
<< "CurrentAbsTheta : " << CurrentAbsTheta << endl
<< "AddArraySize:"<< AddArraySize << " " << m_PreviewControlTime << " " <<m_SamplingPeriod << endl
- << "FinalZMPref :( " <<finalZMPREF[0]
+ << "FinalZMPref :( " <<finalZMPREF[0]
<< " , " <<finalZMPREF[1] << " ) " << ZMPPositions.size() <<endl
<< "InitRightFootAbsPos.z " << InitRightFootAbsolutePosition.z);
- ODEBUG( "lStartingCOMState: " << lStartingCOMState.x[0] << " "
+ ODEBUG( "lStartingCOMState: " << lStartingCOMState.x[0] << " "
<< lStartingCOMState.y[0] << " "
<< lStartingCOMState.z[0] );
-
+
ODEBUG4("ZMP::InitOnLine - Step 4 ","ZMDInitOnLine.txt");
for(unsigned int i=0;i<ZMPPositions.size();i++)
{
@@ -437,7 +437,7 @@ int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
// Smooth ramp
ZMPPositions[CurrentZMPindex].px = startingZMPREF[0] + (finalZMPREF[0] - startingZMPREF[0])*coef;
ZMPPositions[CurrentZMPindex].py = startingZMPREF[1] + (finalZMPREF[1] - startingZMPREF[1])*coef;
- ZMPPositions[CurrentZMPindex].pz = (-startingZMPREF[2] + InitRightFootAbsolutePosition.z) * icoef;
+ ZMPPositions[CurrentZMPindex].pz = (-startingZMPREF[2] + InitRightFootAbsolutePosition.z) * icoef;
ZMPPositions[CurrentZMPindex].theta = CurrentAbsTheta;
ZMPPositions[CurrentZMPindex].time = m_CurrentTime;
ZMPPositions[CurrentZMPindex].stepType = 0;
@@ -447,38 +447,38 @@ int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
FinalCoMStates[CurrentZMPindex].z[1] = 0.0;
FinalCoMStates[CurrentZMPindex].z[2] = 0.0;
- FinalCoMStates[CurrentZMPindex].pitch[0] =
- FinalCoMStates[CurrentZMPindex].pitch[1] =
+ FinalCoMStates[CurrentZMPindex].pitch[0] =
+ FinalCoMStates[CurrentZMPindex].pitch[1] =
FinalCoMStates[CurrentZMPindex].pitch[2] = 0.0;
- FinalCoMStates[CurrentZMPindex].roll[0] =
- FinalCoMStates[CurrentZMPindex].roll[1] =
+ FinalCoMStates[CurrentZMPindex].roll[0] =
+ FinalCoMStates[CurrentZMPindex].roll[1] =
FinalCoMStates[CurrentZMPindex].roll[2] = 0.0;
- FinalCoMStates[CurrentZMPindex].yaw[0] =
+ FinalCoMStates[CurrentZMPindex].yaw[0] =
ZMPPositions[CurrentZMPindex].theta;
- FinalCoMStates[CurrentZMPindex].yaw[1] =
+ FinalCoMStates[CurrentZMPindex].yaw[1] =
FinalCoMStates[CurrentZMPindex].yaw[2] = 0.0;
-
+
// Set Left and Right Foot positions.
- LeftFootAbsolutePositions[CurrentZMPindex] =
+ LeftFootAbsolutePositions[CurrentZMPindex] =
CurrentLeftFootAbsPos;
- RightFootAbsolutePositions[CurrentZMPindex] =
+ RightFootAbsolutePositions[CurrentZMPindex] =
CurrentRightFootAbsPos;
- LeftFootAbsolutePositions[CurrentZMPindex].time =
- RightFootAbsolutePositions[CurrentZMPindex].time =
+ LeftFootAbsolutePositions[CurrentZMPindex].time =
+ RightFootAbsolutePositions[CurrentZMPindex].time =
m_CurrentTime;
- LeftFootAbsolutePositions[CurrentZMPindex].stepType =
+ LeftFootAbsolutePositions[CurrentZMPindex].stepType =
RightFootAbsolutePositions[CurrentZMPindex].stepType = 10;
-
+
m_CurrentTime += m_SamplingPeriod;
CurrentZMPindex++;
}
-
- // The first foot when walking dynamically
+
+ // The first foot when walking dynamically
// does not leave the soil, but needs to be treated for the first phase.
m_RelativeFootPositions.push_back(RelativeFootPositions[0]);
@@ -495,7 +495,7 @@ int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
}
FilterOutValues(ZMPPositions,
FinalZMPPositions,
- true);
+ true);
ODEBUG6("InitOnLine","DebugDataRFPos.txt" );
for(unsigned int i=1;i<RelativeFootPositions.size();i++)
@@ -527,26 +527,26 @@ void ZMPDiscretization::UpdateCurrentSupportFootPosition(RelativeFootPosition aR
for(int k=0;k<2;k++)
for(int l=0;l<2;l++)
Orientation(k,l) = m_CurrentSupportFootPosition(k,l);
-
+
// second position.
MAL_MATRIX_DIM(v,double,2,1);
MAL_MATRIX_DIM(v2,double,2,1);
v(0,0) = aRFP.sx;
v(1,0) = aRFP.sy;
-
+
Orientation = MAL_RET_A_by_B(MM , Orientation);
MAL_C_eq_A_by_B(v2,Orientation, v);
ODEBUG("v :" << v << " "
"v2 : " << v2 << " "
"Orientation : " << Orientation << " "
"CurrentSupportFootPosition: " << m_CurrentSupportFootPosition );
-
-
+
+
for(int k=0;k<2;k++)
for(int l=0;l<2;l++)
m_CurrentSupportFootPosition(k,l) = Orientation(k,l);
-
+
for(int k=0;k<2;k++)
m_CurrentSupportFootPosition(k,2) += v2(k,0);
@@ -559,7 +559,7 @@ void ZMPDiscretization::UpdateCurrentSupportFootPosition(RelativeFootPosition aR
void ZMPDiscretization::OnLine(double, // time,
- deque<ZMPPosition> & ,// FinalZMPPositions,
+ deque<ZMPPosition> & ,// FinalZMPPositions,
deque<COMState> & , //FinalCOMStates,
deque<FootAbsolutePosition> &,//FinalLeftFootAbsolutePositions,
deque<FootAbsolutePosition> &)//FinalRightFootAbsolutePositions)
@@ -568,10 +568,10 @@ void ZMPDiscretization::OnLine(double, // time,
}
/* The interface method which returns an appropriate update of the
- appropriate stacks (ZMPRef, FootPosition) depending on the
+ appropriate stacks (ZMPRef, FootPosition) depending on the
state of the relative steps stack. */
void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosition,
- deque<ZMPPosition> & FinalZMPPositions,
+ deque<ZMPPosition> & FinalZMPPositions,
deque<COMState> & FinalCOMStates,
deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions,
@@ -596,8 +596,8 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
double lTdble=m_Tdble, lTsingle=m_Tsingle;
- ODEBUG6(m_RelativeFootPositions[0].sx << " " <<
- m_RelativeFootPositions[0].sy << " " <<
+ ODEBUG6(m_RelativeFootPositions[0].sx << " " <<
+ m_RelativeFootPositions[0].sy << " " <<
m_RelativeFootPositions[0].theta,"DebugDataRFPos.txt" );
ODEBUG(" OnLineAddFoot: m_RelativeFootPositions.size: " << m_RelativeFootPositions.size());
ODEBUG(" OnLineAddFoot: "<< endl <<
@@ -606,12 +606,12 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
" NewRelativeFootPositions.theta: " << NewRelativeFootPosition.theta
);
if (m_RelativeFootPositions[1].DStime!=0.0)
- {
- lTdble =m_RelativeFootPositions[1].DStime;
- lTsingle =m_RelativeFootPositions[1].SStime;
+ {
+ lTdble =m_RelativeFootPositions[1].DStime;
+ lTsingle =m_RelativeFootPositions[1].SStime;
}
// Compute on the direction of the support foot.
- // double stheta = sin(m_RelativeFootPositions[1].theta*M_PI/180.0);
+ // double stheta = sin(m_RelativeFootPositions[1].theta*M_PI/180.0);
ODEBUG("Time of double support phase in OnLineFootAdd: "<< lTdble);
TimeFirstPhase = lTdble;
@@ -624,7 +624,7 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
m_AngleDiffToSupportFootTheta = CurrentRightFootAbsPos.theta - CurrentLeftFootAbsPos.theta;
m_AngleDiffFromZMPThetaToSupportFootTheta = CurrentRightFootAbsPos.theta - CurrentAbsZMPTheta;
}
- else
+ else
{
WhoIsSupportFoot = 1;// Left
m_vdiffsupppre(0,0) = -CurrentRightFootAbsPos.x + CurrentLeftFootAbsPos.x;
@@ -632,31 +632,31 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
m_AngleDiffToSupportFootTheta = CurrentLeftFootAbsPos.theta - CurrentRightFootAbsPos.theta;
m_AngleDiffFromZMPThetaToSupportFootTheta = CurrentLeftFootAbsPos.theta - CurrentAbsZMPTheta;
}
-
+
double TimeForThisFootPosition = TimeFirstPhase+ lTsingle;
ODEBUG4("TimeFirstPhase: " << TimeFirstPhase << " lTsingle: " << lTsingle,"DebugData.txt");
// Compute the size of cells to add inside the array.
assert(m_SamplingPeriod > 0);
double l2AddArraySize= TimeForThisFootPosition/m_SamplingPeriod;
int AddArraySize = (unsigned int)round(l2AddArraySize);
- ODEBUG("Added part: "<<AddArraySize << " " << l2AddArraySize <<
+ ODEBUG("Added part: "<<AddArraySize << " " << l2AddArraySize <<
" TimeForThisFootPosition " << TimeForThisFootPosition <<
" SamplingPeriod" << m_SamplingPeriod);
ZMPPositions.resize(AddArraySize);
LeftFootAbsolutePositions.resize(AddArraySize);
- RightFootAbsolutePositions.resize(AddArraySize);
-
+ RightFootAbsolutePositions.resize(AddArraySize);
+
m_CurrentAbsTheta+= m_RelativeFootPositions[0].theta;
// m_CurrentAbsTheta = fmod(m_CurrentAbsTheta,180);
// Computes the new ABSOLUTE position of the supporting foot .
UpdateCurrentSupportFootPosition(m_RelativeFootPositions[0]);
-
+
// First Phase of the step cycle.
assert(m_SamplingPeriod > 0);
double dSizeOf1stPhase =TimeFirstPhase/m_SamplingPeriod;
unsigned int SizeOf1stPhase = (unsigned int)round(dSizeOf1stPhase);
- ODEBUG("m_vdiffsupppre : " << m_vdiffsupppre);
+ ODEBUG("m_vdiffsupppre : " << m_vdiffsupppre);
double px0,py0,theta0, delta_x,delta_y;
ODEBUG("FinalZMPPositions.size() = " <<FinalZMPPositions.size());
@@ -664,30 +664,30 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
px0 = FinalZMPPositions.back().px;
py0 = FinalZMPPositions.back().py;
theta0 = FinalZMPPositions.back().theta;
-
+
MAL_VECTOR_DIM(ZMPInFootCoordinates,double,3);
-
+
ZMPInFootCoordinates[0] = m_ZMPNeutralPosition[0];
ZMPInFootCoordinates[1] = m_ZMPNeutralPosition[1];
ZMPInFootCoordinates[2] = 1.0;
-
+
MAL_VECTOR_DIM(ZMPInWorldCoordinates,double,3);
-
- MAL_C_eq_A_by_B(ZMPInWorldCoordinates,m_CurrentSupportFootPosition, ZMPInFootCoordinates);
-
+
+ MAL_C_eq_A_by_B(ZMPInWorldCoordinates,m_CurrentSupportFootPosition, ZMPInFootCoordinates);
+
delta_x = (ZMPInWorldCoordinates(0) - px0)/SizeOf1stPhase;
delta_y = (ZMPInWorldCoordinates(1) - py0)/SizeOf1stPhase;
-
+
ODEBUG("delta_x :"<< delta_x << " delta_y : " << delta_y << " m_CurrentSFP: " <<
- m_CurrentSupportFootPosition << " ZMPInFC : " <<
- ZMPInFootCoordinates << " ZMPinWC : " <<
- ZMPInWorldCoordinates << " px0: " << px0 << " py0:" << py0
+ m_CurrentSupportFootPosition << " ZMPInFC : " <<
+ ZMPInFootCoordinates << " ZMPinWC : " <<
+ ZMPInWorldCoordinates << " px0: " << px0 << " py0:" << py0
);
- ODEBUG4("Step 4 TimeForThisFootPosition " << TimeForThisFootPosition,"DebugData.txt");
-
- // ZMP profile is changed if the stepping over is on, and then
+ ODEBUG4("Step 4 TimeForThisFootPosition " << TimeForThisFootPosition,"DebugData.txt");
+
+ // ZMP profile is changed if the stepping over is on, and then
// depends on the phase during stepping over.
- bool DoIt = 1;
+ bool DoIt = 1;
if (DoIt)
{
if (m_RelativeFootPositions[1].stepType == 3)
@@ -696,7 +696,7 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
delta_x = (m_CurrentSupportFootPosition(0,2)+m_ZMPShift[0] - px0)/SizeOf1stPhase;
//delta_y = (m_CurrentSupportFootPosition(1,2)+(WhoIsSupportFoot)*m_ZMPShift3BeginY - py0)/SizeOf1stPhase;
delta_y = (m_CurrentSupportFootPosition(1,2) - py0)/SizeOf1stPhase;
-
+
}
if (m_RelativeFootPositions[1].stepType == 4)
{
@@ -705,53 +705,53 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
//delta_x = (CurrentSupportFootPosition(0,2)+m_ZMPShift4Begin - px0)/SizeOf1stPhase;
//delta_y = (CurrentSupportFootPosition(1,2)+(WhoIsSupportFoot)*m_ZMPShift3BeginY - py0)/SizeOf1stPhase;
}
-
+
if (m_RelativeFootPositions[1].stepType == 5)
{
delta_x = (m_CurrentSupportFootPosition(0,2)-(m_ZMPShift[0] +m_ZMPShift[2]+
m_ZMPShift[1] +m_ZMPShift[3]) - px0)/SizeOf1stPhase;
delta_y = (m_CurrentSupportFootPosition(1,2)- py0)/SizeOf1stPhase;
- //delta_x = (CurrentSupportFootPosition(0,2)-(m_ZMPShift3Begin +
+ //delta_x = (CurrentSupportFootPosition(0,2)-(m_ZMPShift3Begin +
// m_ZMPShift4Begin+m_ZMPShift3End + m_ZMPShift4End) - px0)/SizeOf1stPhase;
- //delta_y = (CurrentSupportFootPosition(1,2)-(WhoIsSupportFoot)*(m_ZMPShift3BeginY +
+ //delta_y = (CurrentSupportFootPosition(1,2)-(WhoIsSupportFoot)*(m_ZMPShift3BeginY +
// m_ZMPShift4BeginY+m_ZMPShift3EndY + m_ZMPShift4EndY) - py0)/SizeOf1stPhase;
}
-
- }
-
+
+ }
+
ODEBUG4(" GetZMPDiscretization: Step 5 " << AddArraySize << " " ,"DebugData.txt");
- ODEBUG("SizeOf1stPhase: " << SizeOf1stPhase << "dx: " << delta_x << " dy: " << delta_y);
+ ODEBUG("SizeOf1stPhase: " << SizeOf1stPhase << "dx: " << delta_x << " dy: " << delta_y);
- // First phase of the cycle aka Double support phase.
+ // First phase of the cycle aka Double support phase.
for(unsigned int k=0;k<SizeOf1stPhase;k++)
{
-
+
ZMPPositions[CurrentZMPindex].px = px0 + k*delta_x;
ZMPPositions[CurrentZMPindex].py = py0 + k*delta_y;
ZMPPositions[CurrentZMPindex].pz = 0;
ZMPPositions[CurrentZMPindex].theta =theta0;
-
-
+
+
ZMPPositions[CurrentZMPindex].time = m_CurrentTime;
-
+
ZMPPositions[CurrentZMPindex].stepType = m_RelativeFootPositions[1].stepType+10;
-
+
// Right now the foot is not moving during the double support
// TO DO: whatever you need to do ramzi....
LeftFootAbsolutePositions[CurrentZMPindex] = FinalLeftFootAbsolutePositions.back();
-
+
// WARNING : This assume that you are walking on a plane.
LeftFootAbsolutePositions[CurrentZMPindex].z = 0.0;
-
+
RightFootAbsolutePositions[CurrentZMPindex] =FinalRightFootAbsolutePositions.back();
// WARNING : This assume that you are walking on a plane.
RightFootAbsolutePositions[CurrentZMPindex].z = 0.0;
-
- LeftFootAbsolutePositions[CurrentZMPindex].time =
+
+ LeftFootAbsolutePositions[CurrentZMPindex].time =
RightFootAbsolutePositions[CurrentZMPindex].time = m_CurrentTime;
- LeftFootAbsolutePositions[CurrentZMPindex].stepType =
+ LeftFootAbsolutePositions[CurrentZMPindex].stepType =
RightFootAbsolutePositions[CurrentZMPindex].stepType = m_RelativeFootPositions[1].stepType+10;
/*
ofstream aoflocal;
@@ -763,9 +763,9 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
CurrentZMPindex++;
}
//-- End Of First phase.
-
+
// Second Phase of the step cycle aka Single Support Phase.
-
+
// Compute relative feet position for the next step.
double lStepHeight=0;
@@ -794,15 +794,15 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
MAL_MATRIX_DIM(v,double,2,1);
Orientation(0,0) = c; Orientation(0,1) = -s;
Orientation(1,0) = s; Orientation(1,1) = c;
-
+
MAL_MATRIX_DIM(SubOrientation,double,2,2);
MAL_MATRIX_C_eq_EXTRACT_A(SubOrientation,m_CurrentSupportFootPosition,double,0,0,2,2);
- Orientation = MAL_RET_A_by_B(Orientation,SubOrientation) ;
-
+ Orientation = MAL_RET_A_by_B(Orientation,SubOrientation) ;
+
v(0,0) = m_RelativeFootPositions[1].sx;
v(1,0) = m_RelativeFootPositions[1].sy;
MAL_C_eq_A_by_B(vdiffsupp,Orientation,v);
-
+
vrel = vdiffsupp + m_vdiffsupppre;
// Compute relative feet orientation for the next step
@@ -813,7 +813,7 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
vrel(1,0)= 0.0;
RelTheta= 0.0;
NextTheta=0.0;
- lStepHeight = 0.0;
+ lStepHeight = 1.0;
}
ODEBUG(cout << "vrel: " << vrel(0,0) << " " << vrel(1,0));
@@ -824,10 +824,10 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
m_vdiffsupppre = vdiffsupp;
-
+
// m_AngleDiffToSupportFootTheta = NextTheta;
-
-
+
+
// Create the polynomes for the none-support foot.
// Change 08/12/2005: Speed up the modification of X and Y
// for vertical landing of the foot (Kajita-San's trick n 1)
@@ -835,8 +835,8 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
double ModulatedSingleSupportTime = lTsingle * m_ModulationSupportCoefficient;
double EndOfLiftOff = (lTsingle-ModulatedSingleSupportTime)*0.5;
- ODEBUG("ModulatedSingleSupportTime:" << ModulatedSingleSupportTime << " "
- << vrel(0,0) << " "
+ ODEBUG("ModulatedSingleSupportTime:" << ModulatedSingleSupportTime << " "
+ << vrel(0,0) << " "
<< vrel(1,0));
m_FootTrajectoryGenerationStandard->SetParameters(FootTrajectoryGenerationStandard::X_AXIS,
ModulatedSingleSupportTime,vrel(0,0));
@@ -850,7 +850,7 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
EndOfLiftOff,m_Omega);
m_FootTrajectoryGenerationStandard->SetParameters(FootTrajectoryGenerationStandard::OMEGA2_AXIS,
ModulatedSingleSupportTime,2*m_Omega);
-
+
// m_FootTrajectoryGenerationStandard->print();
//m_PolynomeZMPTheta->SetParameters(lTsingle,NextZMPTheta);
@@ -861,13 +861,13 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
int indexinitial = CurrentZMPindex-1;
int SignRHAND=1, SignLHAND=1;
- /*//polynomial planning for the stepover
-
+ /*//polynomial planning for the stepover
+
if (m_RelativeFootPositions[0].stepType==3)
{
StepOverPolyPlanner(m_RelativeFootPositions[0].stepType);
};
- */
+ */
double px02,py02;
px02 = ZMPPositions[CurrentZMPindex-1].px;
py02 = ZMPPositions[CurrentZMPindex-1].py;
@@ -881,28 +881,28 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
ZMPInFootCoordinates[0] = m_ZMPNeutralPosition[0];
ZMPInFootCoordinates[1] = m_ZMPNeutralPosition[1];
ZMPInFootCoordinates[2] = 1.0;
-
+
MAL_VECTOR_DIM(ZMPInWorldCoordinates,double,3);
- MAL_C_eq_A_by_B(ZMPInWorldCoordinates,m_CurrentSupportFootPosition,
- ZMPInFootCoordinates);
+ MAL_C_eq_A_by_B(ZMPInWorldCoordinates,m_CurrentSupportFootPosition,
+ ZMPInFootCoordinates);
ODEBUG4("CSFP: " << m_CurrentSupportFootPosition << endl <<
- "ZMPiWC" << ZMPInWorldCoordinates << endl, "DebugData.txt");
-
+ "ZMPiWC" << ZMPInWorldCoordinates << endl, "DebugData.txt");
+
ZMPPositions[CurrentZMPindex].px = ZMPInWorldCoordinates(0);
ZMPPositions[CurrentZMPindex].py = ZMPInWorldCoordinates(1);
ZMPPositions[CurrentZMPindex].pz = 0;
ZMPPositions[CurrentZMPindex].time = m_CurrentTime;
-
+
if (DoIt)
{
if ((m_RelativeFootPositions[1].stepType == 3)||(m_RelativeFootPositions[1].stepType == 4))
{
-
-
+
+
if (m_RelativeFootPositions[1].stepType == 3)
- {
+ {
delta_x = (m_CurrentSupportFootPosition(0,2)+m_ZMPShift[1] - px02)/SizeOfSndPhase;
delta_y = (m_CurrentSupportFootPosition(1,2) - py02)/SizeOfSndPhase;
//delta_x = (CurrentSupportFootPosition(0,2)+
@@ -919,7 +919,7 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
//delta_y = (CurrentSupportFootPosition(1,2)+
// (WhoIsSupportFoot)*m_ZMPShift4EndY - py02)/SizeOfSndPhase;
}
-
+
ZMPPositions[CurrentZMPindex].px = ZMPPositions[CurrentZMPindex-1].px + delta_x;
ZMPPositions[CurrentZMPindex].py = ZMPPositions[CurrentZMPindex-1].py + delta_y;
ZMPPositions[CurrentZMPindex].pz = 0;
@@ -927,7 +927,7 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
}
- ZMPPositions[CurrentZMPindex].theta = m_PolynomeZMPTheta->Compute(k*m_SamplingPeriod) +
+ ZMPPositions[CurrentZMPindex].theta = m_PolynomeZMPTheta->Compute(k*m_SamplingPeriod) +
ZMPPositions[indexinitial].theta;
ZMPPositions[CurrentZMPindex].stepType = WhoIsSupportFoot*m_RelativeFootPositions[0].stepType;
@@ -952,21 +952,21 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
1);
SignLHAND=-1;
SignRHAND=1;
-
+
}
-
- LeftFootAbsolutePositions[CurrentZMPindex].time =
+
+ LeftFootAbsolutePositions[CurrentZMPindex].time =
RightFootAbsolutePositions[CurrentZMPindex].time = m_CurrentTime;
-
+
m_CurrentTime += m_SamplingPeriod;
CurrentZMPindex++;
}
if (WhoIsSupportFoot==1)
WhoIsSupportFoot = -1;//Right
- else
+ else
WhoIsSupportFoot = 1;// Left
m_RelativeFootPositions.pop_front();
@@ -974,42 +974,42 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
for(unsigned int i=0;i<ZMPPositions.size();i++)
{
COMState aCOMState;
- aCOMState.x[0] =
- aCOMState.x[1] =
+ aCOMState.x[0] =
+ aCOMState.x[1] =
aCOMState.x[2] = 0.0;
- aCOMState.y[0] =
- aCOMState.y[1] =
+ aCOMState.y[0] =
+ aCOMState.y[1] =
aCOMState.y[2] = 0.0;
aCOMState.z[0] = m_ComHeight;
-
- aCOMState.pitch[0] =
+
+ aCOMState.pitch[0] =
aCOMState.pitch[1] =
aCOMState.pitch[2] =
aCOMState.roll[0] =
- aCOMState.roll[1] =
- aCOMState.roll[2] =
+ aCOMState.roll[1] =
+ aCOMState.roll[2] =
aCOMState.yaw[1] =
aCOMState.yaw[2] = 0.0;
-
+
aCOMState.z[1] = aCOMState.z[2] = 0.0;
aCOMState.yaw[0] = ZMPPositions[i].theta;
-
+
FinalCOMStates.push_back(aCOMState);
FinalLeftFootAbsolutePositions.push_back(LeftFootAbsolutePositions[i]);
FinalRightFootAbsolutePositions.push_back(RightFootAbsolutePositions[i]);
-
+
}
FilterOutValues(ZMPPositions,
- FinalZMPPositions,false);
+ FinalZMPPositions,false);
ODEBUG_CODE(DumpReferences(FinalZMPPositions,ZMPPositions));
-
+
if (EndSequence)
{
-
+
// End Phase of the walking includes the filtering.
EndPhaseOfTheWalking(FinalZMPPositions,
FinalCOMStates,
@@ -1022,7 +1022,7 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
void ZMPDiscretization::DumpReferences(deque<ZMPPosition> & FinalZMPPositions,
deque<ZMPPosition> & ZMPPositions)
{
-
+
ofstream dbg_aof("DebugZMPRefPos.dat",ofstream::app);
for(unsigned int i=0;i<ZMPPositions.size();i++)
@@ -1031,7 +1031,7 @@ void ZMPDiscretization::DumpReferences(deque<ZMPPosition> & FinalZMPPositions,
<< ZMPPositions[i].py << endl;
}
dbg_aof.close();
-
+
dbg_aof.open("DebugFinalZMPRefPos.dat",ofstream::app);
for(unsigned int i=0;i<FinalZMPPositions.size();i++)
{
@@ -1039,7 +1039,7 @@ void ZMPDiscretization::DumpReferences(deque<ZMPPosition> & FinalZMPPositions,
<< FinalZMPPositions[i].py << endl;
}
dbg_aof.close();
-
+
}
void ZMPDiscretization::FilterOutValues(deque<ZMPPosition> &ZMPPositions,
@@ -1059,7 +1059,7 @@ void ZMPDiscretization::FilterOutValues(deque<ZMPPosition> &ZMPPositions,
r=i-j+lshift;
if (r<0)
{
-
+
if (InitStep)
{
ltmp[0] += m_ZMPFilterWindow[j]*ZMPPositions[lshift].px;
@@ -1092,7 +1092,7 @@ void ZMPDiscretization::FilterOutValues(deque<ZMPPosition> &ZMPPositions,
ltmp[2] += m_ZMPFilterWindow[j]*ZMPPositions[r].pz;
}
}
-
+
ZMPPosition aZMPPos;
aZMPPos.px = ltmp[0];
aZMPPos.py = ltmp[1];
@@ -1100,7 +1100,7 @@ void ZMPDiscretization::FilterOutValues(deque<ZMPPosition> &ZMPPositions,
aZMPPos.theta = ZMPPositions[i].theta;
aZMPPos.time = ZMPPositions[i].time;
aZMPPos.stepType = ZMPPositions[i].stepType;
-
+
FinalZMPPositions.push_back(aZMPPos);
}
ODEBUG("ZMPPosition.back=( " <<ZMPPositions.back().px << " , " << ZMPPositions.back().py << " )");
@@ -1110,7 +1110,7 @@ void ZMPDiscretization::FilterOutValues(deque<ZMPPosition> &ZMPPositions,
int ZMPDiscretization::OnLineFootChange(double ,//time,
FootAbsolutePosition & ,//aFootAbsolutePosition,
- deque<ZMPPosition> & ,//FinalZMPPositions,
+ deque<ZMPPosition> & ,//FinalZMPPositions,
deque<COMState> & ,//CoMStates,
deque<FootAbsolutePosition> & ,//FinalLeftFootAbsolutePositions,
deque<FootAbsolutePosition> & ,//FinalRightFootAbsolutePositions,
@@ -1146,7 +1146,7 @@ void ZMPDiscretization::EndPhaseOfTheWalking( deque<ZMPPosition> &FinalZMPPosit
assert(m_SamplingPeriod > 0);
double dlAddArraySize = m_Tdble/(2*m_SamplingPeriod);
unsigned int AddArraySize = (unsigned int)round(dlAddArraySize);
-
+
unsigned int currentsize = 0;
unsigned int CurrentZMPindex = 0;
ZMPPositions.resize(currentsize+AddArraySize);
@@ -1163,36 +1163,36 @@ void ZMPDiscretization::EndPhaseOfTheWalking( deque<ZMPPosition> &FinalZMPPosit
// We assume that the last positon of the ZMP
// will the middle of the two last position
// of the support foot.
- ODEBUG("Cur/Prev SuppFootPos (X) : " <<m_CurrentSupportFootPosition(0,2)
+ ODEBUG("Cur/Prev SuppFootPos (X) : " <<m_CurrentSupportFootPosition(0,2)
<< " " << m_PrevCurrentSupportFootPosition(0,2));
- ODEBUG("Cur/Prev SuppFootPos (Y) : " <<m_CurrentSupportFootPosition(1,2)
+ ODEBUG("Cur/Prev SuppFootPos (Y) : " <<m_CurrentSupportFootPosition(1,2)
<< " " << m_PrevCurrentSupportFootPosition(1,2));
pxf = 0.5*(m_CurrentSupportFootPosition(0,2) + m_PrevCurrentSupportFootPosition(0,2));
pyf = 0.5*(m_CurrentSupportFootPosition(1,2) + m_PrevCurrentSupportFootPosition(1,2));
-
+
delta_x = (pxf - px0)/(double)SizeOfEndPhase;
delta_y = (pyf - py0)/(double)SizeOfEndPhase;
-
+
ZMPPositions[0].px = px0 + delta_x;
ZMPPositions[0].py = py0 + delta_y;
ZMPPositions[0].time = m_CurrentTime;
-
+
ZMPPositions[0].theta = FinalZMPPositions.back().theta;
ZMPPositions[0].stepType=0;
CurrentZMPindex++;
- LeftFootAbsolutePosition =
+ LeftFootAbsolutePosition =
FinalLeftFootAbsolutePositions.back();
- RightFootAbsolutePosition =
+ RightFootAbsolutePosition =
FinalRightFootAbsolutePositions.back();
-
- LeftFootAbsolutePosition.time =
+
+ LeftFootAbsolutePosition.time =
RightFootAbsolutePosition.time = m_CurrentTime;
-
- LeftFootAbsolutePosition.stepType =
+
+ LeftFootAbsolutePosition.stepType =
RightFootAbsolutePosition.stepType = 0;
-
+
FinalLeftFootAbsolutePositions.push_back(LeftFootAbsolutePosition);
FinalRightFootAbsolutePositions.push_back(RightFootAbsolutePosition);
@@ -1202,15 +1202,15 @@ void ZMPDiscretization::EndPhaseOfTheWalking( deque<ZMPPosition> &FinalZMPPosit
{
// Set ZMP positions.
- ZMPPositions[CurrentZMPindex].px =
+ ZMPPositions[CurrentZMPindex].px =
ZMPPositions[CurrentZMPindex-1].px + delta_x;
- ZMPPositions[CurrentZMPindex].py =
+ ZMPPositions[CurrentZMPindex].py =
ZMPPositions[CurrentZMPindex-1].py + delta_y;
ZMPPositions[CurrentZMPindex].time = m_CurrentTime;
- ZMPPositions[CurrentZMPindex].theta =
+ ZMPPositions[CurrentZMPindex].theta =
ZMPPositions[CurrentZMPindex-1].theta;
- ZMPPositions[CurrentZMPindex].stepType =0;
+ ZMPPositions[CurrentZMPindex].stepType =0;
// Set CoM Positions.
COMState aCOMState;
@@ -1222,15 +1222,15 @@ void ZMPDiscretization::EndPhaseOfTheWalking( deque<ZMPPosition> &FinalZMPPosit
FinalCOMStates.push_back(aCOMState);
// Set Feet positions.
- LeftFootAbsolutePosition =
+ LeftFootAbsolutePosition =
FinalLeftFootAbsolutePositions.back();
- RightFootAbsolutePosition =
+ RightFootAbsolutePosition =
FinalRightFootAbsolutePositions.back();
-
- LeftFootAbsolutePosition.time =
+
+ LeftFootAbsolutePosition.time =
RightFootAbsolutePosition.time = m_CurrentTime;
- LeftFootAbsolutePosition.stepType =
+ LeftFootAbsolutePosition.stepType =
RightFootAbsolutePosition.stepType = 0;
FinalLeftFootAbsolutePositions.push_back(LeftFootAbsolutePosition);
@@ -1248,14 +1248,14 @@ void ZMPDiscretization::EndPhaseOfTheWalking( deque<ZMPPosition> &FinalZMPPosit
double ldAddArraySize = 3.0*m_PreviewControlTime/m_SamplingPeriod;
AddArraySize = (int)ldAddArraySize;
}
-
+
currentsize = ZMPPositions.size();
ZMPPositions.resize(currentsize+AddArraySize);
ODEBUG4(" GetZMPDiscretization: Step 8 ","DebugData.txt");
for(unsigned int i=0;i<AddArraySize;i++)
- {
+ {
// Set ZMP Positions
ZMPPositions[CurrentZMPindex].px = ZMPPositions[CurrentZMPindex-1].px;
@@ -1275,18 +1275,18 @@ void ZMPDiscretization::EndPhaseOfTheWalking( deque<ZMPPosition> &FinalZMPPosit
FinalCOMStates.push_back(aCOMState);
// Set Feet Positions
- LeftFootAbsolutePosition=
+ LeftFootAbsolutePosition=
FinalLeftFootAbsolutePositions.back();
- RightFootAbsolutePosition =
+ RightFootAbsolutePosition =
FinalRightFootAbsolutePositions.back();
-
- LeftFootAbsolutePosition.time =
+
+ LeftFootAbsolutePosition.time =
RightFootAbsolutePosition.time = m_CurrentTime;
-
- LeftFootAbsolutePosition.stepType =
+
+ LeftFootAbsolutePosition.stepType =
RightFootAbsolutePosition.stepType = 0;
-
+
FinalLeftFootAbsolutePositions.push_back(LeftFootAbsolutePosition);
FinalRightFootAbsolutePositions.push_back(RightFootAbsolutePosition);
@@ -1301,11 +1301,11 @@ void ZMPDiscretization::EndPhaseOfTheWalking( deque<ZMPPosition> &FinalZMPPosit
void ZMPDiscretization::RegisterMethodsForScripting()
{
- std::string aMethodName[3] =
+ std::string aMethodName[3] =
{":prevzmpinitprofil",
":zeroinitprofil",
":previewcontroltime"};
-
+
for(int i=0;i<3;i++)
{
if (!RegisterMethod(aMethodName[i]))
@@ -1341,5 +1341,5 @@ void ZMPDiscretization::CallMethod(std::string &Method, std::istringstream &strm
}
ZMPRefTrajectoryGeneration::CallMethod(Method,strm);
-
+
}
diff --git a/src/patterngeneratorinterfaceprivate.hh b/src/patterngeneratorinterfaceprivate.hh
index af4bf8f87be5d1391de2f1f868185c80ac5995ef..f14dc7b14461115f6b9cbdc252c3d35120b95e6c 100644
--- a/src/patterngeneratorinterfaceprivate.hh
+++ b/src/patterngeneratorinterfaceprivate.hh
@@ -1,10 +1,10 @@
/*
- * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
+ * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
*
* Andrei Herdt
* Benallegue Mehdi
* Olivier Stasse
- * Eiichi Yoshida
+ * Eiichi Yoshida
*
* JRL, CNRS/AIST
*
@@ -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)
*/
/*! \file PatternGeneratorInterface.h
@@ -68,23 +68,23 @@ namespace PatternGeneratorJRL
{
/** @ingroup Interface
- This class is the interface between the Pattern Generator and the
+ This class is the interface between the Pattern Generator and the
external world. In addition to the classical setter and getter for various parameters
there is the possibility to pass commands a string of stream to the method
\a ParseCmd().
-
+
There is a set of functionalities directly supported by the API:
-
-
-
+
+
+
*/
- class PatternGeneratorInterfacePrivate : public virtual PatternGeneratorInterface,
+ class PatternGeneratorInterfacePrivate : public virtual PatternGeneratorInterface,
SimplePluginManager, SimplePlugin
-
- {
+
+ {
public:
-
- /*! Constructor
+
+ /*! Constructor
@param strm: Should provide the file to initialize the preview control,
the path to the VRML model, and the name of the file containing the VRML model.
*/
@@ -92,14 +92,14 @@ namespace PatternGeneratorJRL
/*! Destructor */
~PatternGeneratorInterfacePrivate();
-
- /*! \brief Function to specify steps in the stack of the walking pattern generator.
+
+ /*! \brief Function to specify steps in the stack of the walking pattern generator.
This method is different AddOnLineStep which is the default step add when there is no policy,
or no step available.
*/
void AddStepInStack(double dx, double dy, double theta);
-
- /*! \name High levels function to create automatically stack of steps following specific motions.
+
+ /*! \name High levels function to create automatically stack of steps following specific motions.
@{
*/
/*! \brief This methods generate a stack of steps which make the robot follows an arc.
@@ -116,7 +116,7 @@ namespace PatternGeneratorJRL
double R,
double arc_deg,
int SupportFoot);
-
+
/*! \brief This methods generate a stack of steps which make the robot follows an arc.
The direction of the robot is towards the center of the arc.
The robot is therefore expected to move sideways.
@@ -128,16 +128,16 @@ namespace PatternGeneratorJRL
void CreateArcCenteredInStepStack( double R,
double arc_deg,
int SupportFoot);
-
+
/*! \brief This specifies which foot will be used as the first support of the motion. */
void PrepareForSupportFoot(int SupportFoot);
-
+
/*! \brief This method precomputes all the buffers necessary for walking according to the chosen strategy. */
void FinishAndRealizeStepSequence();
/*! @} */
- /*! Common Initialization of walking.
+ /*! Common Initialization of walking.
@param[out] lStartingCOMPosition: For the starting position on the articular space, returns
the COM position.
@param[out] lStartingZMPPosition: For the starting position on the articular space, returns
@@ -155,25 +155,25 @@ namespace PatternGeneratorJRL
void CommonInitializationOfWalking(COMState & lStartingCOMState,
MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition,
MAL_VECTOR( & ,double) BodyAnglesIni,
- FootAbsolutePosition & InitLeftFootAbsPos,
+ FootAbsolutePosition & InitLeftFootAbsPos,
FootAbsolutePosition & InitRightFootAbsPos,
deque<RelativeFootPosition> & lRelativeFootPositions,
std::vector<double> & lCurrentJointValues,
bool ClearStepStackHandler);
- /*! \name Methods for the control part.
+ /*! \name Methods for the control part.
@{
*/
-
+
/*! \brief Run One Step of the global control loop aka The Main Method To Be Used.
- @param[out] CurrentConfiguration The current configuration of the robot according to
+ @param[out] CurrentConfiguration The current configuration of the robot according to
the implementation of dynamic-JRLJapan. This should be first position and orientation
- of the waist, and then all the DOFs of your robot.
- @param[out] CurrentVelocity The current velocity of the robot according to the
- the implementation of dynamic-JRLJapan.
- @param[out] CurrentAcceleration The current acceleration of the robot according to the
- the implementation of dynamic-JRLJapan.
+ of the waist, and then all the DOFs of your robot.
+ @param[out] CurrentVelocity The current velocity of the robot according to the
+ the implementation of dynamic-JRLJapan.
+ @param[out] CurrentAcceleration The current acceleration of the robot according to the
+ the implementation of dynamic-JRLJapan.
@param[out] ZMPTarget The target ZMP in the waist reference frame.
@return True is there is still some data to send, false otherwise.
*/
@@ -183,13 +183,13 @@ namespace PatternGeneratorJRL
MAL_VECTOR_TYPE(double) & ZMPTarget);
/*! \brief Run One Step of the global control loop aka The Main Method To Be Used.
- @param[out] CurrentConfiguration The current configuration of the robot according to
+ @param[out] CurrentConfiguration The current configuration of the robot according to
the implementation of dynamic-JRLJapan. This should be first position and orientation
- of the waist, and then all the DOFs of your robot.
- @param[out] CurrentVelocity The current velocity of the robot according to the
- the implementation of dynamic-JRLJapan.
- @param[out] CurrentAcceleration The current acceleration of the robot according to the
- the implementation of dynamic-JRLJapan.
+ of the waist, and then all the DOFs of your robot.
+ @param[out] CurrentVelocity The current velocity of the robot according to the
+ the implementation of dynamic-JRLJapan.
+ @param[out] CurrentAcceleration The current acceleration of the robot according to the
+ the implementation of dynamic-JRLJapan.
@param[out] ZMPTarget The target ZMP in the waist reference frame.
@param[out] COMState The CoM state for this motion.
@param[out] LeftFootPosition: Absolute position of the left foot.
@@ -205,13 +205,13 @@ namespace PatternGeneratorJRL
FootAbsolutePosition &RightFootPosition);
/*! \brief Run One Step of the global control loop aka The Main Method To Be Used.
- @param[out] CurrentConfiguration The current configuration of the robot according to
+ @param[out] CurrentConfiguration The current configuration of the robot according to
the implementation of dynamic-JRLJapan. This should be first position and orientation
- of the waist, and then all the DOFs of your robot.
- @param[out] CurrentVelocity The current velocity of the robot according to the
- the implementation of dynamic-JRLJapan.
- @param[out] CurrentAcceleration The current acceleration of the robot according to the
- the implementation of dynamic-JRLJapan.
+ of the waist, and then all the DOFs of your robot.
+ @param[out] CurrentVelocity The current velocity of the robot according to the
+ the implementation of dynamic-JRLJapan.
+ @param[out] CurrentAcceleration The current acceleration of the robot according to the
+ the implementation of dynamic-JRLJapan.
@param[out] ZMPTarget The target ZMP in the waist reference frame.
@param[out] aCOMPosition The CoM position for this motion.
@param[out] LeftFootPosition: Absolute position of the left foot.
@@ -230,7 +230,7 @@ namespace PatternGeneratorJRL
/*! \brief Run One Step of the global control loop aka The Main Method To Be Used.
@param[out] LeftFootPosition: Absolute position of the left foot.
@param[out] RightFootPosition: Absolute position of the right foot.
- @param[out] ZMPRefPos: ZMP position new reference
+ @param[out] ZMPRefPos: ZMP position new reference
@param[out] COMRefPos: COM position new reference.
@return True is there is still some data to send, false otherwise.
*/
@@ -242,16 +242,16 @@ namespace PatternGeneratorJRL
/*! Set the current joint values of the robot.
This method is used to properly initialize the pattern generator.
- It also updates the state of the robot if other control mechanisms
+ It also updates the state of the robot if other control mechanisms
modifies the upper body part and if this should be taken into account
into the pattern generator in the second loop of control. */
void SetCurrentJointValues(MAL_VECTOR( &lCurrentJointValues,double));
/*! \brief Returns the walking mode. */
int GetWalkMode() const;
-
+
/*! \brief Get the leg joint velocity */
- void GetLegJointVelocity(MAL_VECTOR( &dqr,double),
+ void GetLegJointVelocity(MAL_VECTOR( &dqr,double),
MAL_VECTOR( &dql,double)) const;
/*! \brief Read a velocity reference. */
@@ -265,16 +265,16 @@ namespace PatternGeneratorJRL
/*! \brief Read a sequence of steps. */
void ReadSequenceOfSteps(istringstream &strm);
-
- /*! \name On-line steps related methods
+
+ /*! \name On-line steps related methods
@{
*/
/*! \brief Start the creation of steps on line. */
void StartOnLineStepSequencing();
-
+
/*! \brief Stop the creation of steps on line. */
void StopOnLineStepSequencing();
-
+
/*! \brief Add an online step */
void AddOnLineStep(double X, double Y, double Theta);
@@ -282,25 +282,25 @@ namespace PatternGeneratorJRL
The strategy is the following: the step in single support phase at time t
has its landing position changed to \f$ (X,Y,\theta) \f$ in absolute
coordinates (i.e. in the world reference frame of the free flyer of the robot).
- For stability reason there is no guarantee that this method can
+ For stability reason there is no guarantee that this method can
realized the operation. Please see the documentation of the walking pattern generator
- algorithm used.
-
+ algorithm used.
+
If the time falls during a double support phase, the next single support phase is chosen.
-
- @param[in] Time: Time information of the step.
+
+ @param[in] Time: Time information of the step.
@param[in] aFootAbsolutePosition: Absolute position of the foot.
@return If the operation failed the method returns a negative number related
to an error, 0 otherwise.
*/
- int ChangeOnLineStep(double Time,
+ int ChangeOnLineStep(double Time,
FootAbsolutePosition &aFootAbsolutePosition,
double &newtime);
/*! \brief Change online step.
See the above method for the specifications.
This method uses a different format with stream of strings.
- @param[in] Time: Time information of the step.
+ @param[in] Time: Time information of the step.
@return nothing */
void ChangeOnLineStep(istringstream &strm,double &newtime);
@@ -325,13 +325,13 @@ namespace PatternGeneratorJRL
/*! \brief An other method to get the waist position using a matrix. */
void getWaistPositionMatrix(MAL_S4x4_MATRIX( &lWaistAbsPos,double)) const;
-
+
/*!@} */
- /*! \name Handling of the inter-objects relationships.
+ /*! \name Handling of the inter-objects relationships.
@{
*/
-
+
/*! \brief Instanciate the necessary objects. */
void ObjectsInstanciation();
@@ -347,7 +347,7 @@ namespace PatternGeneratorJRL
/*! \brief Get the initial ZMP reference point. */
void getZMPInitialPoint(MAL_S3_VECTOR(&,double) lZMPInitialPoint) const;
- /*! \name System to call a given method based on registration of a method.
+ /*! \name System to call a given method based on registration of a method.
@{
*/
@@ -380,7 +380,7 @@ namespace PatternGeneratorJRL
void setVelocityReference(double x,
double y,
double yaw);
-
+
/*! @} */
@@ -393,13 +393,13 @@ namespace PatternGeneratorJRL
protected:
- /*! \name Methods for interpreter.
+ /*! \name Methods for interpreter.
@{
*/
-
+
/*! \brief Set the shift of the ZMP height for stepping over. */
virtual void m_SetZMPShiftParameters(std::istringstream &strm);
-
+
/*! \brief Set time distribution parameters. */
virtual void m_SetTimeDistrParameters(std::istringstream &strm);
@@ -412,20 +412,22 @@ namespace PatternGeneratorJRL
/*! \brief Read file from Kineoworks. */
virtual void m_ReadFileFromKineoWorks(std::istringstream &strm);
- /*! \brief Specify a sequence of step without asking for
+ /*! \brief Specify a sequence of step without asking for
immediate execution and end sequence. */
virtual void m_PartialStepSequence(istringstream &strm);
/*! \brief This method set PBW's algorithm for ZMP trajectory planning. */
virtual void m_SetAlgoForZMPTraj(istringstream &strm);
- /*! \brief Interface to hrpsys to start the realization of
+ /*! \brief Interface to hrpsys to start the realization of
the stacked of step sequences. */
virtual void m_FinishAndRealizeStepSequence(std::istringstream &strm);
-
+
/*! \brief Realize a sequence of steps. */
virtual void m_StepSequence(std::istringstream &strm);
+ virtual void m_StepStairSequence(std::istringstream &strm);
+
private:
@@ -435,7 +437,7 @@ namespace PatternGeneratorJRL
/*! Buffer needed to perform the stepping over
obstacle. */
std::vector<double> m_ZMPShift;
-
+
/*! Gain factor for the default arm motion while walking. */
double m_GainFactor;
@@ -452,7 +454,7 @@ namespace PatternGeneratorJRL
/*! QP formulation with constraints. */
ZMPConstrainedQPFastFormulation * m_ZMPCQPFF;
-
+
/*! QP formulation with a velocity reference. */
ZMPVelocityReferencedQP * m_ZMPVRQP;
@@ -462,7 +464,7 @@ namespace PatternGeneratorJRL
/*! Specified ZMP starting point. */
MAL_S3_VECTOR_TYPE( double) m_ZMPInitialPoint;
- /*! Boolean stating if the user has specified or not the ZMP initial point.
+ /*! Boolean stating if the user has specified or not the ZMP initial point.
This boolean is set to true when the user is setting the previous value.
Otherwise it is set to false.*/
bool m_ZMPInitialPointSet;
@@ -471,50 +473,50 @@ namespace PatternGeneratorJRL
/*! The Preview Control object. */
PreviewControl *m_PC;
-
+
/*! The object to be used to perform one step of
control, and generates the corrected CoM trajectory. */
ZMPPreviewControlWithMultiBodyZMP *m_ZMPpcwmbz;
/*! Object needed to perform a path provided by Kineo */
GenerateMotionFromKineoWorks *m_GMFKW;
-
+
/*! Conversion between the index of the plan and the robot DOFs. */
std::vector<int> m_ConversionForUpperBodyFromLocalIndexToRobotDOFs;
-
+
/*! Current Actuated Joint values of the robot. */
std::vector<double> m_CurrentActuatedJointValues;
- /*! Position of the waist:
+ /*! Position of the waist:
Relative.*/
MAL_S4x4_MATRIX( m_WaistRelativePos,double);
-
+
/*! Absolute: */
MAL_S4x4_MATRIX( m_WaistAbsPos,double);
-
+
/*! Orientation: */
double m_AbsTheta, m_AbsMotionTheta;
-
+
/*! Position of the motion: */
MAL_S4x4_MATRIX( m_MotionAbsPos,double);
MAL_S4x4_MATRIX( m_MotionAbsOrientation,double);
-
+
/*! Absolute speed:*/
MAL_S4_VECTOR( m_AbsLinearVelocity,double);
MAL_S4_VECTOR( m_AbsAngularVelocity,double);
/*! Aboluste acceleration */
MAL_S4_VECTOR( m_AbsLinearAcc,double);
-
+
/*! Keeps track of the last correct support foot. */
int m_KeepLastCorrectSupportFoot;
-
- /*! Boolean to ensure a correct initialization of the
+
+ /*! Boolean to ensure a correct initialization of the
step's stack. */
bool m_IncorrectInitialization;
- /*! \name Global strategy handlers
+ /*! \name Global strategy handlers
@{
*/
/*! \brief Double stage preview control strategy */
@@ -525,7 +527,7 @@ namespace PatternGeneratorJRL
/*! \brief General handler. */
GlobalStrategyManager *m_GlobalStrategyManager;
-
+
/*! @} */
/*! Store the debug mode. */
@@ -533,11 +535,11 @@ namespace PatternGeneratorJRL
/*! Store the number of degree of freedoms */
int m_DOF;
-
+
/*! Store the height of the arm. */
double m_ZARM;
- /**! \name Time related parameters.
+ /**! \name Time related parameters.
@{
*/
/*! \brief Sampling period of the control loop. */
@@ -545,8 +547,8 @@ namespace PatternGeneratorJRL
/*! \brief Window of the preview control */
double m_PreviewControlTime;
-
- /*! \brief Internal clock.
+
+ /*! \brief Internal clock.
This field is updated every call to RunOneStepOfControl.
It is assumed that this is done every m_SamplingPeriod.
*/
@@ -563,13 +565,13 @@ namespace PatternGeneratorJRL
/*! Discrete size of the preview control window */
unsigned int m_NL;
-
+
/*! Local time while performing the control loop. */
unsigned long int m_count;
-
+
/*! Maximal value for the arms in front of the robot */
- double m_Xmax;
+ double m_Xmax;
/*! Variables used to compute speed for other purposes. */
MAL_VECTOR( m_prev_qr,double);
@@ -595,15 +597,15 @@ namespace PatternGeneratorJRL
bool m_Running;
- /*! \name To handle a new step.
+ /*! \name To handle a new step.
@{
*/
-
+
/*! \brief There is a new user specified step. */
bool m_NewStep;
-
+
/*! \brief X, Y, Theta coordinates of the new step.*/
- double m_NewStepX, m_NewStepY, m_NewTheta;
+ double m_NewStepX, m_NewStepY, m_NewStepZ, m_NewTheta;
/*! @} */
/*! \brief Add automatically the first new step */
@@ -611,20 +613,20 @@ namespace PatternGeneratorJRL
/* ! Store the current relative state of the waist */
COMState m_CurrentWaistState;
-
+
/* ! current time period for the control */
double m_dt;
- /*! \name Internals to deal with several ZMP CoM generation algorithms
+ /*! \name Internals to deal with several ZMP CoM generation algorithms
@{ */
/*! Algorithm to compute ZMP and CoM trajectory */
int m_AlgorithmforZMPCOM;
- /*! Constants
+ /*! Constants
@{ */
/*! Using Preview Control with 2 stages proposed by Shuuji Kajita in 2003. */
static const int ZMPCOM_KAJITA_2003=1;
-
+
/*! Using the preview control with 2 stages proposed by Pierre-Brice Wieber in 2006. */
static const int ZMPCOM_WIEBER_2006=2;
@@ -633,7 +635,7 @@ namespace PatternGeneratorJRL
/*! Using the QP constrained problem resolution proposed by Dimitrov in 2008. */
static const int ZMPCOM_DIMITROV_2008=4;
-
+
/*! Using the velocity referenced QP proposed by Herdt in 2010. */
static const int ZMPCOM_HERDT_2010=5;
/*! @} */
@@ -649,38 +651,38 @@ namespace PatternGeneratorJRL
and feet positions. */
ComAndFootRealization * m_ComAndFootRealization;
- /* \name Object related to stepping over.
+ /* \name Object related to stepping over.
@{
*/
-
+
/*! Planner for stepping over an obstacle. */
StepOverPlanner *m_StOvPl;
/*! Position and parameters related to the obstacle. */
ObstaclePar m_ObstaclePars;
-
+
/*! Boolean on the obstacle's detection */
- bool m_ObstacleDetected;
+ bool m_ObstacleDetected;
/*! Time Distribution factor */
std::vector<double> m_TimeDistrFactor;
/*! Variable for delta feasibility limit */
- double m_DeltaFeasibilityLimit;
+ double m_DeltaFeasibilityLimit;
/*! New time for step interval using Changing On Line Step. */
double m_NewNextStepInterval;
-
+
/* @} */
/*! \brief Foot Trajectory Generator */
LeftAndRightFootTrajectoryGenerationMultiple * m_FeetTrajectoryGenerator;
-
- /*! \name Buffers of Positions.
+
+ /*! \name Buffers of Positions.
@{
*/
-
+
/*! Buffer of ZMP positions */
deque<ZMPPosition> m_ZMPPositions;
@@ -689,10 +691,10 @@ namespace PatternGeneratorJRL
/*! Buffer of absolute foot position. */
deque<FootAbsolutePosition> m_LeftFootPositions, m_RightFootPositions;
-
+
/*! Buffer for the COM position. */
deque<COMState> m_COMBuffer;
-
+
/*! @} */
/*! \brief Reimplement the SimplePlugin interface. */
@@ -707,11 +709,11 @@ namespace PatternGeneratorJRL
protected:
- /*! \name Internal methods which are not to be exposed.
- They are therefore subject to change.
+ /*! \name Internal methods which are not to be exposed.
+ They are therefore subject to change.
@{
*/
-
+
/*! \brief Expansion of the buffers handling Center of Masse positions,
as well as Upper Body Positions. */
void ExpandCOMPositionsQueues(int aNumber);
@@ -721,7 +723,7 @@ namespace PatternGeneratorJRL
MAL_S3_VECTOR( & lStartingCOMPosition,double));
- /*! \brief Fill the internal buffer with the appropriate information depending on the strategy.
+ /*! \brief Fill the internal buffer with the appropriate information depending on the strategy.
The behavior of this method depends on \a m_AlgorithmforZMPCOM.
*/
int CreateZMPReferences(deque<RelativeFootPosition> &lRelativeFootPositions,
@@ -738,7 +740,7 @@ namespace PatternGeneratorJRL
/* @} */
};
-
+
}
diff --git a/tests/TestHerdt2010.cpp b/tests/TestHerdt2010.cpp
index 38062f2072644de03ad31bc2e5c842dc02d51fbf..2208333df5a74789b71323bac5bfcf476b54df4b 100644
--- a/tests/TestHerdt2010.cpp
+++ b/tests/TestHerdt2010.cpp
@@ -85,7 +85,8 @@ protected:
aPGI.ParseCmd(strm2);
}
{
- istringstream strm2(":numberstepsbeforestop 2");
+ // m_TestProfile.m_isStepStairOn = 0;
+ istringstream strm2(":numberstepsbeforestop 1");
aPGI.ParseCmd(strm2);
}
}
@@ -248,7 +249,7 @@ protected:
// {10*200,&TestHerdt2010::walkForward},
// {25*200,&TestHerdt2010::walkForward},
// {35*200,&TestHerdt2010::walkForward},
- {45*200,&TestHerdt2010::stop}
+ {15*200,&TestHerdt2010::stop}
/* {55*200,&TestHerdt2010::walkForward},
{65*200,&TestHerdt2010::walkForward},
{75*200,&TestHerdt2010::walkForward},
diff --git a/tests/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index 93a494eb9318c63f95d8842079d20bb1a540e565..70796b5365801f1e6540e7f6fb58edf6f695fc5e 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -114,7 +114,18 @@ protected:
}
{
- istringstream strm2(":stepseq 0.0 -0.105 0.0 0.2 0.19 0.0 0.2 -0.19 0.0 0.2 0.19 0.0 0.2 -0.19 0.0 0.2 0.19 0.0 0.0 -0.19 0.0");
+ istringstream strm2(":stepseq 0.0 -0.105 0.0 \
+ 0.2 0.0 0.0 \
+ 0.2 0.0 0.0 \
+ ");
+ /* istringstream strm2(":stepseq 0.0 -0.105 0.0 0.0 \
+ 0.2 0.19 0.1 0.0 \
+ 0.2 -0.19 0.1 0.0 \
+ 0.2 0.19 0.1 0.0\
+ 0.2 -0.19 0.1 0.0 \
+ 0.2 0.19 0.1 0.0 \
+ 0.0 -0.19 0.1 0.0");*/
+
aPGI.ParseCmd(strm2);
}
@@ -150,21 +161,21 @@ protected:
/* Stop after 30 seconds the on-line stepping */
- if (m_OneStep.NbOfIt>StoppingTime)
+ if (m_OneStep.NbOfIt>StoppingTime)
{
StopOnLineWalking(*m_PGI);
}
- else
+ else
{
/* Stay on the spot during 5.0 s before stopping. */
- if (m_OneStep.NbOfIt<StoppingTime-200*5.0)
+ if (m_OneStep.NbOfIt<StoppingTime-200*5.0)
{
if (m_OneStep.NbOfIt%200==0)
{
cout << "Progress " << (unsigned int)r << " "<< "\r";
cout.flush();
}
-
+
double triggertime = 9.8*200 + m_deltatime*200;
if ((m_OneStep.NbOfIt>triggertime) &&
m_TestChangeFoot)
@@ -184,7 +195,7 @@ protected:
}
double newtime;
bool stepHandledCorrectly=true;
- try
+ try
{
m_PGI->ChangeOnLineStep(0.805,aFAP,newtime);
}
@@ -201,7 +212,7 @@ protected:
if (m_NbStepsModified==360)
m_TestChangeFoot=false;
}
- else
+ else
{
m_deltatime += 0.005;
}
@@ -209,7 +220,7 @@ protected:
}
}
};
-
+
};
int PerformTests(int argc, char *argv[])