From 9eea73f47eb01e2af8fd830388b76b4499d4b0ef Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Thu, 3 Mar 2011 15:35:54 +0100
Subject: [PATCH] Changed the way velocity signal is called.
---
src/robot-dyn-simu.cpp | 34 +++++++++++++++++++++++++++++++---
src/robot-dyn-simu.h | 2 +-
2 files changed, 32 insertions(+), 4 deletions(-)
diff --git a/src/robot-dyn-simu.cpp b/src/robot-dyn-simu.cpp
index 39cd4c4..dca9690 100644
--- a/src/robot-dyn-simu.cpp
+++ b/src/robot-dyn-simu.cpp
@@ -43,7 +43,7 @@ namespace dynamicgraph
: Device(name)
,CONSTRUCT_SIGNAL_IN(acceleration,ml::Vector)
- ,CONSTRUCT_SIGNAL(velocity,OUT,ml::Vector)
+ ,CONSTRUCT_SIGNAL_OUT(velocity,ml::Vector,sotNOSIGNAL)
{
Entity::signalRegistration( accelerationSIN << velocitySOUT );
@@ -52,6 +52,17 @@ namespace dynamicgraph
addCommand("setVelocity",
makeDirectSetter(*this,&velocity_,
docDirectSetter("velocity","vector")));
+ std::string docstring;
+ /* Command increment. */
+ docstring =
+ "\n"
+ " Integrate dynamics for time step provided as input\n"
+ "\n"
+ " take one floating point number as input\n"
+ "\n";
+ addCommand("increment",
+ command::makeCommandVoid1((Device&)*this,
+ &Device::increment, docstring));
}
@@ -61,12 +72,29 @@ namespace dynamicgraph
os << "RobotDynSimu, nothing more to say yet." << std::endl;
}
+ ml::Vector& RobotDynSimu::
+ velocitySOUT_function( ml::Vector& v, int )
+ {
+ if( velocity_.size()!=state_.size() )
+ {
+ velocity_.resize( state_.size() );
+ velocity_.fill(0.0);
+ }
+ v = velocity_;
+ return v;
+ }
+
/* ---------------------------------------------------------------------- */
void RobotDynSimu::
integrate( const double & dt )
{
const ml::Vector & acceleration = accelerationSIN( controlSIN.getTime() );
+ if( velocity_.size()!=state_.size() )
+ {
+ velocity_.resize( state_.size() );
+ velocity_.fill(0.0);
+ }
assert( velocity_.size() == state_.size()
&& velocity_.size() == acceleration.size() );
@@ -76,14 +104,14 @@ namespace dynamicgraph
for( unsigned int i=6;i<state_.size();++i )
{ state_(i) += velocity_(i)*dt; }
- velocitySOUT.setConstant( velocity_);
+ velocitySOUT.setReady();
}
void RobotDynSimu::
setVelocity( const ml::Vector& v )
{
velocity_ = v;
- velocitySOUT.setConstant( velocity_);
+ velocitySOUT.setReady();
}
} // namespace dyninv
diff --git a/src/robot-dyn-simu.h b/src/robot-dyn-simu.h
index 77c5685..1ee606d 100644
--- a/src/robot-dyn-simu.h
+++ b/src/robot-dyn-simu.h
@@ -71,7 +71,7 @@ namespace dynamicgraph {
public: /* --- SIGNALS --- */
DECLARE_SIGNAL_IN( acceleration,ml::Vector );
- DECLARE_SIGNAL( velocity,OUT,ml::Vector );
+ DECLARE_SIGNAL_OUT( velocity,ml::Vector );
protected:
virtual void integrate( const double & dt );
--
GitLab