From 3898643ef0f14837b491e6d4b3b69ba4a087c652 Mon Sep 17 00:00:00 2001
From: Oscar Ramos <oramos@laas.fr>
Date: Wed, 9 Mar 2011 13:58:49 +0100
Subject: [PATCH] IVIGIT.
---
CMakeLists.txt | 2 +
python/dyndebug.py | 9 +-
src/dynamic_graph/sot/dyninv/__init__.py | 3 +
src/task-dyn-joint-limits.cpp | 215 +++++++++++++++++++++++
src/task-dyn-joint-limits.h | 106 +++++++++++
5 files changed, 332 insertions(+), 3 deletions(-)
create mode 100644 src/task-dyn-joint-limits.cpp
create mode 100644 src/task-dyn-joint-limits.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 135a89a..373de0e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -52,6 +52,7 @@ SET(libs
solver-op-space
zmp-estimator
pseudo-robot-dynamic
+ task-dyn-joint-limits
)
LIST(APPEND LOGGING_WATCHED_TARGETS ${libs})
@@ -67,6 +68,7 @@ SET(headers
controller-pd.h
task-dyn-pd.h
+ task-dyn-joint-limits.h
dynamic-integrator.h
solver-op-space.h
zmp-estimator.h
diff --git a/python/dyndebug.py b/python/dyndebug.py
index 343fdbf..8471e4f 100644
--- a/python/dyndebug.py
+++ b/python/dyndebug.py
@@ -3,11 +3,14 @@ from dynamic_graph.sot.core import *
from dynamic_graph.sot.core.math_small_entities import Derivator_of_Matrix
from dynamic_graph.sot.dynamics import *
from dynamic_graph.sot.dyninv import *
-import dynamic_graph.script_shortcuts
-from dynamic_graph.matlab import matlab
+import script_shortcuts
+from matlab import matlab
+
+#import dynamic_graph.script_shortcuts
+#from dynamic_graph.matlab import matlab
# --- Dynamic parameters ---
-hrp2_14_pkgdatarootdir = "/home/nmansard/compil/devgiri/hpp2/share/hrp2_14"
+hrp2_14_pkgdatarootdir = "/home/oramos/compil/fafnir/hpp2/share/hrp2_14"
modelDir = hrp2_14_pkgdatarootdir
xmlDir = hrp2_14_pkgdatarootdir
modelName = 'HRP2JRLmainsmall.wrl'
diff --git a/src/dynamic_graph/sot/dyninv/__init__.py b/src/dynamic_graph/sot/dyninv/__init__.py
index 484dda0..a900d9f 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 task_dyn_joint_limits import TaskDynJointLimits
+TaskDynJointLimits('')
diff --git a/src/task-dyn-joint-limits.cpp b/src/task-dyn-joint-limits.cpp
new file mode 100644
index 0000000..667bff7
--- /dev/null
+++ b/src/task-dyn-joint-limits.cpp
@@ -0,0 +1,215 @@
+/*
+ * 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 --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+//#define VP_DEBUG
+//#define VP_DEBUG_MODE 15
+#include <sot/core/debug.hh>
+
+#include <dynamic-graph/factory.h>
+
+#include <sot/core/feature-abstract.hh>
+
+#include <sot-dyninv/task-dyn-joint-limits.h>
+#include <sot-dyninv/commands-helper.h>
+
+#include <boost/foreach.hpp>
+
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+namespace dynamicgraph
+{
+ namespace sot
+ {
+ namespace dyninv
+ {
+
+ namespace dg = ::dynamicgraph;
+
+ /* --- DG FACTORY ------------------------------------------------------- */
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskDynJointLimits,"TaskDynJointLimits");
+
+ /* ---------------------------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* ---------------------------------------------------------------------- */
+
+ TaskDynJointLimits::
+ TaskDynJointLimits( const std::string & name )
+ : TaskDynPD(name)
+
+ ,CONSTRUCT_SIGNAL_IN(position,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(velocity,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(referenceInf,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(referenceSup,ml::Vector)
+
+ ,CONSTRUCT_SIGNAL_OUT(normalizedPosition,ml::Vector,positionSIN<<referenceInfSIN<<referenceSupSIN)
+
+ ,previousJ(0u,0u),previousJset(false)
+ {
+ taskSOUT.setFunction( boost::bind(&TaskDynJointLimits::computeTaskDynJointLimits,this,_1,_2) );
+ jacobianSOUT.setFunction( boost::bind(&TaskDynJointLimits::computeTjlJacobian,this,_1,_2) );
+ JdotSOUT.setFunction( boost::bind(&TaskDynJointLimits::computeTjlJdot,this,_1,_2) );
+ taskSOUT.clearDependencies();
+ taskSOUT.addDependency( referenceSupSIN );
+ taskSOUT.addDependency( referenceInfSIN );
+ taskSOUT.addDependency( dtSIN );
+ taskSOUT.addDependency( positionSIN );
+ taskSOUT.addDependency( velocitySIN );
+
+ signalRegistration( referenceSupSIN << dtSIN << referenceInfSIN
+ << positionSIN << velocitySIN << normalizedPositionSOUT );
+ }
+
+ /* ---------------------------------------------------------------------- */
+ /* --- COMPUTATION ------------------------------------------------------ */
+ /* ---------------------------------------------------------------------- */
+
+ dg::sot::VectorMultiBound& TaskDynJointLimits::
+ computeTaskDynJointLimits( dg::sot::VectorMultiBound& res,int time )
+ {
+ sotDEBUGIN(45);
+
+ sotDEBUG(45) << "# In " << getName() << " {" << std::endl;
+ const ml::Vector & position = positionSIN(time);
+ sotDEBUG(35) << "position = " << position << std::endl;
+ const ml::Vector & velocity = velocitySIN(time);
+ sotDEBUG(35) << "velocity = " << velocity << std::endl;
+ const ml::Vector & refInf = referenceInfSIN(time);
+ const ml::Vector & refSup = referenceSupSIN(time);
+ const double & dt = dtSIN(time);
+ const double kt=2/(dt*dt);
+
+ res.resize(position.size());
+ for( unsigned int i=0;i<res.size();++i )
+ {
+ res[i] = dg::sot::MultiBound (kt*(refInf(i)-position(i)-dt*velocity(i)),
+ kt*(refSup(i)-position(i)-dt*velocity(i)));
+ }
+
+ sotDEBUG(15) << "taskU = "<< res << std::endl;
+ sotDEBUGOUT(45);
+ return res;
+ }
+
+ ml::Matrix& TaskDynJointLimits::
+ computeTjlJacobian( ml::Matrix& J,int time )
+ {
+ sotDEBUG(15) << "# In {" << std::endl;
+
+ const ml::Vector& position = positionSIN(time);
+ /*
+ if( featureList.empty())
+ { throw( sotExceptionTask(sotExceptionTask::EMPTY_LIST,
+ "Empty feature list") ) ; }
+
+ try {
+ unsigned int dimJ = J .nbRows();
+ unsigned int nbc = J.nbCols();
+ if( 0==dimJ ){ dimJ = 1; J.resize(dimJ,nbc); }
+ */
+ J.resize(position.size(),position.size());
+ J.setZero();
+ {
+ for ( unsigned int j=6;j<position.size();++j )
+ J(j,j)=1;
+ }
+
+ // catch SOT_RETHROW;
+ sotDEBUG(15) << "# Out }" << std::endl;
+ return J;
+ }
+
+ ml::Matrix& TaskDynJointLimits::
+ computeTjlJdot( ml::Matrix& Jdot,int time )
+ {
+ sotDEBUGIN(15);
+
+ const ml::Matrix& currentJ = jacobianSOUT(time);
+ const double& dt = dtSIN(time);
+
+ if( previousJ.nbRows()!=currentJ.nbRows() ) previousJset = false;
+
+ if( previousJset )
+ {
+ assert( currentJ.nbRows()==previousJ.nbRows()
+ && currentJ.nbCols()==previousJ.nbCols() );
+
+ Jdot .resize( currentJ.nbRows(),currentJ.nbCols() );
+ Jdot = currentJ - previousJ;
+ Jdot *= 1/dt;
+ }
+ else { Jdot.resize(currentJ.nbRows(),currentJ.nbCols() ); Jdot.fill(0); }
+
+ previousJ = currentJ;
+ previousJset = true;
+
+ sotDEBUGOUT(15);
+ return Jdot;
+ }
+
+ ml::Vector& TaskDynJointLimits::
+ //computeNormalizedPosition( ml::Vector& res, int time )
+ normalizedPositionSOUT_function( ml::Vector& res, int time )
+ {
+ const ml::Vector & position = positionSIN(time);
+ // const ml::Vector & velocity = velocitySIN(time);
+ const ml::Vector & refInf = referenceInfSIN(time);
+ const ml::Vector & refSup = referenceSupSIN(time);
+
+ const unsigned int & n = position.size();
+ res.resize( n );
+ assert( n==refInf.size() && n==refSup.size() );
+ for( unsigned int i=0;i<n;++i )
+ {
+ res(i) = ( position(i)-refInf(i) ) / ( refSup(i)-refInf(i) );
+ }
+ return res;
+ }
+
+ /* ------------------------------------------------------------------------ */
+ /* --- DISPLAY ENTITY ----------------------------------------------------- */
+ /* ------------------------------------------------------------------------ */
+
+ void TaskDynJointLimits::
+ display( std::ostream& os ) const
+ {
+ os << "TaskDynJointLimits " << name << ": " << std::endl;
+ }
+
+ /*
+ void TaskDynJointLimits::
+ display( std::ostream& os ) const
+ {
+ os << "TaskDynJointLimits " << name << ": " << std::endl;
+ os << "--- LIST --- " << std::endl;
+ BOOST_FOREACH( dg::sot::FeatureAbstract* feature,featureList )
+ {
+ os << "-> " << feature->getName() << std::endl;
+ }
+ }
+ */
+
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
+
diff --git a/src/task-dyn-joint-limits.h b/src/task-dyn-joint-limits.h
new file mode 100644
index 0000000..2fda522
--- /dev/null
+++ b/src/task-dyn-joint-limits.h
@@ -0,0 +1,106 @@
+/*
+ * 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_TaskDynJointLimits_H__
+#define __sot_dyninv_TaskDynJointLimits_H__
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (task_dyn_joint_limits_EXPORTS)
+# define SOTTASKDYNJOINTLIMITS_EXPORT __declspec(dllexport)
+# else
+# define SOTTASKDYNJOINTLIMITS_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTTASKDYNJOINTLIMITS_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* SOT */
+#include <sot-dyninv/signal-helper.h>
+#include <sot-dyninv/entity-helper.h>
+#include <sot-dyninv/task-dyn-pd.h>
+#include <sot/core/task.hh>
+
+//#include <sot/sotFeatureAbstract.h>
+//#include <sot/sotFlags.h>
+//#include <sot/sotExceptionTask.h>
+
+
+namespace dynamicgraph {
+ namespace sot {
+ namespace dyninv {
+
+ /* --------------------------------------------------------------------- */
+ /* --- CLASS ----------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+
+ class SOTTASKDYNJOINTLIMITS_EXPORT TaskDynJointLimits
+ :public ::dynamicgraph::sot::dyninv::TaskDynPD
+ ,public ::dynamicgraph::EntityHelper<TaskDynJointLimits>
+ {
+
+ public: /* --- CONSTRUCTOR ---- */
+
+ TaskDynJointLimits( const std::string& name );
+
+ public: /* --- ENTITY INHERITANCE --- */
+ typedef TaskDynJointLimits EntityClassName;
+ static const std::string CLASS_NAME;
+ virtual const std::string& getClassName( void ) { return CLASS_NAME; }
+ //virtual void display( std::ostream& os ) const;
+
+ public: /* --- SIGNALS --- */
+
+ DECLARE_SIGNAL_IN(position,ml::Vector);
+ DECLARE_SIGNAL_IN(velocity,ml::Vector);
+ DECLARE_SIGNAL_IN(referenceInf,ml::Vector);
+ DECLARE_SIGNAL_IN(referenceSup,ml::Vector);
+ //DECLARE_SIGNAL_IN(dt,double);
+
+ DECLARE_SIGNAL_OUT(normalizedPosition,ml::Vector);
+
+ public: /* --- COMPUTATION --- */
+ dg::sot::VectorMultiBound& computeTaskDynJointLimits( dg::sot::VectorMultiBound& res,int time );
+ ml::Matrix& computeTjlJacobian( ml::Matrix& J,int time );
+ ml::Matrix& computeTjlJdot( ml::Matrix& Jdot,int time );
+
+ //protected:
+ //dynamicgraph::sot::VectorMultiBound&
+ // taskSOUT_function( dynamicgraph::sot::VectorMultiBound& task,int iter );
+ //std::list< sotFeatureAbstract* > featureList;
+
+ protected:
+ ml::Matrix previousJ;
+ bool previousJset;
+
+ private: /* --- DISPLAY --- */
+ void display( std::ostream& os ) const;
+
+ }; // class TaskDynJointLimits
+
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
+
+
+#endif // #ifndef __sot_dyninv_TaskDynJointLimits_H__
--
GitLab