[Pyrene_Actuator] Add command to change load parameters

examples/pyrene_actuator/PyreneActuator.cpp

@@ -8,8 +8,7 @@ const double PyreneActuator::J = 1.4331;
 const double PyreneActuator::K = 10.6; 
 const double PyreneActuator::F_v = 0.0337;
 const double PyreneActuator::F_s = 0.9089;
-const double PyreneActuator::M =  9.2007e3;
-const double PyreneActuator::L =  20.0e3;
+const double PyreneActuator::M =  9.2007;
 const double PyreneActuator::c_y = -0.0549/M; //TODO /M
 const double PyreneActuator::c_x = 3.0835/M; //TODO /M
 const double PyreneActuator::mu = 1000.0;
@@ -25,6 +24,10 @@ PyreneActuator::PyreneActuator()
   stateNb=2;
   commandNb=1;
 
+  L =  0.0;
+  l_y = c_y;
+  l_x = c_x;
+
   Id.setIdentity();
 
   fu << 0.0, K/J;
@@ -49,13 +52,34 @@ PyreneActuator::PyreneActuator()
   upperCommandBounds << 1.0;
 }
 
+void PyreneActuator::setLoadParam(const double& mass, const double& coordX, const double& coordY)
+{
+  L =  mass;
+  l_y = coordY;
+  l_x = coordX;
+  std::cout << " L : " << L << std::endl; 
+  std::cout << " l_y : " << l_y << std::endl; 
+  std::cout << " l_x : " << l_x << std::endl; 
+}
+
+void PyreneActuator::setLoadMass(const double& mass)
+{
+  L = mass;
+  std::cout << " L : " << L << std::endl; 
+}
+
+void PyreneActuator::removeLoad()
+{
+  L = 0.0;
+}
 
 PyreneActuator::stateVec_t PyreneActuator::computeStateDeriv(double&, const stateVec_t& X,
     const commandVec_t &U)
 {
   stateVec_t dX;
   dX[0] = X[1];
-  dX[1] = ((1/J) * (K * U[0] - F_v * X[1] - F_s * tanh(mu * X[1])) - ((M+L)*g/J) * (cos(X[0])*c_x + sin(X[0])*c_y));
+  dX[1] = (1/J) * (K * U[0] - F_v * X[1] - F_s * tanh(mu * X[1])) - (M*g/J) * (cos(X[0])*c_x + sin(X[0])*c_y) \
+          - (L*g/J) * (cos(X[0])*l_x + sin(X[0])*l_y);
   return dX;
 }
 

examples/pyrene_actuator/PyreneActuator.hh

@@ -7,6 +7,9 @@ class PyreneActuator : public DynamicModel<double,2,1>
 {
 public:
     PyreneActuator();
+    void setLoadParam(const double& mass, const double& coordX, const double& coordY);
+    void setLoadMass(const double& mass);
+    void removeLoad();
     stateVec_t computeStateDeriv(double&, const stateVec_t& X, const commandVec_t &U);
     stateVec_t computeNextState(double& dt, const stateVec_t& X, const commandVec_t &U);
     void computeModelDeriv(double& dt, const stateVec_t& X, const commandVec_t &U);
@@ -19,13 +22,16 @@ public:
     static const double F_v;
     static const double F_s;
     static const double M;
-    static const double L; //load
     static const double c_x;
     static const double c_y;
     static const double mu;
     static const double g;
 
 private:
+    
+    double L; //load
+    double l_x;
+    double l_y;
     stateVec_t Xreal;
     stateMat_t Id;
     stateMat_t A;