Commit eb173fa9 authored by olivier-stasse's avatar olivier-stasse
Browse files

Fix warnings.

parent 1aad544b
......@@ -60,7 +60,7 @@ namespace PatternGeneratorJRL
@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.
*/
PatternGeneratorInterface(CjrlHumanoidDynamicRobot *aHDR) {};
PatternGeneratorInterface(CjrlHumanoidDynamicRobot *) {};
/*! Destructor */
virtual ~PatternGeneratorInterface() {};
......@@ -88,7 +88,7 @@ namespace PatternGeneratorJRL
@param[in] ClearStepStackHandler: Clean the stack of steps after copy.
*/
virtual void CommonInitializationOfWalking(COMState & lStartingCOMState,
MAL_S3_VECTOR(,double) & lStartingZMPPosition,
MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition,
MAL_VECTOR( & ,double) BodyAnglesIni,
FootAbsolutePosition & InitLeftFootAbsPos,
FootAbsolutePosition & InitRightFootAbsPos,
......@@ -112,10 +112,10 @@ namespace PatternGeneratorJRL
@param[out] ZMPTarget The target ZMP in the waist reference frame.
@return True is there is still some data to send, false otherwise.
*/
virtual bool RunOneStepOfTheControlLoop(MAL_VECTOR(,double) & CurrentConfiguration,
MAL_VECTOR(,double) & CurrentVelocity,
MAL_VECTOR(,double) & CurrentAcceleration,
MAL_VECTOR(,double) & ZMPTarget)=0;
virtual bool RunOneStepOfTheControlLoop(MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration,
MAL_VECTOR_TYPE(double) & ZMPTarget)=0;
/*! \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
......@@ -131,10 +131,10 @@ namespace PatternGeneratorJRL
@param[out] RightFootPosition: Absolute position of the right foot.
@return True is there is still some data to send, false otherwise.
*/
virtual bool RunOneStepOfTheControlLoop(MAL_VECTOR(,double) & CurrentConfiguration,
MAL_VECTOR(,double) & CurrentVelocity,
MAL_VECTOR(,double) & CurrentAcceleration,
MAL_VECTOR(,double) &ZMPTarget,
virtual bool RunOneStepOfTheControlLoop(MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration,
MAL_VECTOR_TYPE(double) &ZMPTarget,
COMPosition &COMPosition,
FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition)=0;
......@@ -153,10 +153,10 @@ namespace PatternGeneratorJRL
@param[out] RightFootPosition: Absolute position of the right foot.
@return True is there is still some data to send, false otherwise.
*/
virtual bool RunOneStepOfTheControlLoop(MAL_VECTOR(,double) & CurrentConfiguration,
MAL_VECTOR(,double) & CurrentVelocity,
MAL_VECTOR(,double) & CurrentAcceleration,
MAL_VECTOR(,double) &ZMPTarget,
virtual bool RunOneStepOfTheControlLoop(MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration,
MAL_VECTOR_TYPE(double) &ZMPTarget,
COMState &COMState,
FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition)=0;
......@@ -277,8 +277,8 @@ namespace PatternGeneratorJRL
/*! \brief Returns the ZMP, CoM, left foot absolute position, and right foot absolute position
for the initiale pose.*/
virtual void EvaluateStartingState(COMState & lStartingCOMState,
MAL_S3_VECTOR(,double) & lStartingZMPPosition,
MAL_VECTOR(,double) & lStartingWaistPose,
MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition,
MAL_VECTOR_TYPE(double) & lStartingWaistPose,
FootAbsolutePosition & InitLeftFootAbsPos,
FootAbsolutePosition & InitRightFootAbsPos)=0;
......
......@@ -85,8 +85,8 @@ namespace PatternGeneratorJRL
struct RelativeFootPosition_s
{
double sx,sy,theta;
float SStime;
float DStime;
double SStime;
double DStime;
int stepType; //1:normal walking 2:one step before opbstacle
//3:first leg over obstacle 4:second leg over obstacle 5:one step after obstacle
double DeviationHipHeight;
......
......@@ -82,22 +82,25 @@ void FootTrajectoryGenerationAbstract::CallMethod(std::string &Method,
}
void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
int CurrentAbsoluteIndex,
int IndexInitial,
double ModulatedSingleSupportTime,
int StepType, int LeftOrRight)
void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolutePosition> & , //SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> &, //NoneSupportFootAbsolutePositions,
int , //CurrentAbsoluteIndex,
int , //IndexInitial,
double , //ModulatedSingleSupportTime,
int , //StepType,
int ) //LeftOrRight)
{
LTHROW("FootTrajectoryGenerationAbstract::UpdateFootPosition-1: To be implemented ");
}
void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
int StartIndex, int k,
double LocalInterpolationStartTime,
double ModulatedSingleSupportTime,
int StepType, int LeftOrRight)
void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolutePosition> & ,//SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> & ,//NoneSupportFootAbsolutePositions,
int , // StartIndex,
int , //k,
double , //LocalInterpolationStartTime,
double , //ModulatedSingleSupportTime,
int , //StepType,
int ) //LeftOrRight)
{
LTHROW("FootTrajectoryGenerationAbstract::UpdateFootPosition-2: To be implemented ");
}
......@@ -181,7 +181,7 @@ bool FootTrajectoryGenerationMultiple::Compute(double t, FootAbsolutePosition &
int FootTrajectoryGenerationMultiple::SetNatureInterval(unsigned int IntervalIndex,
int Nature)
{
if ((IntervalIndex<0) || (IntervalIndex>=m_NatureOfIntervals.size()))
if (IntervalIndex>=m_NatureOfIntervals.size())
return -1;
m_NatureOfIntervals[IntervalIndex] = Nature;
return 0;
......@@ -192,7 +192,7 @@ int FootTrajectoryGenerationMultiple::SetNatureInterval(unsigned int IntervalInd
*/
int FootTrajectoryGenerationMultiple::GetNatureInterval(unsigned int IntervalIndex)
{
if ((IntervalIndex<0) || (IntervalIndex>=m_NatureOfIntervals.size()))
if (IntervalIndex>=m_NatureOfIntervals.size())
return -100;
return m_NatureOfIntervals[IntervalIndex];
......@@ -215,7 +215,7 @@ int FootTrajectoryGenerationMultiple::SetParametersWithInitPosInitSpeed(unsigned
double InitPosition,
double InitSpeed)
{
if ((IntervalIndex<0) || (IntervalIndex>=m_SetOfFootTrajectoryGenerationObjects.size()))
if (IntervalIndex>=m_SetOfFootTrajectoryGenerationObjects.size())
return -1;
......@@ -238,8 +238,8 @@ void FootTrajectoryGenerationMultiple::SetAbsoluteTimeReference(double lAbsolute
m_AbsoluteTimeReference = lAbsoluteTimeReference;
}
void FootTrajectoryGenerationMultiple::CallMethod(std::string &Method,
std::istringstream &strm)
void FootTrajectoryGenerationMultiple::CallMethod(std::string &, //Method,
std::istringstream & ) //strm)
{
}
......
......@@ -293,7 +293,8 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
int CurrentAbsoluteIndex,
int IndexInitial,
double ModulatedSingleSupportTime,
int StepType, int LeftOrRight)
int StepType,
int /* LeftOrRight */)
{
unsigned int k = CurrentAbsoluteIndex - IndexInitial;
// Local time
......@@ -470,7 +471,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
int StartIndex, int k,
double LocalInterpolationStartTime,
double ModulatedSingleSupportTime,
int StepType, int LeftOrRight)
int StepType, int /* LeftOrRight */)
{//TODO 0:Update foot position needs to be verified and cleaned
// unsigned int k = CurrentAbsoluteIndex - IndexInitial;
......
......@@ -45,9 +45,9 @@ CoMAndFootOnlyStrategy::~CoMAndFootOnlyStrategy()
{
}
int CoMAndFootOnlyStrategy::InitInterObjects(CjrlHumanoidDynamicRobot *aHDR,
int CoMAndFootOnlyStrategy::InitInterObjects(CjrlHumanoidDynamicRobot * /* aHDR */,
ComAndFootRealization * aCFR,
StepStackHandler * aSSH)
StepStackHandler * /* aSSH */)
{
m_ComAndFootRealization = aCFR;
return 0;
......@@ -55,11 +55,11 @@ int CoMAndFootOnlyStrategy::InitInterObjects(CjrlHumanoidDynamicRobot *aHDR,
int CoMAndFootOnlyStrategy::OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition,
MAL_VECTOR(,double) & ZMPRefPos,
MAL_VECTOR_TYPE(double) & ZMPRefPos,
COMState & finalCOMPosition,
MAL_VECTOR(,double) & CurrentConfiguration,
MAL_VECTOR(,double) & CurrentVelocity,
MAL_VECTOR(,double) & CurrentAcceleration)
MAL_VECTOR_TYPE(double) & ,//CurrentConfiguration,
MAL_VECTOR_TYPE(double) & ,//CurrentVelocity,
MAL_VECTOR_TYPE(double) & )//CurrentAcceleration)
{
ODEBUG("Begin OneGlobalStepOfControl "
<< m_LeftFootPositions->size() << " "
......@@ -184,14 +184,15 @@ int CoMAndFootOnlyStrategy::EndOfMotion()
return 0;
}
void CoMAndFootOnlyStrategy::Setup(deque<ZMPPosition> &aZMPPositions,
deque<COMState> &aCOMBuffer,
deque<FootAbsolutePosition> & aLeftFootAbsolutePositions,
deque<FootAbsolutePosition> & aRightFootAbsolutePositions)
void CoMAndFootOnlyStrategy::Setup(deque<ZMPPosition> &, // aZMPPositions,
deque<COMState> &, // aCOMBuffer,
deque<FootAbsolutePosition> &, // aLeftFootAbsolutePositions,
deque<FootAbsolutePosition> &) // aRightFootAbsolutePositions)
{
}
void CoMAndFootOnlyStrategy::CallMethod(std::string &Method, std::istringstream &astrm)
void CoMAndFootOnlyStrategy::CallMethod(std::string &,//Method,
std::istringstream &)// astrm)
{
}
......
......@@ -69,11 +69,11 @@ namespace PatternGeneratorJRL
*/
int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition,
MAL_VECTOR(,double) & ZMPRefPos,
MAL_VECTOR_TYPE(double) & ZMPRefPos,
COMState & finalCOMState,
MAL_VECTOR(,double) & CurrentConfiguration,
MAL_VECTOR(,double) & CurrentVelocity,
MAL_VECTOR(,double) & CurrentAcceleration);
MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration);
......
......@@ -83,11 +83,11 @@ int DoubleStagePreviewControlStrategy::InitInterObjects(CjrlHumanoidDynamicRobot
int DoubleStagePreviewControlStrategy::OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition,
MAL_VECTOR(,double) & ZMPRefPos,
MAL_VECTOR_TYPE(double) & ZMPRefPos,
COMState & finalCOMState,
MAL_VECTOR(,double) & CurrentConfiguration,
MAL_VECTOR(,double) & CurrentVelocity,
MAL_VECTOR(,double) & CurrentAcceleration)
MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration)
{
// New scheme:
// Update the queue of ZMP ref
......
......@@ -64,11 +64,11 @@ namespace PatternGeneratorJRL
*/
int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition,
MAL_VECTOR(,double) & ZMPRefPos,
MAL_VECTOR_TYPE(double) & ZMPRefPos,
COMState & COMState,
MAL_VECTOR(,double) & CurrentConfiguration,
MAL_VECTOR(,double) & CurrentVelocity,
MAL_VECTOR(,double) & CurrentAcceleration);
MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration);
......
......@@ -86,11 +86,11 @@ namespace PatternGeneratorJRL
*/
virtual int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition,
MAL_VECTOR(,double) & ZMPRefPos,
MAL_VECTOR_TYPE(double) & ZMPRefPos,
COMState & aCOMState,
MAL_VECTOR(,double) & CurrentConfiguration,
MAL_VECTOR(,double) & CurrentVelocity,
MAL_VECTOR(,double) & CurrentAcceleration)=0;
MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration)=0;
......@@ -175,8 +175,8 @@ namespace PatternGeneratorJRL
/*! \name Setter and getter for the jrlHumanoidDynamicRobot object. */
/*! @param[in] aHumanoidDynamicRobot: an object able to compute dynamic parameters
of the robot. */
inline bool setHumanoidDynamicRobot(const CjrlHumanoidDynamicRobot *aHumanoidDynamicRobot)
{ m_HumanoidDynamicRobot = (CjrlHumanoidDynamicRobot *)aHumanoidDynamicRobot;
inline bool setHumanoidDynamicRobot(CjrlHumanoidDynamicRobot *aHumanoidDynamicRobot)
{ m_HumanoidDynamicRobot = aHumanoidDynamicRobot;
return true;}
/*! \brief Returns the object able to compute the dynamic parameters of the robot. */
......
......@@ -39,7 +39,7 @@ namespace PatternGeneratorJRL
{
bool operator() (const CH_Point & s1, const CH_Point &s2)
{
float x1,x2,y1,y2;
double x1,x2,y1,y2;
x1 = s1.col - HRP2CIO_GlobalP0.col;
x2 = s2.col - HRP2CIO_GlobalP0.col;
y1 = s1.row - HRP2CIO_GlobalP0.row;
......@@ -50,9 +50,9 @@ namespace PatternGeneratorJRL
};
void DistanceCHRep(CH_Point & s1,CH_Point & s2,
float & distance1, float &distance2)
double & distance1, double &distance2)
{
float x1,x2,y1,y2;
double x1,x2,y1,y2;
x1 = s1.col - HRP2CIO_GlobalP0.col;
x2 = s2.col - HRP2CIO_GlobalP0.col;
y1 = s1.row - HRP2CIO_GlobalP0.row;
......@@ -63,9 +63,9 @@ namespace PatternGeneratorJRL
}
float CompareCBRep(CH_Point & s1,CH_Point &s2)
double CompareCBRep(CH_Point & s1,CH_Point &s2)
{
float x1,x2,y1,y2;
double x1,x2,y1,y2;
x1 = s1.col - HRP2CIO_GlobalP0.col;
x2 = s2.col - HRP2CIO_GlobalP0.col;
y1 = s1.row - HRP2CIO_GlobalP0.row;
......@@ -120,7 +120,7 @@ namespace PatternGeneratorJRL
CH_Point Current = *it_PtinPolarCoord;
if (CompareCBRep(Current,aVecOfPoints[i])==0.0)
{
float distance1, distance2;
double distance1, distance2;
DistanceCHRep(Current,aVecOfPoints[i],
distance1, distance2);
if (distance1<=distance2)
......@@ -164,7 +164,7 @@ namespace PatternGeneratorJRL
{
CH_Point pi = *it_LPPC;
unsigned char ok=0;
float x1,x2,y1,y2;
double x1,x2,y1,y2;
CH_Point s1;
CH_Point s2;
......
......@@ -38,7 +38,7 @@ namespace PatternGeneratorJRL
*/
typedef struct
{
float col,row; /* col: x, row : y */
double col,row; /* col: x, row : y */
} CH_Point;
typedef std::vector<CH_Point> ConvexHullList;
......
......@@ -298,9 +298,9 @@ int FootConstraintsAsLinearSystem::BuildLinearConstraintInequalities(deque<FootA
int State=0; // State for the system 0:start, 1: Right Support Foot, 2: Left Support Foot,
// 3: Double Support.
int ComputeCH=0;
float lx=0.0, ly=0.0;
float lxcoefs[4] = { 1.0, 1.0, -1.0, -1.0};
float lycoefs[4] = {-1.0, 1.0, 1.0, -1.0};
double lx=0.0, ly=0.0;
double lxcoefs[4] = { 1.0, 1.0, -1.0, -1.0};
double lycoefs[4] = {-1.0, 1.0, 1.0, -1.0};
double prev_xmin=1e7, prev_xmax=-1e7, prev_ymin=1e7, prev_ymax=-1e7;
RESETDEBUG4("ConstraintMax.dat");
......@@ -538,7 +538,8 @@ int FootConstraintsAsLinearSystem::BuildLinearConstraintInequalities(deque<FootA
return 0;
}
void FootConstraintsAsLinearSystem::CallMethod(std::string &Method, std::istringstream &Args)
void FootConstraintsAsLinearSystem::CallMethod(std::string & ,//Method,
std::istringstream & )//Args)
{
// TO BE EXTENDED.
}
......@@ -60,6 +60,7 @@ FootConstraintsAsLinearSystemForVelRef(SimplePluginManager *aSPM,
m_RightFootSize.setHalfSizeInit(lHalfWidthInit,lHalfHeightInit);
m_RightFootSize.setConstraints(ConstraintOnX,ConstraintOnY);
vector3d AnklePosition;
m_RightFoot->getAnklePositionInLocalFrame(AnklePosition);
m_Z = AnklePosition[2];
m_LeftFoot = m_HS->leftFoot();
......@@ -72,15 +73,17 @@ FootConstraintsAsLinearSystemForVelRef(SimplePluginManager *aSPM,
m_LeftFootSize.setHalfSizeInit(lHalfWidthInit,lHalfHeightInit);
m_LeftFootSize.setConstraints(ConstraintOnX,ConstraintOnY);
DSFeetDistance = 0.2;
m_DSFeetDistance = 0.2;
ConvexHullFP.resize(5);
m_ConvexHullFP.resize(5);
//initFPConstrArrays();
//TODO 0: find another condition
if(0)
RESETDEBUG4("Constraints-fCSALS.dat");
{
RESETDEBUG4("Constraints-fCSALS.dat");
}
m_FullDebug = 0;
......@@ -223,20 +226,23 @@ int FootConstraintsAsLinearSystemForVelRef::buildLinearConstraintInequalities(de
//For symmetrical constraints: The points of the left foot are counted clockwise.
//The
float lxcoefsRight[4] = { 1.0, 1.0, -1.0, -1.0};
float lycoefsRight[4] = {-1.0, 1.0, 1.0, -1.0};
float lxcoefsLeft[4] = { 1.0, 1.0, -1.0, -1.0};
float lycoefsLeft[4] = { 1.0, -1.0, -1.0, 1.0};
double lxcoefsRight[4] = { 1.0, 1.0, -1.0, -1.0};
double lycoefsRight[4] = {-1.0, 1.0, 1.0, -1.0};
double lxcoefsLeft[4] = { 1.0, 1.0, -1.0, -1.0};
double lycoefsLeft[4] = { 1.0, -1.0, -1.0, 1.0};
float *lxcoefs, *lycoefs;
double *lxcoefs, *lycoefs;
float CHLeftFPosConstrArrayX[5] = {-0.28, -0.2, 0.0, 0.2, 0.28};
float CHLeftFPosConstrArrayY[5] = {0.2, 0.3, 0.4, 0.3, 0.2};
float CHRightFPosConstrArrayX[5] = {-0.28, -0.2, 0.0, 0.2, 0.28};
float CHRightFPosConstrArrayY[5] = {-0.2, -0.3, -0.4, -0.3, -0.2};
double CHLeftFPosConstrArrayX[5] = {-0.28, -0.2, 0.0, 0.2, 0.28};
double CHLeftFPosConstrArrayY[5] = {0.2, 0.3, 0.4, 0.3, 0.2};
double CHRightFPosConstrArrayX[5] = {-0.28, -0.2, 0.0, 0.2, 0.28};
double CHRightFPosConstrArrayY[5] = {-0.2, -0.3, -0.4, -0.3, -0.2};
vector<CH_Point> TheConvexHull;
double s_t=0,c_t=0;
double lx = 0.0,ly =0.0;
//determine the current support angle
deque<FootAbsolutePosition>::iterator FAP_it;
......@@ -302,7 +308,7 @@ int FootConstraintsAsLinearSystemForVelRef::buildLinearConstraintInequalities(de
{
//TODO: theta = 0
lx = 0.0;
ly = -(double)PrwSupport.Foot*DSFeetDistance/2.0;
ly = -(double)PrwSupport.Foot*m_DSFeetDistance/2.0;
if(PrwSupport.Foot == 1)
{
......@@ -400,25 +406,25 @@ int FootConstraintsAsLinearSystemForVelRef::buildLinearConstraintInequalities(de
}
if(PrwSupport.Foot == 1)
{
CHFPosConstrArrayX = CHLeftFPosConstrArrayX;
CHFPosConstrArrayY = CHLeftFPosConstrArrayY;
m_CHFPosConstrArrayX = CHLeftFPosConstrArrayX;
m_CHFPosConstrArrayY = CHLeftFPosConstrArrayY;
}
else
{
CHFPosConstrArrayX = CHRightFPosConstrArrayX;
CHFPosConstrArrayY = CHRightFPosConstrArrayY;
m_CHFPosConstrArrayX = CHRightFPosConstrArrayX;
m_CHFPosConstrArrayY = CHRightFPosConstrArrayY;
}
//TODO: The interior border does not yet depend on the angle
for(unsigned j=0;j<5;j++)
{
ConvexHullFP[j].col = lx + ( CHFPosConstrArrayX[j] * c_t - CHFPosConstrArrayY[j] * s_t );
ConvexHullFP[j].row = ly + ( CHFPosConstrArrayX[j] * s_t + CHFPosConstrArrayY[j] * c_t );
m_ConvexHullFP[j].col = lx + ( m_CHFPosConstrArrayX[j] * c_t - m_CHFPosConstrArrayY[j] * s_t );
m_ConvexHullFP[j].row = ly + ( m_CHFPosConstrArrayX[j] * s_t + m_CHFPosConstrArrayY[j] * c_t );
}
LinearConstraintInequalityFreeFeet_t aLCIFP;
computeLinearSystem(ConvexHullFP, aLCIFP.D, aLCIFP.Dc, PrwSupport);
computeLinearSystem(m_ConvexHullFP, aLCIFP.D, aLCIFP.Dc, PrwSupport);
aLCIFP.StepNumber = PrwSupport.StepNumber;
......@@ -455,7 +461,7 @@ int FootConstraintsAsLinearSystemForVelRef::buildLinearConstraintInequalities(de
{
LCIFF_it = QueueOfFeetPosInequalities.begin();
IndexConstraint += double(PrwSupport.StepNumber)*MAL_MATRIX_NB_ROWS(LCIFF_it->D);
IndexConstraint += PrwSupport.StepNumber*MAL_MATRIX_NB_ROWS(LCIFF_it->D);
}
NbOfConstraints = IndexConstraint;
......
......@@ -130,31 +130,30 @@ namespace PatternGeneratorJRL
/* ! Reference on the Humanoid Specificities. */
CjrlHumanoidDynamicRobot * m_HS;
std::vector<CH_Point> ConvexHullFP;
std::vector<CH_Point> m_ConvexHullFP;
float* CHFPosConstrArrayX;
float* CHFPosConstrArrayY;
double* m_CHFPosConstrArrayX;
double* m_CHFPosConstrArrayY;
// Find the convex hull for each of the position,
// in order to create the corresponding trajectory.
FootHalfSize m_LeftFootSize, m_RightFootSize;
double m_Z;
double DSFeetDistance;
double m_DSFeetDistance;
// Read humanoid specificities.
CjrlFoot * m_RightFoot;
vector3d AnklePosition;
CjrlFoot * m_LeftFoot;
int State; // State for the system 0:start, 1: Right Support Foot, 2: Left Support Foot,
// 3: Double Support.
int ComputeCH;
float lx, ly;
double m_x, m_y;
double prev_xmin, prev_xmax, prev_ymin, prev_ymax;
// double m_prev_xmin, m_prev_xmax, m_prev_ymin, m_prev_ymax;
double s_t,c_t;
// double s_t,c_t;
unsigned int m_FullDebug;
......
......@@ -139,7 +139,7 @@ void PLDPSolverHerdt::AllocateMemoryForSolver(unsigned int NumberSteps)
m_OptCholesky->SetiL(m_iL);
}
void PLDPSolverHerdt::InitializeSolver(unsigned int NumberSteps)
void PLDPSolverHerdt::InitializeSolver(unsigned int /* NumberSteps*/ )
{
#if 0
// Allocation max:
......@@ -815,10 +815,9 @@ int PLDPSolverHerdt::SolveProblem(deque<LinearConstraintInequalityFreeFeet_t> &
{
IndexFootCstr = m_PreviouslyActivatedConstraints[i]-NbRemovedFootCstr;
}
if (lindex>=0)
{
m_ActivatedConstraints.push_back(lindex);
}
m_ActivatedConstraints.push_back(lindex);
if(IndexFootCstr>=4*m_CardV)
{
m_ActivatedConstraints.push_back(IndexFootCstr);
......@@ -1081,7 +1080,7 @@ void PLDPSolverHerdt::WriteCurrentZMPSolution(string filename,
double* lZMP = new double [lZMP_size];
ofstream aof;
aof.open((char *)filename.c_str(),ofstream::out);
aof.open(filename.c_str(),ofstream::out);
for(unsigned int i=0;i<m_CardV;i++)
......
......@@ -1046,7 +1046,7 @@ void PLDPSolver::WriteCurrentZMPSolution(string filename,
double* lZMP = new double [lZMP_size];
ofstream aof;
aof.open((char *)filename.c_str(),ofstream::out);
aof.open(filename.c_str(),ofstream::out);
for(unsigned int i=0;i<m_CardV;i++)
......