diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 97d7efb78d5eab451f9b6d475113c96beac4adf7..d0fc813c6f2f2d73388b9868096017bf828cba77 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -66,7 +66,6 @@ SET(SOURCES
ZMPRefTrajectoryGeneration/qp-problem.cpp
ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp
- ZMPRefTrajectoryGeneration/door.cpp
MotionGeneration/StepOverPlanner.cpp
MotionGeneration/CollisionDetector.cpp
MotionGeneration/WaistHeightVariation.cpp
diff --git a/src/PreviewControl/rigid-body-system.cpp b/src/PreviewControl/rigid-body-system.cpp
index c097f9588d47a5250cfa8194d87ec0578251725b..380b7aba6130dc4e93b1ff1616763d7d40120e5b 100644
--- a/src/PreviewControl/rigid-body-system.cpp
+++ b/src/PreviewControl/rigid-body-system.cpp
@@ -422,8 +422,8 @@ RigidBodySystem::compute_foot_zero_dynamics( const std::deque<support_state_t> &
// ------------------
linear_dynamics_t * SFDynamics;
linear_dynamics_t * FFDynamics;
- double Spbar[3];//, Sabar[3];
- double Upbar[2];//, Uabar[2];
+ double Spbar[3]={0.0,0.0,0.0};//, Sabar[3];
+ double Upbar[2]={0.0,0.0};//, Uabar[2];
unsigned int SwitchInstant = 0;
std::deque<support_state_t>::const_iterator SS_it =
SupportStates_deq.begin();
@@ -823,48 +823,6 @@ RigidBodySystem::generate_trajectories( double Time, const solution_t & Solution
Solution, PreviewedSupportAngles_deq,
LeftFootTraj_deq, RightFootTraj_deq);
-// linear_dynamics_t LFDyn = LeftFoot_.Dynamics( ACCELERATION );
-// cout<<"LFDyn.S"<<LFDyn.S<<endl;
-// cout<<"LFDyn.U"<<LFDyn.U<<endl;
-// cout<<"Prediction: "<<LFDyn.S(0,0)*CurLeftFoot.x+LFDyn.S(0,1)*CurLeftFoot.dx+LFDyn.S(0,2)*CurLeftFoot.ddx+LFDyn.U(0,0)*Result.Solution_vec[2*N_]<<endl;
-// cout<<"Reality: "<<LeftFootTraj_deq.back().x<<endl;
-// linear_dynamics_t RFDyn = RightFoot_.Dynamics( ACCELERATION );
-// cout<<"CurrentSupport.Phase = "<<CurrentSupport.Phase<<endl;
-// cout<<"CurrentSupport.Foot = "<<CurrentSupport.Foot<<endl;
-// cout<<"CurrentSupport.TimeLimit = "<<CurrentSupport.TimeLimit<<endl;
-// cout<<"RFDyn.S"<<RFDyn.S<<endl;
-// cout<<"RFDyn.U"<<RFDyn.U<<endl;
-// cout<<"Result.Solution_vec[2*N_] = "<<Result.Solution_vec[2*N_]<<endl;
-// cout<<"Prediction: "<<RFDyn.S(0,0)*CurRightFoot.x+RFDyn.S(0,1)*CurRightFoot.dx+RFDyn.S(0,2)*CurRightFoot.ddx+RFDyn.U(0,0)*Result.Solution_vec[2*N_]
-//<<"Prediction: "<<LFDyn.S(0,0)*CurRightFoot.x+LFDyn.S(0,1)*CurRightFoot.dx+LFDyn.S(0,2)*CurRightFoot.ddx+LFDyn.U(0,0)*Result.Solution_vec[2*N_]<<endl;
-// cout<<"Prediction: "<<RFDyn.S(1,0)*CurRightFoot.x+RFDyn.S(1,1)*CurRightFoot.dx+RFDyn.S(1,2)*CurRightFoot.ddx+RFDyn.U(1,0)*Result.Solution_vec[2*N_]
-//<<"Prediction: "<<LFDyn.S(1,0)*CurRightFoot.x+LFDyn.S(1,1)*CurRightFoot.dx+LFDyn.S(1,2)*CurRightFoot.ddx+LFDyn.U(1,0)*Result.Solution_vec[2*N_]<<endl;
-// cout<<"Prediction: "<<RFDyn.S(2,0)*CurRightFoot.x+RFDyn.S(2,1)*CurRightFoot.dx+RFDyn.S(2,2)*CurRightFoot.ddx+RFDyn.U(2,0)*Result.Solution_vec[2*N_]
-//<<"Prediction: "<<LFDyn.S(2,0)*CurRightFoot.x+LFDyn.S(2,1)*CurRightFoot.dx+LFDyn.S(2,2)*CurRightFoot.ddx+LFDyn.U(2,0)*Result.Solution_vec[2*N_]<<endl;
-// cout<<"Prediction: "<<RFDyn.S(3,0)*CurRightFoot.x+RFDyn.S(3,1)*CurRightFoot.dx+RFDyn.S(3,2)*CurRightFoot.ddx+RFDyn.U(3,0)*Result.Solution_vec[2*N_]
-// <<"Prediction: "<<LFDyn.S(3,0)*CurRightFoot.x+LFDyn.S(3,1)*CurRightFoot.dx+LFDyn.S(3,2)*CurRightFoot.ddx+LFDyn.U(3,0)*Result.Solution_vec[2*N_]<<endl;
-// cout<<"Prediction: "<<RFDyn.S(4,0)*CurRightFoot.x+RFDyn.S(4,1)*CurRightFoot.dx+RFDyn.S(4,2)*CurRightFoot.ddx+RFDyn.U(4,0)*Result.Solution_vec[2*N_]
-//<<"Prediction: "<<LFDyn.S(4,0)*CurRightFoot.x+LFDyn.S(4,1)*CurRightFoot.dx+LFDyn.S(4,2)*CurRightFoot.ddx+LFDyn.U(4,0)*Result.Solution_vec[2*N_]<<endl;
-// cout<<"Prediction: "<<RFDyn.S(5,0)*CurRightFoot.x+RFDyn.S(5,1)*CurRightFoot.dx+RFDyn.S(5,2)*CurRightFoot.ddx+RFDyn.U(5,0)*Result.Solution_vec[2*N_]
-// <<"Prediction: "<<LFDyn.S(5,0)*CurRightFoot.x+LFDyn.S(5,1)*CurRightFoot.dx+LFDyn.S(5,2)*CurRightFoot.ddx+LFDyn.U(5,0)*Result.Solution_vec[2*N_]<<endl;
-// cout<<"Prediction: "<<RFDyn.S(6,0)*CurRightFoot.x+RFDyn.S(6,1)*CurRightFoot.dx+RFDyn.S(6,2)*CurRightFoot.ddx+RFDyn.U(6,0)*Result.Solution_vec[2*N_]
-// <<"Prediction: "<<LFDyn.S(6,0)*CurRightFoot.x+LFDyn.S(6,1)*CurRightFoot.dx+LFDyn.S(6,2)*CurRightFoot.ddx+LFDyn.U(6,0)*Result.Solution_vec[2*N_]<<endl;
-// cout<<"Prediction: "<<RFDyn.S(7,0)*CurRightFoot.x+RFDyn.S(7,1)*CurRightFoot.dx+RFDyn.S(7,2)*CurRightFoot.ddx+RFDyn.U(7,0)*Result.Solution_vec[2*N_]
-// <<"Prediction: "<<LFDyn.S(7,0)*CurRightFoot.x+LFDyn.S(7,1)*CurRightFoot.dx+LFDyn.S(7,2)*CurRightFoot.ddx+LFDyn.U(7,0)*Result.Solution_vec[2*N_]<<endl;
-// cout<<"Prediction: "<<RFDyn.S(8,0)*CurRightFoot.x+RFDyn.S(8,1)*CurRightFoot.dx+RFDyn.S(8,2)*CurRightFoot.ddx+RFDyn.U(8,0)*Result.Solution_vec[2*N_]
-// <<"Prediction: "<<LFDyn.S(8,0)*CurRightFoot.x+LFDyn.S(8,1)*CurRightFoot.dx+LFDyn.S(8,2)*CurRightFoot.ddx+LFDyn.U(8,0)*Result.Solution_vec[2*N_]<<endl;
-// cout<<"Prediction: "<<RFDyn.S(9,0)*CurRightFoot.x+RFDyn.S(9,1)*CurRightFoot.dx+RFDyn.S(9,2)*CurRightFoot.ddx+RFDyn.U(9,0)*Result.Solution_vec[2*N_]
-// <<"Prediction: "<<LFDyn.S(9,0)*CurRightFoot.x+LFDyn.S(9,1)*CurRightFoot.dx+LFDyn.S(9,2)*CurRightFoot.ddx+LFDyn.U(9,0)*Result.Solution_vec[2*N_]<<endl;
-// cout<<"Prediction: "<<RFDyn.S(10,0)*CurRightFoot.x+RFDyn.S(10,1)*CurRightFoot.dx+RFDyn.S(10,2)*CurRightFoot.ddx+RFDyn.U(10,0)*Result.Solution_vec[2*N_]
-// <<"Prediction: "<<LFDyn.S(10,0)*CurRightFoot.x+LFDyn.S(10,1)*CurRightFoot.dx+LFDyn.S(10,2)*CurRightFoot.ddx+LFDyn.U(10,0)*Result.Solution_vec[2*N_]<<endl;
-// cout<<"Prediction: "<<RFDyn.S(11,0)*CurRightFoot.x+RFDyn.S(11,1)*CurRightFoot.dx+RFDyn.S(11,2)*CurRightFoot.ddx+RFDyn.U(11,0)*Result.Solution_vec[2*N_]
-// <<"Prediction: "<<LFDyn.S(11,0)*CurRightFoot.x+LFDyn.S(11,1)*CurRightFoot.dx+LFDyn.S(11,2)*CurRightFoot.ddx+LFDyn.U(11,0)*Result.Solution_vec[2*N_]<<endl;
-// cout<<"Prediction: "<<RFDyn.S(12,0)*CurRightFoot.x+RFDyn.S(12,1)*CurRightFoot.dx+RFDyn.S(12,2)*CurRightFoot.ddx+RFDyn.U(12,0)*Result.Solution_vec[2*N_]
-// <<"Prediction: "<<LFDyn.S(12,0)*CurRightFoot.x+LFDyn.S(12,1)*CurRightFoot.dx+LFDyn.S(12,2)*CurRightFoot.ddx+LFDyn.U(12,0)*Result.Solution_vec[2*N_]<<endl;
-// cout<<"Prediction: "<<RFDyn.S(13,0)*CurRightFoot.x+RFDyn.S(13,1)*CurRightFoot.dx+RFDyn.S(13,2)*CurRightFoot.ddx+RFDyn.U(13,0)*Result.Solution_vec[2*N_]
-// <<"Prediction: "<<LFDyn.S(13,0)*CurRightFoot.x+LFDyn.S(13,1)*CurRightFoot.dx+LFDyn.S(13,2)*CurRightFoot.ddx+LFDyn.U(13,0)*Result.Solution_vec[2*N_]<<endl;
-// cout<<"Reality: "<<RightFootTraj_deq.back().ddx<<endl;
-
return 0;
}
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 9b12ffe89a30ce24594e37934dc9c85396d22b3d..0ddd1ea3bc5416ffe5694d200f6adc891d89f98e 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -109,9 +109,6 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
VRQPGenerator_->Ponderation( 0.000001, IntermedQPMat::COP_CENTERING );
VRQPGenerator_->Ponderation( 0.00001, IntermedQPMat::JERK_MIN );
- Door_.initialize( -M_PI/2.0 );
- Door_.SamplingTime( QP_T_ );
-
// Register method to handle
const unsigned int NbMethods = 3;
string aMethodName[NbMethods] =
@@ -400,16 +397,9 @@ ZMPVelocityReferencedQP::OnLine(double Time,
PreviewedSupportStates_deq, PreviewedSupportAngles_deq );
- // BUILD DOOR CONSTRAINTS:
- // -----------------------
-// VRQPGenerator_->build_constraints_door( Time, Door_, PreviewedSupportStates_deq, Problem_ );
-
-
// SOLVE PROBLEM:
// --------------
solution_t Result;
-
-// Problem_.solve( QPProblem_s::LSSOL, Result, QPProblem_s::ALL );
Problem_.solve( QPProblem_s::QLD, Result, QPProblem_s::ALL );
Problem_.reset();
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
index 704c62c40a67fe5b17af0cd195976971c749a34d..8c4139f9cc6e56413bed2062a257398738f9ea55 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
@@ -41,7 +41,6 @@
#include <privatepgtypes.h>
#include <ZMPRefTrajectoryGeneration/generator-vel-ref.hh>
#include <Mathematics/intermediate-qp-matrices.hh>
-#include <ZMPRefTrajectoryGeneration/door.hh>
#include <jrl/walkgen/pgtypes.hh>
namespace PatternGeneratorJRL
@@ -172,9 +171,6 @@ namespace PatternGeneratorJRL
/// \brief Final optimization problem
QPProblem Problem_;
- /// \brief Door
- Door Door_;
-
/// \brief Additional term on the acceleration of the CoM
MAL_VECTOR(PerturbationAcceleration_,double);
diff --git a/src/ZMPRefTrajectoryGeneration/door.cpp b/src/ZMPRefTrajectoryGeneration/door.cpp
deleted file mode 100644
index 28194419a7e338b044c015e0d5b3fab8608c058c..0000000000000000000000000000000000000000
--- a/src/ZMPRefTrajectoryGeneration/door.cpp
+++ /dev/null
@@ -1,68 +0,0 @@
-/*
- * door.cpp
- *
- * Created on: 10 mai 2011
- * Author: syrine
- */
-
-#include <ZMPRefTrajectoryGeneration/door.hh>
-
-using namespace std;
-
-
-Door::Door()
-{}
-
-
-void
-Door::initialize( double 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);
-
- double xp = FrameBase_.Position[X];
- double yp = FrameBase_.Position[Y];
-
- FrameBase_.InvHp = -FrameBase_.InvRx*xp-FrameBase_.InvRy*yp;
-
-}
-
-
-void
-Door::build_rotation_matrices(double Time, int N, boost_ublas::matrix<double> & R )
-{
-
- double DesVelDoor = 0.0;
- if(Time > 8.0)
- DesVelDoor = M_PI/400.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;
- 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
deleted file mode 100644
index 4dc546f7ab780a2e2285412c410e407525f971ee..0000000000000000000000000000000000000000
--- a/src/ZMPRefTrajectoryGeneration/door.hh
+++ /dev/null
@@ -1,95 +0,0 @@
-/*
- * door.hh
- *
- * Created on: 10 mai 2011
- * Author: syrine
- */
-
-#ifndef DOOR_HH_
-#define DOOR_HH_
-
-#include <jrl/mal/matrixabstractlayer.hh>
-#include <cmath>
-
-class Door
-{
-
- //
- // Public types:
- //
-public:
- const static unsigned int X = 0;
- const static unsigned int Y = 1;
- const static unsigned int Z = 2;
-
- /// \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();
-
- /// \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 );
-
- /// \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:
-
- /// \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_;
-
-};
-
-#endif /* DOOR_HH_ */
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index c0d77c5be832191960e8fff7cc76131f70851516..ba61b935174f16ff783eb40a598a465472aca2d5 100755
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -325,291 +325,6 @@ 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)
-{
-
- // 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);
-
- // Build selection matrix W
- IntermedQPMat::state_variant_t & State = IntermedData_->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(unsigned 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;
- }
-
-
- int NbStepsPreviewed = SupportStates_deq.back().StepNumber;
-
- /// 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(unsigned 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;
- }
-
- const RigidBody & CoM = Robot_->CoM();
- const linear_dynamics_t & PosDynamics = CoM.Dynamics( 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
- // -----------------------
- Door::frame_t & FrameBase = Door.FrameBase();
- boost_ublas::vector<double> HXT = FrameBase.InvRx;
- boost_ublas::vector<double> HXRT = trans(prod(HXT,R));
-
-
- // Build diagonal matrix out of HXRT
- boost_ublas::matrix<double> Diagx(N_,N_,false);
- Diagx.clear();
- for(unsigned int i=0;i<N_;i++)
- Diagx(i,i) = HXRT(i);
-
- 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(FrameBase.InvRy);
- boost_ublas::vector<double> HYRT = trans(prod(HYT,R));
-
- boost_ublas::matrix<double> Diagy(N_,N_,false);
- Diagy.clear();
-
-
- for(unsigned 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);
-
- //compute the term HPTR
- boost_ublas::vector<double> HPT = trans(FrameBase.InvHp);
- boost_ublas::vector<double> HPTR = trans(prod(HPT,R));
-
- // 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);
-
- //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);
-
-
- boost_ublas::matrix<double> WWx(NbPrwSteps,NbPrwSteps,false);
- boost_ublas::matrix<double> WWy(NbPrwSteps,NbPrwSteps,false);
-
- int line = 0;
- for(unsigned 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(unsigned 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));
-
- // 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;
-
- // 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);
-
-
- //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(unsigned 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
- unsigned 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 );
-// cout<<WWx<<WWy<<D5<<WWH<<endl;
- // 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);
- //
- //
- // 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);
-
-
-}
-
-
void
GeneratorVelRef::build_constraints_cop(const linear_inequality_t & IneqCoP,
unsigned int NbStepsPreviewed, QPProblem & Pb)
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
index a6bbbe327f81a3201fd1cee83df0b19e2e5db765..3f7f19960642669bbc4f55955cdabdf665d85c82 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
@@ -39,7 +39,6 @@
#include <PreviewControl/rigid-body.hh>
#include <Mathematics/intermediate-qp-matrices.hh>
#include <Mathematics/relative-feet-inequalities.hh>
-#include <ZMPRefTrajectoryGeneration/door.hh>
#include <privatepgtypes.h>
#include <cmath>
@@ -99,15 +98,6 @@ namespace PatternGeneratorJRL
const std::deque<support_state_t> & PrwSupportStates_deq,
const std::deque<double> & PrwSupportAngles_deq);
- /// \brief Generate door constraints
- ///
- /// \param[in] Time
- /// \param[in] Door
- /// \param[in] SupportStates_deq
- /// \param[out] Pb
- void build_constraints_door(double Time,
- Door & Door, const std::deque<support_state_t> & SupportStates_deq, QPProblem & Pb);
-
/// \brief Build the constant part of the objective
///
/// \param[in] Pb