Commit e40a7d2a authored by NoelieRamuzat's avatar NoelieRamuzat
Browse files

Merge branch 'topic/ddp' into devel

parents 921e9598 73ff46e3
Subproject commit 679949e28a91c334edf3588753cf294aec2fc4c7
Subproject commit dc8b946d456d2c41ad12b819111b005148c68031
......@@ -53,3 +53,4 @@ You can find the full installation procedure in the <a href="md_doc_installation
Quick instructions on how to run a test can be found <a href="md_doc_running.html">here</a>.
Instructions for running a simulation or an experiment using the DDP on the right elbow of Pyrene can be found <a href="md_doc_ddpRun.html">here</a>.
# Running the DDP on the right elbow joint of Pyrene
In the following, we demonstrate how to run a test with sot-torque-control, <a href="https://github.com/stack-of-tasks/ddp-actuator-solver">ddp-actuator-solver</a> and <a href="https://gepgitlab.laas.fr/pyrene-dev/talos-torque-control.git">talos-torque-control</a>.
## Start the simulation
Start the simulation with the robot in the half-sitting position:
```
roslaunch talos_data talos_gazebo.launch start_half_sitting:=true
```
If you are on the real robot just ignore this step.
## Start the SoT in torque mode
To start the SoT in simulation in torque mode:
```
roslaunch roscontrol_sot_talos sot_talos_controller_gazebo_effort.launch
```
To start the SoT on the real robot in torque mode:
```
roslaunch roscontrol_sot_talos sot_talos_controller_effort.launch
```
## Run the test
First of all, you need to go to the folder where your script is.
For running the ddp test of talos-torque-control, assuming you are in the root directory:
```
cd script
```
Then, you can just run the test, specifying that you are in simulation mode:
```
python test_ddp_sinu_effort.py simu
```
Or on the real robot:
```
python test_ddp_sinu_effort.py robot
```
This will launch the test making the robot executing a sinusoid with its right arm (on the elbow joint) using the DDP solver.
This one computes a trajectory avoiding the limits in position, velocity and torque of the robot.
The script also saves the dynamic graph in /tmp/sot_ddp_talos_effort.pdf.
You can add loads on the arm of the robot and see how it will react: by degrading the sinusoid movement of its elbow to respect its limits.
To make the loads taken into account by the DDP formulation you should use the following commands:
```
rosrun dynamic_graph_bridge run_command
robot.ddp_ctrl.setLoadParam(30.0,-0.021481595, 0.10)
```
It will add a load of 30kg at the coordinates (-0.021481595, 0.10) with respect to the elbow joint (10cm ahead on the arm from the joint, almost centered).
To remove the load just run:
```
robot.ddp_ctrl.removeLoad()
```
## Other
More information on how to use the SoT and how to work on Talos can be found <a href="https://wiki.laas.fr/robots/Pyrene">in the robot wiki page</a> (you need LAAS permissions to access this).
......@@ -21,7 +21,6 @@
#include <sot/core/robot-utils.hh>
#include <sot/core/causal-filter.hh>
#include <ddp-actuator-solver/ddpsolver.hh>
#include <ddp-actuator-solver/ddpsolver.hh>
#include <ddp-actuator-solver/examples/dctemp.hh>
......
/*
* 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_ddp_pyrene_actuator_EXPORTS)
# define SOTDDPPYRENEACTUATORSOLVER_EXPORT __declspec(dllexport)
# else
# define SOTDDPPYRENEACTUATORSOLVER_EXPORT __declspec(dllimport)
# endif
#else
# define SOTDDPPYRENEACTUATORSOLVER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <vector>
#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/core/causal-filter.hh>
#include <ddp-actuator-solver/ddpsolver.hh>
#include <ddp-actuator-solver/examples/pyreneCostFunction.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_IN(tau_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(tau, dynamicgraph::Vector);
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream &os) const;
protected:
dynamicgraph::Vector m_previous_tau;
double m_dt;
bool m_initSucceeded;
DDPSolver<double, 2, 1>::stateVec_t 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.
*/
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
} // 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
......
......@@ -9,45 +9,6 @@
#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>
#if DEBUG
#define ODEBUG(x) std::cout << x << std::endl
......
/*
* 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/>.
*/
#define EIGEN_NO_MALLOC
#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>
#include <math.h>
#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_pyrene_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 << m_tau_desSIN
#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(DdpPyreneActuatorSolver, "DdpPyreneActuatorSolver");
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_IN (tau_des, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_OUT(tau, dynamicgraph::Vector, ALL_INPUT_SIGNALS),
m_dt(1e-3),
m_T(50),
m_stopCrit(1e-3),
m_iterMax(10),
m_solver(m_model, m_cost,
DISABLE_FULLDDP,
DISABLE_QPBOX)
{
RESETDEBUG5();
Entity::signalRegistration( ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS );
m_zeroState.setZero();
/* 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")));
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.",
"Mass of the load [g].",
"X coordinate of the Load",
"Y coordinate of the Load")));
addCommand("setLoadMass",
makeCommandVoid1(*this, &DdpPyreneActuatorSolver::setLoadMass,
docCommandVoid1("Set the Load mass.",
"Mass of the load [g].")));
addCommand("removeLoad",
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;
}
/* --- COMMANDS ---------------------------------------------------------- */
DEFINE_SIGNAL_OUT_FUNCTION(tau, dynamicgraph::Vector)
{
if (!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal tau before initialization!");
return s;
}
/// ---- 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);
/// Desired torque
const dynamicgraph::Vector &
tau_des = m_tau_desSIN(iter);
DDPSolver<double, 2, 1>::stateVec_t xinit, xDes;
xinit << pos_joint_measure(0),
dx_joint_measure(0);
xDes << pos_des, 0.0;
//ODEBUG(xDes);
m_solver.initSolver(xinit, xDes);
//ODEBUG("FirstInitSolver");
/// --- Solve the DDP ---
m_solver.solveTrajectory();
//ODEBUG("Trajectory solved");
/// --- Get the command ---
DDPSolver<double, 2, 1>::traj lastTraj;
lastTraj = m_solver.getLastSolvedTrajectory();
//ODEBUG("getLastSolvedTrajectory");
DDPSolver<double, 2, 1>::commandVecTab_t uList;
uList = lastTraj.uList;
s = m_previous_tau;
double tau_ddp = uList[0](0,0);
if (!isnan(tau_ddp))
{
s[25] = tau_ddp;
}
m_previous_tau = s;
//ODEBUG(s);
return s;
}
void DdpPyreneActuatorSolver::param_init(const double &timestep,
const int &T,
const int &nbItMax,
const double &stopCriteria)
{
if (!m_pos_desSIN.isPlugged())
return SEND_MSG("Init failed: signal pos_des is not plugged", MSG_TYPE_ERROR);
if (!m_pos_joint_measureSIN.isPlugged())
return SEND_MSG("Init failed: signal pos_joint_measure is not plugged", MSG_TYPE_ERROR);
if (!m_dx_joint_measureSIN.isPlugged())
return SEND_MSG("Init failed: signal dx_joint_measure is not plugged", MSG_TYPE_ERROR);
m_previous_tau.resize(32);
m_previous_tau.setZero();
m_T = T;
m_dt = timestep;
m_iterMax = nbItMax;
m_stopCrit = stopCriteria;
m_cost.setTauLimit(70.0);
m_cost.setJointLimit(0.0, -2.35619449019);
m_cost.setJointVelLimit(30.0, -30.0);
m_solver.FirstInitSolver( m_zeroState, m_zeroState,
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);
}
void DdpPyreneActuatorSolver::setLoadMass(const double& mass)
{
m_model.setLoadMass(mass);
}
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
<< " 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