diff --git a/AUTHORS b/AUTHORS
index aa5e5ff69636a87d8b96257a8c291bfa6026af0f..db9a50df56ef574250ae44d4de5ab38a56b6d2c6 100644
--- a/AUTHORS
+++ b/AUTHORS
@@ -3,3 +3,4 @@ This package was written by and with the assistance of:
* Nicolas Mansard nicolas.mansard@laas.fr
* Layale Saab lsaab@laas.fr
* Olivier Stasse olivier.stasse@aist.go.jp
+* Oscar E. Ramos P. oramos@laas.fr
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 31759a57fcaf0c4bd2a3e16cb8777fee9066821a..42869ef38face864ac8ad6255111242015d1cc19 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -56,6 +56,7 @@ SET(libs
task-dyn-joint-limits
task-dyn-limits
task-dyn-inequality
+ task-dyn-passing-point
solver-op-space
solver-dyn-reduced
@@ -89,6 +90,7 @@ SET(headers
task-dyn-joint-limits.h
task-dyn-limits.h
task-dyn-inequality.h
+ task-dyn-passing-point.h
solver-op-space.h
solver-dyn-reduced.h
@@ -107,6 +109,7 @@ SET(headers
list(APPEND task-dyn-limits_plugins_dependencies task-dyn-pd)
list(APPEND task-dyn-inequality_plugins_dependencies task-dyn-pd)
list(APPEND task-dyn-joint-limits_plugins_dependencies task-dyn-pd)
+list(APPEND task-dyn-passing-point_plugins_dependencies task-dyn-pd)
list(APPEND pseudo-robot-dynamic_plugins_dependencies dynamic-integrator)
list(APPEND contact-selecter_plugins_dependencies solver-dyn-reduced)
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index c067ea49de13b62e85f832ef7ffa711c8eb50946..62cc04b16436ffc75e2d8911bac7c6f5e753ebaf 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -73,6 +73,7 @@ ENDFOREACH(lib)
INSTALL(FILES
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/__init__.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/meta_task_dyn_6d.py
+ ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/meta_task_dyn_passing_point.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/meta_tasks_dyn.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/meta_tasks_dyn_relative.py
${CMAKE_CURRENT_BINARY_DIR}/../python/robot_specific.py
diff --git a/src/dynamic_graph/sot/dyninv/__init__.py b/src/dynamic_graph/sot/dyninv/__init__.py
index f74125252a5d9b2b06f72b2856c35f54a780a550..cddb9586dae035a60851740d748131473b6c0c87 100755
--- a/src/dynamic_graph/sot/dyninv/__init__.py
+++ b/src/dynamic_graph/sot/dyninv/__init__.py
@@ -9,6 +9,7 @@ from zmp_estimator import ZmpEstimator
from robot_dyn_simu import RobotDynSimu
from task_dyn_joint_limits import TaskDynJointLimits
from task_dyn_limits import TaskDynLimits
+from task_dyn_passing_point import TaskDynPassingPoint
from task_joint_limits import TaskJointLimits
from task_inequality import TaskInequality
from feature_projected_line import FeatureProjectedLine
diff --git a/src/dynamic_graph/sot/dyninv/meta_task_dyn_passing_point.py b/src/dynamic_graph/sot/dyninv/meta_task_dyn_passing_point.py
new file mode 100644
index 0000000000000000000000000000000000000000..a5e2aa83ba85c42c6fb95327ea5df4153c033420
--- /dev/null
+++ b/src/dynamic_graph/sot/dyninv/meta_task_dyn_passing_point.py
@@ -0,0 +1,69 @@
+from dynamic_graph import plug
+from dynamic_graph.sot.core import GainAdaptive, OpPointModifier
+from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, rpy2tr
+from dynamic_graph.sot.core.meta_task_6d import MetaTask6d
+from dynamic_graph.sot.dyninv import TaskDynPassingPoint
+
+from numpy import matrix, identity, zeros, eye, array, sqrt, radians, arccos, linalg, inner
+
+
+class MetaTaskDynPassingPoint(MetaTask6d):
+ def createTask(self):
+ self.task = TaskDynPassingPoint('task'+self.name)
+ self.task.dt.value = 1e-3
+ def createGain(self):
+ pass
+ def plugEverything(self):
+ self.feature.setReference(self.featureDes.name)
+ plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
+ plug(self.dyn.signal('J'+self.opPoint), self.feature.signal('Jq'))
+ self.task.add(self.feature.name)
+ plug(self.dyn.velocity, self.task.qdot)
+ ''' Dummy initialization '''
+ self.task.duration.value = 1
+ self.task.velocityDes.value = (0,0,0,0,0,0)
+ self.task.initialTime.value = 0
+ #self.task.currentTime.value = 0
+
+ def __init__(self,*args):
+ MetaTask6d.__init__(self,*args)
+
+
+def goto6dPP(task, position, velocity, duration, current):
+ if isinstance(position,matrix): position = vectorToTuple(position)
+ if( len(position)==3 ):
+ ''' If only position specification '''
+ M=eye(4)
+ M[0:3,3] = position
+ else:
+ ''' If there is position and orientation '''
+ M = array( rpy2tr(*position[3:7]) )
+ M[0:3,3] = position[0:3]
+ task.feature.selec.value = "111111"
+ task.task.controlSelec.value = "111111"
+ task.featureDes.position.value = matrixToTuple(M)
+ task.task.duration.value = duration
+ task.task.initialTime.value = current
+ task.task.velocityDes.value = velocity
+ task.task.resetJacobianDerivative()
+
+
+def gotoNdPP(task, position, velocity, selec, duration, current):
+ if isinstance(position,matrix): position = vectorToTuple(position)
+ if( len(position)==3 ):
+ M=eye(4)
+ M[0:3,3] = position
+ else:
+ M = array( rpy2tr(*position[3:7]) )
+ M[0:3,3] = position[0:3]
+ if isinstance(selec,str):
+ task.feature.selec.value = selec
+ task.task.controlSelec.value = selec
+ else:
+ task.feature.selec.value = toFlags(selec)
+ task.task.controlSelec.value = toFlags(selec)
+ task.featureDes.position.value = matrixToTuple(M)
+ task.task.duration.value = duration
+ task.task.initialTime.value = current
+ task.task.velocityDes.value = velocity
+ task.task.resetJacobianDerivative()
diff --git a/src/task-dyn-passing-point.cpp b/src/task-dyn-passing-point.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..80c307db4dbb4e1236075597dbc212bdaa7cc8d3
--- /dev/null
+++ b/src/task-dyn-passing-point.cpp
@@ -0,0 +1,209 @@
+/*
+ * Copyright 2012, Oscar E. Ramos Ponce, 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 <dynamic-graph/factory.h>
+
+#include <sot/core/debug.hh>
+#include <sot/core/feature-abstract.hh>
+
+#include <sot-dyninv/task-dyn-passing-point.h>
+#include <sot-dyninv/commands-helper.h>
+
+#include <boost/foreach.hpp>
+#include <Eigen/Dense>
+
+
+namespace dynamicgraph
+{
+ namespace sot
+ {
+ namespace dyninv
+ {
+
+ namespace dg = ::dynamicgraph;
+
+ /* --- DG FACTORY ------------------------------------------------------- */
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskDynPassingPoint,"TaskDynPassingPoint");
+
+ /* ---------------------------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* ---------------------------------------------------------------------- */
+
+ TaskDynPassingPoint::
+ TaskDynPassingPoint( const std::string & name )
+ : TaskDynPD(name)
+
+ ,CONSTRUCT_SIGNAL_IN(velocityDes, ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(duration, double)
+ ,CONSTRUCT_SIGNAL_IN(initialTime, double)
+
+ ,CONSTRUCT_SIGNAL_OUT(velocityCurrent, ml::Vector,
+ qdotSIN<<jacobianSOUT )
+ ,CONSTRUCT_SIGNAL_OUT(velocityDesired, ml::Vector,
+ velocityDesSIN<<controlSelectionSIN )
+
+ ,previousTask(0u)
+ {
+ taskSOUT.setFunction( boost::bind(&TaskDynPassingPoint::computeTaskSOUT,this,_1,_2) );
+ taskSOUT.addDependency( velocityCurrentSOUT );
+ taskSOUT.addDependency( durationSIN );
+ taskSOUT.addDependency( velocityDesiredSOUT );
+
+ signalRegistration( velocityDesSIN << durationSIN << initialTimeSIN
+ << velocityCurrentSOUT << velocityDesiredSOUT );
+
+ }
+
+
+ /* ---------------------------------------------------------------------- */
+ /* --- SIGNALS ---------------------------------------------------------- */
+ /* ---------------------------------------------------------------------- */
+
+ /* Current velocity using the Jacobian: dx_0 = J*dq */
+ ml::Vector& TaskDynPassingPoint::
+ velocityCurrentSOUT_function( ml::Vector& velocityCurrent, int time )
+ {
+ sotDEBUGIN(15);
+
+ const ml::Vector & qdot = qdotSIN(time);
+ const ml::Matrix & J = jacobianSOUT(time);
+ velocityCurrent.resize( J.nbRows() );
+ J.multiply(qdot, velocityCurrent);
+
+ sotDEBUGOUT(15);
+ return velocityCurrent;
+ }
+
+
+ /* Select the correct components of the desired velocity according to 'selec' */
+ ml::Vector& TaskDynPassingPoint::
+ velocityDesiredSOUT_function( ml::Vector& velocityDesired, int time )
+ {
+ sotDEBUGIN(15);
+
+ const ml::Vector & velocityDesiredFull = velocityDesSIN(time);
+ const Flags &fl = controlSelectionSIN(time);
+
+ unsigned int dim = 0;
+ for( int i=0;i<6;++i ) if( fl(i) ) dim++;
+ velocityDesired.resize( dim );
+
+ unsigned int cursor = 0;
+ for( unsigned int i=0;i<6;++i )
+ { if( fl(i) ) velocityDesired(cursor++) = velocityDesiredFull(i); }
+
+ sotDEBUGOUT(15);
+ return velocityDesired;
+ }
+
+
+ /* Task computation */
+ dg::sot::VectorMultiBound& TaskDynPassingPoint::
+ computeTaskSOUT( dg::sot::VectorMultiBound& task, int time )
+ {
+ sotDEBUGIN(15);
+
+ const double& fullDuration = durationSIN(time);
+ const double& n0 = initialTimeSIN(time);
+ const double& dt = dtSIN(time);
+ const ml::Vector & e = errorSOUT(time);
+ const ml::Vector & vel_current = velocityCurrentSOUT(time);
+ const ml::Vector & vel_desired = velocityDesiredSOUT(time);
+
+ double T = fabs( fullDuration-(time-n0)*dt );
+ //double T = fullDuration-(time-n0)*dt;
+ //std::cout << "duration left: " << T << std::endl;
+
+ assert( e.size() == vel_current.size() && e.size() == vel_desired.size() );
+ task.resize( e.size() );
+
+ if(previousTask.size() != task.size())
+ previousTask.resize( e.size() );
+
+ /* --- Using a pseudoinverse --- */
+ // Eigen::MatrixXd M_t = Eigen::MatrixXd::Zero(2*e.size(),2*e.size());
+ // Eigen::VectorXd V_ddx;
+ // Eigen::VectorXd b;
+
+ // V_ddx.resize(2*e.size());
+ // b.resize(2*e.size());
+
+ // for( unsigned int i=0; i<task.size(); ++i)
+ // {
+ // M_t(i,i) = T*T/3.0;
+ // M_t(i,i+e.size())= T*T/6.0;
+ // M_t(i+e.size(),i) = T/2.0;
+ // M_t(i+e.size(),i+e.size()) = T/2;
+
+ // b(i) = -e(i) - vel_current(i)*T;
+ // b(i+e.size()) = vel_desired(i)-vel_current(i);
+ // }
+
+ // //V_ddx = M_t.colPivHouseholderQr().solve(b);
+ // Eigen::ColPivHouseholderQR<Eigen::MatrixXd> colPiv(M_t);
+ // colPiv.setThreshold(1e-3);
+ // V_ddx = colPiv.solve(b);
+
+ // V_ddx = M_t.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
+
+ // std::cout << " == M_t:" << std::endl << M_t << std::endl;
+ // std::cout << " == b:" << std::endl << b << std::endl;
+ // std::cout << " == V_ddx:" << std::endl<< V_ddx << std::endl;
+
+ // for (unsigned int i=0; i<task.size(); ++i)
+ // {
+ // task[i] = V_ddx(i);
+ // }
+
+ /* --- Computing ddx(0) "manually" --- */
+ for( unsigned int i=0;i<task.size(); ++i )
+ {
+ if (T>dt)
+ {
+ task[i] = - 6*e(i)/(T*T) - 2/T*(vel_desired(i)+2*vel_current(i)) ;
+ previousTask(i) = - 6*e(i)/(T*T) - 2/T*(vel_desired(i)+2*vel_current(i)) ;
+ }
+ else
+ {
+ task[i] = previousTask(i);
+ //task[i] = 0;
+ }
+ }
+
+ sotDEBUGOUT(15);
+ return task;
+ }
+
+
+ /* ---------------------------------------------------------------------- */
+ /* --- ENTITY ----------------------------------------------------------- */
+ /* ---------------------------------------------------------------------- */
+
+ void TaskDynPassingPoint::
+ display( std::ostream& os ) const
+ {
+ os << "TaskDynPassingPoint " << 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-passing-point.h b/src/task-dyn-passing-point.h
new file mode 100644
index 0000000000000000000000000000000000000000..902960e1700ee9faff8c78c93465029368ae6405
--- /dev/null
+++ b/src/task-dyn-passing-point.h
@@ -0,0 +1,100 @@
+/*
+ * Copyright 2012, Oscar E. Ramos Ponce, 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/>.
+ */
+
+/*! \file task-dyn-passing-point.h
+ \brief Defines a task based on time constraints as well as the initial
+ and final position and velocity.
+*/
+
+
+#ifndef __sot_dyninv_TaskDynPassingPoint_H__
+#define __sot_dyninv_TaskDynPassingPoint_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (task_dyn_passing_point_EXPORTS)
+# define SOTTASKDYNPASSINGPOINT_EXPORT __declspec(dllexport)
+# else
+# define SOTTASKDYNPASSINGPOINT_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTTASKDYNPASSINGPOINT_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>
+
+namespace dynamicgraph {
+ namespace sot {
+ namespace dyninv {
+
+ /* --------------------------------------------------------------------- */
+ /* --- CLASS ----------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+
+ class SOTTASKDYNPASSINGPOINT_EXPORT TaskDynPassingPoint
+ :public ::dynamicgraph::sot::dyninv::TaskDynPD
+ ,public ::dynamicgraph::EntityHelper<TaskDynPassingPoint>
+ {
+
+ public:
+ /*! Constructor
+ @param name: Name of the task (string)
+ */
+ TaskDynPassingPoint( const std::string & name );
+
+ /* --- ENTITY INHERITANCE --- */
+ typedef TaskDynPassingPoint EntityClassName;
+ static const std::string CLASS_NAME;
+ virtual void display( std::ostream& os ) const;
+ virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+
+ /* --- SIGNALS --- */
+ DECLARE_SIGNAL_IN(velocityDes, ml::Vector);
+ DECLARE_SIGNAL_IN(duration, double);
+ DECLARE_SIGNAL_IN(initialTime, double);
+
+ DECLARE_SIGNAL_OUT(velocityCurrent, ml::Vector);
+ DECLARE_SIGNAL_OUT(velocityDesired, ml::Vector);
+
+
+ protected:
+ /* --- COMPUTATION --- */
+ dg::sot::VectorMultiBound& computeTaskSOUT( dg::sot::VectorMultiBound& task, int iter );
+
+ private:
+ ml::Vector previousTask;
+
+ }; // class TaskDynPassingPoint
+
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
+
+
+#endif // #ifndef __sot_dyninv_TaskDynPassingPoint_H__