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); }