see previous commit...

src/PreviewControl/rigid-body-system.cpp

@@ -27,11 +27,12 @@
 using namespace PatternGeneratorJRL;
 using namespace std;
 
-RigidBodySystem::RigidBodySystem( SimplePluginManager *SPM, CjrlHumanoidDynamicRobot *aHS ):
+RigidBodySystem::RigidBodySystem( SimplePluginManager * SPM, CjrlHumanoidDynamicRobot * aHS, SupportFSM * FSM ):
     Mass_(0),CoMHeight_(0),T_(0),Tr_(0),Ta_(0),N_(0),
-    OFTG_(0)
+    OFTG_(0), FSM_(0)
 {
 
+  FSM_ = FSM;
   OFTG_ = new OnLineFootTrajectoryGeneration(SPM,aHS->leftFoot());
 
 }
@@ -56,6 +57,7 @@ RigidBodySystem::initialize(  )
   OFTG_->SetDoubleSupportTime(T_);
   OFTG_->QPSamplingPeriod(T_);
   OFTG_->FeetDistance(0.2);
+  OFTG_->StepHeight( 0.05 );
 
   // Initialize dynamics
   // -------------------
@@ -65,15 +67,43 @@ RigidBodySystem::initialize(  )
   compute_dyn_cjerk();
   compute_dyn_cop();
 
+  precompute_trajectory();
   LeftFoot_.NbSamplingsPreviewed( N_ );
   LeftFoot_.initialize();
   RightFoot_.NbSamplingsPreviewed( N_ );
   RightFoot_.initialize();
 
 
+
 }
 
 
+int
+RigidBodySystem::precompute_trajectory()
+{
+
+  // Vertical foot trajectory of a stance phase
+  // starting from the begin of the simple support phase
+  // and ending at the end of the double support phase.
+  double SSPeriod = FSM_->StepPeriod()-T_;
+  unsigned int NbInstantsSS = (unsigned int)(SSPeriod/T_)+1;
+
+  OFTG_->SetParameters(FootTrajectoryGenerationStandard::Z_AXIS, SSPeriod, OFTG_->StepHeight() );
+
+  FlyingFootTrajectory_.resize(NbInstantsSS);
+  std::deque<rigid_body_state_t>::iterator FTIt;
+  FTIt = FlyingFootTrajectory_.begin();
+  for( unsigned int i = 0; i < NbInstantsSS; i++)
+    {
+      FTIt->Z[0] = OFTG_->Compute(FootTrajectoryGenerationStandard::Z_AXIS,i*T_);
+      FTIt->Z[2] = OFTG_->ComputeSecDerivative(FootTrajectoryGenerationStandard::Z_AXIS,i*T_);
+      FTIt++;
+    }
+
+  return 0;
+
+}
+
 int
 RigidBodySystem::update( const SupportFSM * FSM, support_state_t & CurrentSupport, double Time )
 {

src/PreviewControl/rigid-body-system.hh

@@ -44,7 +44,7 @@ namespace PatternGeneratorJRL
     //
   public:
 
-    RigidBodySystem( SimplePluginManager *SPM, CjrlHumanoidDynamicRobot *aHS );
+    RigidBodySystem( SimplePluginManager *SPM, CjrlHumanoidDynamicRobot *aHS, SupportFSM * FSM );
 
     ~RigidBodySystem();
 
@@ -162,6 +162,9 @@ namespace PatternGeneratorJRL
     int compute_dyn_pol_feet( linear_dynamics_t & LeftFootDynamics, linear_dynamics_t & RightFootDynamics,
         const SupportFSM * FSM, const support_state_t & CurrentSupport, double Time );
 
+    /// \brief Precompute trajectories
+    int precompute_trajectory();
+
     /// \brief Compute a row of the dynamic matrix Sp
     int compute_sbar( double * Spbar, double * Sabar, double T, double Td );
 
@@ -179,13 +182,14 @@ namespace PatternGeneratorJRL
     CoM_,
     LeftFoot_,
     RightFoot_;
-    
+
     /// \brief Center of Pressure dynamics
     linear_dynamics_t
     CoPDynamics_;
 
-    /// \brief Standard polynomial trajectories for the feet.
-    OnLineFootTrajectoryGeneration * OFTG_;
+    /// \brief Fixed trajectories
+    std::deque<rigid_body_state_t>
+    FlyingFootTrajectory_;
 
     /// \brief Total robot mass
     double Mass_;
@@ -206,6 +210,12 @@ namespace PatternGeneratorJRL
     /// \brief Nb previewed samples
     unsigned int N_;
 
+    /// \brief Standard polynomial trajectories for the feet.
+    OnLineFootTrajectoryGeneration * OFTG_;
+
+    /// \brief Finite State Machine
+    SupportFSM * FSM_;
+
   };
 }
 #endif /* _RIGID_BODY_SYSTEM_ */

src/PreviewControl/rigid-body.cpp

@@ -28,7 +28,7 @@ using namespace PatternGeneratorJRL;
 using namespace std;
 
 RigidBody::RigidBody():
-        T_(0),Tr_(0),Ta_(0),N_(0),Mass_(0)
+        T_(0),Tr_(0),Ta_(0),N_(16),Mass_(0)
 {
 
   PositionDynamics_.Type = POSITION;
@@ -146,7 +146,7 @@ rigid_body_state_t::operator=(const rigid_body_state_s & RB)
 
   for(unsigned int i=0;i<3;i++)
     {
-      X(i) = RB.X(i);
+      X[i] = RB.X[i];
       Y[i] = RB.Y[i];
       Z[i] = RB.Z[i];
 

src/PreviewControl/rigid-body.hh

@@ -49,15 +49,15 @@ namespace PatternGeneratorJRL
   {
     /// \name Translational degrees of freedom
     /// \{
-    boost_ublas::vector<double> X;
-    boost_ublas::vector<double> Y;
-    boost_ublas::vector<double> Z;
+    boost_ublas::bounded_vector<double, 3> X;
+    boost_ublas::bounded_vector<double, 3> Y;
+    boost_ublas::bounded_vector<double, 3> Z;
     /// \}
     /// \name Rotational degrees of freedom
     /// \{
-    boost_ublas::vector<double> Pitch;
-    boost_ublas::vector<double> Roll;
-    boost_ublas::vector<double> Yaw;
+    boost_ublas::bounded_vector<double, 3> Pitch;
+    boost_ublas::bounded_vector<double, 3> Roll;
+    boost_ublas::bounded_vector<double, 3> Yaw;
     /// \}
 
     struct rigid_body_state_s & operator=(const rigid_body_state_s &RB);
@@ -169,6 +169,9 @@ namespace PatternGeneratorJRL
     /// \brief State
     rigid_body_state_t State_;
 
+    /// \brief Trajectory
+    std::deque<rigid_body_state_t> Trajectory_;
+
     /// \name Dynamics
     /// \{
     linear_dynamics_t