Commit 56039c62 authored by Oscar E. Ramos P's avatar Oscar E. Ramos P
Browse files

Add TaskDynPassingPoint as a dynamic task

There is meta_task_dyn_passing_point to help in the initialization
of this task
parent 8a6f9bd2
......@@ -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
......@@ -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)
......
......@@ -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
......
......@@ -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
......
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()
/*
* 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
/*
* 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__
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment