Initialization

- Add initialization 
- Add acceleration and position dynamics

src/PreviewControl/rigid-body.cpp

@@ -28,9 +28,14 @@ using namespace PatternGeneratorJRL;
 using namespace std;
 
 RigidBody::RigidBody():
-    T_(0),Ta_(0),N_(0),Mass_(0)
+    T_(0),Tr_(0),Ta_(0),N_(0),Mass_(0)
 {
 
+  PositionDynamics_.Type = POSITION;
+  VelocityDynamics_.Type = VELOCITY;
+  AccelerationDynamics_.Type = ACCELERATION;
+  JerkDynamics_.Type = JERK;
+
 }
 
 
@@ -73,12 +78,16 @@ RigidBody::increment_state(double Control)
 // ACCESSORS:
 // ----------
 RigidBody::linear_dynamics_t const &
-RigidBody::Dynamics( const int type ) const
+RigidBody::Dynamics( const int Type ) const
 {
-  switch(type)
+  switch(Type)
   {
+  case POSITION:
+    return PositionDynamics_;
   case VELOCITY:
     return VelocityDynamics_;
+  case ACCELERATION:
+    return AccelerationDynamics_;
   case JERK:
     return JerkDynamics_;
   }
@@ -88,12 +97,16 @@ RigidBody::Dynamics( const int type ) const
 }
 
 RigidBody::linear_dynamics_t &
-RigidBody::Dynamics( const int type )
+RigidBody::Dynamics( const int Type )
 {
-  switch(type)
+  switch(Type)
   {
+  case POSITION:
+    return PositionDynamics_;
   case VELOCITY:
     return VelocityDynamics_;
+  case ACCELERATION:
+    return AccelerationDynamics_;
   case JERK:
     return JerkDynamics_;
   }
@@ -136,14 +149,13 @@ void
 RigidBody::rigid_body_state_t::reset()
 {
 
-  // Redimension
   X.resize(3,false);
   Y.resize(3,false);
   Z.resize(3,false);
   Yaw.resize(3,false);
   Pitch.resize(3,false);
   Roll.resize(3,false);
-  // Set to zero
+
   X.clear();
   Y.clear();
   Z.clear();

src/PreviewControl/rigid-body.hh

@@ -96,7 +96,7 @@ namespace PatternGeneratorJRL
       /// \brief State matrix
       boost_ublas::matrix<double> S;
 
-      unsigned int Type;
+      int Type;
     };
     typedef linear_dynamics_s linear_dynamics_t;
     /// \}
@@ -136,9 +136,8 @@ namespace PatternGeneratorJRL
 
     /// \name Accessors
     /// \{
-
-    linear_dynamics_t const & Dynamics( int type ) const;
-    linear_dynamics_t & Dynamics( int type );
+    linear_dynamics_t const & Dynamics( int Type ) const;
+    linear_dynamics_t & Dynamics( int Type );
 
     inline double const & SamplingPeriodSim( ) const
     { return T_; }
@@ -149,6 +148,11 @@ namespace PatternGeneratorJRL
     { return Ta_; }
     inline void SamplingPeriodAct( double Ta )
     { Ta_ = Ta; }
+
+    inline double const & Mass( ) const
+    { return Mass_; }
+    inline void Mass( double Mass )
+    { Mass_ = Mass; }
     /// \}
 
     
@@ -178,8 +182,6 @@ namespace PatternGeneratorJRL
     
     /// \brief Sampling period actuators
     double Ta_;
-    
-    
 
     /// \brief Nb previewed samples
     unsigned int N_;