Commit 9593f31e authored by NoelieRamuzat's avatar NoelieRamuzat Committed by Noëlie RAMUZAT
Browse files

[Pyrene_Actuator] Add setter for gains and limits

parent 64205b75
......@@ -28,6 +28,7 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <vector>
#include <tsid/utils/stop-watch.hpp>
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
......@@ -82,18 +83,28 @@ class SOTDDPPYRENEACTUATORSOLVER_EXPORT DdpPyreneActuatorSolver
* @param T Size of the preview window (in nb of timestep).
* @param nbItMax Maximum number of iterations.
* @param stopCriteria The value of the stopping criteria.
* @param tauLim Maximum torque to be applied on joint.
*/
void param_init(const double &timestep,
const int &T,
const int &nbItMax,
const double &stopCriteria);
// /* --- SETTER LIM --- */
void setTorqueLimit(const double& tau);
void setJointLimit(const double& upperLim, const double& lowerLim);
void setJointVelLimit(const double& upperLim, const double& lowerLim);
// /* --- SETTER LOAD --- */
void setLoadParam(const double& mass, const double& coordX, const double& coordY);
void setLoadMass(const double& mass);
void removeLoad();
// /* --- SETTER GAINS --- */
void setCostGainState(const dynamicgraph::Vector& Q);
void setCostGainStateConstraint(const dynamicgraph::Vector& W);
void setCostGainCommand(const dynamicgraph::Vector& R);
void setCostGainTorqueConstraint(const dynamicgraph::Vector& P);
};
} // namespace torque_control
} // namespace sot
......
......@@ -101,6 +101,20 @@ DdpPyreneActuatorSolver(const std::string &name)
"Size of the preview window (in nb of samples)",
"Max. nb. of iterations",
"Stopping criteria")));
addCommand("setTorqueLimit",
makeCommandVoid1(*this, &DdpPyreneActuatorSolver::setTorqueLimit,
docCommandVoid1("Set the Torque limit.",
"Limit of the motor torque.")));
addCommand("setJointLimit",
makeCommandVoid2(*this, &DdpPyreneActuatorSolver::setJointLimit,
docCommandVoid2("Set the angular limits of the joint.",
"Upper limit",
"Lower limit.")));
addCommand("setJointVelLimit",
makeCommandVoid2(*this, &DdpPyreneActuatorSolver::setJointVelLimit,
docCommandVoid2("Set the angular velocity limits of the joint.",
"Upper limit",
"Lower limit.")));
addCommand("setLoadParam",
makeCommandVoid3(*this, &DdpPyreneActuatorSolver::setLoadParam,
docCommandVoid3("Setter of the Load parameters.",
......@@ -115,6 +129,23 @@ DdpPyreneActuatorSolver(const std::string &name)
makeCommandVoid0(*this, &DdpPyreneActuatorSolver::removeLoad,
docCommandVoid0("Remove the Load.")));
addCommand("setCostGainState",
makeCommandVoid1(*this, &DdpPyreneActuatorSolver::setCostGainState,
docCommandVoid1("Set the Gain of the state cost matrix.",
"Matrix of Gains.")));
addCommand("setCostGainCommand",
makeCommandVoid1(*this, &DdpPyreneActuatorSolver::setCostGainCommand,
docCommandVoid1("Set the Gain of the command cost matrix.",
"Matrix of Gains.")));
addCommand("setCostGainStateConstraint",
makeCommandVoid1(*this, &DdpPyreneActuatorSolver::setCostGainStateConstraint,
docCommandVoid1("Set the Gain of the constraints on the state.",
"Matrix of Gains.")));
addCommand("setCostGainTorqueConstraint",
makeCommandVoid1(*this, &DdpPyreneActuatorSolver::setCostGainTorqueConstraint,
docCommandVoid1("Set the Gain of the torque constraints.",
"Matrix of Gains.")));
m_initSucceeded = true;
}
......@@ -196,6 +227,21 @@ void DdpPyreneActuatorSolver::param_init(const double &timestep,
m_T , m_dt, m_iterMax, m_stopCrit);
}
void DdpPyreneActuatorSolver::setTorqueLimit(const double& tau)
{
m_cost.setTauLimit(tau);
}
void DdpPyreneActuatorSolver::setJointLimit(const double& upperLim, const double& lowerLim)
{
m_cost.setJointLimit(upperLim, lowerLim);
}
void DdpPyreneActuatorSolver::setJointVelLimit(const double& upperLim, const double& lowerLim)
{
m_cost.setJointVelLimit(upperLim, lowerLim);
}
void DdpPyreneActuatorSolver::setLoadParam(const double& mass, const double& coordX, const double& coordY)
{
m_model.setLoadParam(mass, coordX, coordY);
......@@ -210,6 +256,31 @@ void DdpPyreneActuatorSolver::removeLoad()
{
m_model.removeLoad();
}
void DdpPyreneActuatorSolver::setCostGainState(const dynamicgraph::Vector& Q)
{
const CostFunction<double,2,1>::stateMat_t Q_new = Eigen::Map<const CostFunction<double,2,1>::stateMat_t, Eigen::Unaligned >(Q.data(),2,1);
m_cost.setCostGainState(Q_new);
}
void DdpPyreneActuatorSolver::setCostGainStateConstraint(const dynamicgraph::Vector& W)
{
const CostFunction<double,2,1>::stateMat_t W_new = Eigen::Map<const CostFunction<double,2,1>::stateMat_t, Eigen::Unaligned >(W.data(),2,1);
m_cost.setCostGainStateConstraint(W_new);
}
void DdpPyreneActuatorSolver::setCostGainCommand(const dynamicgraph::Vector& R)
{
const CostFunction<double,2,1>::commandMat_t R_new = Eigen::Map<const CostFunction<double,2,1>::commandMat_t, Eigen::Unaligned >(R.data(),1);
m_cost.setCostGainCommand(R_new);
}
void DdpPyreneActuatorSolver::setCostGainTorqueConstraint(const dynamicgraph::Vector& P)
{
const CostFunction<double,2,1>::commandMat_t P_new = Eigen::Map<const CostFunction<double,2,1>::commandMat_t, Eigen::Unaligned >(P.data(),1);
m_cost.setCostGainTorqueConstraint(P_new);
}
void DdpPyreneActuatorSolver::display(std::ostream &os) const
{
os << " T: " << m_T
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment