Unverified Commit 81280edc authored by olivier stasse's avatar olivier stasse Committed by GitHub
Browse files

Merge pull request #54 from jmirabel/master

Update matrix operators and entity Event.
parents 5733488a 273550d6
......@@ -88,6 +88,7 @@ SET(NEWHEADERS
sot/core/periodic-call-entity.hh
sot/core/trajectory.hh
sot/core/switch.hh
sot/core/variadic-op.hh
)
INSTALL(FILES ${NEWHEADERS}
DESTINATION include/sot/core
......
......@@ -24,6 +24,7 @@
#include <dynamic-graph/pool.h>
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
#include <sot/core/config.hh>
......@@ -57,6 +58,12 @@ namespace dynamicgraph {
" Get list of signals\n";
addCommand ("list", new command::Getter<Event, std::string>
(*this, &Event::getSignalsByName, docstring));
docstring =
"\n"
" Triggers an event only when condition goes from False to True\n";
addCommand ("setOnlyUp", new command::Setter<Event, bool>
(*this, &Event::setOnlyUp, docstring));
}
~Event () {}
......@@ -87,6 +94,11 @@ namespace dynamicgraph {
return oss.str();
}
void setOnlyUp (const bool& up)
{
onlyUp_ = up;
}
private:
typedef SignalBase<int>* Trigger_t;
typedef std::vector<Trigger_t> Triggers_t;
......@@ -95,11 +107,14 @@ namespace dynamicgraph {
{
const bool& val = conditionSIN (time);
ret = (val != lastVal_);
bool trigger = onlyUp_ ? (!lastVal_ && val) : ret;
if (ret) {
lastVal_ = val;
for (Triggers_t::const_iterator _s = triggers.begin();
_s != triggers.end(); ++_s)
(*_s)->recompute (time);
if (trigger) {
for (Triggers_t::const_iterator _s = triggers.begin();
_s != triggers.end(); ++_s)
(*_s)->recompute (time);
}
}
return ret;
}
......@@ -109,7 +124,7 @@ namespace dynamicgraph {
Triggers_t triggers;
SignalPtr <bool, int> conditionSIN;
bool lastVal_;
bool lastVal_, onlyUp_;
};
} // namespace sot
} // namespace dynamicgraph
......
......@@ -201,6 +201,7 @@ namespace dynamicgraph {
defineFreeFloatingJoints(const unsigned int& jointIdFirst,
const unsigned int& jointIdLast = -1);
virtual void defineNbDof( const unsigned int& nbDof );
virtual const unsigned int& getNbDof() const { return nbJoints; }
/*! @} */
public: /* --- CONTROL --- */
......
......@@ -28,36 +28,40 @@
#include <dynamic-graph/command-getter.h>
#include <sot/core/config.hh>
#include <sot/core/variadic-op.hh>
namespace dynamicgraph {
namespace sot {
/// Switch
template <typename Value, typename Time = int>
class SOT_CORE_DLLAPI Switch : public dynamicgraph::Entity
class SOT_CORE_DLLAPI Switch : public VariadicAbstract<Value,Value,Time>
{
DYNAMIC_GRAPH_ENTITY_DECL();
typedef VariadicAbstract<Value,Value,Time> Base;
Switch (const std::string& name) :
Entity (name),
Base (name,CLASS_NAME),
selectionSIN(NULL,"Switch("+name+")::input(int)::selection"),
boolSelectionSIN(NULL,"Switch("+name+")::input(bool)::boolSelection"),
signalSOUT ("Switch("+name+")::output(" + typeName() + ")::sout")
boolSelectionSIN(NULL,"Switch("+name+")::input(bool)::boolSelection")
{
signalSOUT.setFunction (boost::bind (&Switch::signal, this, _1, _2));
signalRegistration (selectionSIN << boolSelectionSIN << signalSOUT);
this->signalRegistration (selectionSIN << boolSelectionSIN);
this->SOUT.setFunction (boost::bind(&Switch::signal,this,_1,_2));
this->SOUT.addDependency (selectionSIN);
this->SOUT.addDependency (boolSelectionSIN);
using command::makeCommandVoid1;
std::string docstring =
"\n"
" Set number of input signals\n";
addCommand ("setSignalNumber", makeCommandVoid1
(*this, &Switch::setSignalNumber, docstring));
this->addCommand ("setSignalNumber", makeCommandVoid1
(*(Base*)this, &Base::setSignalNumber, docstring));
docstring =
"\n"
" Get number of input signals\n";
addCommand ("getSignalNumber",
new command::Getter<Switch, int> (*this, &Switch::getSignalNumber, docstring));
this->addCommand ("getSignalNumber",
new command::Getter<Base, int> (*this, &Base::getSignalNumber, docstring));
}
~Switch () {}
......@@ -69,39 +73,7 @@ namespace dynamicgraph {
"Dynamically select a given signal based on a input information.\n";
}
void setSignalNumber (const int& n)
{
assert (n>=0);
const std::size_t oldSize = signals.size();
for (std::size_t i = n; i < oldSize; ++i)
{
std::ostringstream oss; oss << "sin" << i;
signalDeregistration(oss.str());
delete signals[i];
}
signals.resize(n,NULL);
for (std::size_t i = oldSize; i < (std::size_t)n; ++i)
{
assert (signals[i]==NULL);
std::ostringstream oss;
oss << "Switch("<< getName()<< ")::input(" << typeName() << ")::sin" << i;
signals[i] = new Signal_t (NULL,oss.str());
signalRegistration(*signals[i]);
}
}
int getSignalNumber () const
{
return (int)signals.size();
}
private:
typedef SignalPtr<Value, Time> Signal_t;
typedef std::vector<Signal_t*> Signals_t;
static const std::string& typeName ();
Value& signal (Value& ret, const Time& time)
{
int sel;
......@@ -111,18 +83,15 @@ namespace dynamicgraph {
const bool& b = boolSelectionSIN(time);
sel = b ? 1 : 0;
}
if (sel < 0 || sel >= int(signals.size()))
if (sel < 0 || sel >= int(this->signalsIN.size()))
throw std::runtime_error ("Signal selection is out of range.");
ret = (*signals[sel]) (time);
ret = this->signalsIN[sel]->access (time);
return ret;
}
Signals_t signals;
SignalPtr <int, Time> selectionSIN;
SignalPtr <bool, Time> boolSelectionSIN;
Signal <Value, Time> signalSOUT;
};
} // namespace sot
} // namespace dynamicgraph
......
/*
* Copyright 2018,
* Mirabel Joseph
*
* CNRS/AIST
*
* This file is part of sot-core.
* sot-core 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-core 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-core. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef SOT_CORE_VARIADICOP_HH
#define SOT_CORE_VARIADICOP_HH
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#include <sot/core/flags.hh>
#include <dynamic-graph/entity.h>
#include <sot/core/pool.hh>
#include <dynamic-graph/all-signals.h>
#include <sot/core/matrix-geometry.hh>
/* STD */
#include <string>
#include <boost/function.hpp>
namespace dynamicgraph {
namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
template< typename Tin, typename Tout, typename Time >
class VariadicAbstract
:public Entity
{
public: /* --- CONSTRUCTION --- */
static std::string getTypeInName ( void );
static std::string getTypeOutName( void );
VariadicAbstract( const std::string& name , const std::string& className)
: Entity(name)
,SOUT( className+"("+name+")::output("+getTypeOutName()+")::sout")
,baseSigname(className+"("+name+")::input("+getTypeInName()+")::")
{
signalRegistration( SOUT );
}
virtual ~VariadicAbstract( void )
{
for (std::size_t i = 0; i < signalsIN.size(); ++i) {
_removeSignal (i);
delete signalsIN[i];
}
};
public: /* --- SIGNAL --- */
SignalTimeDependent<Tout,int> SOUT;
std::size_t addSignal ()
{
std::ostringstream oss; oss << "sin" << signalsIN.size();
return addSignal (oss.str());
}
std::size_t addSignal (const std::string& name)
{
signal_t* sig = new signal_t (NULL, baseSigname + name);
try {
_declareSignal(sig);
signalsIN.push_back (sig);
// names.push_back (name);
return signalsIN.size()-1;
} catch (const ExceptionAbstract&) {
delete sig;
throw;
}
}
void removeSignal ()
{
assert (signalsIN.size()>0);
_removeSignal (signalsIN().size()-1);
// names.pop_back();
signalsIN.pop_back();
}
void setSignalNumber (const int& n)
{
assert (n>=0);
const std::size_t oldSize = signalsIN.size();
for (std::size_t i = n; i < oldSize; ++i)
_removeSignal (i);
signalsIN.resize(n,NULL);
// names.resize(n);
for (std::size_t i = oldSize; i < (std::size_t)n; ++i)
{
assert (signalsIN[i]==NULL);
std::ostringstream oss;
oss << baseSigname << "sin" << i;
// names[i] = oss.str();
// signal_t* s = new signal_t (NULL,names[i]);
signal_t* s = new signal_t (NULL,oss.str());
signalsIN[i] = s;
_declareSignal (s);
}
}
int getSignalNumber () const
{
return (int)signalsIN.size();
}
protected:
typedef SignalPtr<Tin,int> signal_t;
std::vector< signal_t* > signalsIN;
// Use signal->shortName instead
// std::vector< std::string > names;
private:
void _removeSignal (const std::size_t i)
{
// signalDeregistration(names[i]);
signalDeregistration(signalsIN[i]->shortName());
SOUT.removeDependency (*signalsIN[i]);
delete signalsIN[i];
}
void _declareSignal (signal_t* s)
{
signalRegistration(*s);
SOUT.addDependency (*s);
}
const std::string baseSigname;
};
template< typename Operator >
class VariadicOp
:public VariadicAbstract<typename Operator::Tin, typename Operator::Tout, int>
{
Operator op;
typedef typename Operator::Tin Tin;
typedef typename Operator::Tout Tout;
typedef VariadicOp<Operator> Self;
public: /* --- CONSTRUCTION --- */
typedef VariadicAbstract<Tin,Tout,int> Base;
// static std::string getTypeInName ( void ) { return Operator::nameTypeIn (); }
// static std::string getTypeOutName( void ) { return Operator::nameTypeOut(); }
static const std::string CLASS_NAME;
virtual const std::string& getClassName () const { return CLASS_NAME; }
std::string getDocString () const { return op.getDocString ();}
VariadicOp( const std::string& name )
: Base(name, CLASS_NAME)
{
this->SOUT.setFunction (boost::bind(&Self::computeOperation,this,_1,_2));
op.initialize(this,this->commandMap);
}
virtual ~VariadicOp( void ) {};
protected:
Tout& computeOperation( Tout& res,int time )
{
std::vector< const Tin* > in (this->signalsIN.size());
for (std::size_t i = 0; i < this->signalsIN.size(); ++i) {
const Tin& x = this->signalsIN[i]->access (time);
in[i] = &x;
}
op(in,res);
return res;
}
};
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef SOT_CORE_VARIADICOP_HH
......@@ -24,6 +24,7 @@
#include <sot/core/unary-op.hh>
#include <sot/core/binary-op.hh>
#include <sot/core/variadic-op.hh>
#include <sot/core/matrix-geometry.hh>
......@@ -94,13 +95,25 @@ namespace dynamicgraph {
{
void operator()( const Tin& m,Vector& res ) const
{
assert( (imin<=imax) && (imax <= m.size()) );
res.resize( imax-imin );
for( int i=imin;i<imax;++i ) res(i-imin)=m(i);
res.resize(size);
Vector::Index r=0;
for (std::size_t i = 0; i < idxs.size(); ++i) {
const Vector::Index& R = idxs[i].first;
const Vector::Index& nr = idxs[i].second;
assert( (nr>=0) && (R+nr <= m.size()) );
res.segment(r,nr) = m.segment(R,nr);
r += nr;
}
assert (r == size);
}
int imin,imax;
void setBounds( const int & m,const int & M ) { imin = m; imax = M; }
typedef std::pair <Vector::Index,Vector::Index> segment_t;
typedef std::vector <segment_t> segments_t;
segments_t idxs;
Vector::Index size;
void setBounds( const int & m,const int & M ) { idxs = segments_t (1, segment_t(m, M-m)); size = M-m; }
void addBounds( const int & m,const int & M ) { idxs .push_back( segment_t(m, M-m)); size += M-m; }
void addSpecificCommands(Entity& ent,
Entity::CommandMap_t& commandMap )
......@@ -112,7 +125,12 @@ namespace dynamicgraph {
= boost::bind( &VectorSelecter::setBounds,this,_1,_2 );
doc = docCommandVoid2("Set the bound of the selection [m,M[.","int (min)","int (max)");
ADD_COMMAND( "selec", makeCommandVoid2(ent,setBound,doc) );
boost::function< void( const int&, const int& ) > addBound
= boost::bind( &VectorSelecter::setBounds,this,_1,_2 );
doc = docCommandVoid2("Add a segment to be selected [m,M[.","int (min)","int (max)");
ADD_COMMAND( "addSelec", makeCommandVoid2(ent,setBound,doc) );
}
VectorSelecter () : size (0) {}
};
REGISTER_UNARY_OP( VectorSelecter,Selec_of_vector );
......@@ -241,11 +259,7 @@ namespace dynamicgraph {
{
void operator()( const dg::Vector& r,dg::Matrix & res )
{
unsigned imax=r.size(),jmax=r.size();
if(( nbr!=0)&&(nbc!=0)) { imax=nbr; jmax=nbc; }
res.resize(imax,jmax);
for( unsigned int i=0;i<imax;++i )
for( unsigned int j=0;j<jmax;++j ) if( i==j ) res(i,i)=r(i); else res(i,j)=0;
res = r.asDiagonal();
}
public:
Diagonalizer( void ) : nbr(0),nbc(0) {}
......@@ -328,12 +342,10 @@ namespace dynamicgraph {
{
void operator()( const MatrixHomogeneous& M,dg::Vector& res )
{
MatrixRotation R; R = M.linear();
VectorUTheta r(R);
dg::Vector t(3); t = M.translation();
res.resize(6);
for( int i=0;i<3;++i ) res(i)=t(i);
for( int i=0;i<3;++i ) res(i+3)=r.angle()*r.axis()(i);
res.resize(6);
VectorUTheta r(M.linear());
res.head<3>() = M.translation();
res.tail<3>() = r.angle()*r.axis();
}
};
REGISTER_UNARY_OP( HomogeneousMatrixToVector,MatrixHomoToPoseUTheta);
......@@ -357,11 +369,12 @@ namespace dynamicgraph {
void operator()( const dg::Vector& v,MatrixHomogeneous& res )
{
assert( v.size()>=6 );
Eigen::Affine3d trans;
trans = Eigen::Translation3d(v.head<3>());
dg::Vector ruth = v.tail<3>();
Eigen::Affine3d R(Eigen::AngleAxisd(ruth.norm(), ruth.normalized()));
res = R*trans;
res.translation() = v.head<3>();
double theta = v.tail<3>().norm();
if (theta > 0)
res.linear() = Eigen::AngleAxisd(theta, v.tail<3>() / theta).matrix();
else
res.linear().setIdentity();
}
};
REGISTER_UNARY_OP(PoseUThetaToMatrixHomo,PoseUThetaToMatrixHomo);
......@@ -371,12 +384,10 @@ namespace dynamicgraph {
{
void operator()( const MatrixHomogeneous& M,Vector& res )
{
MatrixRotation R; R = M.linear();
VectorQuaternion r(R);
dg::Vector t(3); t = M.translation();
res.resize(7);
for( int i=0;i<3;++i ) res(i)=t(i);
for( int i=0;i<4;++i ) res(i+3)=r.coeffs()(i);
res.head<3>() = M.translation();
Eigen::Map<VectorQuaternion> q (res.tail<4>().data());
q = M.linear();
}
};
REGISTER_UNARY_OP(MatrixHomoToPoseQuaternion,MatrixHomoToPoseQuaternion);
......@@ -443,7 +454,7 @@ namespace dynamicgraph {
: public UnaryOpHeader<MatrixHomogeneous,Matrix>
{
void operator()( const MatrixHomogeneous& M,dg::Matrix& res )
{ res=(dg::Matrix&)M; }
{ res=M.matrix(); }
};
REGISTER_UNARY_OP(HomoToMatrix,HomoToMatrix);
......@@ -650,24 +661,6 @@ namespace dynamicgraph {
/* --- MULTIPLICATION --------------------------------------------------- */
template< typename T>
struct Multiplier
: public BinaryOpHeader<T,T,T>
{
void operator()( const T& v1,const T& v2,T& res ) const { res = v1*v2; }
};
template<> void Multiplier<double>::
operator()( const double& v1,const double& v2,double& res ) const
{ res=v1; res*=v2; }
REGISTER_BINARY_OP(Multiplier<dynamicgraph::Matrix>,Multiply_of_matrix);
REGISTER_BINARY_OP(Multiplier<dynamicgraph::Vector>,Multiply_of_vector);
REGISTER_BINARY_OP(Multiplier<MatrixRotation>,Multiply_of_matrixrotation);
REGISTER_BINARY_OP(Multiplier<MatrixHomogeneous>,Multiply_of_matrixHomo);
REGISTER_BINARY_OP(Multiplier<MatrixTwist>,Multiply_of_matrixtwist);
REGISTER_BINARY_OP(Multiplier<VectorQuaternion>,Multiply_of_quaternion);
REGISTER_BINARY_OP(Multiplier<double>,Multiply_of_double);
template< typename F,typename E>
struct Multiplier_FxE__E
: public BinaryOpHeader<F,E,E>
......@@ -745,58 +738,6 @@ namespace dynamicgraph {
};
REGISTER_BINARY_OP(VectorStack,Stack_of_vector);
/* --- STACK ------------------------------------------------------------ */
struct VectorMix
: public BinaryOpHeader<dynamicgraph::Vector,dynamicgraph::Vector,dynamicgraph::Vector>
{
public:
std::vector<bool> useV1;
std::vector<int> idx1, idx2;
void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector& v2,dynamicgraph::Vector& res ) const
{
res.resize(useV1.size());
std::size_t k1=0, k2=0;
for (std::size_t i = 0; i < useV1.size(); ++i)
{
if (useV1[i]) {
assert (k1 < idx1.size());
res[i] = v1[idx1[k1]];
++k1;
} else {
assert (k2 < idx2.size());
res[i] = v2[idx2[k2]];
++k2;
}
}
assert (k1 == idx1.size());
assert (k2 == idx2.size());
}
void addSelec1( const int & i) { useV1.push_back(true ); idx1.push_back(i); }
void addSelec2( const int & i) { useV1.push_back(false); idx2.push_back(i); }
void addSpecificCommands(Entity& ent,
Entity::CommandMap_t& commandMap )
{
using namespace dynamicgraph::command;
boost::function< void( const int& ) > selec1
= boost::bind( &VectorMix::addSelec1,this,_1 );
boost::function< void( const int& ) > selec2
= boost::bind( &VectorMix::addSelec2,this,_1 );
ADD_COMMAND( "addSelec1",
makeCommandVoid1(ent, selec1,
docCommandVoid1("append value from vector 1.",
"int (index in vector 1)")));
ADD_COMMAND( "addSelec2",
makeCommandVoid1(ent, selec2,
docCommandVoid1("append value from vector 2.",
"int (index in vector 2)")));
}
};
REGISTER_BINARY_OP(VectorMix,Mix_of_vector);
/* ---------------------------------------------------------------------- */
struct Composer
......@@ -804,14 +745,8 @@ namespace dynamicgraph {
{
void operator() ( const dynamicgraph::Matrix& R,const dynamicgraph::Vector& t, MatrixHomogeneous& H ) const
{
for( int i=0;i<3;++i )
{
H(i,3)=t(i);
for( int j=0;j<3;++j )
H(i,j) = R(i,j);
H(3,i) = 0;
}
H(3,3)=1.;
H.linear() = R;