NextStep::
NextStep( const std::string & name )
:Entity(name)
,footPrintList()
,period(PERIOD_DEFAULT)
,timeLastIntroduction( 0 )
,mode(MODE_3D)
,state( STATE_STOPED )
,zeroStepPosition( ZERO_STEP_POSITION_DEFAULT )
,rfMref0()
,lfMref0()
,twoHandObserver( name )
,verbose(0x0)
,referencePositionLeftSIN( NULL,"NextStep("+name+")::input(vector)::posrefleft" )
,referencePositionRightSIN( NULL,"NextStep("+name+")::input(vector)::posrefright" )
,contactFootSIN( NULL,"NextStep("+name+")::input(uint)::contactfoot" )
,triggerSOUT( "NextStep("+name+")::input(dummy)::trigger" )
{
sotDEBUGIN(5);
triggerSOUT.setFunction( boost::bind(&NextStep::triggerCall,this,_1,_2) );
signalRegistration( referencePositionLeftSIN<<referencePositionRightSIN
<<contactFootSIN<<triggerSOUT );
signalRegistration( twoHandObserver.getSignals() );
referencePositionLeftSIN.plug( &twoHandObserver.referencePositionLeftSOUT );
referencePositionRightSIN.plug( &twoHandObserver.referencePositionRightSOUT );
sotDEBUGOUT(5);
}
Kalman::Kalman( const std::string& name )
: Entity(name)
,measureSIN (NULL,"Kalman("+name+")::input(vector)::y")
,modelTransitionSIN( NULL,"Kalman("+name+")::input(matrix)::F" )
,modelMeasureSIN( NULL,"Kalman("+name+")::input(matrix)::H" )
,noiseTransitionSIN( NULL,"Kalman("+name+")::input(matrix)::Q" )
,noiseMeasureSIN( NULL,"Kalman("+name+")::input(matrix)::R" )
,statePredictedSIN (0, "Kalman("+name+")::input(vector)::x_pred")
,observationPredictedSIN (0, "Kalman("+name+")::input(vector)::y_pred")
,varianceUpdateSOUT ("Kalman("+name+")::output(vector)::P")
,stateUpdateSOUT ("Kalman("+name+")::output(vector)::x_est"),
stateEstimation_ (),
stateVariance_ ()
{
sotDEBUGIN(15);
varianceUpdateSOUT.setFunction
(boost::bind(&Kalman::computeVarianceUpdate,
this,_1,_2));
stateUpdateSOUT.setFunction (boost::bind(&Kalman::computeStateUpdate,
this, _1, _2));
signalRegistration( measureSIN << observationPredictedSIN
<< modelTransitionSIN
<< modelMeasureSIN << noiseTransitionSIN
<< noiseMeasureSIN << statePredictedSIN
<< stateUpdateSOUT << varianceUpdateSOUT );
std::string docstring =
" Set initial state estimation\n"
"\n"
" input:\n"
" - a vector: initial state\n";
addCommand ("setInitialState",
new Setter <Kalman, Vector> (*this,
&Kalman::setStateEstimation,
docstring));
docstring =
" Set variance of initial state estimation\n"
"\n"
" input:\n"
" - a matrix: variance covariance matrix\n";
addCommand ("setInitialVariance",
new Setter <Kalman, Matrix> (*this,
&Kalman::setStateVariance,
docstring));
sotDEBUGOUT(15);
}
GradientAscent::
GradientAscent( const std::string& n )
:Entity(n)
,gradientSIN(NULL, "GradientAscent(" + n + ")::input(vector)::gradient")
,learningRateSIN(NULL, "GradientAscent(" + n + ")::input(double)::learningRate")
,init(false)
,refresherSINTERN( "GradientAscent("+n+")::intern(dummy)::refresher" )
,valueSOUT(
boost::bind(&GradientAscent::update,this,_1,_2),
gradientSIN << refresherSINTERN, "GradientAscent(" + n + ")::output(vector)::value")
{
// Register signals into the entity.
signalRegistration(gradientSIN << learningRateSIN << valueSOUT);
refresherSINTERN.setDependencyType( TimeDependency<int>::ALWAYS_READY );
}
FeatureProjectedLine::
FeatureProjectedLine( const std::string& pointName )
: FeatureAbstract( pointName )
,CONSTRUCT_SIGNAL_IN(xa,MatrixHomogeneous)
,CONSTRUCT_SIGNAL_IN(xb,MatrixHomogeneous)
,CONSTRUCT_SIGNAL_IN(Ja,dg::Matrix)
,CONSTRUCT_SIGNAL_IN(Jb,dg::Matrix)
,CONSTRUCT_SIGNAL_IN(xc,dg::Vector)
{
jacobianSOUT.addDependency( xaSIN );
jacobianSOUT.addDependency( xbSIN );
jacobianSOUT.addDependency( xcSIN );
jacobianSOUT.addDependency( JaSIN );
jacobianSOUT.addDependency( JbSIN );
errorSOUT.addDependency( xaSIN );
errorSOUT.addDependency( xbSIN );
errorSOUT.addDependency( xcSIN );
signalRegistration( xaSIN << xbSIN << xcSIN << JaSIN << JbSIN );
}
RosJointState::RosJointState (const std::string& n)
: Entity (n),
// do not use callbacks, so do not create a useless spinner
nh_ (rosInit (false)),
state_ (0, MAKE_SIGNAL_STRING(name, true, "Vector", "state")),
publisher_ (nh_, "dynamic_graph/joint_states", 5),
jointState_ (),
trigger_ (boost::bind (&RosJointState::trigger, this, _1, _2),
sotNOSIGNAL,
MAKE_SIGNAL_STRING(name, true, "int", "trigger")),
rate_ (ROS_JOINT_STATE_PUBLISHER_RATE),
lastPublicated_ ()
{
try {
lastPublicated_ = ros::Time::now ();
} catch (const std::exception& exc) {
throw std::runtime_error ("Failed to call ros::Time::now ():" +
std::string (exc.what ()));
}
signalRegistration (state_ << trigger_);
trigger_.setNeedUpdateFromAllChildren (true);
// Fill header.
jointState_.header.seq = 0;
jointState_.header.stamp.sec = 0;
jointState_.header.stamp.nsec = 0;
jointState_.header.frame_id = "";
std::string docstring =
"\n"
" Retrieve joint names using robot model contained in a Dynamic entity\n"
"\n"
" Input:\n"
" - dynamic entity name (i.e. robot.dynamic.name)\n"
"\n";
addCommand ("retrieveJointNames",
new command::RetrieveJointNames (*this, docstring));
}
void addSignal()
{
signalRegistration(m_sigdSIN << m_sigdTimeDepSOUT);
}
