[Pyrene_Actuator] Change mass, add setter limits and gains

examples/pyrene_actuator/PyreneActuator.cpp

@@ -13,11 +13,11 @@ const double PyreneActuator::J_m = 0.21;
 const double PyreneActuator::F_vm = 5.6714;
 const double PyreneActuator::F_sm = 4.0420;
 const double PyreneActuator::offset_m = 0.5572;
-const double PyreneActuator::M = 9.2007;
+const double PyreneActuator::M = 4.5667;
 const double PyreneActuator::c_y = 1.1005/M;
 const double PyreneActuator::c_x = -0.0981/M; 
 const double PyreneActuator::mu = 1000.0;
-const double PyreneActuator::g = -9.81;
+const double PyreneActuator::g = 9.81;
 
 /*
  * x0 -> actuator position

examples/pyrene_actuator/pyreneCostFunction.cpp

@@ -10,7 +10,7 @@ const double CostFunctionPyreneActuator::offset_m = 0.5572;
 
 CostFunctionPyreneActuator::CostFunctionPyreneActuator()
 {
-    Q << 100.0, 0.0,
+    Q << 10.0, 0.0,
          0.0, 0.01; 
     W << 1.0, 0.0,
          0.0, 0.01; 
@@ -29,18 +29,39 @@ CostFunctionPyreneActuator::CostFunctionPyreneActuator()
     alphaTau = 0.01;
 }
 
-void CostFunctionPyreneActuator::setTauLimit(double limit)
+
+void CostFunctionPyreneActuator::setCostGainState(const stateMat_t& Q_new)
+{
+    Q = Q_new;
+}
+
+void CostFunctionPyreneActuator::setCostGainStateConstraint(const stateMat_t& W_new)
+{
+    W = W_new;
+}
+
+void CostFunctionPyreneActuator::setCostGainCommand(const commandMat_t& R_new)
+{
+    R = R_new;
+}
+
+void CostFunctionPyreneActuator::setCostGainTorqueConstraint(const commandMat_t& P_new)
+{
+    P = P_new;
+}
+
+void CostFunctionPyreneActuator::setTauLimit(const double& limit)
 {
     tauLim = limit;
 }
 
-void CostFunctionPyreneActuator::setJointLimit(double limitUp, double limitDown)
+void CostFunctionPyreneActuator::setJointLimit(const double& limitUp, const double& limitDown)
 {
     jointLim.push_back(limitUp);
     jointLim.push_back(limitDown);
 }
 
-void CostFunctionPyreneActuator::setJointVelLimit(double limitUp, double limitDown)
+void CostFunctionPyreneActuator::setJointVelLimit(const double& limitUp, const double& limitDown)
 {
     jointVelLim.push_back(limitUp);
     jointVelLim.push_back(limitDown);

examples/pyrene_actuator/pyreneCostFunction.hh

@@ -8,11 +8,18 @@ class CostFunctionPyreneActuator : public CostFunction<double,2,1>
 {
 public:
     CostFunctionPyreneActuator();
+
     void computeCostAndDeriv(const stateVec_t& X,const stateVec_t& Xdes, const commandVec_t& U);
     void computeFinalCostAndDeriv(const stateVec_t& X,const stateVec_t& Xdes);
-    void setTauLimit(double limit);
-    void setJointLimit(double limitUp, double limitDown);
-    void setJointVelLimit(double limitUp, double limitDown);
+
+    void setCostGainState(const stateMat_t& Q);
+    void setCostGainStateConstraint(const stateMat_t& W);
+    void setCostGainCommand(const commandMat_t& R);
+    void setCostGainTorqueConstraint(const commandMat_t& P);
+
+    void setTauLimit(const double& limit);
+    void setJointLimit(const double& limitUp, const double& limitDown);
+    void setJointVelLimit(const double& limitUp, const double& limitDown);
     void computeConstraintsAndDeriv(const stateVec_t& X);
     void computeTauConstraintsAndDeriv(const commandVec_t& U);