diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 1ab2976450388a9ba65d09a0bbfce3fb1882fb12..b8f148332220dd9c61de3b25ab6ad07896581815 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -407,22 +407,8 @@ ZMPVelocityReferencedQP::OnLine(double Time,
// --------------
QPProblem_s::solution_t Result;
Problem_.solve( QPProblem_s::QLD , Result );
-
-
- char FileName[1024];
- sprintf(FileName,"/tmp/Problem%f.dat", Time);
- Problem_.dump_problem(FileName);
- cout<<"time:"<<Time<<endl;
-
Problem_.reset();
- for(int i=0;i<PrwSupportStates_deq.back().StepNumber;i++)
- {
- cout<<"X"<<i<<":"<<Result.Solution_vec(2*QP_N_+i)<<endl;
- cout<<"Y"<<i<<":"<<Result.Solution_vec(2*QP_N_+PrwSupportStates_deq.back().StepNumber+i)<<endl;
- cout<<"LMult."<<Result.ConstrLagr_vec<<endl;
- }
-
// INTERPOLATE THE NEXT COMPUTED COM STATE:
// ----------------------------------------
diff --git a/src/ZMPRefTrajectoryGeneration/door.cpp b/src/ZMPRefTrajectoryGeneration/door.cpp
index 6c44cf474294f66c150318fe727013b3241b431b..ddee95e7cb8e05b2f7c8a48e65a0b69c9ee4c84b 100644
--- a/src/ZMPRefTrajectoryGeneration/door.cpp
+++ b/src/ZMPRefTrajectoryGeneration/door.cpp
@@ -18,67 +18,51 @@ void
Door::initialize( double Orientation)
{
- FrameBase_.Orientation_ = Orientation;
+ FrameBase_.Orientation = Orientation;
- FrameBase_.InvRx_.resize(2,false);
- FrameBase_.InvRx_(0) = cos(Orientation);
- FrameBase_.InvRx_(1) = -sin(Orientation);
- FrameBase_.InvRy_.resize(2,false);
- FrameBase_.InvRy_(0) = sin(Orientation);
- FrameBase_.InvRy_(1) = cos(Orientation);
+ FrameBase_.InvRx.resize(2,false);
+ FrameBase_.InvRx(0) = cos(Orientation);
+ FrameBase_.InvRx(1) = -sin(Orientation);
+ FrameBase_.InvRy.resize(2,false);
+ FrameBase_.InvRy(0) = sin(Orientation);
+ FrameBase_.InvRy(1) = cos(Orientation);
- double xp = FrameBase_.Position(X);
- double yp = FrameBase_.Position(Y);
- double zp = FrameBase_.Position(Z);
- FrameBase_.InvHp_ = -FrameBase_.InvRx_*xp-FrameBase_.InvRy_*yp;
+ double xp = FrameBase_.Position[X];
+ double yp = FrameBase_.Position[Y];
-}
+ FrameBase_.InvHp = -FrameBase_.InvRx*xp-FrameBase_.InvRy*yp;
+}
void
-Door::build_door_matrix(double Time, int N, boost_ublas::matrix<double> & R, double RelativAngle, double & Xdoortip, double & Ydoortip )
+Door::build_rotation_matrices(double Time, int N, boost_ublas::matrix<double> & R )
{
-// double angle;
-// angle = DesVelDoor*Time;
-// cout<<"angle door"<< angle << endl;
- double DesVelDoor = 0.0;
- if(Time > 8.0)
- DesVelDoor = M_PI/300.0;
-
- double CurrentDoorAngle = FrameDoor_.Orientation();
- double Theta;
- for(unsigned int j=0;j<N;j++)
- {
- Theta = DesVelDoor*(j+1)*SamplingTime_ + CurrentDoorAngle;
- cout<<"Theta:"<<Theta<<endl;
- R(0,j) = sin(Theta);
- R(1,j) = -cos(Theta);
- }
-
- FrameDoor_.Orientation(CurrentDoorAngle+DesVelDoor*SamplingTime_);
-// Rrot(0) = sin(DesVelDoor*(Time)+ RelativAngle);
-// Rrot(1) = -cos(DesVelDoor*(Time)+ RelativAngle);
-// // L largeur de la porte
-
- double L = 1.0;
- // door tip position in global frame
- Xdoortip = L*sin(DesVelDoor*(Time))*cos(FrameBase_.Orientation_)- L*cos( DesVelDoor*(Time))*sin(FrameBase_.Orientation_)+FrameBase_.Position(X);
- Ydoortip = L*sin(DesVelDoor*(Time))*sin(FrameBase_.Orientation_)+ L*cos( DesVelDoor*(Time))*cos(FrameBase_.Orientation_)-FrameBase_.Position(Y);
- cout<<"Xdoortip"<<Xdoortip<< endl;
- cout<<"Ydoortip"<<Ydoortip<< endl;
+ double DesVelDoor = 0.0;
+ if(Time > 8.0)
+ DesVelDoor = M_PI/300.0;
+
+ double CurrentDoorAngle = FrameDoor_.Orientation;
+ double Theta;
+ for(int j=0;j<N;j++)
+ {
+ Theta = DesVelDoor*(j+1)*SamplingTime_ + CurrentDoorAngle;
+ R(0,j) = sin(Theta);
+ R(1,j) = -cos(Theta);
+ }
+ FrameDoor_.Orientation = CurrentDoorAngle+DesVelDoor*SamplingTime_;
}
Door::frame_s::frame_s()
{
- Orientation_ = 0.0;//-M_PI/2.0;
- dOrientation_ = 0.0;
- ddOrientation_ = 0.0;
- Position_[X] = 0.6;
- Position_[Y] = 0.5;
- Position_[Z] = 0.0;
+ Orientation = 0.0;
+ dOrientation = 0.0;
+ ddOrientation = 0.0;
+ Position[X] = 0.6;
+ Position[Y] = 0.5;
+ Position[Z] = 0.0;
}
diff --git a/src/ZMPRefTrajectoryGeneration/door.hh b/src/ZMPRefTrajectoryGeneration/door.hh
index c59dda1d670732cc9880d56d338aa1d821fa1d2b..4dc546f7ab780a2e2285412c410e407525f971ee 100644
--- a/src/ZMPRefTrajectoryGeneration/door.hh
+++ b/src/ZMPRefTrajectoryGeneration/door.hh
@@ -14,145 +14,81 @@
class Door
{
- //
- // Public types:
- //
+ //
+ // Public types:
+ //
public:
- const static unsigned int X = 0;
- const static unsigned int Y = 1;
- const static unsigned int Z = 2;
+ const static unsigned int X = 0;
+ const static unsigned int Y = 1;
+ const static unsigned int Z = 2;
- //
- // Public methods:
- //
+ /// \brief Frame
+ struct frame_s
+ {
+
+ frame_s();
+
+ /// \brief Orientation around the vertical axis
+ double Orientation;
+ // \brief Rotational velocity around the vertical axis
+ double dOrientation;
+ // \brief Rotational acceleration around the vertical axis
+ double ddOrientation;
+
+ /// \brief Position of the origin
+ double Position[3];
+
+ /// \brief x rotation vector of the inv. transformation matrix InvH
+ boost_ublas::vector<double> InvRx;
+ /// \brief y rotation vector of the inv. transformation matrix InvH
+ boost_ublas::vector<double> InvRy;
+ /// \brief Translation component vector of InvH matrix
+ boost_ublas::vector<double> InvHp;
+
+ };
+ typedef frame_s frame_t;
+
+
+ //
+ // Public methods:
+ //
public:
- Door(); //constructeur
-
- void initialize( double Orientation);
-
-
- ///
- void build_door_matrix(double Time, int N, boost_ublas::matrix<double> & R, double RelativAngle, double & Xdoortip, double & Ydoortip);
- /// void doormatrices_construct();
-
- struct frame_s
- {
-
- //
- // Public methods
- //
- public:
-
- frame_s();// constructeur de la structure
-
- inline double const & Position(unsigned int Axis) const
- { return Position_[Axis]; };
- inline void Position( unsigned int Axis, double Position )
- { Position_[Axis] = Position; };
-
- inline double const Orientation() const
- { return Orientation_; };
- inline void Orientation( const double Orientation )
- { Orientation_ = Orientation; };
-
- inline double const dOrientation() const
- { return dOrientation_; };
- inline void dOrientation( const double dOrientation )
- { dOrientation_ = dOrientation; };
-
- inline double const ddOrientation() const
- { return ddOrientation_; };
- inline void ddOrientation( const double ddOrientation )
- { ddOrientation_ = ddOrientation; };
-
- //TODO:
- // inline boost_ublas::vector<double> const Position() const
- //{ return Postion_[3]; };
- //inline void Postion( const boost_ublas::vector<double> Postion_[3] )
- //{ Postion_[3] = Postion_[3]; };
-//
-// inline boost_ublas::matrix<double> const H() const
-// { return H; };
-// inline void H( const boost_ublas::matrix<double> H )
-// { H = H_; };
-//
-// inline boost_ublas::matrix<double> const Hinv() const
-// { return Hinv; };
-// inline void Hinv( const boost_ublas::matrix<double> Hinv )
-// { Hinv = Hinv; };
-
- /// \brief Orientation around the vertical axis
- double Orientation_;
- // \brief Rotational velocity around the vertical axis
- double dOrientation_;
- // \brief Rotational acceleration around the vertical axis
- double ddOrientation_;
-
- /// \brief Position of the origin
- double Position_[3];
-// /// \brief transformation matrix from
-// boost_ublas::matrix<double> H_;
-// /// \brief Inverse of H
-// boost_ublas::matrix<double> Hinv_;
-
- /// \brief x rotation vector of the inv. transformation matrix InvH
- boost_ublas::vector<double> InvRx_;
- /// \brief y rotation vector of the inv. transformation matrix InvH
- boost_ublas::vector<double> InvRy_;
- /// \brief Translation component vector of InvH matrix
- boost_ublas::vector<double> InvHp_;
-
- };
- typedef frame_s frame_t;
-
- /// Definition des matrices correspondant à la porte
-
-
- boost_ublas::vector<double> & InvRx()
- { return FrameBase_.InvRx_; };
- boost_ublas::vector<double> & InvRy()
- { return FrameBase_.InvRy_; };
- boost_ublas::vector<double> & InvHp()
- { return FrameBase_.InvHp_; };
-
-
- inline void SamplingTime( const double SamplingTime )
- { SamplingTime_ = SamplingTime; }
-
- /// \name Matrices defining the evolution
- /// \{
-// inline double const Mass() const
-// { return Mass_; };
-// inline void Mass( const double Mass )
-// { Mass_ = Mass; };
-//
-// inline double const Inertia() const
-// { return Inertia_; };
-// inline void Inertia( const double Mass )
-// { Inertia_ = Inertia; };
- /// \}
-
-
- //
- // Private members:
- //
-private:
+ Door();
+
+ /// \brief
+ void initialize( double Orientation );
+
+ /// \brief Build rotation matrices for the whole preview period
+ void build_rotation_matrices( double Time, int N, boost_ublas::matrix<double> & R );
- /// Frame of the door
- frame_t FrameBase_;
- /// Frame of the door
- frame_t FrameDoor_;
-// /// Mass
-// double Mass_;
-// /// Inertia
-// double Inertia_;
- /// Wrist
- /// TODO: Wrist undefined
- frame_t FrameWrist_;
+ /// \name Accessors
+ /// \{
+ inline frame_t & FrameBase()
+ { return FrameBase_; }
+ inline frame_t & FrameDoor()
+ { return FrameDoor_; }
+ inline frame_t & FrameWrist()
+ { return FrameWrist_; }
+ inline void SamplingTime( const double SamplingTime )
+ { SamplingTime_ = SamplingTime; }
+ /// \}
+
+
+ //
+ // Private members:
+ //
+private:
- double SamplingTime_;
+ /// \brief Frame of the door
+ frame_t FrameBase_;
+ /// \brief Frame of the door
+ frame_t FrameDoor_;
+ /// \brief Frame of the wrist
+ frame_t FrameWrist_;
+ /// \brief Extern sampling time
+ double SamplingTime_;
};
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index 579e69ce847dcc2f406558bb5c3236202c28e84d..335fe75562daff8942fe4b0264da32b13ef34d15 100755
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -397,387 +397,285 @@ GeneratorVelRef::build_inequalities_feet(linear_inequality_t & Inequalities,
void
-GeneratorVelRef::build_constraints_door(double Time,
- Door & Door, const std::deque<support_state_t> & SupportStates_deq, QPProblem & Pb)
+GeneratorVelRef::build_constraints_door(double Time, Door & Door,
+ const std::deque<support_state_t> & SupportStates_deq, QPProblem & Pb)
{
-// // Build rotation matrix Rrot
-// boost_ublas::vector<double> Rrot(2,false);
-// Rrot.clear();
-
-
- // Build rotation matrix R
- boost_ublas::matrix<double> R(2,N_,false);
- R.clear();
- double Xdoortip = 0;
- double Ydoortip = 0;
- Door.build_door_matrix(Time, N_, R, 0.0, Xdoortip, Ydoortip);
-// cout << "Xdoortip" << Xdoortip << endl;
-// cout << "Ydoortip" << Ydoortip << endl;
-// cout << " R" << R << endl;
-
-// // build door linear equation
-//
-// double RrotHx = prod(Door.InvRx(),Rrot);
-// double RrotHy = prod(Door.InvRy(),Rrot);
-// double RrotHp = prod(Door.InvRp(),Rrot);
-
- // Build selection matrix W
-
- IntermedQPMat::state_variant_t & State = Matrices_.State();
-
- const int & NbPrwSteps = SupportStates_deq.back().StepNumber;
- boost_ublas::matrix<double> W(N_,NbPrwSteps,false);
- W.clear();
- bool Empty = true;
- for(int j=0;j<NbPrwSteps;j++)
- {
-
- for(int i=0;i<N_;i++)
- {
- if(State.V(i,j)== 1 && Empty == true)
- {
- Empty = false;
- W(i,j)=1;
- }
- else
- W(i,j)=0;
- }
- Empty = true;
- }
-
-// cout << "W" << W << endl;
-// cout << "State.V" << State.V << endl;
-
-
- int NbStepsPreviewed = SupportStates_deq.back().StepNumber;
-// cout << "NbStepsPreviewed " << NbStepsPreviewed << endl;
-
- /// security distance distances from door's edges D1 and D2
- boost_ublas::vector<double> D1(N_,false);
- boost_ublas::vector<double> D2(N_,false);
- boost_ublas::vector<double> D3(N_,false);
- boost_ublas::vector<double> D4(N_,false);
- boost_ublas::vector<double> D5(NbStepsPreviewed,false);
- boost_ublas::vector<double> D6(NbStepsPreviewed,false);
- boost_ublas::vector<double> D7(NbStepsPreviewed,false);
-
- for(int i=0;i<N_;i++)
- {
- D1(i) = 0.5;
- D2(i) = 0.2;
- D3(i) = 0.45;
- D4(i) = 0.55;
- }
+ // Build rotation matrix R
+ boost_ublas::matrix<double> R(2,N_,false);
+ R.clear();
+ Door.build_rotation_matrices(Time, N_, R);
+ // // build door linear equation
+ //
+ // double RrotHx = prod(Door.InvRx(),Rrot);
+ // double RrotHy = prod(Door.InvRy(),Rrot);
+ // double RrotHp = prod(Door.InvRp(),Rrot);
- for(int i=0;i< NbStepsPreviewed;i++)
- {
+ // Build selection matrix W
+ IntermedQPMat::state_variant_t & State = Matrices_.State();
- D5(i) = 0.5;
- D6(i) = 0.3;
- D7(i) = 0.7;
- }
-
- const IntermedQPMat::dynamics_t & PosDynamics = Matrices_.Dynamics(IntermedQPMat::POSITION);
-
- // Number of inequality constraints
- linear_inequality_t DoorConstraints;
-// DoorConstraints.resize(4*N_+3*NbPrwSteps,2*(N_+NbPrwSteps),false);
-
- DoorConstraints.resize(NbStepsPreviewed,2*(NbStepsPreviewed),false);
- DoorConstraints.clear();
-
- // Compute term diag(RH)Sx
- // -----------------------
- boost_ublas::vector<double> HXT = Door.InvRx();
- boost_ublas::vector<double> HXRT = trans(prod(HXT,R));
-
-// cout << "HXRT " << HXRT << endl;
-// cout << " R " << R << endl;
-
-
-
- // Build diagonal matrix out of HXRT
- boost_ublas::matrix<double> Diagx(N_,N_,false);
- Diagx.clear();
- for(int i=0;i<N_;i++)
- {
- Diagx(i,i) = HXRT(i);
- }
-
-// cout << " HXRT " << HXRT << endl;
-// cout << " Diagx " << Diagx << endl;
- boost_ublas::matrix<double> RHXSXi = prod(Diagx,PosDynamics.S);
- boost_ublas::vector<double> RHXSX = prod(RHXSXi,State.CoM.x);
-
- // Compute term diag(RH)Sy
- // -----------------------
- boost_ublas::vector<double> HYT = trans(Door.InvRy());
- boost_ublas::vector<double> HYRT = trans(prod(HYT,R));
-// cout << " HYT " << HYT << endl;
-// cout << " Door.InvRy() " << Door.InvRy() << endl;
-// cout << "R " << R << endl;
-// cout << " HXT " << HXT << endl;
-// cout << "Door.InvRx() " << Door.InvRx() << endl;
-// cout << "trans Door.InvRx() " << trans(Door.InvRx()) << endl;
-
- boost_ublas::matrix<double> Diagy(N_,N_,false);
- Diagy.clear();
-
-
- for(int i=0;i<N_;i++)
- {
- Diagy(i,i) = HYRT(i);
- }
-
- boost_ublas::matrix<double> RHYSYi = prod(Diagy,PosDynamics.S);
- boost_ublas::vector<double> RHYSY = prod(RHYSYi,State.CoM.y);
-// cout << "Dynamics.S" << PosDynamics.S << endl;
-// cout << "PosDynamics.S" << PosDynamics.S << endl;
-// cout << "prod (PosDynamics.S,State.CoM.y)" << prod (PosDynamics.S,State.CoM.y) << endl;
-// cout << " State.CoM.y " << State.CoM.y << endl;
-// cout << "Diagy " <<Diagy << endl;
-// cout << "Diagx " <<Diagx << endl;
-
- //compute the term HPTR
- boost_ublas::vector<double> HPT = trans(Door.InvHp());
- boost_ublas::vector<double> HPTR = trans(prod(HPT,R));
-
-// cout << " HPTR " << HPTR << endl;
-// cout << " Diagy " << Diagy << endl;
-
- // COM constraints right side
-
- boost_ublas::vector<double> COM_CONSTR_R1 = D1+D2-RHXSX-RHYSY- HPTR;
- boost_ublas::vector<double> COM_CONSTR_R2 = D2-COM_CONSTR_R1;
-
- // COM constraints left side
-
- //compute term HxTRU
- boost_ublas::vector<double> HXTR = trans(HXRT);
- boost_ublas::matrix<double> HXTRU = prod(Diagx,PosDynamics.U);
-
- //compute term HyTRU
-
- boost_ublas::vector<double> HYTR = trans(HYRT);
- boost_ublas::matrix<double> HYTRU = prod(Diagy,PosDynamics.U);
+ const int & NbPrwSteps = SupportStates_deq.back().StepNumber;
+ boost_ublas::matrix<double> W(N_,NbPrwSteps,false);
+ W.clear();
+ bool Empty = true;
+ for(int j=0;j<NbPrwSteps;j++)
+ {
+ for(int i=0;i<N_;i++)
+ {
+ if(State.V(i,j)== 1 && Empty == true)
+ {
+ Empty = false;
+ W(i,j)=1;
+ }
+ else
+ W(i,j)=0;
+ }
+ Empty = true;
+ }
- //Foot constraints with respect to the door
- boost_ublas::matrix<double> HXRTW = prod(Diagx,W);
- boost_ublas::matrix<double> HYRTW = prod(Diagy,W);
- boost_ublas::vector<double> WWH= prod(HPTR,W);
+ int NbStepsPreviewed = SupportStates_deq.back().StepNumber;
- boost_ublas::matrix<double> WWx(NbPrwSteps,NbPrwSteps,false);
- boost_ublas::matrix<double> WWy(NbPrwSteps,NbPrwSteps,false);
+ /// security distance distances from door's edges D1 and D2
+ boost_ublas::vector<double> D1(N_,false);
+ boost_ublas::vector<double> D2(N_,false);
+ boost_ublas::vector<double> D3(N_,false);
+ boost_ublas::vector<double> D4(N_,false);
+ boost_ublas::vector<double> D5(NbStepsPreviewed,false);
+ boost_ublas::vector<double> D6(NbStepsPreviewed,false);
+ boost_ublas::vector<double> D7(NbStepsPreviewed,false);
- int line = 0;
- for(int i=0;i<N_;i++)
- {
- for(int j=0;j<NbPrwSteps;j++)
- {
- if(W(i,j)==1 )
- {
- for(int p=0;p<NbPrwSteps;p++)
- {
- WWx(line,p)= HXRTW(i,p);
- WWy(line,p)= HYRTW(i,p);
- }
- line++;
- }
+ for(int i=0;i<N_;i++)
+ {
+ D1(i) = 0.5;
+ D2(i) = 0.2;
+ D3(i) = 0.45;
+ D4(i) = 0.55;
+ }
- }
- }
+ for(int i=0;i< NbStepsPreviewed;i++)
+ {
+ D5(i) = 0.5;
+ D6(i) = 0.3;
+ D7(i) = 0.7;
+ }
- R.clear();
+ const IntermedQPMat::dynamics_t & PosDynamics = Matrices_.Dynamics(IntermedQPMat::POSITION);
- // lateral constraints with D3 and D4
- // ............................
- Door.build_door_matrix(Time, N_, R, M_PI/2.00, Xdoortip, Ydoortip );
- // Compute term diag(RH)Sx
- // -----------------------
- // boost_ublas::vector<double> HXRT2 = trans(prod(HXT,R));
-
- boost_ublas::matrix<double> RT = trans(R);
- boost_ublas::vector<double> HXRT2 = prod(RT,HXT);
- // cout << "HXRT2 " << HXRT2 << endl;
- // cout << "RT " << RT << endl;
- // cout << "R " << R << endl;
- // cout << "HXT"<<HXT<<endl;
- // Build diagonal matrix out of HXRT
- boost_ublas::matrix<double> Diagx1(N_,N_,false);
- Diagx1.clear();
- for(int i=0;i<N_;i++)
- {
- Diagx1(i,i) = HXRT2(i);
- }
+ // Number of inequality constraints
+ linear_inequality_t DoorConstraints;
+ // DoorConstraints.resize(4*N_+3*NbPrwSteps,2*(N_+NbPrwSteps),false);
-// cout << " Diagx1 " << Diagx1 << endl;
+ DoorConstraints.resize(NbStepsPreviewed,2*(NbStepsPreviewed),false);
+ DoorConstraints.clear();
-// cout << " Diagx1 " << Diagx1 << endl;
-// cout << " HXRT2 " << HXRT2 << endl;
+ // Compute term diag(RH)Sx
+ // -----------------------
+ Door::frame_t & FrameBase = Door.FrameBase();
+ boost_ublas::vector<double> HXT = FrameBase.InvRx;
+ boost_ublas::vector<double> HXRT = trans(prod(HXT,R));
- RHXSXi = prod(Diagx1,PosDynamics.S);
- boost_ublas::vector<double> RHXSX2 = prod(RHXSXi,State.CoM.x);
- // Compute term diag(RH)Sy
- // -----------------------
- boost_ublas::vector<double> HYRT2 = trans(prod(HYT,R));
- boost_ublas::matrix<double> Diagy1(N_,N_,false);
- Diagy1.clear();
- for(int i=0;i<N_;i++)
- {
- Diagy1(i,i) = HYRT2(i);
- }
+ // Build diagonal matrix out of HXRT
+ boost_ublas::matrix<double> Diagx(N_,N_,false);
+ Diagx.clear();
+ for(int i=0;i<N_;i++)
+ Diagx(i,i) = HXRT(i);
-// cout << " Diagy1 " << Diagx1 << endl;
-// cout << " HYRT2 " << HYRT2 << endl;
+ boost_ublas::matrix<double> RHXSXi = prod(Diagx,PosDynamics.S);
+ boost_ublas::vector<double> RHXSX = prod(RHXSXi,State.CoM.x);
- RHYSYi = prod(Diagy1,PosDynamics.S);
- boost_ublas::vector<double> RHYSY2 = prod(RHYSYi,State.CoM.y);
+ // Compute term diag(RH)Sy
+ // -----------------------
+ boost_ublas::vector<double> HYT = trans(FrameBase.InvRy);
+ boost_ublas::vector<double> HYRT = trans(prod(HYT,R));
- //compute the term HPTR
- boost_ublas::vector<double> HPTR2 = trans(prod(HPT,R));
+ boost_ublas::matrix<double> Diagy(N_,N_,false);
+ Diagy.clear();
- // COM constraints right side
- boost_ublas::vector<double> COM_CONSTR_R1_2 = D3+D4-RHXSX2-RHYSY2- HPTR2;
- boost_ublas::vector<double> COM_CONSTR_R2_2 = D4-COM_CONSTR_R1_2;
+ for(int i=0;i<N_;i++)
+ {
+ Diagy(i,i) = HYRT(i);
+ }
- // COM constraints left side
+ boost_ublas::matrix<double> RHYSYi = prod(Diagy,PosDynamics.S);
+ boost_ublas::vector<double> RHYSY = prod(RHYSYi,State.CoM.y);
- //compute term HxTRU
- boost_ublas::vector<double> HXTR2 = trans(HXRT2);
- boost_ublas::matrix<double> HXTRU2 = prod(Diagx1,PosDynamics.U);
- //compute term HyTRU
+ //compute the term HPTR
+ boost_ublas::vector<double> HPT = trans(FrameBase.InvHp);
+ boost_ublas::vector<double> HPTR = trans(prod(HPT,R));
- boost_ublas::vector<double> HYTR2 = trans(HYRT2);
- boost_ublas::matrix<double> HYTRU2 = prod(Diagy1,PosDynamics.U);
+ // COM constraints right side
-// cout << " PosDynamics.U " << PosDynamics.U << endl;
-// cout << " PosDynamics.S " << PosDynamics.S << endl;
-// cout << " State.CoM.x " << State.CoM.x << endl;
-// cout << " State.CoM.y " << State.CoM.y << endl;
+ boost_ublas::vector<double> COM_CONSTR_R1 = D1+D2-RHXSX-RHYSY- HPTR;
+ boost_ublas::vector<double> COM_CONSTR_R2 = D2-COM_CONSTR_R1;
+ // COM constraints left side
- //Foot constraints with respect to Cadre
-
- boost_ublas::matrix<double> HXRTW2=prod(Diagx1,W);
- boost_ublas::matrix<double> HYRTW2= prod(Diagy1,W);
- boost_ublas::vector<double> WWH2= prod(HPTR2,W);
- boost_ublas::matrix<double> WWx2(NbPrwSteps,NbPrwSteps,false);
- boost_ublas::matrix<double> WWy2(NbPrwSteps,NbPrwSteps,false);
+ //compute term HxTRU
+ boost_ublas::vector<double> HXTR = trans(HXRT);
+ boost_ublas::matrix<double> HXTRU = prod(Diagx,PosDynamics.U);
+ //compute term HyTRU
-//
-// cout << "HXRTW2" << HXRTW2 << endl;
-// cout << "Diagx1" << Diagx1 << endl;
-
- int line1 = 0;
- for(int i=0;i<N_;i++)
- {
-
- for(int j=0;j<NbPrwSteps;j++)
- {
- if(W(i,j)==1 )
- {
- for(int p=0;p<NbPrwSteps;p++)
- {
- WWx2(line1,p)= HXRTW2(i,p);
- WWy2(line1,p)= HYRTW2(i,p);
-
- }
- line1++;
- }
-
- }
- }
-
-
-// cout << "HXTRU" << HXTRU << endl;
-// cout << "HYTRU" << HYTRU << endl;
-// cout << "HXTRU2" << HXTRU2 << endl;
-// cout << "HYTRU2" << HYTRU2 << endl;
-// cout << "HXRTW" << HXRTW << endl;
-// cout << "HYRTW" << HYRTW << endl;
-// cout << "HXRTW2" <<HXRTW2 << endl;
-// cout << "HYRTW2" << HYRTW2 << endl;
-
-
- int NbConstraints = Pb.NbConstraints();
-// cout<<"NbConstraints: "<<NbConstraints<<endl;
-//
-//// in front of door1
-//
- Pb.add_term(HXTRU,QPProblem::MATRIX_DU,NbConstraints,0);
- Pb.add_term(HYTRU,QPProblem::MATRIX_DU,NbConstraints,N_);
- Pb.add_term(COM_CONSTR_R2,QPProblem::VECTOR_DS,NbConstraints);
+ boost_ublas::vector<double> HYTR = trans(HYRT);
+ boost_ublas::matrix<double> HYTRU = prod(Diagy,PosDynamics.U);
-// cout << "COM_CONSTR_R2" << COM_CONSTR_R2 << endl;
-// cout << "HXTRU" << HXTRU << endl;
-// cout << "HYTRU" << HYTRU << endl;
-//
-//// in front of door2
-//
-// NbConstraints = Pb.NbConstraints();
-// Pb.add_term(-HXTRU,QPProblem::MATRIX_DU,NbConstraints,0);
-// Pb.add_term(-HYTRU,QPProblem::MATRIX_DU,NbConstraints,N_);
-// Pb.add_term(COM_CONSTR_R1,QPProblem::VECTOR_DS,NbConstraints);
+ //Foot constraints with respect to the door
+ boost_ublas::matrix<double> HXRTW = prod(Diagx,W);
+ boost_ublas::matrix<double> HYRTW = prod(Diagy,W);
+ boost_ublas::vector<double> WWH= prod(HPTR,W);
-// cout<<"-HXTRU"<<HXTRU<<endl;
-// cout<<"-HYTRU"<<HYTRU<<endl;
-// cout<<"COM_CONSTR_R1"<<-COM_CONSTR_R1<<endl;
+ boost_ublas::matrix<double> WWx(NbPrwSteps,NbPrwSteps,false);
+ boost_ublas::matrix<double> WWy(NbPrwSteps,NbPrwSteps,false);
- //int NbStepsPreviewed = SupportStates_deq.back().StepNumber;
-//
-// NbConstraints = Pb.NbConstraints();
-// Pb.add_term(HXTRU2,QPProblem::MATRIX_DU,NbConstraints,0);
-// Pb.add_term(HYTRU2,QPProblem::MATRIX_DU,NbConstraints,N_);
-// Pb.add_term(COM_CONSTR_R2_2,QPProblem::VECTOR_DS,NbConstraints);
+ int line = 0;
+ for(int i=0;i<N_;i++)
+ {
+ for(int j=0;j<NbPrwSteps;j++)
+ {
+ if(W(i,j)==1 )
+ {
+ for(int p=0;p<NbPrwSteps;p++)
+ {
+ WWx(line,p)= HXRTW(i,p);
+ WWy(line,p)= HYRTW(i,p);
+ }
+ line++;
+ }
+ }
+ }
+
+
+ // lateral constraints with D3 and D4:
+ // -----------------------------------
+ R.clear();
+ Door.build_rotation_matrices( Time, N_, R );
+ // Compute term diag(RH)Sx
+ // -----------------------
+ // boost_ublas::vector<double> HXRT2 = trans(prod(HXT,R));
+
+ boost_ublas::matrix<double> RT = trans(R);
+ boost_ublas::vector<double> HXRT2 = prod(RT,HXT);
+
+ // Build diagonal matrix out of HXRT
+ boost_ublas::matrix<double> Diagx1(N_,N_,false);
+ Diagx1.clear();
+ for(int i=0;i<N_;i++)
+ Diagx1(i,i) = HXRT2(i);
+
+
+ RHXSXi = prod(Diagx1,PosDynamics.S);
+ boost_ublas::vector<double> RHXSX2 = prod(RHXSXi,State.CoM.x);
+
+ // Compute term diag(RH)Sy:
+ // ------------------------
+ boost_ublas::vector<double> HYRT2 = trans(prod(HYT,R));
+ boost_ublas::matrix<double> Diagy1(N_,N_,false);
+ Diagy1.clear();
+ for(unsigned int i=0;i<N_;i++)
+ Diagy1(i,i) = HYRT2(i);
+
+
+ RHYSYi = prod(Diagy1,PosDynamics.S);
+ boost_ublas::vector<double> RHYSY2 = prod(RHYSYi,State.CoM.y);
+ //compute the term HPTR
+ boost_ublas::vector<double> HPTR2 = trans(prod(HPT,R));
-// NbConstraints = Pb.NbConstraints();
-// Pb.add_term(-HXTRU2,QPProblem::MATRIX_DU,NbConstraints,0);
-// Pb.add_term(-HYTRU2,QPProblem::MATRIX_DU,NbConstraints,N_);
-// Pb.add_term(COM_CONSTR_R1_2,QPProblem::VECTOR_DS,NbConstraints);
+ // COM constraints right side
+ boost_ublas::vector<double> COM_CONSTR_R1_2 = D3+D4-RHXSX2-RHYSY2- HPTR2;
+ boost_ublas::vector<double> COM_CONSTR_R2_2 = D4-COM_CONSTR_R1_2;
- NbConstraints = Pb.NbConstraints();
-// cout<<"HXRTW:"<<HXRTW<<endl;
-// cout<<"HYRTW:"<<HYRTW<<endl;
-// cout<<"D5:"<<D5<<endl;
+ // COM constraints left side
+ //compute term HxTRU
+ boost_ublas::vector<double> HXTR2 = trans(HXRT2);
+ boost_ublas::matrix<double> HXTRU2 = prod(Diagx1,PosDynamics.U);
+ //compute term HyTRU
+ boost_ublas::vector<double> HYTR2 = trans(HYRT2);
+ boost_ublas::matrix<double> HYTRU2 = prod(Diagy1,PosDynamics.U);
- NbConstraints = Pb.NbConstraints();
-// cout<<"NbConstraints: "<<NbConstraints<<endl;
- D5=D5-WWH;
+
+ //Foot constraints with respect to Cadre
+ boost_ublas::matrix<double> HXRTW2=prod(Diagx1,W);
+ boost_ublas::matrix<double> HYRTW2= prod(Diagy1,W);
+ boost_ublas::vector<double> WWH2= prod(HPTR2,W);
+ boost_ublas::matrix<double> WWx2(NbPrwSteps,NbPrwSteps,false);
+ boost_ublas::matrix<double> WWy2(NbPrwSteps,NbPrwSteps,false);
+
+
+ int line1 = 0;
+ for(int i=0;i<N_;i++)
+ {
+
+ for(int j=0;j<NbPrwSteps;j++)
+ {
+ if(W(i,j)==1)
+ {
+ for(int p=0;p<NbPrwSteps;p++)
+ {
+ WWx2(line1,p)= HXRTW2(i,p);
+ WWy2(line1,p)= HYRTW2(i,p);
+ }
+ line1++;
+ }
+ }
+ }
+
+
+ // Front of door 1
+ int NbConstraints = Pb.NbConstraints();
+ Pb.add_term(HXTRU,QPProblem::MATRIX_DU,NbConstraints,0);
+ Pb.add_term(HYTRU,QPProblem::MATRIX_DU,NbConstraints,N_);
+ Pb.add_term(COM_CONSTR_R2,QPProblem::VECTOR_DS,NbConstraints);
+
+ //// in front of door2
+ //
+ // NbConstraints = Pb.NbConstraints();
+ // Pb.add_term(-HXTRU,QPProblem::MATRIX_DU,NbConstraints,0);
+ // Pb.add_term(-HYTRU,QPProblem::MATRIX_DU,NbConstraints,N_);
+ // Pb.add_term(COM_CONSTR_R1,QPProblem::VECTOR_DS,NbConstraints);
+
+
+ //int NbStepsPreviewed = SupportStates_deq.back().StepNumber;
+ //
+ // NbConstraints = Pb.NbConstraints();
+ // Pb.add_term(HXTRU2,QPProblem::MATRIX_DU,NbConstraints,0);
+ // Pb.add_term(HYTRU2,QPProblem::MATRIX_DU,NbConstraints,N_);
+ // Pb.add_term(COM_CONSTR_R2_2,QPProblem::VECTOR_DS,NbConstraints);
+
+
+ // NbConstraints = Pb.NbConstraints();
+ // Pb.add_term(-HXTRU2,QPProblem::MATRIX_DU,NbConstraints,0);
+ // Pb.add_term(-HYTRU2,QPProblem::MATRIX_DU,NbConstraints,N_);
+ // Pb.add_term(COM_CONSTR_R1_2,QPProblem::VECTOR_DS,NbConstraints);
+
+ NbConstraints = Pb.NbConstraints();
+ D5=D5-WWH;
+ Pb.add_term(WWx,QPProblem::MATRIX_DU,NbConstraints,2*N_);
+ Pb.add_term(WWy,QPProblem::MATRIX_DU,NbConstraints,2*N_+NbStepsPreviewed);
+ Pb.add_term(-D5,QPProblem::VECTOR_DS,NbConstraints);
+
+// NbConstraints = Pb.NbConstraints();
+// D6=D6-WWH2;
+// Pb.add_term(WWx2,QPProblem::MATRIX_DU,NbConstraints,2*N_);
+// Pb.add_term(WWy2,QPProblem::MATRIX_DU,NbConstraints,2*N_+NbStepsPreviewed);
+// Pb.add_term(-D6,QPProblem::VECTOR_DS,NbConstraints);
+//
//
- Pb.add_term(WWx,QPProblem::MATRIX_DU,NbConstraints,2*N_);
- Pb.add_term(WWy,QPProblem::MATRIX_DU,NbConstraints,2*N_+NbStepsPreviewed);
- Pb.add_term(-D5,QPProblem::VECTOR_DS,NbConstraints);
-
-// NbConstraints = Pb.NbConstraints();
-// D6=D6-WWH2;
-// Pb.add_term(WWx2,QPProblem::MATRIX_DU,NbConstraints,2*N_);
-// Pb.add_term(WWy2,QPProblem::MATRIX_DU,NbConstraints,2*N_+NbStepsPreviewed);
-// Pb.add_term(-D6,QPProblem::VECTOR_DS,NbConstraints);
-
-
-// cout<<"-D6"<<-D6<<endl;
-// cout<<"WWy2"<<WWy2<<endl;
-// cout<<"WWx2"<<WWx2<<endl;
-// cout<<"WWH2"<<WWH2<<endl;
-
-// NbConstraints = Pb.NbConstraints();
-// D7=D7-WWH2;
-// Pb.add_term(-WWx2,QPProblem::MATRIX_DU,NbConstraints,2*N_);
-// Pb.add_term(-WWy2,QPProblem::MATRIX_DU,NbConstraints,2*N_+NbStepsPreviewed);
-// Pb.add_term(D7,QPProblem::VECTOR_DS,NbConstraints);
-// cout<<"D7"<<D7<<endl;
+// NbConstraints = Pb.NbConstraints();
+// D7=D7-WWH2;
+// Pb.add_term(-WWx2,QPProblem::MATRIX_DU,NbConstraints,2*N_);
+// Pb.add_term(-WWy2,QPProblem::MATRIX_DU,NbConstraints,2*N_+NbStepsPreviewed);
+// Pb.add_term(D7,QPProblem::VECTOR_DS,NbConstraints);
+
}
diff --git a/tests/TestHerdt2010.cpp b/tests/TestHerdt2010.cpp
index 10f664bb1b4577e9e8d65bbea0c722f1e9f4500d..c60078694502436c7e17f7811448021940e57143 100644
--- a/tests/TestHerdt2010.cpp
+++ b/tests/TestHerdt2010.cpp
@@ -82,14 +82,14 @@ protected:
void startTurningLeft(PatternGeneratorInterface &aPGI)
{
{
- istringstream strm2(":setVelReference 0.2 0.0 0.1");
+ istringstream strm2(":setVelReference 0.2 0.0 0.0");
aPGI.ParseCmd(strm2);
}
}
void startTurningRight(PatternGeneratorInterface &aPGI)
{
{
- istringstream strm2(":setVelReference 0.2 0.0 -0.1");
+ istringstream strm2(":setVelReference 0.2 0.0 -0.0");
aPGI.ParseCmd(strm2);
}
}
@@ -97,7 +97,7 @@ protected:
void startTurningRightOnSpot(PatternGeneratorInterface &aPGI)
{
{
- istringstream strm2(":setVelReference 0.0 0.0 10.0");
+ istringstream strm2(":setVelReference 0.2 0.0 0.0");
aPGI.ParseCmd(strm2);
}
}