IVIGIT.

CMakeLists.txt

@@ -52,6 +52,7 @@ SET(libs
 	solver-op-space
 	zmp-estimator
 	pseudo-robot-dynamic
+	robot-dyn-simu
   )
 
 LIST(APPEND LOGGING_WATCHED_TARGETS ${libs})
@@ -71,6 +72,7 @@ SET(headers
 	solver-op-space.h
 	zmp-estimator.h
 	pseudo-robot-dynamic.h
+	robot-dyn-simu.h
 )
 
 # Add subdirectories.

src/dynamic_graph/sot/dyninv/__init__.py

@@ -15,3 +15,6 @@ SolverOpSpace('')
 
 from zmp_estimator import ZmpEstimator
 ZmpEstimator('')
+
+from robot_dyn_simu import RobotDynSimu
+RobotDynSimu('')

src/robot-dyn-simu.cpp

+/*
+ * Copyright 2011, Nicolas Mansard, LAAS-CNRS
+ *
+ * This file is part of sot-dyninv.
+ * sot-dyninv 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-dyninv 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-dyninv.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <sot/core/debug.hh>
+
+#include <dynamic-graph/factory.h>
+#include <sot-dyninv/robot-dyn-simu.h>
+#include <sot-dyninv/commands-helper.h>
+#include <sot-dyninv/mal-to-eigen.h>
+
+
+namespace dynamicgraph
+{
+  namespace sot
+  {
+    namespace dyninv
+    {
+
+      namespace dg = ::dynamicgraph;
+      using namespace dg;
+
+      /* --- DG FACTORY ------------------------------------------------------- */
+      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RobotDynSimu,"RobotDynSimu");
+
+      /* --- CONSTRUCTION ----------------------------------------------------- */
+      /* --- CONSTRUCTION ----------------------------------------------------- */
+      /* --- CONSTRUCTION ----------------------------------------------------- */
+      RobotDynSimu::
+      RobotDynSimu( const std::string & name )
+	: Device(name)
+
+	,CONSTRUCT_SIGNAL_IN(acceleration,ml::Vector)
+	,CONSTRUCT_SIGNAL(velocity,OUT,ml::Vector)
+
+      {
+	Entity::signalRegistration( accelerationSIN << velocitySOUT );
+
+	/* --- COMMANDS --- */
+	addCommand("setVelocity",
+		   makeDirectSetter(*this,&velocity_,
+				    docDirectSetter("velocity","vector")));
+
+      }
+
+      void RobotDynSimu::
+      display( std::ostream& os ) const
+      {
+	os << "RobotDynSimu, nothing more to say yet." << std::endl;
+      }
+
+      /* ---------------------------------------------------------------------- */
+      void RobotDynSimu::
+      integrate( const double & dt )
+      {
+	const ml::Vector & acceleration = accelerationSIN( controlSIN.getTime() );
+
+	assert( velocity_.size() == state_.size()
+		&& velocity_.size() == acceleration.size() );
+
+	velocity_ += acceleration*dt;
+
+	integrateRollPitchYaw(state_, velocity_, dt);
+	for( unsigned int i=6;i<state_.size();++i )
+	  { state_(i) += velocity_(i)*dt; }
+
+	velocitySOUT.setConstant( velocity_);
+      }
+
+      void RobotDynSimu::
+      setVelocity( const ml::Vector& v )
+      {
+	velocity_ = v;
+	velocitySOUT.setConstant( velocity_);
+      }
+
+    } // namespace dyninv
+  } // namespace sot
+} // namespace dynamicgraph

src/robot-dyn-simu.h

+/*
+ * Copyright 2011, Nicolas Mansard, LAAS-CNRS
+ *
+ * This file is part of sot-dyninv.
+ * sot-dyninv 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-dyninv 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-dyninv.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_dyninv_RobotDynSimu_H__
+#define __sot_dyninv_RobotDynSimu_H__
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+#  if defined (pseudo_robot_dynamic_EXPORTS)
+#    define SOTROBOTDYNSIMU_EXPORT __declspec(dllexport)
+#  else
+#    define SOTROBOTDYNSIMU_EXPORT __declspec(dllimport)
+#  endif
+#else
+#  define SOTROBOTDYNSIMU_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+
+/* SOT */
+#include <sot-dyninv/signal-helper.h>
+#include <sot-dyninv/entity-helper.h>
+#include <sot-dyninv/dynamic-integrator.h>
+#include <sot/core/device.hh>
+
+
+namespace dynamicgraph {
+  namespace sot {
+    namespace dyninv {
+
+      /* --------------------------------------------------------------------- */
+      /* --- CLASS ----------------------------------------------------------- */
+      /* --------------------------------------------------------------------- */
+
+      class SOTROBOTDYNSIMU_EXPORT RobotDynSimu
+	:public ::dynamicgraph::sot::Device
+	,public ::dynamicgraph::EntityHelper<RobotDynSimu>
+	{
+
+	public: /* --- CONSTRUCTOR ---- */
+
+	  RobotDynSimu( const std::string & name );
+
+	public: /* --- ENTITY INHERITANCE --- */
+
+	  static const std::string CLASS_NAME;
+	  virtual void display( std::ostream& os ) const;
+	  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+
+	  typedef ::dynamicgraph::EntityHelper<RobotDynSimu>::EntityClassName
+	    EntityClassName;
+
+	public:  /* --- SIGNALS --- */
+
+	  DECLARE_SIGNAL_IN( acceleration,ml::Vector );
+	  DECLARE_SIGNAL( velocity,OUT,ml::Vector );
+
+	protected:
+	  virtual void integrate( const double & dt );
+
+	private:
+	  ml::Vector velocity_;
+
+	public:
+	  void setVelocity( const ml::Vector& v );
+
+	}; // class RobotDynSimu
+    } // namespace dyninv
+  } // namespace sot
+} // namespace dynamicgraph
+
+
+
+#endif // #ifndef __sot_dyninv_RobotDynSimu_H__