From 1147ca833ae2ac9203a26bde2b9f5da6ad9151c2 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Tue, 15 Mar 2011 16:03:40 +0100
Subject: [PATCH] IVIGIT.
---
CMakeLists.txt | 2 +
src/dynamic_graph/sot/dyninv/__init__.py | 3 +
src/task-inequality.cpp | 171 +++++++++++++++++++++++
src/task-inequality.h | 90 ++++++++++++
4 files changed, 266 insertions(+)
create mode 100644 src/task-inequality.cpp
create mode 100644 src/task-inequality.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index d3f6020..c142d50 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -58,6 +58,7 @@ SET(libs
solver-kine
task-joint-limits
+ task-inequality
)
LIST(APPEND LOGGING_WATCHED_TARGETS ${libs})
@@ -82,6 +83,7 @@ SET(headers
solver-op-space.h
task-joint-limits.h
+ task-inequality.h
solver-kine.h
)
diff --git a/src/dynamic_graph/sot/dyninv/__init__.py b/src/dynamic_graph/sot/dyninv/__init__.py
index 603be4f..cbee415 100755
--- a/src/dynamic_graph/sot/dyninv/__init__.py
+++ b/src/dynamic_graph/sot/dyninv/__init__.py
@@ -27,3 +27,6 @@ TaskDynJointLimits('')
from task_joint_limits import TaskJointLimits
TaskJointLimits('')
+
+from task_inequality import TaskInequality
+TaskInequality('')
diff --git a/src/task-inequality.cpp b/src/task-inequality.cpp
new file mode 100644
index 0000000..14767af
--- /dev/null
+++ b/src/task-inequality.cpp
@@ -0,0 +1,171 @@
+/*
+ * 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-dyninv/task-inequality.h>
+
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+namespace dynamicgraph
+{
+ namespace sot
+ {
+ namespace dyninv
+ {
+
+ namespace dg = ::dynamicgraph;
+
+ /* --- DG FACTORY ------------------------------------------------------- */
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskInequality,"TaskInequality");
+
+ /* ---------------------------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* ---------------------------------------------------------------------- */
+
+ TaskInequality::
+ TaskInequality( const std::string & name )
+ : Task(name)
+
+ ,CONSTRUCT_SIGNAL_IN(referenceInf,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(referenceSup,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(dt,double)
+ ,CONSTRUCT_SIGNAL_IN(selec,Flags)
+
+ ,CONSTRUCT_SIGNAL_OUT(normalizedPosition,ml::Vector,
+ errorSOUT<<referenceInfSIN<<referenceSupSIN)
+ ,CONSTRUCT_SIGNAL_OUT(size,int,
+ errorSOUT<<selecSIN)
+ {
+ taskSOUT.setFunction( boost::bind(&TaskInequality::computeTask,this,_1,_2) );
+ taskSOUT.addDependency( selecSIN );
+ taskSOUT.addDependency( dtSIN );
+ taskSOUT.addDependency( referenceSupSIN );
+ taskSOUT.addDependency( referenceInfSIN );
+ taskSOUT.addDependency( sizeSOUT );
+
+ jacobianSOUT.setFunction( boost::bind(&TaskInequality::computeJacobian,this,_1,_2) );
+ jacobianSOUT.addDependency( selecSIN );
+ jacobianSOUT.addDependency( sizeSOUT );
+
+ controlGainSIN = 1.0;
+ signalRegistration( referenceSupSIN << referenceInfSIN << dtSIN
+ << selecSIN << sizeSOUT << normalizedPositionSOUT );
+ }
+
+ /* ---------------------------------------------------------------------- */
+ /* --- COMPUTATION ------------------------------------------------------ */
+ /* ---------------------------------------------------------------------- */
+ int& TaskInequality::
+ sizeSOUT_function(int& res, int time)
+ {
+ const Flags & selec = selecSIN(time);
+ const int size = errorSOUT(time).size();
+ res=0;
+ for( int i=0;i<size;++i )
+ {
+ if(selec(i)) res++;
+ }
+ return res;
+ }
+
+ dg::sot::VectorMultiBound& TaskInequality::
+ computeTask( dg::sot::VectorMultiBound& res,int time )
+ {
+ const ml::Vector & error = errorSOUT(time);
+ const ml::Vector & refInf = referenceInfSIN(time);
+ const ml::Vector & refSup = referenceSupSIN(time);
+ const Flags & selec = selecSIN(time);
+ const int insize = error.size(), outsize=sizeSOUT(time);
+ const double K = 1.0/(dtSIN(time)*controlGainSIN(time));
+ assert( insize==(int)refInf.size() && insize==(int)refSup.size() );
+
+ res.resize(outsize); int idx=0;
+ for( int i=0;i<insize;++i )
+ {
+ if( selec(i) )
+ res[idx++] = dg::sot::MultiBound( (refInf(i)-error(i))*K,
+ (refSup(i)-error(i))*K );
+ }
+
+ sotDEBUG(15) << "taskU = "<< res << std::endl;
+ return res;
+ }
+
+ ml::Matrix& TaskInequality::
+ computeJacobian( ml::Matrix& res,int time )
+ {
+ const Flags & selec = selecSIN(time);
+ ml::Matrix Jin; Task::computeJacobian(Jin,time);
+ const int insize = Jin.nbRows(), outsize=sizeSOUT(time), nbc=Jin.nbCols();
+ //assert( );
+
+ res.resize(outsize,nbc); int idx=0;
+ for( int i=0;i<insize;++i )
+ {
+ if( selec(i) )
+ {
+ for( int j=0;j<nbc;++j )
+ res(idx,j) = Jin(i,j);
+ idx++;
+ }
+ }
+
+ return res;
+ }
+
+ ml::Vector& TaskInequality::
+ normalizedPositionSOUT_function( ml::Vector& res, int time )
+ {
+ const ml::Vector & error = errorSOUT(time);
+ const ml::Vector & refInf = referenceInfSIN(time);
+ const ml::Vector & refSup = referenceSupSIN(time);
+ const Flags & selec = selecSIN(time);
+ const int insize = error.size(), outsize=sizeSOUT(time);
+ assert( insize==(int)refInf.size() && insize==(int)refSup.size() );
+
+ res.resize( outsize ); int idx=0;
+ for( int i=0;i<insize;++i )
+ { if(selec(i))
+ res(idx++) = ( error(i)-refInf(i) ) / ( error(i)-refInf(i) ); }
+ return res;
+ }
+
+ /* ------------------------------------------------------------------------ */
+ /* --- DISPLAY ENTITY ----------------------------------------------------- */
+ /* ------------------------------------------------------------------------ */
+
+ void TaskInequality::
+ display( std::ostream& os ) const
+ {
+ os << "TaskInequality " << name << ": " << std::endl;
+ }
+
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
+
diff --git a/src/task-inequality.h b/src/task-inequality.h
new file mode 100644
index 0000000..eefcd61
--- /dev/null
+++ b/src/task-inequality.h
@@ -0,0 +1,90 @@
+/*
+ * 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_TaskInequality_H__
+#define __sot_dyninv_TaskInequality_H__
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (task_dyn_inequality_EXPORTS)
+# define SOTTASKDYNINEQUALITY_EXPORT __declspec(dllexport)
+# else
+# define SOTTASKDYNINEQUALITY_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTTASKDYNINEQUALITY_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* SOT */
+#include <sot-dyninv/signal-helper.h>
+#include <sot-dyninv/entity-helper.h>
+#include <sot/core/task.hh>
+#include <sot/core/flags.hh>
+
+namespace dynamicgraph {
+ namespace sot {
+ namespace dyninv {
+
+ /* --------------------------------------------------------------------- */
+ /* --- CLASS ----------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+
+ class SOTTASKDYNINEQUALITY_EXPORT TaskInequality
+ :public Task
+ ,public EntityHelper<TaskInequality>
+ {
+
+ public: /* --- CONSTRUCTOR ---- */
+
+ TaskInequality( const std::string& name );
+
+ public: /* --- ENTITY INHERITANCE --- */
+
+ 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(referenceInf,ml::Vector);
+ DECLARE_SIGNAL_IN(referenceSup,ml::Vector);
+ DECLARE_SIGNAL_IN(dt,double);
+ DECLARE_SIGNAL_IN(selec,Flags);
+
+ DECLARE_SIGNAL_OUT(normalizedPosition,ml::Vector);
+ DECLARE_SIGNAL_OUT(size,int);
+
+ public: /* --- COMPUTATION --- */
+ dg::sot::VectorMultiBound&
+ computeTask( dg::sot::VectorMultiBound& res,int time );
+ ml::Matrix&
+ computeJacobian( ml::Matrix& res,int time );
+
+ }; // class TaskInequality
+
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
+
+
+#endif // #ifndef __sot_dyninv_TaskInequality_H__
--
GitLab