diff --git a/src/PreviewControl/rigid-body-system.cpp b/src/PreviewControl/rigid-body-system.cpp
index 93bf72cbf016444e121460cfee8a0745ad61fd18..c78af9c2e0aa5784979e8439166748c4a03dd955 100644
--- a/src/PreviewControl/rigid-body-system.cpp
+++ b/src/PreviewControl/rigid-body-system.cpp
@@ -27,15 +27,18 @@
using namespace PatternGeneratorJRL;
using namespace std;
-RigidBodySystem::RigidBodySystem()
+RigidBodySystem::RigidBodySystem():
+ Mass_(0),CoMHeight_(0),T_(0),Tr_(0),Ta_(0),N_(0)
{
+ initialize();
+
}
RigidBodySystem::~RigidBodySystem()
{
-
+
}
@@ -43,6 +46,12 @@ void
RigidBodySystem::initialize()
{
+ // Initialize total mass
+ CoM_.Mass( Mass_ );
+
+ // Initialize dynamics
+ RigidBody::linear_dynamics_t & Position = CoM_.Dynamics( RigidBody::POSITION );
+ initialize_dynamics( Position );
RigidBody::linear_dynamics_t & Velocity = CoM_.Dynamics( RigidBody::VELOCITY );
initialize_dynamics( Velocity );
RigidBody::linear_dynamics_t & COP = CoM_.Dynamics( RigidBody::COP );
@@ -54,7 +63,7 @@ RigidBodySystem::initialize()
void
-RigidBodySystem::initialize_dynamics( RigidBody::linear_dynamics_t & Dynamics)
+RigidBodySystem::initialize_dynamics( RigidBody::linear_dynamics_t & Dynamics )
{
bool preserve = true;
@@ -66,62 +75,73 @@ RigidBodySystem::initialize_dynamics( RigidBody::linear_dynamics_t & Dynamics)
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;
-
- }
+ {
+ case RigidBody::POSITION:
+ for(unsigned int i=0;i<N_;i++)
+ {
+ Dynamics.S(i,0) = 1; Dynamics.S(i,1) =(i+1)* T_; Dynamics.S(i,2) = ((i+1)* T_)*((i+1)* T_)/2;
+ 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 ;
+ else
+ Dynamics.U(i,j) = Dynamics.UT(j,i) = 0.0;
+ }
+ break;
+ 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)
-{
-
-}
+//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
index 0359ab7fc52205eff4f7895eb43d9112c7eedf93..93a597bb6c9abaec81b280e8debc7f5b0d05721d 100644
--- a/src/PreviewControl/rigid-body-system.hh
+++ b/src/PreviewControl/rigid-body-system.hh
@@ -43,30 +43,28 @@ namespace PatternGeneratorJRL
~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
+// /// \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 and mutators
/// \{
inline const RigidBody operator ()() const
{return CoM_;};
@@ -82,6 +80,21 @@ namespace PatternGeneratorJRL
{ return Ta_; }
inline void SamplingPeriodAct( double Ta )
{ Ta_ = Ta; }
+
+ inline unsigned const & NbSamplingsPreviewed( ) const
+ { return N_; }
+ inline void NbSamplingsPreviewed( unsigned N )
+ { N_ = N; }
+
+ inline double const & Mass( ) const
+ { return Mass_; }
+ inline void Mass( double Mass )
+ { Mass_ = Mass; }
+
+ inline double const & CoMHeight( ) const
+ { return CoMHeight_; }
+ inline void CoMHeight( double Height )
+ { CoMHeight_ = Height; }
/// \}
@@ -93,7 +106,7 @@ namespace PatternGeneratorJRL
/// \brief Initialize dynamics matrices
///
/// \param[in] Dynamics Matrices to be filled
- void initialize_dynamics( RigidBody::linear_dynamics_t & Dynamics);
+ void initialize_dynamics( RigidBody::linear_dynamics_t & Dynamics );
//
// Private members
@@ -103,6 +116,9 @@ namespace PatternGeneratorJRL
/// \brief Body
RigidBody CoM_;
+ /// \brief Total robot mass
+ double Mass_;
+
/// \brief CoMHeight
double CoMHeight_;