Merge branch 'topic/toes' of github.com:jrl-umi3218/jrl-walkgen into topic/toes

Conflicts:
	src/Mathematics/relative-feet-inequalities.cpp
	src/Mathematics/relative-feet-inequalities.hh

src/Mathematics/FootHalfSize.cpp

@@ -49,13 +49,14 @@ FootHalfSize::~FootHalfSize()
 }
 
 void FootHalfSize::setHalfSizeInit(double lHalfWidthInit,
-			      double lHalfHeightInit)
+                                   double lHalfHeightInit,
+                                   double DSFeetDistance)
 {
   m_HalfHeightInit = lHalfHeightInit;
   m_HalfWidthInit = lHalfWidthInit;
   
   updateHalfSize();
-  updateHalfHeightDS();
+  updateHalfHeightDS(DSFeetDistance);
 }
 
 void FootHalfSize::updateHalfSize()
@@ -67,20 +68,18 @@ void FootHalfSize::updateHalfSize()
   m_HalfHeight -= m_ConstraintsOnY;
 }
 
-void FootHalfSize::updateHalfHeightDS()
+void FootHalfSize::updateHalfHeightDS(double DSFeetDistance)
 {
-  double DSFeetDistance = 0.2;
-  
   m_HalfHeightDS = m_HalfHeight+DSFeetDistance/2.0;
 }
 
-void FootHalfSize::setConstraints(double OnX, double OnY)
+void FootHalfSize::setConstraints(double OnX, double OnY, double DSFeetDistance)
 {
   m_ConstraintsOnX = OnX;
   m_ConstraintsOnY = OnY;
 
   updateHalfSize();
-  updateHalfHeightDS();
+  updateHalfHeightDS(DSFeetDistance);
 
 }
 

src/Mathematics/FootHalfSize.hh

@@ -38,13 +38,15 @@ namespace PatternGeneratorJRL
     ~FootHalfSize();
     
     void setHalfSizeInit(double lHalfWidth,
-			 double lHalfHeight);
+                         double lHalfHeight,
+                         double DSFeetDistance);
 
     void setConstraints(double OnX,
-			double OnY);
+                        double OnY,
+                        double DSFeetDistance);
     
   protected:
-    void updateHalfHeightDS();
+    void updateHalfHeightDS(double DSFeetDistance);
     void updateHalfSize();
 
   private:

src/Mathematics/relative-feet-inequalities.cpp

@@ -48,15 +48,29 @@ RelativeFeetInequalities::RelativeFeetInequalities( SimplePluginManager *aSPM,
   DSFeetDistance_ = 0.2;
   SecurityMarginX_ = 0.04;
   SecurityMarginY_ = 0.04;
-
+  
+  double DefaultFPosEdgesX[5] = {-0.28, -0.2, 0.0, 0.2, 0.28};
+  double DefaultFPosEdgesY[5] = {-0.2, -0.3, -0.4, -0.3, -0.2};
+  
+  for(int i=0;i<5;i++)
+    {
+      LeftFPosEdgesX_[i] = DefaultFPosEdgesX[i];
+      LeftFPosEdgesY_[i] = DefaultFPosEdgesY[i];
+      
+      RightFPosEdgesX_[i] =  DefaultFPosEdgesX[i];
+      RightFPosEdgesY_[i] = -DefaultFPosEdgesY[i];
+    }
+  
   set_feet_dimensions( aHS );
 
   init_convex_hulls();
-
+  
   // Register method to handle
   string aMethodName[] =
-    {":setfeetconstraint"};
-
+    {":setfeetconstraint",
+     ":setDSFeetDistance",
+     ":setFPosEdges"};
+  
   for(int i=0;i<1;i++)
     {
       if (!RegisterMethod(aMethodName[i]))
@@ -83,20 +97,14 @@ RelativeFeetInequalities::init_convex_hulls()
 
   // Feet polygonal hulls:
   // ---------------------
-  double LeftFPosEdgesX[nbVertFeet] = {-0.28, -0.2, 0.0, 0.2, 0.28};
-  double LeftFPosEdgesY[nbVertFeet] = {-0.2, -0.3, -0.4, -0.3, -0.2};
-  double RightFPosEdgesX[nbVertFeet] = {-0.28, -0.2, 0.0, 0.2, 0.28};
-  double RightFPosEdgesY[nbVertFeet] = {0.2, 0.3, 0.4, 0.3, 0.2};
-
-  FootPosEdges_.LeftDS.resize(nbVertFeet);
-  FootPosEdges_.LeftSS.resize(nbVertFeet);
-  FootPosEdges_.RightDS.resize(nbVertFeet);
-  FootPosEdges_.RightSS.resize(nbVertFeet);
-
-  FootPosEdges_.LeftDS.set_vertices(LeftFPosEdgesX,LeftFPosEdgesY);
-  FootPosEdges_.LeftSS.set_vertices(LeftFPosEdgesX,LeftFPosEdgesY);
-  FootPosEdges_.RightDS.set_vertices(RightFPosEdgesX,RightFPosEdgesY);
-  FootPosEdges_.RightSS.set_vertices(RightFPosEdgesX,RightFPosEdgesY);
+  FootPosEdges_.LeftDS.resize( nbVertFeet );
+  FootPosEdges_.LeftSS.resize( nbVertFeet );
+  FootPosEdges_.RightDS.resize( nbVertFeet );
+  FootPosEdges_.RightSS.resize( nbVertFeet );
+  FootPosEdges_.LeftDS.set_vertices ( LeftFPosEdgesX_, LeftFPosEdgesY_          );
+  FootPosEdges_.LeftSS.set_vertices ( LeftFPosEdgesX_, LeftFPosEdgesY_          );
+  FootPosEdges_.RightDS.set_vertices( RightFPosEdgesX_, RightFPosEdgesY_        );
+  FootPosEdges_.RightSS.set_vertices( RightFPosEdgesX_, RightFPosEdgesY_        );
 
 
   // ZMP polygonal hulls:
@@ -164,10 +172,10 @@ RelativeFeetInequalities::set_feet_dimensions( CjrlHumanoidDynamicRobot *aHS )
     }
   LeftFoot->getSoleSize( WidthHalf,HeightHalf );
 
-  LeftFootSize_.setHalfSizeInit( WidthHalf,HeightHalf );
-  LeftFootSize_.setConstraints( SecurityMarginX_,SecurityMarginY_ );
-  RightFootSize_.setHalfSizeInit( WidthHalf,HeightHalf );
-  RightFootSize_.setConstraints( SecurityMarginX_,SecurityMarginY_ );
+  LeftFootSize_.setHalfSizeInit( WidthHalf, HeightHalf, DSFeetDistance_                 );
+  LeftFootSize_.setConstraints( SecurityMarginX_, SecurityMarginY_, DSFeetDistance_     );
+  RightFootSize_.setHalfSizeInit( WidthHalf, HeightHalf, DSFeetDistance_                );
+  RightFootSize_.setConstraints( SecurityMarginX_, SecurityMarginY_, DSFeetDistance_    );
 
   return 0;
 
@@ -325,12 +333,47 @@ RelativeFeetInequalities::CallMethod( std::string &Method, std::istringstream &A
           Args >> SecurityMarginX_;
           Args >> SecurityMarginY_;
 
-          RightFootSize_.setConstraints( SecurityMarginX_, SecurityMarginY_ );
-          LeftFootSize_.setConstraints( SecurityMarginX_, SecurityMarginY_ );
+          RightFootSize_.setConstraints( SecurityMarginX_, SecurityMarginY_ , DSFeetDistance_ );
+          LeftFootSize_.setConstraints( SecurityMarginX_, SecurityMarginY_, DSFeetDistance_ );
           init_convex_hulls();
           cout << "Security margin On X: " << SecurityMarginX_
                << " Security margin On Y: " << SecurityMarginY_ << endl;
         }
     }
+  else if( Method == ":setDSFeetDistance" )
+    {
+      Args >> DSFeetDistance_;
+      init_convex_hulls();
+      cout << "DSFeetDistance = " << DSFeetDistance_ << endl;
+    }
+  else if( Method == ":setFPosEdges" )
+    {
+      string lCmd;
+      Args >> lCmd;
 
+      if (lCmd == "X")
+        {
+          cout << "LeftFeftFPosEdgesX = ";
+          for(int i=0;i<5;i++)
+            {
+              Args >> LeftFPosEdgesX_[i];
+              RightFPosEdgesX_[i] = LeftFPosEdgesX_[i];
+              cout << LeftFPosEdgesX_[i] << "  ";
+            }
+          cout << endl;
+          init_convex_hulls();
+        }
+      else if (lCmd == "Y")
+        {
+          cout << "LeftFeftFPosEdgesY = ";
+          for(int i=0;i<5;i++)
+            {
+              Args >> LeftFPosEdgesY_[i];
+              RightFPosEdgesY_[i] = -LeftFPosEdgesY_[i];
+              cout << LeftFPosEdgesY_[i] << "  ";
+            }
+          cout << endl;
+          init_convex_hulls();
+        }
+    }
 }

src/Mathematics/relative-feet-inequalities.hh

@@ -135,9 +135,13 @@ namespace PatternGeneratorJRL
     };
     struct edges_s FootPosEdges_, ZMPPosEdges_;
 
+
     /// \brief Polyhedral hull
     convex_hull_t CoMHull_;
 
+    double LeftFPosEdgesX_[5], LeftFPosEdgesY_[5];
+    double RightFPosEdgesX_[5], RightFPosEdgesY_[5];
+
     /// \brief Half foot size
     FootHalfSize LeftFootSize_, RightFootSize_;
 

src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h

@@ -122,6 +122,9 @@ namespace PatternGeneratorJRL
 
     solution_t & Solution()
     { return Solution_; }
+
+    inline const int & QP_N(void) const
+    { return QP_N_; }
     /// \}