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

[Pyrene_Actuator] Creation of the ddp solver for the actuator of pyrene

Entity using the ddp_solver associated with the pyrene actuator (see ddp-actuator-solver/example/pyrene_actuator)
parent 485b2856
/*
* Copyright 2019
*
* LAAS-CNRS
*
* Noelie RAMUZAT
* This file is part of sot-torque-control.
* See license file.
*/
#ifndef _SOT_DDP_PYRENE_ACTUATOR_SOLVER_H
#define _SOT_DDP_PYRENE_ACTUATOR_SOLVER_H
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (sot_simple_ddp_actuator_EXPORTS)
# define SOTDDPPYRENEACTUATORSOLVER_EXPORT __declspec(dllexport)
# else
# define SOTDDPPYRENEACTUATORSOLVER_EXPORT __declspec(dllimport)
# endif
#else
# define SOTDDPPYRENEACTUATORSOLVER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <tsid/utils/stop-watch.hpp>
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
#include <sot/torque_control/utils/causal-filter.hh>
#include <ddp-actuator-solver/ddpsolver.hh>
#include <ddp-actuator-solver/examples/costFunction.hh>
#include <ddp-actuator-solver/examples/PyreneActuator.hh>
namespace dynamicgraph {
namespace sot {
namespace torque_control {
class SOTDDPPYRENEACTUATORSOLVER_EXPORT DdpPyreneActuatorSolver
: public :: dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/** Constructor */
DdpPyreneActuatorSolver(const std::string &name);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(pos_des, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(pos_joint_measure, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dx_joint_measure, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(tau, dynamicgraph::Vector);
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream &os) const;
protected:
double m_dt;
DDPSolver<double, 2, 1>::stateVec_t m_xinit, m_xDes, m_x, m_zeroState;
DDPSolver<double, 2, 1>::commandVec_t m_u;
unsigned int m_T;
double m_stopCrit;
unsigned int m_iterMax;
PyreneActuator m_model;
CostFunctionPyreneActuator m_cost;
DDPSolver<double, 2, 1> m_solver;
/** Initialize the DDP.
* @param timestep Control period (in seconds).
* @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,
const double &tauLim);
};
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
#endif // _SOT_SIMPLE_DDP_ACTUATOR_SOLVER_H
......@@ -71,6 +71,7 @@ SET(plugins
IF(DDP_ACTUATOR_SOLVER_FOUND)
SET(plugins ${plugins} ddp-actuator-solver )
SET(plugins ${plugins} ddp_pyrene_actuator_solver )
ENDIF(DDP_ACTUATOR_SOLVER_FOUND)
#Plugins compilation, link, and installation
......
......@@ -3,49 +3,6 @@
*
*/
#include <Eigen/Dense>
#include <dynamic-graph/factory.h>
#include <sot/core/debug.hh>
#include <sot/torque_control/commands-helper.hh>
#include <sot/torque_control/ddp-actuator-solver.hh>
#include <ddp-actuator-solver/examples/dctemp.hh>
#include <ddp-actuator-solver/examples/costtemp.hh>
#if DEBUG
#define ODEBUG(x) std::cout << x << std::endl
#else
#define ODEBUG(x)
#endif
#define ODEBUG3(x) std::cout << x << std::endl
#define DBGFILE "/tmp/debug-ddp_actuator_solver.dat"
#define RESETDEBUG5() \
{ \
std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::out); \
DebugFile.close(); \
}
#define ODEBUG5FULL(x) \
{ \
std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::app); \
DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ << "):" << x << std::endl; \
DebugFile.close(); \
}
#define ODEBUG5(x) \
{ \
std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::app); \
DebugFile << x << std::endl; \
DebugFile.close(); \
}
#define RESETDEBUG4()
#define ODEBUG4FULL(x)
#define ODEBUG4(x)
#include <ddp-actuator-solver/examples/dctemp.hh>
#include <ddp-actuator-solver/examples/costtemp.hh>
......
/*
* Copyright 2018-, Olivier Stasse LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
#include <Eigen/Dense>
#include <dynamic-graph/factory.h>
#include <sot/core/debug.hh>
#include <sot/torque_control/commands-helper.hh>
#include <sot/torque_control/ddp_pyrene_actuator_solver.hh>
#if DEBUG
#define ODEBUG(x) std::cout << x << std::endl
#else
#define ODEBUG(x)
#endif
#define ODEBUG3(x) std::cout << x << std::endl
#define DBGFILE "/tmp/debug-simple_ddp_actuator_solver.dat"
#define RESETDEBUG5() { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::out); \
DebugFile.close();}
#define ODEBUG5FULL(x) { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::app); \
DebugFile << __FILE__ << ":" \
<< __FUNCTION__ << "(#" \
<< __LINE__ << "):" << x << std::endl; \
DebugFile.close();}
#define ODEBUG5(x) { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::app); \
DebugFile << x << std::endl; \
DebugFile.close();}
#define RESETDEBUG4()
#define ODEBUG4FULL(x)
#define ODEBUG4(x)
namespace dynamicgraph
{
namespace sot
{
namespace torque_control
{
namespace dynamicgraph = ::dynamicgraph;
using namespace dynamicgraph;
using namespace dynamicgraph::command;
using namespace Eigen;
#define ALL_INPUT_SIGNALS m_pos_desSIN << m_pos_joint_measureSIN << m_dx_joint_measureSIN
#define ALL_OUTPUT_SIGNALS m_tauSOUT
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef DdpPyreneActuatorSolver EntityClassName;
/* --- DG FACTORY ------------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SimpleDdpActuatorSolver, "SimpleDdpActuatorSolver");
DdpPyreneActuatorSolver::
DdpPyreneActuatorSolver(const std::string &name)
: Entity(name),
CONSTRUCT_SIGNAL_IN (pos_des, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN (pos_joint_measure, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN (dx_joint_measure, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_OUT(tau, dynamicgraph::Vector, m_pos_desSIN),
m_dt(1e-3),
m_T(50),
m_stopCrit(1e-3),
m_iterMax(100),
m_solver(m_model, m_cost,
DISABLE_FULLDDP,
DISABLE_QPBOX)
{
RESETDEBUG5();
Entity::signalRegistration( ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS );
m_zeroState.setZero();
m_firstIter = true;
/* Commands. */
addCommand("init",
makeCommandVoid4(*this, &DdpPyreneActuatorSolver::param_init,
docCommandVoid4("Initialize the DDP solver.",
"Control timestep [s].",
"Size of the preview window (in nb of samples)",
"Max. nb. of iterations",
"Stopping criteria")));
}
/* --- COMMANDS ---------------------------------------------------------- */
DEFINE_SIGNAL_OUT_FUNCTION(tau, dynamicgraph::Vector)
{
/// ---- Get the information -----
/// Desired position
const dynamicgraph::Vector &
pos_des = m_pos_desSIN(iter);
/// Measured joint position
const dynamicgraph::Vector &
pos_joint_measure = m_pos_joint_measureSIN(iter);
/// Measured joint speed
const dynamicgraph::Vector &
dx_joint_measure = m_dx_joint_measureSIN(iter);
m_xinit << pos_joint_measure(0),
dx_joint_measure(0);
m_xDes << pos_des, 0.0;
ODEBUG5(xinit);
ODEBUG5("");
ODEBUG5(xDes);
m_solver.FirstInitSolver(xinit, xDes, m_T, m_dt, m_iterMax, m_stopCrit);
ODEBUG5("FirstInitSolver");
/// --- Solve the DDP ---
m_solver.solveTrajectory();
ODEBUG5("Trajectory solved");
/// --- Get the command ---
DDPSolver<double, 2, 1>::traj lastTraj;
lastTraj = m_solver.getLastSolvedTrajectory();
ODEBUG5("getLastSolvedTrajectory");
DDPSolver<double, 2, 1>::commandVecTab_t uList;
uList = lastTraj.uList;
ODEBUG5("uList");
//s = uList[0];
s.resize(32);
s << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, uList[0], 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
//s.setZero(32);
ODEBUG5(s);
return s;
}
void SimpleDdpActuatorSolver::param_init(const double &timestep,
const int &T,
const int &nbItMax,
const double &stopCriteria,
const double &tauLim)
{
m_T = T;
m_dt = timestep;
m_iterMax = nbItMax;
m_stopCrit = stopCriteria;
m_cost.setTauLimit(tauLim);
m_solver.FirstInitSolver( m_zeroState, m_zeroState,
m_T , m_dt, m_iterMax, m_stopCrit);
}
void SimpleDdpActuatorSolver::display(std::ostream &os) const
{
os << " T: " << m_T
<< " timestep: " << m_dt
<< " nbItMax: " << m_iterMax
<< " stopCriteria: " << m_stopCrit << std::endl;
}
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
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