diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 21a225f476da156643780924ab5af7e2304b54d6..81bb1530a3748643066759269d195890afb20664 100644
--- a/src/CMakeLists.txt
+++ b/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
diff --git a/src/PreviewControl/rigid-body-system.cpp b/src/PreviewControl/rigid-body-system.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..93bf72cbf016444e121460cfee8a0745ad61fd18
--- /dev/null
+++ b/src/PreviewControl/rigid-body-system.cpp
@@ -0,0 +1,127 @@
+/*
+ * 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)
+{
+
+}
diff --git a/src/PreviewControl/rigid-body-system.hh b/src/PreviewControl/rigid-body-system.hh
new file mode 100644
index 0000000000000000000000000000000000000000..0359ab7fc52205eff4f7895eb43d9112c7eedf93
--- /dev/null
+++ b/src/PreviewControl/rigid-body-system.hh
@@ -0,0 +1,124 @@
+/*
+ * 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_ */
diff --git a/src/PreviewControl/rigid-body.cpp b/src/PreviewControl/rigid-body.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..031e203d5882e4d5a30a82260a75786d27b149fe
--- /dev/null
+++ b/src/PreviewControl/rigid-body.cpp
@@ -0,0 +1,154 @@
+/*
+ * 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();
+
+}
diff --git a/src/PreviewControl/rigid-body.hh b/src/PreviewControl/rigid-body.hh
new file mode 100644
index 0000000000000000000000000000000000000000..d317aba8cc017cd4c052503801d5b711a949ee05
--- /dev/null
+++ b/src/PreviewControl/rigid-body.hh
@@ -0,0 +1,192 @@
+/*
+ * 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_ */
diff --git a/src/privatepgtypes.h b/src/privatepgtypes.h
index 01524ecb42873d0e0ec1dff27ab86fa596441ccb..681e37ed99cd514924bd8737dbabaf3afb0dacd4 100644
--- a/src/privatepgtypes.h
+++ b/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);