Commit e2b20d98 authored by Rohan Budhiraja's avatar Rohan Budhiraja Committed by andreadelprete
Browse files

[se3-trajectory-generator] add spline(deals with only translation part).

parent d6639249
...@@ -40,6 +40,7 @@ ...@@ -40,6 +40,7 @@
#include <sot/torque_control/utils/vector-conversions.hh> #include <sot/torque_control/utils/vector-conversions.hh>
#include <sot/torque_control/utils/logger.hh> #include <sot/torque_control/utils/logger.hh>
#include <sot/torque_control/utils/trajectory-generators.hh> #include <sot/torque_control/utils/trajectory-generators.hh>
#include <parametriccurves/spline.hpp>
#include <map> #include <map>
#include <initializer_list> #include <initializer_list>
#include "boost/assign.hpp" #include "boost/assign.hpp"
...@@ -68,6 +69,7 @@ namespace dynamicgraph { ...@@ -68,6 +69,7 @@ namespace dynamicgraph {
/* --- SIGNALS --- */ /* --- SIGNALS --- */
DECLARE_SIGNAL_IN(initial_value, dynamicgraph::Vector); DECLARE_SIGNAL_IN(initial_value, dynamicgraph::Vector);
DECLARE_SIGNAL(x, OUT, dynamicgraph::Vector); DECLARE_SIGNAL(x, OUT, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(trigger, bool);
DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);
...@@ -79,6 +81,10 @@ namespace dynamicgraph { ...@@ -79,6 +81,10 @@ namespace dynamicgraph {
/* --- COMMANDS --- */ /* --- COMMANDS --- */
void playTrajectoryFile(const std::string& fileName); void playTrajectoryFile(const std::string& fileName);
void startSpline();
void setSpline(const std::string& filename,
const double& timeToInitConf,
const Eigen::MatrixXd& init_rotation);
/** Print the current value of the specified component. */ /** Print the current value of the specified component. */
void getValue(const int& id); void getValue(const int& id);
...@@ -148,7 +154,8 @@ namespace dynamicgraph { ...@@ -148,7 +154,8 @@ namespace dynamicgraph {
TG_LIN_CHIRP, TG_LIN_CHIRP,
TG_TRIANGLE, TG_TRIANGLE,
TG_CONST_ACC, TG_CONST_ACC,
TG_TEXT_FILE TG_TEXT_FILE,
TG_SPLINE
}; };
bool m_initSucceeded; /// true if the entity has been successfully initialized bool m_initSucceeded; /// true if the entity has been successfully initialized
...@@ -158,6 +165,12 @@ namespace dynamicgraph { ...@@ -158,6 +165,12 @@ namespace dynamicgraph {
unsigned int m_nv; /// size of velocity vector unsigned int m_nv; /// size of velocity vector
unsigned int m_iterLast; /// last iter index unsigned int m_iterLast; /// last iter index
double m_t;
Eigen::Matrix3d m_splineRotation;
parametriccurves::Spline<double, Eigen::Dynamic>* m_splineTrajGen;
bool m_splineReady;
std::vector<TG_Status> m_status; /// status of the component std::vector<TG_Status> m_status; /// status of the component
std::vector<AbstractTrajectoryGenerator*> m_currentTrajGen; std::vector<AbstractTrajectoryGenerator*> m_currentTrajGen;
std::vector<NoTrajectoryGenerator*> m_noTrajGen; std::vector<NoTrajectoryGenerator*> m_noTrajGen;
......
...@@ -50,18 +50,22 @@ namespace dynamicgraph ...@@ -50,18 +50,22 @@ namespace dynamicgraph
SE3TrajectoryGenerator(const std::string& name) SE3TrajectoryGenerator(const std::string& name)
: Entity(name) : Entity(name)
,CONSTRUCT_SIGNAL_IN(initial_value,dynamicgraph::Vector) ,CONSTRUCT_SIGNAL_IN(initial_value,dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(trigger,bool)
,CONSTRUCT_SIGNAL(x, OUT, dynamicgraph::Vector) ,CONSTRUCT_SIGNAL(x, OUT, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_xSOUT) ,CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_xSOUT)
,CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_xSOUT) ,CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_xSOUT)
,m_firstIter(true) ,m_firstIter(true)
,m_initSucceeded(false) ,m_initSucceeded(false)
,m_splineReady(false)
,m_t(0)
,m_np(12) ,m_np(12)
,m_nv(6) ,m_nv(6)
,m_iterLast(0) ,m_iterLast(0)
{ {
BIND_SIGNAL_TO_FUNCTION(x, OUT, dynamicgraph::Vector); BIND_SIGNAL_TO_FUNCTION(x, OUT, dynamicgraph::Vector);
Entity::signalRegistration( m_xSOUT << m_dxSOUT << m_ddxSOUT << m_initial_valueSIN); Entity::signalRegistration( m_xSOUT << m_dxSOUT << m_ddxSOUT << m_initial_valueSIN
<<m_triggerSIN );
/* Commands. */ /* Commands. */
addCommand("init", addCommand("init",
...@@ -78,6 +82,12 @@ namespace dynamicgraph ...@@ -78,6 +82,12 @@ namespace dynamicgraph
makeCommandVoid1(*this, &SE3TrajectoryGenerator::playTrajectoryFile, makeCommandVoid1(*this, &SE3TrajectoryGenerator::playTrajectoryFile,
docCommandVoid1("Play the trajectory read from the specified text file.", docCommandVoid1("Play the trajectory read from the specified text file.",
"(string) File name, path included"))); "(string) File name, path included")));
addCommand("setSpline",
makeCommandVoid3(*this, &SE3TrajectoryGenerator::setSpline,
docCommandVoid3("Load serialized spline from file",
"(string) filename",
"(double) time to initial conf",
"(Matrix) orientation of the point")));
addCommand("startSinusoid", addCommand("startSinusoid",
makeCommandVoid3(*this, &SE3TrajectoryGenerator::startSinusoid, makeCommandVoid3(*this, &SE3TrajectoryGenerator::startSinusoid,
...@@ -146,6 +156,7 @@ namespace dynamicgraph ...@@ -146,6 +156,7 @@ namespace dynamicgraph
m_noTrajGen[i] = new NoTrajectoryGenerator(1); m_noTrajGen[i] = new NoTrajectoryGenerator(1);
m_currentTrajGen[i] = m_noTrajGen[i]; m_currentTrajGen[i] = m_noTrajGen[i];
} }
m_splineTrajGen = new parametriccurves::Spline<double,Eigen::Dynamic>();
m_textFileTrajGen = new TextFileTrajectoryGenerator(dt, m_np); m_textFileTrajGen = new TextFileTrajectoryGenerator(dt, m_np);
m_initSucceeded = true; m_initSucceeded = true;
} }
...@@ -184,11 +195,22 @@ namespace dynamicgraph ...@@ -184,11 +195,22 @@ namespace dynamicgraph
} }
else if(iter == m_iterLast) else if(iter == m_iterLast)
{ {
if (m_triggerSIN(iter)==true && m_splineReady) startSpline();
if(m_status[0]==TG_TEXT_FILE) if(m_status[0]==TG_TEXT_FILE)
{ {
for(unsigned int i=0; i<m_np; i++) for(unsigned int i=0; i<m_np; i++)
s(i) = m_textFileTrajGen->getPos()[i]; s(i) = m_textFileTrajGen->getPos()[i];
} }
else if(m_status[0]==TG_SPLINE)
{
const Eigen::Vector3d& t= (*m_splineTrajGen)(m_t);
s.segment<3>(0) = m_splineRotation.row(0);
s.segment<3>(4) = m_splineRotation.row(1);
s.segment<3>(8) = m_splineRotation.row(2);
s[3] = t[0];
s[7] = t[1];
s[11]= t[2];
}
else else
for(unsigned int i=0; i<m_np; i++) for(unsigned int i=0; i<m_np; i++)
s(i) = m_currentTrajGen[i]->getPos()(0); s(i) = m_currentTrajGen[i]->getPos()(0);
...@@ -196,7 +218,7 @@ namespace dynamicgraph ...@@ -196,7 +218,7 @@ namespace dynamicgraph
return s; return s;
} }
m_iterLast = iter; m_iterLast = iter;
if (m_triggerSIN(iter)==true && m_splineReady) startSpline();
if(m_status[0]==TG_TEXT_FILE) if(m_status[0]==TG_TEXT_FILE)
{ {
const VectorXd& xRef = m_textFileTrajGen->compute_next_point(); const VectorXd& xRef = m_textFileTrajGen->compute_next_point();
...@@ -212,6 +234,38 @@ namespace dynamicgraph ...@@ -212,6 +234,38 @@ namespace dynamicgraph
if(m_textFileTrajGen->isTrajectoryEnded()) if(m_textFileTrajGen->isTrajectoryEnded())
SEND_MSG("Text file trajectory ended.", MSG_TYPE_INFO); SEND_MSG("Text file trajectory ended.", MSG_TYPE_INFO);
} }
else if(m_status[0]==TG_SPLINE)
{
m_t += m_dt;
if(!m_splineTrajGen->checkRange(m_t))
{
const Eigen::Vector3d& t= (*m_splineTrajGen)(m_splineTrajGen->tmax());
s.segment<3>(0) = m_splineRotation.row(0);
s.segment<3>(4) = m_splineRotation.row(1);
s.segment<3>(8) = m_splineRotation.row(2);
s[3] = t[0];
s[7] = t[1];
s[11]= t[2];
for(unsigned int i=0; i<m_np; i++)
{
m_noTrajGen[i]->set_initial_point(s(i));
m_status[i] = TG_STOP;
}
m_splineReady =false;
SEND_MSG("Spline trajectory ended. Remember to turn off the trigger.", MSG_TYPE_INFO);
m_t =0;
}
else
{
const Eigen::Vector3d& t= (*m_splineTrajGen)(m_t);
s.segment<3>(0) = m_splineRotation.row(0);
s.segment<3>(4) = m_splineRotation.row(1);
s.segment<3>(8) = m_splineRotation.row(2);
s[3] = t[0];
s[7] = t[1];
s[11]= t[2];
}
}
else else
{ {
for(unsigned int i=0; i<m_np; i++) for(unsigned int i=0; i<m_np; i++)
...@@ -250,6 +304,12 @@ namespace dynamicgraph ...@@ -250,6 +304,12 @@ namespace dynamicgraph
for(unsigned int i=0; i<m_nv; i++) for(unsigned int i=0; i<m_nv; i++)
s(i) = m_textFileTrajGen->getVel()[i]; s(i) = m_textFileTrajGen->getVel()[i];
} }
else if(m_status[0]==TG_SPLINE)
{
const Eigen::Vector3d& t= m_splineTrajGen->derivate(m_t,1);
s.segment<3>(0) = t;
s.segment<3>(3).setZero();
}
else else
for(unsigned int i=0; i<m_nv; i++) for(unsigned int i=0; i<m_nv; i++)
s(i) = m_currentTrajGen[i]->getVel()(0); s(i) = m_currentTrajGen[i]->getVel()(0);
...@@ -275,6 +335,12 @@ namespace dynamicgraph ...@@ -275,6 +335,12 @@ namespace dynamicgraph
for(unsigned int i=0; i<m_nv; i++) for(unsigned int i=0; i<m_nv; i++)
s(i) = m_textFileTrajGen->getAcc()[i]; s(i) = m_textFileTrajGen->getAcc()[i];
} }
else if(m_status[0]==TG_SPLINE)
{
const Eigen::Vector3d& t= m_splineTrajGen->derivate(m_t,2);
s.segment<3>(0) = t;
s.segment<3>(3).setZero();
}
else else
for(unsigned int i=0; i<m_nv; i++) for(unsigned int i=0; i<m_nv; i++)
s(i) = m_currentTrajGen[i]->getAcc()(0); s(i) = m_currentTrajGen[i]->getAcc()(0);
...@@ -336,6 +402,73 @@ namespace dynamicgraph ...@@ -336,6 +402,73 @@ namespace dynamicgraph
} }
} }
void SE3TrajectoryGenerator::setSpline(const std::string& fileName,
const double& timeToInitConf,
const Eigen::MatrixXd& init_rotation)
{
if(!m_initSucceeded)
return SEND_MSG("Cannot start sinusoid before initialization!",MSG_TYPE_ERROR);
for(unsigned int i=0; i<m_np; i++)
if(m_status[i]!=TG_STOP)
return SEND_MSG("You cannot control component "+toString(i)+" because it is already controlled.", MSG_TYPE_ERROR);
if(!m_splineTrajGen->loadFromFile(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;
m_splineReady = true;
m_splineRotation = init_rotation;
if(timeToInitConf < 0)
{
SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
return;
}
const VectorXd& t= (*m_splineTrajGen)(0.0);
VectorXd xInit(12);
xInit.segment<3>(0) = m_splineRotation.row(0);
xInit.segment<3>(4) = m_splineRotation.row(1);
xInit.segment<3>(8) = m_splineRotation.row(2);
xInit[3] = t[0];
xInit[7] = t[1];
xInit[11]= t[2];
for(unsigned int i=0; i<m_np; i++)
if(fabs(xInit[i] - m_currentTrajGen[i]->getPos()(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_np; i++)
{
m_minJerkTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
m_minJerkTrajGen[i]->set_final_point(xInit[i]);
m_minJerkTrajGen[i]->set_trajectory_time(timeToInitConf);
m_status[i] = TG_MIN_JERK;
m_currentTrajGen[i] = m_minJerkTrajGen[i];
}
return;
}
SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
}
void SE3TrajectoryGenerator::startSpline()
{
if(m_status[0]==TG_SPLINE) return;
m_t = 0.0;
for(unsigned int i=0; i<m_np; i++)
{
m_status[i] = TG_SPLINE;
}
}
void SE3TrajectoryGenerator::startSinusoid(const int& id, const double& xFinal, const double& time) void SE3TrajectoryGenerator::startSinusoid(const int& id, const double& xFinal, const double& time)
{ {
if(!m_initSucceeded) if(!m_initSucceeded)
...@@ -474,6 +607,9 @@ namespace dynamicgraph ...@@ -474,6 +607,9 @@ namespace dynamicgraph
m_noTrajGen[i]->set_initial_point(m_currentTrajGen[i]->getPos()); m_noTrajGen[i]->set_initial_point(m_currentTrajGen[i]->getPos());
m_status[i] = TG_STOP; m_status[i] = TG_STOP;
m_currentTrajGen[i] = m_noTrajGen[i]; m_currentTrajGen[i] = m_noTrajGen[i];
m_splineReady=false;
m_t=0.0;
} }
/* ------------------------------------------------------------------- */ /* ------------------------------------------------------------------- */
......
Supports Markdown
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