Skip to content
Snippets Groups Projects
Commit 30d0a416 authored by Andrei Herdt's avatar Andrei Herdt Committed by Olivier Stasse
Browse files

Changes due to changed type names (functionality untouched)

parent 4440e734
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 44 additions and 46 deletions
src/Mathematics/FootConstraintsAsLinearSystemForVelRef.cpp
+ 3
− 3
View file @ 30d0a416
......@@ -162,7 +162,7 @@ int FootConstraintsAsLinearSystemForVelRef::FindSimilarConstraints(MAL_MATRIX(&A
int FootConstraintsAsLinearSystemForVelRef::computeLinearSystem(vector<CH_Point> aVecOfPoints,
MAL_MATRIX(&D,double),
MAL_MATRIX(&Dc,double),
SupportState_t PrwSupport)
support_state_t PrwSupport)
{
double dx,dy,dc,x1,y1,x2,y2;
......@@ -228,9 +228,9 @@ int FootConstraintsAsLinearSystemForVelRef::buildLinearConstraintInequalities(de
QueueOfLConstraintInequalitiesFreeFeet,
deque<LinearConstraintInequalityFreeFeet_t> &
QueueOfFeetPosInequalities,
ReferenceAbsoluteVelocity & RefVel,
reference_t & RefVel,
double StartingTime, double m_QP_N,
SupportFSM * SupportFSM, SupportState_t CurrentSupport, SupportState_t & PrwSupport,
SupportFSM * SupportFSM, support_state_t CurrentSupport, support_state_t & PrwSupport,
deque<double> &PreviewedSupportAngles,
int & NbOfConstraints)
{
......
src/Mathematics/FootConstraintsAsLinearSystemForVelRef.h
+ 3
− 3
View file @ 30d0a416
......@@ -87,7 +87,7 @@ namespace PatternGeneratorJRL
int computeLinearSystem(std::vector<CH_Point> aVecOfPoints,
MAL_MATRIX(&D,double),
MAL_MATRIX(&Dc,double),
SupportState_t PrwSupport);
support_state_t PrwSupport);
/*! Build a queue of constraint Inequalities based on a list of Foot Absolute
Position.
......@@ -98,10 +98,10 @@ namespace PatternGeneratorJRL
QueueOfLConstraintInequalities,
std::deque<LinearConstraintInequalityFreeFeet_t> &
QueueOfFeetPosInequalities,
ReferenceAbsoluteVelocity & RefVel,
reference_t & RefVel,
double StartingTime,
double m_QP_N,
SupportFSM * SupportFSM, SupportState_t CurrentSupport, SupportState_t & PrwSupport,
SupportFSM * SupportFSM, support_state_t CurrentSupport, support_state_t & PrwSupport,
std::deque<double> &PreviewedSupportAngles,
int & NbOfConstraints);
......
src/Mathematics/PLDPHerdt.cpp
+ 3
− 3
View file @ 30d0a416
......@@ -293,7 +293,7 @@ int PLDPSolverHerdt::PrecomputeiPuPx(unsigned int NumberSteps)
int PLDPSolverHerdt::ComputeInitialSolution(deque<LinearConstraintInequalityFreeFeet_t> & QueueOfLConstraintInequalitiesFreeFeet,
deque<SupportFeet_t> & QueueOfSupportFeet, unsigned int NumberSteps,
deque<supportfoot_t> & QueueOfSupportFeet, unsigned int NumberSteps,
double *XkYk)
{
/*! The initial solution of the problem is given by identical zmp and feet positions that are lying inside the feasible zones defined by the constraints on the feet placements.
......@@ -304,7 +304,7 @@ int PLDPSolverHerdt::ComputeInitialSolution(deque<LinearConstraintInequalityFree
LCIFF_it = QueueOfLConstraintInequalitiesFreeFeet.begin();
//Current support foot coordinates
deque<SupportFeet_t>::iterator CurSF_it;
deque<supportfoot_t>::iterator CurSF_it;
CurSF_it = QueueOfSupportFeet.end();
CurSF_it--;
......@@ -691,7 +691,7 @@ double PLDPSolverHerdt::ComputeAlpha(vector<unsigned int> & NewActivatedConstrai
return Alpha;
}
int PLDPSolverHerdt::SolveProblem(deque<LinearConstraintInequalityFreeFeet_t> & QueueOfLConstraintInequalitiesFreeFeet,
deque<SupportFeet_t> & QueueOfSupportFeet,
deque<supportfoot_t> & QueueOfSupportFeet,
double *CstPartOfTheCostFunction,
unsigned int NbOfConstraints,
double *LinearPartOfConstraints,
......
src/Mathematics/PLDPSolverHerdt.h
+ 3
− 2
View file @ 30d0a416
......@@ -35,6 +35,7 @@
#include <deque>
#include <Mathematics/OptCholesky.h>
#include <jrl/walkgen/pgtypes.hh>
#include <privatepgtypes.h>
namespace Optimization
{
......@@ -58,7 +59,7 @@ namespace Optimization
/*! \brief Solve the optimization problem
*/
int SolveProblem(std::deque<PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_t> & QueueOfLConstraintInequalitiesFreeFeet,
std::deque<PatternGeneratorJRL::SupportFeet_t> & QueueOfSupportFeet,
std::deque<PatternGeneratorJRL::supportfoot_t> & QueueOfSupportFeet,
double *CstPartOfTheCostFunction,
unsigned int NbOfConstraints,
double *LinearPartOfConstraints,
......@@ -74,7 +75,7 @@ namespace Optimization
*/
/*! Compute the initial solution */
int ComputeInitialSolution(std::deque<PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_t> & QueueOfLConstraintInequalitiesFreeFeet,
std::deque<PatternGeneratorJRL::SupportFeet_t> & QueueOfSupportFeet, unsigned int NumberSteps,
std::deque<PatternGeneratorJRL::supportfoot_t> & QueueOfSupportFeet, unsigned int NumberSteps,
double *XkYk);
/*! Precompite iPuPx */
......
src/PreviewControl/SupportFSM.cpp
+ 16
− 16
View file @ 30d0a416
......@@ -45,35 +45,35 @@ SupportFSM::SupportFSM(const double &SamplingPeriod)
m_T = SamplingPeriod;
m_eps = 0.00000001;
m_FullDebug = 0;
}
SupportFSM::~SupportFSM()
{
}
void SupportFSM::setSupportState(const double &Time, const int &pi,
SupportState_t & Support, const ReferenceAbsoluteVelocity & RefVel)
support_state_t & Support, const reference_t & vel_ref)
{
double eps = 1e-6;
Support.StateChanged = false;
m_ReferenceGiven = false;
if(fabs(RefVel.x)>0||fabs(RefVel.y)>0)
m_ReferenceGiven = true;
if(m_ReferenceGiven == true && Support.Phase == 0 && (Support.TimeLimit-Time-m_eps)>m_DSSSDuration)
if(fabs(vel_ref.local.x)>eps||fabs(vel_ref.local.y)>eps||fabs(vel_ref.local.yaw)>eps)
m_ReferenceGiven = true;
if(m_ReferenceGiven == true && Support.Phase == 0 && (Support.TimeLimit-Time-eps)>m_DSSSDuration)
{
Support.TimeLimit = Time+m_DSSSDuration;
}
//FSM
if(Time+m_eps+pi*m_T >= Support.TimeLimit)
if(Time+eps+pi*m_T >= Support.TimeLimit)
{
//SS->DS
if(Support.Phase == 1 && m_ReferenceGiven == false && Support.StepsLeft==0)
......@@ -103,15 +103,15 @@ void SupportFSM::setSupportState(const double &Time, const int &pi,
Support.StepsLeft = Support.StepsLeft-1;
}
}
if(m_FullDebug>0)
{
ofstream aof;
aof.open("SupportStates.dat", ios::app);
aof << "Time: "<<Time<<" PrwTime: "<<Time+pi*m_T
<<" CSF: "<<Support.Foot<<" CTL: "<<Support.TimeLimit
<<" SL: "<<Support.StepsLeft<<" *SF: "<<Support.Foot
<<" SN: "<<Support.StepNumber;
<<" CSF: "<<Support.Foot<<" CTL: "<<Support.TimeLimit
<<" SL: "<<Support.StepsLeft<<" *SF: "<<Support.Foot
<<" SN: "<<Support.StepNumber;
aof << endl;
aof.close();
}
......
src/PreviewControl/SupportFSM.h
+ 2
− 5
View file @ 30d0a416
......@@ -43,10 +43,7 @@ namespace PatternGeneratorJRL
/*! \brief Initialize the previewed state. */
void setSupportState(const double &Time, const int &pi,
SupportState_t & Support, const ReferenceAbsoluteVelocity & RefVel);
///*! \brief Numerical precision */
double m_eps;
support_state_t & Support, const reference_t & Ref);
/*! \brief constants for the durations in the support phases */
double m_DSDuration, m_SSPeriod, m_DSSSDuration;
......@@ -55,7 +52,7 @@ namespace PatternGeneratorJRL
unsigned int m_NbOfStepsSSDS;
private:
/*! \Brief Sampling duration */
double m_T;
......
src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp
+ 9
− 9
View file @ 30d0a416
......@@ -84,7 +84,7 @@ void OrientationsPreview::previewOrientations(const double &Time,
deque<double> &PreviewedSupportAngles,
const COMState &TrunkState,
COMState &TrunkStateT,
const SupportFSM * SupportFSM ,SupportState_t CurrentSupport,
const SupportFSM * SupportFSM ,support_state_t CurrentSupport,
deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
deque<FootAbsolutePosition> &RightFootAbsolutePositions)
{
......@@ -315,17 +315,17 @@ void OrientationsPreview::previewOrientations(const double &Time,
}
}
void OrientationsPreview::verifyAccelerationOfHipJoint(const ReferenceAbsoluteVelocity_t &Ref,
void OrientationsPreview::verifyAccelerationOfHipJoint(const reference_t &Ref,
const COMState &TrunkState,
COMState &TrunkStateT,
SupportState_t CurrentSupport)
support_state_t CurrentSupport)
{
if(CurrentSupport.Phase!=0)
{
//Verify change in velocity against the maximal acceleration
if(fabs(Ref.yaw-TrunkState.yaw[1]) > 2.0/3.0*m_T*m_uaLimitHipYaw)
if(fabs(Ref.local.yaw-TrunkState.yaw[1]) > 2.0/3.0*m_T*m_uaLimitHipYaw)
{
double signRotAccTrunk = (Ref.yaw-TrunkState.yaw[1] < 0.0)?-1.0:1.0;
double signRotAccTrunk = (Ref.local.yaw-TrunkState.yaw[1] < 0.0)?-1.0:1.0;
TrunkStateT.yaw[1] = TrunkState.yaw[1] + signRotAccTrunk * 2.0/3.0*m_T* m_uaLimitHipYaw;
......@@ -334,7 +334,7 @@ void OrientationsPreview::verifyAccelerationOfHipJoint(const ReferenceAbsoluteVe
ofstream aof;
aof.open("/tmp/verifyAccelerationOfHipJoint.dat",ofstream::app);
aof<<" TrunkStateT.yaw[1]: "<<TrunkStateT.yaw[1] <<" "
<<" Ref.yaw: "<<Ref.yaw <<" "
<<" Ref.local.yaw: "<<Ref.local.yaw <<" "
<<" m_signRotAccTrunk: "<<m_signRotAccTrunk <<" "
<<" 2.0/3.0*m_T* m_uaLimitHipYaw: "<<2.0/3.0*m_T* m_uaLimitHipYaw <<" "
<<endl;
......@@ -342,7 +342,7 @@ void OrientationsPreview::verifyAccelerationOfHipJoint(const ReferenceAbsoluteVe
}
}
else
TrunkStateT.yaw[1] = Ref.yaw;
TrunkStateT.yaw[1] = Ref.local.yaw;
}
else//No rotations in a double support phase
......@@ -352,7 +352,7 @@ void OrientationsPreview::verifyAccelerationOfHipJoint(const ReferenceAbsoluteVe
}
bool OrientationsPreview::verifyAngleOfHipJoint(SupportState_t CurrentSupport,
bool OrientationsPreview::verifyAngleOfHipJoint(support_state_t CurrentSupport,
const COMState &TrunkState, COMState &TrunkStateT,
double CurrentSupportFootAngle,
unsigned int StepNumber)
......@@ -398,7 +398,7 @@ void OrientationsPreview::verifyVelocityOfHipJoint(const double &Time,
COMState &,
const double &PreviewedSupportFoot,
const unsigned int &StepNumber,
SupportState_t CurrentSupport,
support_state_t CurrentSupport,
const double &CurrentRightFootAngle,
const double &CurrentLeftFootAngle,
const double &CurrentLeftFootVelocity,
......
src/ZMPRefTrajectoryGeneration/OrientationsPreview.h
+ 5
− 5
View file @ 30d0a416
......@@ -55,13 +55,13 @@ namespace PatternGeneratorJRL
void previewOrientations(const double &Time,
std::deque<double> &PreviewedSupportAngles,
const COMState &TrunkState, COMState &TrunkStateT,
const SupportFSM * SupportFSM, SupportState_t CurrentSupport,
const SupportFSM * SupportFSM, support_state_t CurrentSupport,
std::deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
std::deque<FootAbsolutePosition> &RightFootAbsolutePositions);
void verifyAccelerationOfHipJoint(const ReferenceAbsoluteVelocity_t &Ref,
void verifyAccelerationOfHipJoint(const reference_t &Ref,
const COMState &TrunkState, COMState &TrunkStateT,
SupportState_t CurrentSupport);
support_state_t CurrentSupport);
private:
/*! Angular limitations of the hip joints*/
......@@ -122,14 +122,14 @@ namespace PatternGeneratorJRL
unsigned int m_FullDebug;
bool verifyAngleOfHipJoint(SupportState_t CurrentSupport,
bool verifyAngleOfHipJoint(support_state_t CurrentSupport,
const COMState &TrunkState, COMState &TrunkStateT,
double CurrentSupportFootAngle,
unsigned int StepNumber);
void verifyVelocityOfHipJoint(const double &Time, COMState &TrunkStateT,
const double &PreviewedSupportFoot, const unsigned int &StepNumber,
SupportState_t CurrentSupport,
support_state_t CurrentSupport,
const double &CurrentRightFootAngle, const double &CurrentLeftFootAngle,
const double &CurrentLeftFootVelocity,
const double &CurrentRightFootVelocity);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment