diff --git a/CMakeLists.txt b/CMakeLists.txt
index a055218b97a17ccb5d7d887a72871388dd587230..6fcbafe7bd1f6d318e94dc382c50c0da7bcfe9d5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -62,6 +62,7 @@ SET(libs
solver-kine
task-joint-limits
task-inequality
+ task-weight
feature-projected-line
contact-selecter
@@ -93,6 +94,7 @@ SET(headers
task-joint-limits.h
task-inequality.h
+ task-weight.h
solver-kine.h
feature-projected-line.h
@@ -108,7 +110,7 @@ list(APPEND task-dyn-joint-limits_plugins_dependencies task-dyn-pd)
list(APPEND pseudo-robot-dynamic_plugins_dependencies dynamic-integrator)
list(APPEND contact-selecter_plugins_dependencies solver-dyn-reduced)
-foreach(lib task-inequality task-dyn-pd task-joint-limits)
+foreach(lib task-weight task-inequality task-dyn-pd task-joint-limits)
IF(WIN32)
list(APPEND ${lib}_ext_plugins_dependencies
${DYNAMIC_GRAPH_PLUGINDIR}/task${CMAKE_LINK_LIBRARY_SUFFIX})
diff --git a/src/task-weight.cpp b/src/task-weight.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..53e11042e4533e30ea97fa9749597f58c57726c3
--- /dev/null
+++ b/src/task-weight.cpp
@@ -0,0 +1,178 @@
+/*
+ * 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-weight.h>
+#include <dynamic-graph/all-commands.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(TaskWeight,"TaskWeight");
+
+ /* ---------------------------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* ---------------------------------------------------------------------- */
+
+ TaskWeight::
+ TaskWeight( const std::string & name )
+ : Task(name)
+ {
+ taskSOUT.setFunction( boost::bind(&TaskWeight::computeTask,this,_1,_2) );
+ jacobianSOUT.setFunction( boost::bind(&TaskWeight::computeJacobian,this,_1,_2) );
+
+ commandMap.erase("add");
+ addCommand("add",
+ makeCommandVoid2(*this,&TaskWeight::addTask,
+ docCommandVoid2("Add a sub task with weight","task name","weight")));
+ addCommand("setWeight",
+ makeCommandVoid2(*this,&TaskWeight::setWeight,
+ docCommandVoid2("Modify the weight of a task","task name","weight")));
+ }
+
+ /* ---------------------------------------------------------------------- */
+ /* --- COMPUTATION ------------------------------------------------------ */
+ /* ---------------------------------------------------------------------- */
+
+ void TaskWeight::
+ addTask(const std::string& name,const double & w)
+ {
+ Task& task =
+ dynamic_cast<Task&> (PoolStorage::getInstance()->getTask(name));
+ TaskContener tw;
+ tw.task = &task;
+ tw.weight = w;
+
+ taskList.push_back(tw);
+ }
+
+ void TaskWeight::
+ setWeight(const std::string& name,const double & w)
+ {
+ BOOST_FOREACH(TaskContener& tc,taskList)
+ {
+ if( tc.task->getName() == name )
+ { tc.weight = w; }
+ }
+ }
+
+ dg::sot::VectorMultiBound& TaskWeight::
+ computeTask( dg::sot::VectorMultiBound& res,int time )
+ {
+ int cursorError = 0;
+ int dimError = res .size();
+ if( 0==dimError ){ dimError = 1; res.resize(dimError); }
+
+ for( std::list< TaskContener >::iterator iter = taskList.begin();
+ iter!=taskList.end(); ++iter )
+ {
+ TaskContener & tw = *iter;
+ Task & task = *tw.task;
+ double wi = tw.weight;
+
+ const dg::sot::VectorMultiBound & partialTask = task.taskSOUT(time);
+ const int dim = partialTask.size();
+
+ while( cursorError+dim>dimError ) // DEBUG It was >=
+ { dimError *= 2; res.resize(dimError,false); }
+
+ for( int k=0;k<dim;++k )
+ {
+ res[cursorError] = partialTask[k];
+ res[cursorError].boundSingle *= wi;
+ res[cursorError].boundSup *= wi;
+ res[cursorError].boundInf *= wi;
+ cursorError++;
+ }
+ }
+
+ /* If too much memory has been allocated, resize. */
+ res .resize(cursorError,false);
+ return res;
+ }
+
+ ml::Matrix& TaskWeight::
+ computeJacobian( ml::Matrix& res,int time )
+ {
+ int cursorError = 0;
+ int dimJ = res .nbRows(), colJ = res.nbCols();
+ if( 0==dimJ ){ dimJ = 1; res.resize(dimJ,colJ); }
+
+ for( std::list< TaskContener >::iterator iter = taskList.begin();
+ iter!=taskList.end(); ++iter )
+ {
+ TaskContener & tw = *iter;
+ Task & task = *tw.task;
+ double wi = tw.weight;
+
+
+ const ml::Matrix & partialJ = task.jacobianSOUT(time);
+ const int dim = partialJ.nbRows();
+ if(colJ==0) colJ=partialJ.nbCols();
+
+ while( cursorError+dim>dimJ ) // DEBUG It was >=
+ { dimJ *= 2; res.resize(dimJ,colJ,false); }
+
+ for( int k=0;k<dim;++k )
+ {
+ for( int l=0;l<colJ; ++l )
+ res(cursorError,l) = partialJ(k,l) * wi;
+ cursorError++;
+ }
+ }
+
+ /* If too much memory has been allocated, resize. */
+ res .resize(cursorError,colJ,false);
+ return res;
+ }
+
+
+ /* ------------------------------------------------------------------------ */
+ /* --- DISPLAY ENTITY ----------------------------------------------------- */
+ /* ------------------------------------------------------------------------ */
+
+ void TaskWeight::
+ display( std::ostream& os ) const
+ {
+ os << "TaskWeight " << name << ": " << std::endl;
+ }
+
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
+
diff --git a/src/task-weight.h b/src/task-weight.h
new file mode 100644
index 0000000000000000000000000000000000000000..39c9f14936dcade5801844d316b76174c59c788f
--- /dev/null
+++ b/src/task-weight.h
@@ -0,0 +1,95 @@
+
+/*
+ * 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_TaskWeight_H__
+#define __sot_dyninv_TaskWeight_H__
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (task_weight_EXPORTS)
+# define SOTTASKDYNWEIGHT_EXPORT __declspec(dllexport)
+# else
+# define SOTTASKDYNWEIGHT_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTTASKDYNWEIGHT_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 SOTTASKDYNWEIGHT_EXPORT TaskWeight
+ :public Task
+ ,public EntityHelper<TaskWeight>
+ {
+
+ public: /* --- CONSTRUCTOR ---- */
+
+ TaskWeight( 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 --- */
+
+ public: /* --- COMPUTATION --- */
+ dg::sot::VectorMultiBound&
+ computeTask( dg::sot::VectorMultiBound& res,int time );
+ ml::Matrix&
+ computeJacobian( ml::Matrix& res,int time );
+
+
+ struct TaskContener
+ {
+ Task* task;
+ double weight;
+ };
+ std::list< TaskContener > taskList;
+
+ void addTask(const std::string& name,const double & w);
+ void setWeight(const std::string& name,const double & w);
+
+
+ }; // class TaskWeight
+
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
+
+
+#endif // #ifndef __sot_dyninv_TaskWeight_H__