Commit e566126c authored by Francois Keith's avatar Francois Keith
Browse files

Add signals for the maximal/minimal joint velocity/torques.

parent 025c60fe
......@@ -207,6 +207,10 @@ class SOTDYNAMIC_EXPORT Dynamic
dg::SignalTimeDependent<double,int> footHeightSOUT;
dg::SignalTimeDependent<ml::Vector,int> upperJlSOUT;
dg::SignalTimeDependent<ml::Vector,int> lowerJlSOUT;
dg::SignalTimeDependent<ml::Vector,int> upperVlSOUT;
dg::SignalTimeDependent<ml::Vector,int> lowerVlSOUT;
dg::SignalTimeDependent<ml::Vector,int> upperTlSOUT;
dg::SignalTimeDependent<ml::Vector,int> lowerTlSOUT;
dg::Signal<ml::Vector,int> inertiaRotorSOUT;
dg::Signal<ml::Vector,int> gearRatioSOUT;
......@@ -234,6 +238,12 @@ class SOTDYNAMIC_EXPORT Dynamic
ml::Vector& getUpperJointLimits( ml::Vector& res,const int& time );
ml::Vector& getLowerJointLimits( ml::Vector& res,const int& time );
ml::Vector& getUpperVelocityLimits( ml::Vector& res,const int& time );
ml::Vector& getLowerVelocityLimits( ml::Vector& res,const int& time );
ml::Vector& getUpperTorqueLimits( ml::Vector& res,const int& time );
ml::Vector& getLowerTorqueLimits( ml::Vector& res,const int& time );
ml::Vector& computeTorqueDrift( ml::Vector& res,const int& time );
public: /* --- PARAMS --- */
......
......@@ -124,6 +124,22 @@ Dynamic( const std::string & name, bool build )
sotNOSIGNAL,
"sotDynamic("+name+")::output(vector)::lowerJl" )
,upperVlSOUT( boost::bind(&Dynamic::getUpperVelocityLimits,this,_1,_2),
sotNOSIGNAL,
"sotDynamic("+name+")::output(vector)::upperVl" )
,lowerVlSOUT( boost::bind(&Dynamic::getLowerVelocityLimits,this,_1,_2),
sotNOSIGNAL,
"sotDynamic("+name+")::output(vector)::lowerVl" )
,upperTlSOUT( boost::bind(&Dynamic::getUpperTorqueLimits,this,_1,_2),
sotNOSIGNAL,
"sotDynamic("+name+")::output(vector)::upperTl" )
,lowerTlSOUT( boost::bind(&Dynamic::getLowerTorqueLimits,this,_1,_2),
sotNOSIGNAL,
"sotDynamic("+name+")::output(vector)::lowerTl" )
,inertiaRotorSOUT( "sotDynamic("+name+")::output(matrix)::inertiaRotor" )
,gearRatioSOUT( "sotDynamic("+name+")::output(matrix)::gearRatio" )
,inertiaRealSOUT( boost::bind(&Dynamic::computeInertiaReal,this,_1,_2),
......@@ -158,6 +174,10 @@ Dynamic( const std::string & name, bool build )
signalRegistration(footHeightSOUT);
signalRegistration(upperJlSOUT);
signalRegistration(lowerJlSOUT);
signalRegistration(upperVlSOUT);
signalRegistration(lowerVlSOUT);
signalRegistration(upperTlSOUT);
signalRegistration(lowerTlSOUT);
signalRegistration(inertiaSOUT);
signalRegistration(inertiaRealSOUT);
signalRegistration(inertiaRotorSOUT);
......@@ -1271,6 +1291,70 @@ getLowerJointLimits(ml::Vector& res, const int&)
return res;
}
ml::Vector& Dynamic::
getUpperVelocityLimits(ml::Vector& res, const int&)
{
sotDEBUGIN(15);
const unsigned int NBJ = m_HDR->numberDof();
res.resize( NBJ );
for( unsigned int i=0;i<NBJ;++i )
{
res(i)=m_HDR->upperVelocityBoundDof( i );
}
sotDEBUG(15) << "upperVelocityLimit (" << NBJ << ")=" << res <<endl;
sotDEBUGOUT(15);
return res;
}
ml::Vector& Dynamic::
getLowerVelocityLimits(ml::Vector& res, const int&)
{
sotDEBUGIN(15);
const unsigned int NBJ = m_HDR->numberDof();
res.resize( NBJ );
for( unsigned int i=0;i<NBJ;++i )
{
res(i)=m_HDR->lowerVelocityBoundDof( i );
}
sotDEBUG(15) << "lowerVelocityLimit (" << NBJ << ")=" << res <<endl;
sotDEBUGOUT(15);
return res;
}
ml::Vector& Dynamic::
getUpperTorqueLimits(ml::Vector& res, const int&)
{
sotDEBUGIN(15);
const unsigned int NBJ = m_HDR->numberDof();
res.resize( NBJ );
for( unsigned int i=0;i<NBJ;++i )
{
res(i)=m_HDR->upperTorqueBoundDof( i );
}
sotDEBUG(15) << "upperTorqueLimit (" << NBJ << ")=" << res <<endl;
sotDEBUGOUT(15);
return res;
}
ml::Vector& Dynamic::
getLowerTorqueLimits(ml::Vector& res, const int&)
{
sotDEBUGIN(15);
const unsigned int NBJ = m_HDR->numberDof();
res.resize( NBJ );
for( unsigned int i=0;i<NBJ;++i )
{
res(i)=m_HDR->lowerTorqueBoundDof( i );
}
sotDEBUG(15) << "lowerTorqueLimit (" << NBJ << ")=" << res <<endl;
sotDEBUGOUT(15);
return res;
}
ml::Vector& Dynamic::
computeTorqueDrift( ml::Vector& tauDrift,const int & iter )
{
......
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