From 06b9db7ded2b188187dc4bed176eb5b20d2cef80 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Tue, 13 Sep 2011 12:49:47 +0200
Subject: [PATCH] IVIGIt.
---
CMakeLists.txt | 2 +
src/dynamic_graph/sot/dyninv/__init__.py | 3 +
src/task-dyn-inequality.cpp | 187 +++++++++++++++++++++++
src/task-dyn-inequality.h | 89 +++++++++++
4 files changed, 281 insertions(+)
create mode 100644 src/task-dyn-inequality.cpp
create mode 100644 src/task-dyn-inequality.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 53f1b13..443a657 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -55,6 +55,7 @@ SET(libs
task-dyn-pd
task-dyn-joint-limits
task-dyn-limits
+ task-dyn-inequality
solver-op-space
solver-dyn-reduced
@@ -84,6 +85,7 @@ SET(headers
task-dyn-pd.h
task-dyn-joint-limits.h
task-dyn-limits.h
+ task-dyn-inequality.h
solver-op-space.h
solver-dyn-reduced.h
diff --git a/src/dynamic_graph/sot/dyninv/__init__.py b/src/dynamic_graph/sot/dyninv/__init__.py
index 02e6170..edfbe21 100755
--- a/src/dynamic_graph/sot/dyninv/__init__.py
+++ b/src/dynamic_graph/sot/dyninv/__init__.py
@@ -4,6 +4,9 @@ ControllerPD('')
from task_dyn_pd import TaskDynPD
TaskDynPD('')
+from task_dyn_inequality import TaskDynInequality
+TaskDynInequality('')
+
from dynamic_integrator import DynamicIntegrator
DynamicIntegrator('')
diff --git a/src/task-dyn-inequality.cpp b/src/task-dyn-inequality.cpp
new file mode 100644
index 0000000..2a22b9a
--- /dev/null
+++ b/src/task-dyn-inequality.cpp
@@ -0,0 +1,187 @@
+/*
+ * 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-dyn-inequality.h>
+
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+namespace dynamicgraph
+{
+ namespace sot
+ {
+ namespace dyninv
+ {
+
+ namespace dg = ::dynamicgraph;
+
+ /* --- DG FACTORY ------------------------------------------------------- */
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskDynInequality,"TaskDynInequality");
+
+ /* ---------------------------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* ---------------------------------------------------------------------- */
+
+ TaskDynInequality::
+ TaskDynInequality( const std::string & name )
+ : TaskDynPD(name)
+
+ ,CONSTRUCT_SIGNAL_IN(referenceInf,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(referenceSup,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(selec,Flags)
+
+ //,CONSTRUCT_SIGNAL_OUT(size,int,
+ // errorSOUT<<selecSIN)
+ ,sizeSOUT( boost::bind(& TaskDynInequality::sizeSOUT_function,this,_1,_2),
+ errorSOUT<<selecSIN,getClassName()+"("+getName()+")::output(int)::size" )
+
+ {
+ taskSOUT.setFunction( boost::bind(&TaskDynInequality::computeTaskDyn,this,_1,_2) );
+ taskSOUT.addDependency( selecSIN );
+ taskSOUT.addDependency( referenceSupSIN );
+ taskSOUT.addDependency( referenceInfSIN );
+ taskSOUT.addDependency( sizeSOUT );
+
+ jacobianSOUT.setFunction( boost::bind(&TaskDynInequality::computeJacobian,this,_1,_2) );
+ jacobianSOUT.addDependency( selecSIN );
+ jacobianSOUT.addDependency( sizeSOUT );
+
+ controlGainSIN = 1.0;
+ selecSIN = true;
+ signalRegistration( referenceSupSIN << referenceInfSIN
+ << selecSIN << sizeSOUT );
+ }
+
+ /* ---------------------------------------------------------------------- */
+ /* --- COMPUTATION ------------------------------------------------------ */
+ /* ---------------------------------------------------------------------- */
+ int& TaskDynInequality::
+ 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& TaskDynInequality::
+ computeTaskDyn( dg::sot::VectorMultiBound& res,int iter )
+ {
+ ml::Vector dummy;
+ const bool withInf = referenceInfSIN, withSup = referenceSupSIN;
+ MultiBound::SupInfType bound = withInf ? MultiBound::BOUND_INF : MultiBound::BOUND_SUP;
+
+ const ml::Vector & error = errorSOUT(iter);
+ const ml::Vector & errorDot = errorDotSOUT(iter);
+ const ml::Vector & refInf = withInf ? referenceInfSIN(iter) : dummy;
+ const ml::Vector & refSup = withSup ? referenceSupSIN(iter) : dummy;
+ const Flags & selec = selecSIN(iter);
+ const int insize = error.size(), outsize=sizeSOUT(iter);
+ const double dt = dtSIN(iter)*controlGainSIN(iter), dt2 = 2/(dt*dt);
+ assert( !withInf || insize==(int)refInf.size() );
+ assert( !withSup || insize==(int)refSup.size() );
+
+ using std::cout; using std::endl;
+ // int iterShow = 76;
+ // if(iter==iterShow)
+ // {
+ // cout << "e = " << error << endl;
+ // cout << "edot = " << errorDot << endl;
+ // cout << "l = " << refInf << endl;
+ // cout << "u = " << refSup << endl;
+ // }
+ res.resize(outsize); int idx=0;
+ for( int i=0;i<insize;++i )
+ {
+ if( selec(i) )
+ {
+ const double & e = error(i), & u = refSup(i),
+ & l = refInf(i), & ed = errorDot(idx);
+ const double de = ed*dt;
+
+ //const double EPS = 1e-4;
+ //assert( e<=u+EPS && e>=l-EPS );
+
+ double inf,sup;
+ if(! withSup ) { sup = 0; }
+ else /*if( e+de/2<=u || e>u)*/ { sup = (u-e-de)*dt2; }
+ //else { sup = ed*ed/(2*(e-u)); }
+
+ if(! withInf ) { inf = 0; }
+ else /*if(e<l || e+de/2>=l )*/ { inf = (l-e-de)*dt2; }
+ //else { inf = ed*ed/(2*(e-l)); }
+
+ // if(i==0)
+ // {
+ // std::cout << inf << " " << sup;
+ // if( e+de/2>=l ) std::cout << " B";
+ // if( e<l ) std::cout << " C";
+ // std::cout << endl;
+ // }
+
+ if( withInf && withSup )
+ res[idx++] = dg::sot::MultiBound( inf,sup );
+ else
+ res[idx++] = dg::sot::MultiBound( inf+sup,bound );
+ }
+ }
+
+ sotDEBUG(15) << "taskU = "<< res << std::endl;
+ return res;
+ }
+
+ ml::Matrix& TaskDynInequality::
+ computeJacobian( ml::Matrix& res,int iter )
+ {
+ const Flags & selec = selecSIN(iter);
+ ml::Matrix Jin; TaskDynPD::computeJacobian(Jin,iter);
+ const int insize = Jin.nbRows(), outsize=sizeSOUT(iter), nbc=Jin.nbCols();
+ assert( insize>=outsize );
+
+ 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;
+ }
+
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
+
diff --git a/src/task-dyn-inequality.h b/src/task-dyn-inequality.h
new file mode 100644
index 0000000..b121d76
--- /dev/null
+++ b/src/task-dyn-inequality.h
@@ -0,0 +1,89 @@
+/*
+ * 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_TaskDynInequality_H__
+#define __sot_dyninv_TaskDynInequality_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>
+#include <sot-dyninv/task-dyn-pd.h>
+
+namespace dynamicgraph {
+ namespace sot {
+ namespace dyninv {
+
+ /* --------------------------------------------------------------------- */
+ /* --- CLASS ----------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+
+ class SOTTASKDYNINEQUALITY_EXPORT TaskDynInequality
+ :public TaskDynPD
+ ,public EntityHelper<TaskDynInequality>
+ {
+
+ public: /* --- CONSTRUCTOR ---- */
+
+ TaskDynInequality( const std::string& name );
+
+ public: /* --- ENTITY INHERITANCE --- */
+
+ static const std::string CLASS_NAME;
+ virtual const std::string& getClassName( void ) { return CLASS_NAME; }
+
+ public: /* --- SIGNALS --- */
+
+ DECLARE_SIGNAL_IN(referenceInf,ml::Vector);
+ DECLARE_SIGNAL_IN(referenceSup,ml::Vector);
+ DECLARE_SIGNAL_IN(selec,Flags);
+
+ DECLARE_SIGNAL_OUT(normalizedPosition,ml::Vector);
+ DECLARE_SIGNAL_OUT(size,int);
+
+ public: /* --- COMPUTATION --- */
+ dg::sot::VectorMultiBound&
+ computeTaskDyn( dg::sot::VectorMultiBound& res,int time );
+ ml::Matrix&
+ computeJacobian( ml::Matrix& res,int time );
+
+ }; // class TaskDynInequality
+
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
+
+
+#endif // #ifndef __sot_dyninv_TaskDynInequality_H__
--
GitLab