Commit 4b6f9c15 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Remove nd-trajectory-generator.

parent 97df29d7
Pipeline #4731 failed with stage
in 0 seconds
......@@ -93,7 +93,6 @@ SET(${LIBRARY_NAME}_HEADERS
include/sot/torque_control/numerical-difference.hh
include/sot/torque_control/motor-model.hh
include/sot/torque_control/joint-trajectory-generator.hh
include/sot/torque_control/nd-trajectory-generator.hh
include/sot/torque_control/se3-trajectory-generator.hh
include/sot/torque_control/free-flyer-locator.hh
include/sot/torque_control/inverse-dynamics-balance-controller.hh
......
# Copyright 2014, Andrea Del Prete, 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
# General Lesser 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/>.
/*
* Copyright 2017,Thomas Flayols, 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/>.
*/
#ifndef __sot_torque_control_nd_trajectory_generator_H__
#define __sot_torque_control_nd_trajectory_generator_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (nd_position_controller_EXPORTS)
# define SOTNDTRAJECTORYGENERATOR_EXPORT __declspec(dllexport)
# else
# define SOTNDTRAJECTORYGENERATOR_EXPORT __declspec(dllimport)
# endif
#else
# define SOTNDTRAJECTORYGENERATOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <map>
#include "boost/assign.hpp"
#include <parametric-curves/spline.hpp>
#include <parametric-curves/constant.hpp>
#include <parametric-curves/text-file.hpp>
#include <parametric-curves/minimum-jerk.hpp>
#include <parametric-curves/linear-chirp.hpp>
#include <parametric-curves/infinite-sinusoid.hpp>
#include <parametric-curves/infinite-const-acc.hpp>
/* HELPER */
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
//#include <sot/torque_control/utils/trajectory-generators.hh>
namespace dynamicgraph {
namespace sot {
namespace torque_control {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTNDTRAJECTORYGENERATOR_EXPORT NdTrajectoryGenerator
:public::dynamicgraph::Entity
{
typedef NdTrajectoryGenerator EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
NdTrajectoryGenerator( const std::string & name );
void init(const double& dt, const unsigned int& n);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(initial_value, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(trigger, bool);
DECLARE_SIGNAL(x, OUT, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);
protected:
DECLARE_SIGNAL_OUT_FUNCTION(x, dynamicgraph::Vector);
public:
/* --- COMMANDS --- */
void playTrajectoryFile(const std::string& fileName);
void playSpline(const std::string& fileName);
void setSpline(const std::string& filename, const double& timeToInitConf);
void startSpline();
/** Print the current value of the specified component. */
void getValue(const int& id);
/** Move a component to a position with a minimum-jerk trajectory.
* @param id integer index.
* @param xFinal The desired final position of the component.
* @param time The time to go from the current position to xFinal [sec].
*/
void move(const int& id, const double& xFinal, const double& time);
/** Start an infinite sinusoidal trajectory.
* @param id integer index.
* @param xFinal The position of the component corresponding to the max amplitude of the sinusoid.
* @param time The time to go from the current position to xFinal [sec].
*/
void startSinusoid(const int& id, const double& xFinal, const double& time);
/** Start an infinite triangle trajectory.
* @param id integer index.
* @param xFinal The position of the component corresponding to the max amplitude of the trajectory.
* @param time The time to go from the current position to xFinal [sec].
*/
//void startTriangle(const int& id, const double& xFinal, const double& time, const double& Tacc);
/** Start an infinite trajectory with piece-wise constant acceleration.
* @param id integer index.
* @param xFinal The position of the component corresponding to the max amplitude of the trajectory.
* @param time The time to go from the current position to xFinal [sec].
* @param Tacc The time during witch acceleration is keept constant [sec].
*/
void startConstAcc(const int& id, const double& xFinal, const double& time);
/** Start a linear-chirp trajectory, that is a sinusoidal trajectory with frequency
* being a linear function of time.
* @param id integer index.
* @param xFinal The position of the component corresponding to the max amplitude of the sinusoid [rad].
* @param f0 The initial (min) frequency of the sinusoid [Hz]
* @param f1 The final (max) frequency of the sinusoid [Hz]
* @param time The time to get from f0 to f1 [sec]
*/
void startLinearChirp(const int& id, const double& xFinal, const double& f0, const double& f1, const double& time);
/** Stop the motion of the specified component. If id is -1
* it stops the trajectory of all the vector.
* @param id integer index.
* */
void stop(const int& id);
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
Entity::sendMsg("[NdTrajectoryGenerator-"+name+"] "+msg, t, file, line);
}
protected:
enum JTG_Status
{
JTG_STOP,
JTG_SINUSOID,
JTG_MIN_JERK,
JTG_LIN_CHIRP,
//JTG_TRIANGLE,
JTG_CONST_ACC,
JTG_TEXT_FILE,
JTG_SPLINE
};
bool m_initSucceeded; /// true if the entity has been successfully initialized
bool m_firstIter; /// true if it is the first iteration, false otherwise
double m_dt; /// control loop time step.
double m_t; /// current control loop time.
unsigned int m_n; /// size of ouput vector
unsigned int m_iterLast; /// last iter index
bool m_splineReady; /// true if the spline has been successfully loaded.
std::vector<JTG_Status> m_status; /// status of the component
std::vector<parametriccurves::AbstractCurve<double, dynamicgraph::sot::Vector1d>* > m_currentTrajGen;
std::vector<parametriccurves::Constant<double, 1>* > m_noTrajGen;
std::vector<parametriccurves::MinimumJerk<double, 1>* > m_minJerkTrajGen;
std::vector<parametriccurves::InfiniteSinusoid<double,1>* > m_sinTrajGen;
std::vector<parametriccurves::LinearChirp<double,1>*> m_linChirpTrajGen;
//std::vector<parametriccurves::InfiniteTriangular<double,1>* > m_triangleTrajGen;
std::vector<parametriccurves::InfiniteConstAcc<double,1>* > m_constAccTrajGen;
parametriccurves::TextFile<double, Eigen::Dynamic>* m_textFileTrajGen;
parametriccurves::Spline<double, Eigen::Dynamic>* m_splineTrajGen;
}; // class NdTrajectoryGenerator
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_nd_trajectory_generator_H__
......@@ -45,7 +45,6 @@ SET(plugins
inverse-dynamics-balance-controller
joint-torque-controller
joint-trajectory-generator
nd-trajectory-generator
numerical-difference
position-controller
se3-trajectory-generator
......
/*
* Copyright 2017,Thomas Flayols, 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 <dynamic-graph/factory.h>
#include <sot/core/debug.hh>
#include <sot/torque_control/nd-trajectory-generator.hh>
#include <sot/torque_control/commands-helper.hh>
#include <sot/core/stop-watch.hh>
namespace dynamicgraph
{
namespace sot
{
namespace torque_control
{
namespace dynamicgraph = ::dynamicgraph;
using namespace dynamicgraph;
using namespace dynamicgraph::command;
using namespace std;
using namespace Eigen;
#define PROFILE_ND_POSITION_DESIRED_COMPUTATION "NdTrajGen: traj computation"
#define DOUBLE_INF std::numeric_limits<double>::max()
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef NdTrajectoryGenerator EntityClassName;
/* --- DG FACTORY ---------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(NdTrajectoryGenerator,
"NdTrajectoryGenerator");
/* ------------------------------------------------------------------- */
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
NdTrajectoryGenerator::
NdTrajectoryGenerator(const std::string& name)
: Entity(name)
,CONSTRUCT_SIGNAL_IN(initial_value,dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(trigger,bool)
,CONSTRUCT_SIGNAL(x, OUT, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_xSOUT)
,CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_xSOUT)
,m_initSucceeded(false)
,m_firstIter(true)
,m_t(0)
,m_n(1)
,m_iterLast(0)
,m_splineReady(false)
{
BIND_SIGNAL_TO_FUNCTION(x, OUT, dynamicgraph::Vector);
Entity::signalRegistration( m_xSOUT << m_dxSOUT << m_ddxSOUT << m_initial_valueSIN
<<m_triggerSIN);
/* Commands. */
addCommand("init",
makeCommandVoid2(*this, &NdTrajectoryGenerator::init,
docCommandVoid2("Initialize the entity.",
"Time period in seconds (double)",
"size of output vector signal (int)")));
addCommand("getValue",
makeCommandVoid1(*this, &NdTrajectoryGenerator::getValue,
docCommandVoid1("Get the current value of the specified index.",
"index (int)")));
addCommand("playTrajectoryFile",
makeCommandVoid1(*this, &NdTrajectoryGenerator::playTrajectoryFile,
docCommandVoid1("Play the trajectory read from the specified text file.",
"(string) File name, path included")));
addCommand("startSinusoid",
makeCommandVoid3(*this, &NdTrajectoryGenerator::startSinusoid,
docCommandVoid3("Start an infinite sinusoid motion.",
"(int) index",
"(double) final value",
"(double) time to reach the final value in sec")));
addCommand("setSpline",
makeCommandVoid2(*this, &NdTrajectoryGenerator::setSpline,
docCommandVoid2("Load serialized spline from file",
"(string) filename",
"(double) time to initial conf")));
/* addCommand("startTriangle",
makeCommandVoid4(*this, &NdTrajectoryGenerator::startTriangle,
docCommandVoid4("Start an infinite triangle wave.",
"(int) index",
"(double) final values",
"(double) time to reach the final value in sec",
"(double) time to accelerate in sec")));
*/
addCommand("startConstAcc",
makeCommandVoid3(*this, &NdTrajectoryGenerator::startConstAcc,
docCommandVoid3("Start an infinite trajectory with piece-wise constant acceleration.",
"(int) index",
"(double) final values",
"(double) time to reach the final value in sec")));
addCommand("startLinChirp",
makeCommandVoid5(*this, &NdTrajectoryGenerator::startLinearChirp,
docCommandVoid5("Start a linear-chirp motion.",
"(int) index",
"(double) final values",
"(double) initial frequency [Hz]",
"(double) final frequency [Hz]",
"(double) trajectory time [sec]")));
addCommand("move",
makeCommandVoid3(*this, &NdTrajectoryGenerator::move,
docCommandVoid3("Move component corresponding to index to the specified value with a minimum jerk trajectory.",
"(int) index",
"(double) final values",
"(double) time to reach the final value in sec")));
addCommand("stop",
makeCommandVoid1(*this, &NdTrajectoryGenerator::stop,
docCommandVoid1("Stop the motion of the specified index, or of all components of the vector if index is equal to -1.",
"(int) index")));
}
void NdTrajectoryGenerator::init(const double& dt, const unsigned int& n)
{
if(dt<=0.0)
return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
if(n<1)
return SEND_MSG("n must be at least 1", MSG_TYPE_ERROR);
m_firstIter = true;
m_dt = dt;
m_n = n;
m_status.resize(m_n,JTG_STOP);
m_minJerkTrajGen.resize(m_n);
m_sinTrajGen.resize(m_n);
//m_triangleTrajGen.resize(m_n);
m_constAccTrajGen.resize(m_n);
m_linChirpTrajGen.resize(m_n);
m_currentTrajGen.resize(m_n);
m_noTrajGen.resize(m_n);
for(unsigned int i=0; i<m_n; i++)
{
m_minJerkTrajGen[i] = new parametriccurves::MinimumJerk<double,1>(5.0);
m_sinTrajGen[i] = new parametriccurves::InfiniteSinusoid<double,1>(5.0);
//m_triangleTrajGen[i] = new parametriccurves::InfiniteTriangle<double,1>(5.0);
m_constAccTrajGen[i] = new parametriccurves::InfiniteConstAcc<double,1>(5.0);
m_linChirpTrajGen[i] = new parametriccurves::LinearChirp<double,1>(5.0);
m_noTrajGen[i] = new parametriccurves::Constant<double,1>(5.0);
m_currentTrajGen[i] = m_noTrajGen[i];
//Set infinite time for infinite trajectories
m_noTrajGen[i]->setTimePeriod(DOUBLE_INF);
m_constAccTrajGen[i]->setTimePeriod(DOUBLE_INF);
m_sinTrajGen[i]->setTimePeriod(DOUBLE_INF);
}
m_splineTrajGen = new parametriccurves::Spline<double,Eigen::Dynamic>();
m_textFileTrajGen = new parametriccurves::TextFile<double, Eigen::Dynamic>(dt, n);
m_initSucceeded = true;
}
/* ------------------------------------------------------------------- */
/* --- SIGNALS ------------------------------------------------------- */
/* ------------------------------------------------------------------- */
DEFINE_SIGNAL_OUT_FUNCTION(x, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization!");
return s;
}
getProfiler().start(PROFILE_ND_POSITION_DESIRED_COMPUTATION);
{
if(s.size()!=m_n)
s.resize(m_n);
// at first iteration store initial values
if(m_firstIter)
{
const dynamicgraph::Vector& initial_value = m_initial_valueSIN(iter);
if(initial_value.size()!=m_n)
{
SEND_MSG("Unexpected size of input signal initial_value: "+toString(initial_value.size()),MSG_TYPE_ERROR);
getProfiler().stop(PROFILE_ND_POSITION_DESIRED_COMPUTATION);
return s;
}
for(unsigned int i=0; i<m_n; i++)
m_currentTrajGen[i]->setInitialPoint(initial_value(i));
m_firstIter = false;
}
else if(iter == static_cast<int>(m_iterLast))
{
if (m_triggerSIN(iter)==true && m_splineReady) startSpline();
if(m_status[0]==JTG_TEXT_FILE)
{
s = (*m_textFileTrajGen)(m_t);
}
else if(m_status[0]==JTG_SPLINE)
{
s = (*m_splineTrajGen)(m_t);
}
else
for(unsigned int i=0; i<m_n; i++)
s(i) = (*m_currentTrajGen[i])(m_t)[0];
getProfiler().stop(PROFILE_ND_POSITION_DESIRED_COMPUTATION);
return s;
}
m_iterLast = iter;
m_t += m_dt;
if (m_triggerSIN(iter)==true && m_splineReady) startSpline();
if(m_status[0]==JTG_TEXT_FILE)
{
if(!m_textFileTrajGen->checkRange(m_t))
{
s = (*m_textFileTrajGen)(m_textFileTrajGen->tmax());
for(unsigned int i=0; i<m_n; i++)
{
m_noTrajGen[i]->setInitialPoint(s(i));
m_status[i] = JTG_STOP;
}
SEND_MSG("Text file trajectory ended.", MSG_TYPE_INFO);
m_t =0;
}
else
s = (*m_textFileTrajGen)(m_t);
}
else if(m_status[0]==JTG_SPLINE)
{
if(!m_splineTrajGen->checkRange(m_t))
{
s = (*m_splineTrajGen)(m_splineTrajGen->tmax());
for(unsigned int i=0; i<m_n; i++)
{
m_noTrajGen[i]->setInitialPoint(s(i));
m_status[i] = JTG_STOP;
}
m_splineReady =false;
SEND_MSG("Spline trajectory ended. Remember to turn off the trigger.", MSG_TYPE_INFO);
m_t =0;
}
else
s = (*m_splineTrajGen)(m_t);
}
else
{
for(unsigned int i=0; i<m_n; i++)
{
if(!m_currentTrajGen[i]->checkRange(m_t))
{
s(i) = (*m_currentTrajGen[i])(m_currentTrajGen[i]->tmax())[0];
m_currentTrajGen[i] = m_noTrajGen[i];
m_noTrajGen[i]->setInitialPoint(s(i));
m_status[i] = JTG_STOP;
SEND_MSG("Trajectory of index "+toString(i)+" ended.", MSG_TYPE_INFO);
}
else
s(i) = (*m_currentTrajGen[i])(m_t)[0];
}
}
}
getProfiler().stop(PROFILE_ND_POSITION_DESIRED_COMPUTATION);
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(dx, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
std::ostringstream oss("Cannot compute signal positionDes before initialization! iter:");
oss << iter;
SEND_WARNING_STREAM_MSG(oss.str());
return s;
}
if(s.size()!=m_n)
s.resize(m_n);
if(m_status[0]==JTG_TEXT_FILE)
{
s = m_textFileTrajGen->derivate(m_t, 1);
}
else if(m_status[0]==JTG_SPLINE)
s = m_splineTrajGen->derivate(m_t, 1);
else
for(unsigned int i=0; i<m_n; i++)
s(i) = m_currentTrajGen[i]->derivate(m_t, 1)[0];
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(ddx, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
std::ostringstream oss("Cannot compute signal positionDes before initialization! iter:");
oss << iter;
SEND_WARNING_STREAM_MSG(oss.str());
return s;
}
if(s.size()!=m_n)
s.resize(m_n);
if(m_status[0]==JTG_TEXT_FILE)
{
s = m_textFileTrajGen->derivate(m_t, 2);
}
else if(m_status[0]==JTG_SPLINE)
s = m_splineTrajGen->derivate(m_t, 2);
else
for(unsigned int i=0; i<m_n; i++)
s(i) = m_currentTrajGen[i]->derivate(m_t, 2)[0];
return s;
}
/* ------------------------------------------------------------------- */
/* --- COMMANDS ------------------------------------------------------ */
/* ------------------------------------------------------------------- */
void NdTrajectoryGenerator::getValue(const int& id)
{
if(id<0 || id>=static_cast<int>(m_n))
return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
SEND_MSG("Current value of component "+toString(id)+" is "+toString( (*m_currentTrajGen[id])(m_t)[0]) , MSG_TYPE_INFO);
}
void NdTrajectoryGenerator::playTrajectoryFile(const std::string& fileName)
{
if(!m_initSucceeded)
return SEND_MSG("Cannot start sinusoid before initialization!",MSG_TYPE_ERROR);
for(unsigned int i=0; i<m_n; i++)
if(m_status[i]!=JTG_STOP)
return SEND_MSG("You cannot control component "+toString(i)+" because it is already controlled.", MSG_TYPE_ERROR);
if(!m_textFileTrajGen->loadTextFile(fileName))
return SEND_MSG("Error while loading text file "+fileName, MSG_TYPE_ERROR);
// check current configuration is not too far from initial trajectory configuration
bool needToMoveToInitConf = false;
const VectorXd& xInit = m_textFileTrajGen->getInitialPoint();
for(unsigned int i=0; i<m_n; i++)
if(fabs(xInit[i] - (*m_currentTrajGen[i])(m_t)[0]) > 0.001)
{
needToMoveToInitConf = true;
SEND_MSG("Component "+ toString(i) +" is too far from initial configuration so first i will move it there.", MSG_TYPE_WARNING);
}
// if necessary move joints to initial configuration
if(needToMoveToInitConf)
{
for(unsigned int i=0; i<m_n; i++)
{
// if(!isJointInRange(i, xInit[i]))
// return;
m_minJerkTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
m_minJerkTrajGen[i]->setFinalPoint(xInit[i]);
m_minJerkTrajGen[i]->setTimePeriod(4.0);
m_status[i] = JTG_MIN_JERK;