Merge branch 'topic/new-classes' into topic/door

src/CMakeLists.txt

@@ -47,6 +47,8 @@ SET(SOURCES
   PreviewControl/OptimalControllerSolver.cpp
   PreviewControl/ZMPPreviewControlWithMultiBodyZMP.cpp
   PreviewControl/LinearizedInvertedPendulum2D.cpp
+  PreviewControl/rigid-body.cpp
+  PreviewControl/rigid-body-system.cpp
   PreviewControl/SupportFSM.cpp
   ZMPRefTrajectoryGeneration/ZMPRefTrajectoryGeneration.cpp
   ZMPRefTrajectoryGeneration/ZMPDiscretization.cpp

src/PreviewControl/rigid-body-system.cpp

+/*
+ * Copyright 2011
+ *
+ * Andrei Herdt
+ *
+ * JRL, CNRS/AIST, INRIA Grenoble-Rhone-Alpes
+ *
+ * This file is part of walkGenJrl.
+ * walkGenJrl is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * walkGenJrl is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Lesser Public License for more details.
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with walkGenJrl.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/// Simulate a rigid body
+
+#include <PreviewControl/rigid-body-system.hh>
+
+using namespace PatternGeneratorJRL;
+using namespace std;
+
+RigidBodySystem::RigidBodySystem()
+{
+
+}
+
+
+RigidBodySystem::~RigidBodySystem()
+{
+  
+}
+
+
+void
+RigidBodySystem::initialize()
+{
+
+  RigidBody::linear_dynamics_t & Velocity = CoM_.Dynamics( RigidBody::VELOCITY );
+  initialize_dynamics( Velocity );
+  RigidBody::linear_dynamics_t & COP = CoM_.Dynamics( RigidBody::COP );
+  initialize_dynamics( COP );
+  RigidBody::linear_dynamics_t & Jerk = CoM_.Dynamics( RigidBody::JERK );
+  initialize_dynamics( Jerk );
+
+}
+
+
+void
+RigidBodySystem::initialize_dynamics( RigidBody::linear_dynamics_t & Dynamics)
+{
+
+  bool preserve = true;
+  Dynamics.U.resize(N_,N_,!preserve);
+  Dynamics.U.clear();
+  Dynamics.UT.resize(N_,N_,!preserve);
+  Dynamics.UT.clear();
+  Dynamics.S.resize(N_,3,!preserve);
+  Dynamics.S.clear();
+
+  switch(Dynamics.Type)
+    {
+    case RigidBody::VELOCITY:
+      for(unsigned int i=0;i<N_;i++)
+        {
+          Dynamics.S(i,0) = 0.0; Dynamics.S(i,1) = 1.0; Dynamics.S(i,2) = (i+1)*T_;
+          for(unsigned int j=0;j<N_;j++)
+            if (j<=i)
+              Dynamics.U(i,j) = Dynamics.UT(j,i) = (2*(i-j)+1)*T_*T_*0.5 ;
+            else
+              Dynamics.U(i,j) = Dynamics.UT(j,i) = 0.0;
+        }
+      break;
+
+    case RigidBody::COP:
+      for(unsigned int i=0;i<N_;i++)
+        {
+          Dynamics.S(i,0) = 1.0; Dynamics.S(i,1) = (i+1)*T_; Dynamics.S(i,2) = (i+1)*(i+1)*T_*T_*0.5-CoMHeight_/9.81;
+          for(unsigned int j=0;j<N_;j++)
+            if (j<=i)
+              Dynamics.U(i,j) = Dynamics.UT(j,i) = (1 + 3*(i-j) + 3*(i-j)*(i-j)) * T_*T_*T_/6.0 - T_*CoMHeight_/9.81;
+            else
+              Dynamics.U(i,j) = Dynamics.UT(j,i) = 0.0;
+        }
+      break;
+
+    case RigidBody::JERK:
+      for(unsigned int i=0;i<N_;i++)
+        {
+          Dynamics.S(i,0) = 0.0; Dynamics.S(i,1) = 0.0; Dynamics.S(i,2) = 0.0;
+          for(unsigned int j=0;j<N_;j++)
+            if (j==i)
+              Dynamics.U(i,j) = Dynamics.UT(j,i) = 1.0;
+            else
+              Dynamics.U(i,j) = Dynamics.UT(j,i) = 0.0;
+        }
+      break;
+
+    }
+
+}
+
+
+int 
+RigidBodySystem::interpolate(deque<COMState> &COMStates,
+						deque<ZMPPosition> &ZMPRefPositions,
+						int CurrentPosition,
+						double CX, double CY)
+{
+  
+  return 0;
+  
+}
+
+
+void
+RigidBodySystem::increment_state(double Control)
+{
+
+}

src/PreviewControl/rigid-body-system.hh

+/*
+ * Copyright 2011
+ *
+ * Andrei Herdt
+ *
+ * JRL, CNRS/AIST, INRIA Grenoble-Rhone-Alpes
+ *
+ * This file is part of walkGenJrl.
+ * walkGenJrl is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * walkGenJrl is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Lesser Public License for more details.
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with walkGenJrl.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/// \doc Simulate a rigid body 
+
+
+#ifndef _RIGID_BODY_SYSTEM_
+#define _RIGID_BODY_SYSTEM_
+
+#include <PreviewControl/rigid-body.hh>
+
+namespace PatternGeneratorJRL
+{
+  class  RigidBodySystem
+  {
+
+    //
+    // Public methods
+    //
+  public:
+
+    RigidBodySystem();
+
+    ~RigidBodySystem();
+
+    /// \brief Initialize
+    ///
+    /// \return 0
+    void initialize();
+
+    /// \brief Interpolate
+    int interpolate(std::deque<COMState> &COMStates,
+		      std::deque<ZMPPosition> &ZMPRefPositions,
+		      int CurrentPosition,
+		      double CX, double CY);
+
+    /// \brief Increment the state
+    ///
+    /// \param[in] Control Control vector
+    void increment_state( double Control );
+
+    /// \brief Decouple degree of freedom by injected trajectory
+    ///
+    /// \param[in] Axis The axis to be decoupled
+    /// \param[in] Trajectory The injected trajectory
+    ///
+    /// \return 0
+    int inject_trajectory( unsigned int Axis, boost_ublas::vector<double> Trajectory );
+
+    /// \name Accessors
+    /// \{
+    inline const RigidBody operator ()() const
+    {return CoM_;};
+    inline void operator ()( RigidBody Body )
+    {CoM_ = Body;};
+
+    inline double const & SamplingPeriodSim( ) const
+    { return T_; }
+    inline void SamplingPeriodSim( double T )
+    { T_ = T; }
+
+    inline double const & SamplingPeriodAct( ) const
+    { return Ta_; }
+    inline void SamplingPeriodAct( double Ta )
+    { Ta_ = Ta; }
+    /// \}
+
+    
+    //
+    // Private methods
+    //
+  private:
+
+    /// \brief Initialize dynamics matrices
+    ///
+    /// \param[in] Dynamics Matrices to be filled
+    void initialize_dynamics( RigidBody::linear_dynamics_t & Dynamics);
+
+    //
+    // Private members
+    //
+  private:
+
+    /// \brief Body
+    RigidBody CoM_;
+    
+    /// \brief CoMHeight
+    double CoMHeight_;
+
+    /// \brief Sampling period simulation
+    double T_;
+
+    /// \brief Recalculation period
+    /// The state is incremented with respect to this parameter
+    double Tr_;
+    
+    /// \brief Sampling period actuators
+    double Ta_;
+    
+    /// \brief Nb previewed samples
+    unsigned int N_;
+
+  };
+}
+#endif /* _RIGID_BODY_SYSTEM_ */

src/PreviewControl/rigid-body.cpp

+/*
+ * Copyright 2011
+ *
+ * Andrei Herdt
+ *
+ * JRL, CNRS/AIST, INRIA Grenoble-Rhone-Alpes
+ *
+ * This file is part of walkGenJrl.
+ * walkGenJrl is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * walkGenJrl is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Lesser Public License for more details.
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with walkGenJrl.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/// Simulate a rigid body
+
+#include <PreviewControl/rigid-body.hh>
+
+using namespace PatternGeneratorJRL;
+using namespace std;
+
+RigidBody::RigidBody():
+    T_(0),Ta_(0),N_(0),Mass_(0)
+{
+
+}
+
+
+RigidBody::~RigidBody()
+{
+  
+}
+
+
+int 
+RigidBody::initialize()
+{
+  
+  return 0;
+
+}
+
+
+int 
+RigidBody::interpolate(deque<COMState> &COMStates,
+						deque<ZMPPosition> &ZMPRefPositions,
+						int CurrentPosition,
+						double CX, double CY)
+{
+  
+  return 0;
+  
+}
+
+
+RigidBody::rigid_body_state_t
+RigidBody::increment_state(double Control)
+{
+
+  return State_;
+  
+}
+
+
+// ACCESSORS:
+// ----------
+RigidBody::linear_dynamics_t const &
+RigidBody::Dynamics( const int type ) const
+{
+  switch(type)
+  {
+  case VELOCITY:
+    return VelocityDynamics_;
+  case JERK:
+    return JerkDynamics_;
+  }
+
+  // Default
+  return VelocityDynamics_;
+}
+
+RigidBody::linear_dynamics_t &
+RigidBody::Dynamics( const int type )
+{
+  switch(type)
+  {
+  case VELOCITY:
+    return VelocityDynamics_;
+  case JERK:
+    return JerkDynamics_;
+  }
+
+  // Default
+  return VelocityDynamics_;
+}
+
+
+// INTERNAL TYPE METHODS:
+// ----------------------
+RigidBody::rigid_body_state_s::rigid_body_state_s()
+{
+
+  reset();
+
+}
+
+
+struct RigidBody::rigid_body_state_s &
+RigidBody::rigid_body_state_t::operator=(const RigidBody::rigid_body_state_s & RB)
+{
+
+  for(unsigned int i=0;i<3;i++)
+    {
+      X(i) = RB.X(i);
+      Y[i] = RB.Y[i];
+      Z[i] = RB.Z[i];
+
+      Yaw[i] = RB.Yaw[i];
+      Pitch[i] = RB.Pitch[i];
+      Roll[i] = RB.Roll[i];
+    };
+  return *this;
+
+}
+
+
+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();
+  Yaw.clear();
+  Pitch.clear();
+  Roll.clear();
+
+}

src/PreviewControl/rigid-body.hh

+/*
+ * Copyright 2011
+ *
+ * Andrei Herdt
+ *
+ * JRL, CNRS/AIST, INRIA Grenoble-Rhone-Alpes
+ *
+ * This file is part of walkGenJrl.
+ * walkGenJrl is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * walkGenJrl is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Lesser Public License for more details.
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with walkGenJrl.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/// \doc Simulate a rigid body 
+
+
+#ifndef _RIGID_BODY_
+#define _RIGID_BODY_
+
+#include <jrl/mal/matrixabstractlayer.hh>
+#include <deque>
+#include <jrl/walkgen/pgtypes.hh>
+#include <privatepgtypes.h>
+
+namespace PatternGeneratorJRL
+{
+  class RigidBody
+  {
+
+    //
+    // Public types
+    //
+  public:
+
+    /// \name Axes
+    /// \{
+    const static int X_AXIS = 0;
+    const static int Y_AXIS = 1;
+    const static int Z_AXIS = 2;
+    const static int PITCH = 3;
+    const static int ROLL = 4;
+    const static int YAW = 5;
+    /// \}
+
+    /// \name Axes
+    /// \{
+    const static int POSITION = 10;
+    const static int VELOCITY = 11;
+    const static int ACCELERATION = 12;
+    const static int JERK = 13;
+    const static int COP = 14;
+    /// \}
+
+    /// \brief State vectors
+    struct rigid_body_state_s
+    {
+      /// \name Translational degrees of freedom
+      /// \{
+      boost_ublas::vector<double> X;
+      boost_ublas::vector<double> Y;
+      boost_ublas::vector<double> Z;
+      /// \}
+      /// \name Rotational degrees of freedom
+      /// \{
+      boost_ublas::vector<double> Pitch;
+      boost_ublas::vector<double> Roll;
+      boost_ublas::vector<double> Yaw;
+      /// \}
+
+      struct rigid_body_state_s & operator=(const rigid_body_state_s &RB);
+
+      void reset();
+
+      rigid_body_state_s();
+    };
+    typedef struct rigid_body_state_s rigid_body_state_t;
+
+    /// \name Dynamics matrices
+    /// \{
+    struct linear_dynamics_s
+    {
+      /// \brief Control matrix
+      boost_ublas::matrix<double> U;
+      /// \brief Transpose of control matrix
+      boost_ublas::matrix<double> UT;
+
+      /// \brief State matrix
+      boost_ublas::matrix<double> S;
+
+      unsigned int Type;
+    };
+    typedef linear_dynamics_s linear_dynamics_t;
+    /// \}
+
+    //
+    // Public methods
+    //
+  public:
+
+    RigidBody();
+
+    ~RigidBody();
+
+    /// \brief Initialize
+    ///
+    /// \return 0
+    int initialize();
+
+    /// \brief Interpolate
+    int interpolate(std::deque<COMState> &COMStates,
+		      std::deque<ZMPPosition> &ZMPRefPositions,
+		      int CurrentPosition,
+		      double CX, double CY);
+
+    /// \brief Increment the state
+    ///
+    /// \param[in] Control Control vector
+    rigid_body_state_t increment_state( double Control );
+
+    /// \brief Decouple degree of freedom by injected trajectory
+    ///
+    /// \param[in] Axis The axis to be decoupled
+    /// \param[in] Trajectory The injected trajectory
+    ///
+    /// \return 0
+    int inject_trajectory( unsigned int Axis, boost_ublas::vector<double> Trajectory );
+
+    /// \name Accessors
+    /// \{
+
+    linear_dynamics_t const & Dynamics( int type ) const;
+    linear_dynamics_t & Dynamics( int type );
+
+    inline double const & SamplingPeriodSim( ) const
+    { return T_; }
+    inline void SamplingPeriodSim( double T )
+    { T_ = T; }
+
+    inline double const & SamplingPeriodAct( ) const
+    { return Ta_; }
+    inline void SamplingPeriodAct( double Ta )
+    { Ta_ = Ta; }
+    /// \}
+
+    
+    //
+    // Private members
+    //
+  private:
+
+    /// \brief State
+    rigid_body_state_t State_;
+
+    /// \name Dynamics
+    /// \{
+    linear_dynamics_t
+    PositionDynamics_,
+    VelocityDynamics_,
+    AccelerationDynamics_,
+    JerkDynamics_;
+    /// \}
+    
+    /// \brief Sampling period simulation
+    double T_;
+
+    /// \brief Recalculation period
+    /// The state is incremented with respect to this parameter
+    double Tr_;
+    
+    /// \brief Sampling period actuators
+    double Ta_;
+    
+    
+
+    /// \brief Nb previewed samples
+    unsigned int N_;
+    
+    /// \brief Mass
+    double Mass_;
+
+  };
+}
+#endif /* _RIGID_BODY_ */

src/privatepgtypes.h

@@ -72,7 +72,7 @@ namespace PatternGeneratorJRL
   };
   typedef struct support_state_s support_state_t;
 
-  /// \brief Support state of the robot at a certain point in time
+  /// \brief State of the center of mass
   struct com_s
   {
     MAL_VECTOR(x,double);