Add new object Door

src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp

@@ -105,6 +105,7 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *lSPM,
   VRQPGenerator_->Ponderation( 0.000001, IntermedQPMat::COP_CENTERING );
   VRQPGenerator_->Ponderation( 0.00001, IntermedQPMat::JERK_MIN );
 
+  Door_.initialize(-3.14/2);
 
   // Register method to handle
   string aMethodName[2] =
@@ -392,6 +393,13 @@ ZMPVelocityReferencedQP::OnLine(double time,
           PreviewedSupportAngles_deq );
 
 
+      // BUILD DOOR CONSTRAINTS:
+      // -----------------------
+      double DesVelDoor = 0.0;
+      VRQPGenerator_->build_constraints_door( time, DesVelDoor,
+		      Door_, PrwSupportStates_deq, Problem_);
+
+
       // SOLVE PROBLEM:
       // --------------
       QPProblem_s::solution_t Result;

src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h

@@ -45,6 +45,7 @@
 #include <privatepgtypes.h>
 #include <ZMPRefTrajectoryGeneration/generator-vel-ref.hh>
 #include <Mathematics/intermediate-qp-matrices.hh>
+#include <ZMPRefTrajectoryGeneration/door.hh>
 
 namespace PatternGeneratorJRL
 {
@@ -156,6 +157,9 @@ 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);