From 3ebc53340e60eb4f707b3c7198490ad84d7bc876 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Wed, 2 Mar 2011 16:24:39 +0100
Subject: [PATCH] IVIGIT.
---
CMakeLists.txt | 2 +
src/dynamic_graph/sot/dyninv/__init__.py | 3 +
src/robot-dyn-simu.cpp | 91 +++++++++++++++++++++++
src/robot-dyn-simu.h | 92 ++++++++++++++++++++++++
4 files changed, 188 insertions(+)
create mode 100644 src/robot-dyn-simu.cpp
create mode 100644 src/robot-dyn-simu.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 135a89a..f598ac6 100644
--- a/CMakeLists.txt
+++ b/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.
diff --git a/src/dynamic_graph/sot/dyninv/__init__.py b/src/dynamic_graph/sot/dyninv/__init__.py
index 484dda0..7d781f6 100755
--- a/src/dynamic_graph/sot/dyninv/__init__.py
+++ b/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('')
diff --git a/src/robot-dyn-simu.cpp b/src/robot-dyn-simu.cpp
new file mode 100644
index 0000000..39cd4c4
--- /dev/null
+++ b/src/robot-dyn-simu.cpp
@@ -0,0 +1,91 @@
+/*
+ * 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
diff --git a/src/robot-dyn-simu.h b/src/robot-dyn-simu.h
new file mode 100644
index 0000000..77c5685
--- /dev/null
+++ b/src/robot-dyn-simu.h
@@ -0,0 +1,92 @@
+/*
+ * 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__
--
GitLab