17 #define EIGEN_NO_MALLOC
18 #include <Eigen/Dense>
19 #include <dynamic-graph/factory.h>
20 #include <sot/core/debug.hh>
26 #define ODEBUG(x) std::cout << x << std::endl
30 #define ODEBUG3(x) std::cout << x << std::endl
32 #define DBGFILE "/tmp/debug-ddp_pyrene_actuator_solver.dat"
34 #define RESETDEBUG5() { std::ofstream DebugFile; \
35 DebugFile.open(DBGFILE,std::ofstream::out); \
37 #define ODEBUG5FULL(x) { std::ofstream DebugFile; \
38 DebugFile.open(DBGFILE,std::ofstream::app); \
39 DebugFile << __FILE__ << ":" \
40 << __FUNCTION__ << "(#" \
41 << __LINE__ << "):" << x << std::endl; \
43 #define ODEBUG5(x) { std::ofstream DebugFile; \
44 DebugFile.open(DBGFILE,std::ofstream::app); \
45 DebugFile << x << std::endl; \
49 #define ODEBUG4FULL(x)
62 using namespace Eigen;
64 #define ALL_INPUT_SIGNALS m_pos_desSIN << m_pos_joint_measureSIN << m_dx_joint_measureSIN << m_tau_desSIN
66 #define ALL_OUTPUT_SIGNALS m_tauSOUT
79 CONSTRUCT_SIGNAL_IN (pos_joint_measure,
dynamicgraph::Vector),
80 CONSTRUCT_SIGNAL_IN (dx_joint_measure,
dynamicgraph::Vector),
87 m_solver(m_model, m_cost,
100 docCommandVoid4(
"Initialize the DDP solver.",
101 "Control timestep [s].",
102 "Size of the preview window (in nb of samples)",
103 "Max. nb. of iterations",
104 "Stopping criteria")));
105 addCommand(
"setTorqueLimit",
107 docCommandVoid1(
"Set the Torque limit.",
108 "Limit of the motor torque.")));
109 addCommand(
"setJointLimit",
111 docCommandVoid2(
"Set the angular limits of the joint.",
114 addCommand(
"setJointVelLimit",
116 docCommandVoid2(
"Set the angular velocity limits of the joint.",
119 addCommand(
"setLoadParam",
121 docCommandVoid3(
"Setter of the Load parameters.",
122 "Mass of the load [g].",
123 "X coordinate of the Load",
124 "Y coordinate of the Load")));
125 addCommand(
"setLoadMass",
127 docCommandVoid1(
"Set the Load mass.",
128 "Mass of the load [g].")));
129 addCommand(
"removeLoad",
131 docCommandVoid0(
"Remove the Load.")));
133 addCommand(
"setCostGainState",
135 docCommandVoid1(
"Set the Gain of the state cost matrix.",
136 "Matrix of Gains.")));
137 addCommand(
"setCostGainCommand",
139 docCommandVoid1(
"Set the Gain of the command cost matrix.",
140 "Matrix of Gains.")));
141 addCommand(
"setCostGainStateConstraint",
143 docCommandVoid1(
"Set the Gain of the constraints on the state.",
144 "Matrix of Gains.")));
145 addCommand(
"setCostGainTorqueConstraint",
147 docCommandVoid1(
"Set the Gain of the torque constraints.",
148 "Matrix of Gains.")));
156 if (!m_initSucceeded)
158 SEND_WARNING_STREAM_MSG(
"Cannot compute signal tau before initialization!");
164 const dynamicgraph::Vector &
165 pos_des = m_pos_desSIN(iter);
167 const dynamicgraph::Vector &
168 pos_joint_measure = m_pos_joint_measureSIN(iter);
170 const dynamicgraph::Vector &
171 dx_joint_measure = m_dx_joint_measureSIN(iter);
173 const dynamicgraph::Vector &
174 tau_des = m_tau_desSIN(iter);
176 DDPSolver<double, 2, 1>::stateVec_t xinit, xDes;
178 xinit << pos_joint_measure(0),
181 xDes << pos_des, 0.0;
184 m_solver.initSolver(xinit, xDes);
188 m_solver.solveTrajectory();
192 DDPSolver<double, 2, 1>::traj lastTraj;
193 lastTraj = m_solver.getLastSolvedTrajectory();
196 DDPSolver<double, 2, 1>::commandVecTab_t uList;
197 uList = lastTraj.uList;
200 double tau_ddp = uList[0](0,0);
215 const double &stopCriteria)
217 if (!m_pos_desSIN.isPlugged())
218 return SEND_MSG(
"Init failed: signal pos_des is not plugged", MSG_TYPE_ERROR);
219 if (!m_pos_joint_measureSIN.isPlugged())
220 return SEND_MSG(
"Init failed: signal pos_joint_measure is not plugged", MSG_TYPE_ERROR);
221 if (!m_dx_joint_measureSIN.isPlugged())
222 return SEND_MSG(
"Init failed: signal dx_joint_measure is not plugged", MSG_TYPE_ERROR);
231 m_cost.setJointLimit(0.0, -2.35619449019);
232 m_cost.setJointVelLimit(30.0, -30.0);
244 m_cost.setJointLimit(upperLim, lowerLim);
249 m_cost.setJointVelLimit(upperLim, lowerLim);
254 m_model.setLoadParam(mass, coordX, coordY);
269 const CostFunction<double,2,1>::stateMat_t Q_new = Eigen::Map<const CostFunction<double,2,1>::stateMat_t, Eigen::Unaligned >(Q.data(),2,1);
270 m_cost.setCostGainState(Q_new);
275 const CostFunction<double,2,1>::stateMat_t W_new = Eigen::Map<const CostFunction<double,2,1>::stateMat_t, Eigen::Unaligned >(W.data(),2,1);
276 m_cost.setCostGainStateConstraint(W_new);
281 const CostFunction<double,2,1>::commandMat_t R_new = Eigen::Map<const CostFunction<double,2,1>::commandMat_t, Eigen::Unaligned >(R.data(),1);
282 m_cost.setCostGainCommand(R_new);
287 const CostFunction<double,2,1>::commandMat_t P_new = Eigen::Map<const CostFunction<double,2,1>::commandMat_t, Eigen::Unaligned >(P.data(),1);
288 m_cost.setCostGainTorqueConstraint(P_new);
294 <<
" timestep: " <<
m_dt
296 <<
" stopCriteria: " <<
m_stopCrit << std::endl;
void setCostGainStateConstraint(const dynamicgraph::Vector &W)
void setLoadParam(const double &mass, const double &coordX, const double &coordY)
void setJointVelLimit(const double &upperLim, const double &lowerLim)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW DdpPyreneActuatorSolver(const std::string &name)
dynamicgraph::Vector m_previous_tau
virtual void display(std::ostream &os) const
void setCostGainTorqueConstraint(const dynamicgraph::Vector &P)
DDPSolver< double, 2, 1 > m_solver
void setJointLimit(const double &upperLim, const double &lowerLim)
void setCostGainCommand(const dynamicgraph::Vector &R)
DDPSolver< double, 2, 1 >::stateVec_t m_zeroState
CostFunctionPyreneActuator m_cost
void param_init(const double ×tep, const int &T, const int &nbItMax, const double &stopCriteria)
void setTorqueLimit(const double &tau)
void setCostGainState(const dynamicgraph::Vector &Q)
void setLoadMass(const double &mass)
#define ALL_OUTPUT_SIGNALS
#define ALL_INPUT_SIGNALS
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
AdmittanceController EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")