Skip to content
Snippets Groups Projects
Commit 6c156dca authored by Nicolas Mansard's avatar Nicolas Mansard
Browse files

Moved the .h in the source, and added the namespace dynamicgraph.

parent f2776267
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 2749 additions and 2410 deletions
CMakeLists.txt
+ 17
− 1
View file @ 6c156dca
......@@ -56,8 +56,24 @@ SET(libs
LIST(APPEND LOGGING_WATCHED_TARGETS ${libs})
SET(headers
commands-helper.h
entity-helper.h
signal-helper.h
mal-to-eigen.h
stack-template.h
stack-template.t.cpp
controller-pd.h
task-dyn-pd.h
dynamic-integrator.h
solver-op-space.h
zmp-estimator.h
pseudo-robot-dynamic.h
)
# Add subdirectories.
ADD_SUBDIRECTORY(include)
ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(unitTesting)
......
include/sot-dyninv/stack-template.h deleted 100644 → 0
+ 0
− 192
View file @ f2776267
/*
* 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_StackTemplate_H__
#define __sot_dyninv_StackTemplate_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (dynamic_interpretor_EXPORTS)
# define SOTSTACKTEMPLATE_EXPORT __declspec(dllexport)
# else
# define SOTSTACKTEMPLATE_EXPORT __declspec(dllimport)
# endif
#else
# define SOTSTACKTEMPLATE_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <sot-dyninv/signal-helper.h>
namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dg = dynamicgraph;
template< typename TaskGeneric >
class SOTSTACKTEMPLATE_EXPORT Stack
{
protected:
typedef std::vector<TaskGeneric*> Stack_t;
typedef typename Stack_t::iterator StackIterator_t;
typedef typename Stack_t::const_iterator StackConstIterator_t;
/*! \brief List of task (controllers) managed by the stack of tasks. */
Stack_t stack;
/*! \brief Number of joints to be used to compute the control law. */
int nbDofs;
public:
/*! \brief Number of joints by default. */
static const unsigned int NB_JOINTS_DEFAULT; // = 48;
public: /* --- Constructors --- */
Stack( void );
~Stack( void ) { /* TODO!! */ }
public: /* --- Modifiors --- */
/*! \name Methods to handle the stack.
@{
*/
/*! \brief Push the task in the stack.
It has a lowest priority than the previous ones.
If this is the first task, then it has the highest
priority. */
virtual void push( TaskGeneric& task );
/*! \brief Pop the task from the stack.
This method removes the task with the smallest
priority in the task. The other are projected
in the null-space of their predecessors. */
virtual TaskGeneric& pop( void );
/*! \brief This method allows to know if a task exists or not */
virtual bool exist( const TaskGeneric& task );
/*! \brief Remove a task regardless to its position in the stack.
It removes also the signals connected to the output signal of this stack.*/
virtual void remove( const TaskGeneric& task );
/*! \brief This method makes the task to swap with the task having the
immediate superior priority. */
virtual void up( const TaskGeneric& task );
/*! \brief This method makes the task to swap with the task having the
immediate inferior priority. */
virtual void down( const TaskGeneric& task );
/*! \brief Remove all the tasks from the stack. */
virtual void clear( void );
/*! Change the number of DOF, ie the field nbDofs. */
virtual void defineNbDof( const int& nbDof );
/*! @} */
dg::SignalBase<int>* tatata;
int titit;
typedef std::list< const dg::SignalBase<int>* > TaskDependancyList_t;
int totol;
/*! \brief Return the signal to be added/removed from the dependancy
* list of the control signal. */
virtual TaskDependancyList_t getTaskDependancyList( const TaskGeneric& task ) = 0;
virtual void addDependancy( const TaskDependancyList_t& depList ) = 0;
virtual void removeDependancy( const TaskDependancyList_t& depList ) = 0;
virtual void resetReady( void ) = 0;
public: /* --- DISPLAY --- */
/*! \name Methods to display the stack of tasks.
@{
*/
/*! Display the stack of tasks in text mode as a tree. */
virtual void display( std::ostream& os ) const;
/*! \brief Write the priority between tasks in the outstream os. */
virtual std::ostream &
writeGraph(const std::string & name, std::ostream & os) const;
/*! @} */
public: /* --- COMMANDS --- */
/*! @{ */
/* To use the macro, the typedef sot::Stack<TaskSpec> stack_t has to be set,
* as well as the EntityClassName. */
//#define ADD_COMMANDS_FOR_THE_STACK /// Code in the .t.cpp
/*! \brief This method deals with the command line.
The command given in argument is send to the stack of tasks by the shell.
The command understood by sot are:
<ul>
<li> Tasks
<ul>
<li> push <task> : Push a task in the stack (FILO).
<li> pop : Remove the task push in the stack.
<li> down <task> : Make the task have a higher priority, i.e.
swap with the task immediatly superior in priority.
<li> up <task> : Make the task have a lowest priority, i.e.
swap with the task immediatly inferior in priority.
<li> rm <task> : Remove the task from the stack.
</ul>
*/
virtual bool stackCommandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
void pushByTaskName( const std::string& taskName );
void removeByTaskName( const std::string& taskName );
void upByTaskName( const std::string& taskName );
void downByTaskName( const std::string& taskName );
/*! @} */
}; // class StackTemplate
template< typename TaskGeneric >
std::ostream&
operator<< ( std::ostream& os,const Stack<TaskGeneric>& sot )
{
sot.display(os);
return os;
}
} // namespace sot
#include <sot-dyninv/stack-template.t.cpp>
#endif // #ifndef __sot_dyninv_StackTemplate_H__
include/sot-dyninv/stack-template.t.cpp deleted 100644 → 0
+ 0
− 368
View file @ f2776267
/*
* 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_StackTemplate_TCC__
#define __sot_dyninv_StackTemplate_TCC__
#include <dynamic-graph/pool.h>
#include <sot-core/debug.h>
#include <sot-core/task-abstract.h>
namespace sot
{
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
template< typename TaskGeneric >
const unsigned int Stack<TaskGeneric>::NB_JOINTS_DEFAULT = 46;
/* --------------------------------------------------------------------- */
/* --- CONSTRUCTION ---------------------------------------------------- */
/* --------------------------------------------------------------------- */
template< typename TaskGeneric >
Stack<TaskGeneric>::
Stack( void )
:stack()
,nbDofs( NB_JOINTS_DEFAULT )
{
}
/* --------------------------------------------------------------------- */
/* --- STACK MANIPULATION --- */
/* --------------------------------------------------------------------- */
template< typename TaskGeneric >
void Stack<TaskGeneric>::
push( TaskGeneric& task )
{
stack.push_back( &task );
addDependancy( getTaskDependancyList( task ) );
resetReady();
}
template< typename TaskGeneric >
TaskGeneric& Stack<TaskGeneric>::
pop( void )
{
TaskGeneric* res = stack.back();
stack.pop_back();
removeDependancy( getTaskDependancyList( *res ) );
resetReady();
return *res;
}
template< typename TaskGeneric >
bool Stack<TaskGeneric>::
exist( const TaskGeneric& key )
{
StackIterator_t it;
for ( it=stack.begin();stack.end()!=it;++it )
{
if( *it == &key ) { return true; }
}
return false;
}
template< typename TaskGeneric >
void Stack<TaskGeneric>::
remove( const TaskGeneric& key )
{
bool find =false;
StackIterator_t it;
for ( it=stack.begin();stack.end()!=it;++it )
{
if( *it == &key ) { find=true; break; }
}
if(! find ){ return; }
stack.erase( it );
removeDependancy( getTaskDependancyList( key ) );
resetReady();
}
template< typename TaskGeneric >
void Stack<TaskGeneric>::
up( const TaskGeneric& key )
{
bool find =false;
StackIterator_t it;
for ( it=stack.begin();stack.end()!=it;++it )
{
if( *it == &key ) { find=true; break; }
}
if( stack.begin()==it ) { return; }
if(! find ){ return; }
StackIterator_t pos=it; pos--;
TaskGeneric * task = *it;
stack.erase( it );
stack.insert( pos,task );
resetReady();
}
template< typename TaskGeneric >
void Stack<TaskGeneric>::
down( const TaskGeneric& key )
{
bool find =false;
StackIterator_t it;
for ( it=stack.begin();stack.end()!=it;++it )
{
if( *it == &key ) { find=true; break; }
}
if( stack.end()==it ) { return; }
if(! find ){ return; }
StackIterator_t pos=it; pos++;
TaskGeneric* task=*it;
stack.erase( it );
if( stack.end()==pos ){ stack.push_back(task); }
else
{
pos++;
stack.insert( pos,task );
}
resetReady();
}
template< typename TaskGeneric >
void Stack<TaskGeneric>::
clear( void )
{
for ( StackIterator_t it=stack.begin();stack.end()!=it;++it )
{
removeDependancy( getTaskDependancyList( **it ) );
}
stack.clear();
resetReady();
}
template< typename TaskGeneric >
void Stack<TaskGeneric>::
defineNbDof( const int& nbDof )
{
assert(nbDof >= 0);
nbDofs = nbDof;
resetReady();
}
/* --------------------------------------------------------------------- */
/* --- DISPLAY --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
template< typename TaskGeneric >
void Stack<TaskGeneric>::
display( std::ostream& os ) const
{
os << "+-----------------"<<std::endl<<"+ SOT "
<< std::endl<< "+-----------------"<<std::endl;
for ( StackConstIterator_t it=this->stack.begin();
this->stack.end()!=it;++it )
//BOOST_FOREACH( TaskGeneric* task, stack )
{
os << "| " << (*it)->getName() <<std::endl;
}
}
/* --------------------------------------------------------------------- */
/* --- COMMAND --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#define ACT_BY_TASK_NAME(ACT) \
template< typename TaskGeneric > \
void Stack<TaskGeneric>:: \
ACT##ByTaskName( const std::string& tname ) \
{ \
dg::Entity & taska = dg::g_pool.getEntity( tname ); \
TaskGeneric & task = dynamic_cast<TaskGeneric&>( taska ); \
ACT(task); \
}
ACT_BY_TASK_NAME(push)
ACT_BY_TASK_NAME(remove)
ACT_BY_TASK_NAME(up)
ACT_BY_TASK_NAME(down)
template< typename TaskGeneric >
bool Stack<TaskGeneric>::
stackCommandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os )
{
sotDEBUGIN(15);
if( cmdLine == "help")
{
os << "Stack of Tasks: "<<std::endl
<< " - push <task>"<< std::endl
<< " - pop"<< std::endl
<< " - down <task>"<< std::endl
<< " - up <task>"<< std::endl
<< " - rm <task>"<< std::endl
<< " - clear"<< std::endl
<< " - display"<< std::endl
<< " - nbDofs <nb> "<<std::endl;
}
else if( cmdLine == "clear")
{
clear();
}
else if( cmdLine == "push")
{
std::string tname; cmdArgs >> tname;
pushByTaskName(tname);
}
else if( cmdLine == "up")
{
std::string tname; cmdArgs >> tname;
upByTaskName(tname);
}
else if( cmdLine == "down")
{
std::string tname; cmdArgs >> tname;
downByTaskName(tname);
}
else if( cmdLine == "rm")
{
std::string tname; cmdArgs >> tname;
removeByTaskName(tname);
}
else if( cmdLine == "pop")
{
TaskGeneric& task = pop();
os << "Remove : "<< task << std::endl;
}
else if( cmdLine == "nbDofs")
{
cmdArgs>>std::ws;
if( cmdArgs.good() )
{
unsigned int nbDof;
cmdArgs >> nbDof;
defineNbDof(nbDof);
} else { os << "nbDofs = "<< nbDofs <<std::endl; }
}
else if( cmdLine == "display")
{
display(os);
}
else return false;
return true;
sotDEBUGOUT(15);
}
template< typename TaskGeneric >
std::ostream& Stack<TaskGeneric>::
writeGraph( const std::string & name, std::ostream& os ) const
{
StackConstIterator_t iter;
for( iter = stack.begin(); iter!=stack.end();++iter )
{
const TaskGeneric & task = **iter;
StackConstIterator_t nextiter =iter;
nextiter++;
if (nextiter!=stack.end())
{
TaskGeneric & nexttask = **nextiter;
os << "\t\t\t" << task.getName() << " -> " << nexttask.getName()
<< " [color=red]" << std::endl;
}
}
os << "\t\tsubgraph cluster_Tasks {" <<std::endl
<< "\t\t\tsubgraph cluster_" << name << " {" << std::endl
<< "\t\t\t\tcolor=lightsteelblue1; label=\"" << name
<< "\"; style=filled;" << std::endl;
for( iter = stack.begin(); iter!=stack.end();++iter )
{
const TaskGeneric & task = **iter;
os << "\t\t\t\t" << task.getName()
<< " [ label = \"" << task.getName() << "\" ," << std::endl
<< "\t\t\t\t fontcolor = black, color = black, "
<< "fillcolor = magenta, style=filled, shape=box ]" << std::endl;
}
os << "\t\t\t}" << std::endl;
os << "\t\t\t}" <<std::endl;
return os;
}
/* To use the macro, the typedef sot::Stack<TaskSpec> stack_t has to be set,
* as well as the EntityClassName. */
#define ADD_COMMANDS_FOR_THE_STACK \
addCommand("dispStack", \
makeCommandVerbose(*this,&stack_t::display, \
docCommandVerbose("Guess what?"))); \
addCommand("up", \
makeCommandVoid1(*this, \
(void (EntityClassName::*)(const std::string&)) \
&stack_t::upByTaskName, \
docCommandVoid1("Up the named task.", \
"string (task name)"))); \
addCommand("down", \
makeCommandVoid1(*this, \
(void (EntityClassName::*)(const std::string&)) \
&stack_t::downByTaskName, \
docCommandVoid1("Down the named task.", \
"string (task name)"))); \
addCommand("push", \
makeCommandVoid1(*this, \
(void (EntityClassName::*)(const std::string&)) \
&stack_t::pushByTaskName, \
docCommandVoid1("Push the named task.", \
"string (task name)"))); \
addCommand("rm", \
makeCommandVoid1(*this, \
(void (EntityClassName::*)(const std::string&)) \
&stack_t::removeByTaskName, \
docCommandVoid1("Remove the named task.", \
"string (task name)"))); \
addCommand("clear", \
makeCommandVoid0(*this, \
(void (EntityClassName::*)(void)) \
&stack_t::clear, \
docCommandVoid0("Clear the stack from all task."))); \
addCommand("pop", \
makeCommandVoid0(*this, \
(void (EntityClassName::*)(void)) \
&stack_t::pop, \
docCommandVoid0("Remove the last (lowest) task of the stack."))); \
\
addCommand("setSize", \
makeCommandVoid1(*this, \
(void (EntityClassName::*)(const int&)) \
&stack_t::defineNbDof, \
docCommandVoid1("??? TODO", \
"int"))); \
addCommand("getSize", \
makeDirectGetter(*this,&nbDofs, \
docDirectGetter("size","int")))
} // namespace sot
#endif // #ifndef __sot_dyninv_StackTemplate_TCC__
python/dyndebug.py
+ 9
− 9
View file @ 6c156dca
......@@ -3,8 +3,8 @@ from dynamic_graph.sot.core import *
from dynamic_graph.sot.core.math_small_entities import Derivator_of_Matrix
from dynamic_graph.sot.dynamics import *
from dynamic_graph.sot.dyninv import *
import script_shortcuts
from matlab import matlab
import dynamic_graph.script_shortcuts
from dynamic_graph.matlab import matlab
# --- Dynamic parameters ---
hrp2_14_pkgdatarootdir = "/home/nmansard/compil/devgiri/hpp2/share/hrp2_14"
......@@ -26,9 +26,9 @@ robot.resize(robotDim)
robot.set( ( 0.0274106623863, 0.143843868989, 0.646921914726, 0.00221064938462, 0.101393756965, 1.36729741242e-05, -0.00911630330837, -0.0914099637938, -0.471978743283, 0.840380192617, -0.470232799053, 0.0896624078591, 0.00950781802257, 0.0911102868041, -0.469450351848, 0.835307995386, -0.467686190904, -0.0938029466367, -8.75646582964e-05, 0.00326431877374, -7.83380820086e-06, 0.000194742578013, 0.258370257314, -0.175099102142, -6.1173425555e-05, -0.524953548768, 3.1825099713e-06, -0.000257600047423, -3.41210481921e-06, 0.258367271628, 0.174322097546, -8.89023954997e-05, -0.524983690846, -3.46102941488e-07, -0.000265401439772, 1.00498481453e-06 ) )
robot_old=robot
robot_wrapper=robot
robot = PseudoRobotDynamic("dynint")
robot.replace("robot",True)
robot.replace(robot_wrapper.name,True)
dt=1e-3
robot.dt.value = dt
......@@ -304,16 +304,16 @@ tr.add('sot.control','')
# --- RUN ------------------------------------------------
def inc():
robot_old.increment(dt)
tr.triger.recompute( robot_old.control.time )
robot_wrapper.increment(dt)
tr.triger.recompute( robot_wrapper.control.time )
featureComDes.errorIN.value = (0.06,0,0.8)
sot.push('taskCom')
for i in range(2100):
inc()
#for i in range(2100):
# inc()
tr.dump()
#tr.dump()
src/CMakeLists.txt
+ 13
− 0
View file @ 6c156dca
......@@ -20,7 +20,20 @@ IF(CMAKE_BUILD_TYPE STREQUAL "DEBUG")
ADD_DEFINITIONS(-DDEBUG=2)
ENDIF(CMAKE_BUILD_TYPE STREQUAL "DEBUG")
# --- HEADERS
FOREACH(header ${headers})
get_filename_component(headerName ${header} NAME)
set(src_path ${${PROJECT_NAME}_SOURCE_DIR}/src/${header})
set(bin_path ${${PROJECT_NAME}_BINARY_DIR}/include/${PROJECT_NAME}/${headerName})
set(install_path ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME})
execute_process(COMMAND ${CMAKE_COMMAND} -E remove ${bin_path})
execute_process(COMMAND ${CMAKE_COMMAND} -E create_symlink ${src_path} ${bin_path} )
install(FILES ${src_path} DESTINATION ${install_path}
PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
ENDFOREACH(header)
INCLUDE_DIRECTORIES(${CMAKE_BINARY_DIR}/include)
# --- COMPILATION OF PLUGINS
FOREACH(lib ${libs})
ADD_LIBRARY(${lib} SHARED ${lib}.cpp)
......
include/sot-dyninv/commands-helper.h → src/commands-helper.h
+ 18
− 16
View file @ 6c156dca
......@@ -26,22 +26,24 @@
#include <boost/function.hpp>
/* --- HELPER --------------------------------------------------------------- */
namespace sot {
namespace dyninv {
using ::dynamicgraph::command::makeDirectGetter;
using ::dynamicgraph::command::docDirectGetter;
using ::dynamicgraph::command::makeDirectSetter;
using ::dynamicgraph::command::docDirectSetter;
using ::dynamicgraph::command::makeCommandVoid0;
using ::dynamicgraph::command::docCommandVoid0;
using ::dynamicgraph::command::makeCommandVoid1;
using ::dynamicgraph::command::docCommandVoid1;
using ::dynamicgraph::command::makeCommandVoid2;
using ::dynamicgraph::command::docCommandVoid2;
using ::dynamicgraph::command::makeCommandVerbose;
using ::dynamicgraph::command::docCommandVerbose;
} // namespace dyninv
} // namespace sot
namespace dynamicgraph {
namespace sot {
namespace dyninv {
using ::dynamicgraph::command::makeDirectGetter;
using ::dynamicgraph::command::docDirectGetter;
using ::dynamicgraph::command::makeDirectSetter;
using ::dynamicgraph::command::docDirectSetter;
using ::dynamicgraph::command::makeCommandVoid0;
using ::dynamicgraph::command::docCommandVoid0;
using ::dynamicgraph::command::makeCommandVoid1;
using ::dynamicgraph::command::docCommandVoid1;
using ::dynamicgraph::command::makeCommandVoid2;
using ::dynamicgraph::command::docCommandVoid2;
using ::dynamicgraph::command::makeCommandVerbose;
using ::dynamicgraph::command::docCommandVerbose;
} // namespace dyninv
} // namespace sot
} // namespace dynamicgraph
......
src/controller-pd.cpp
+ 269
− 266
View file @ 6c156dca
......@@ -15,279 +15,282 @@
*/
#include <sot-dyninv/controller-pd.h>
#include <sot-core/debug.h>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <sot-dyninv/commands-helper.h>
namespace sot
namespace dynamicgraph
{
namespace dyninv
namespace sot
{
namespace dg = ::dynamicgraph;
using namespace dg;
/* --- DG FACTORY ------------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControllerPD,"ControllerPD");
/* --- CONSTRUCTION ----------------------------------------------------- */
/* --- CONSTRUCTION ----------------------------------------------------- */
/* --- CONSTRUCTION ----------------------------------------------------- */
ControllerPD::
ControllerPD( const std::string & name )
: Entity(name)
,CONSTRUCT_SIGNAL_IN(Kp,ml::Vector)
,CONSTRUCT_SIGNAL_IN(Kd,ml::Vector)
,CONSTRUCT_SIGNAL_IN(position,ml::Vector)
,CONSTRUCT_SIGNAL_IN(positionRef,ml::Vector)
,CONSTRUCT_SIGNAL_IN(velocity,ml::Vector)
,CONSTRUCT_SIGNAL_IN(velocityRef,ml::Vector)
,CONSTRUCT_SIGNAL_OUT(control,ml::Vector,
KpSIN << KdSIN << positionSIN << positionRefSIN
<< velocitySIN << velocityRefSIN )
{
Entity::signalRegistration( KpSIN << KdSIN << positionSIN << positionRefSIN
<< velocitySIN << velocityRefSIN
<< controlSOUT );
/* Commands. */
addCommand("getSize",
makeDirectGetter(*this,&_dimension,
docDirectGetter("dimension","int")));
addCommand("setSize",
makeDirectSetter(*this, &_dimension,
docDirectSetter("dimension","int")));
addCommand("velocityOnly",
makeCommandVoid0(*this,&ControllerPD::setGainVelocityOnly,
docCommandVoid0("Set a friction-style controller.")));
addCommand("stdGain",
makeCommandVoid1(*this,&ControllerPD::setStandardGains,
docCommandVoid1("Set spefic gains.",
"string (in low|medium|high)")));
}
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
ml::Vector& ControllerPD::
controlSOUT_function( ml::Vector &tau, int iter )
{
sotDEBUGIN(15);
const ml::Vector& Kp = KpSIN(iter);
const ml::Vector& Kd = KdSIN(iter);
const ml::Vector& position = positionSIN(iter);
const ml::Vector& desiredposition = positionRefSIN(iter);
const unsigned int size = Kp.size();
assert( _dimension == (int)size );
assert( size==Kp.size() ); assert( size==Kd.size() );
assert( size==position.size() ); assert( size==desiredposition.size() );
bool useVelocity = velocitySIN;
ml::Vector velocity;
bool useVelocityDesired = false;
ml::Vector desiredvelocity;
if( useVelocity ) // TODO: there is a useless copy here. Use a pointer?
{
velocity = velocitySIN(iter);
assert( size==velocity.size() );
useVelocityDesired = velocityRefSIN;
if( useVelocityDesired )
{
desiredvelocity = velocityRefSIN(iter);
assert( size==desiredvelocity.size() );
}
}
tau.resize(size); double dv; // TODO: use dv from start to avoid the copy.
for(unsigned i = 0u; i < size; ++i)
{
tau(i) = Kp(i)*(desiredposition(i)-position(i));
if( useVelocity )
{
dv= velocity(i);
if( useVelocityDesired )
dv-=desiredvelocity(i);
tau(i) -= Kd(i)*dv;
}
}
sotDEBUG(15) << "p = " << position << std::endl;
sotDEBUG(15) << "pd = " << desiredposition << std::endl;
if( useVelocity ) {sotDEBUG(15) << "v= " << velocity << std::endl;}
if( useVelocityDesired ) { sotDEBUG(15) << "vd= " << velocity << std::endl; }
sotDEBUG(15) << "kp = " << Kp << std::endl;
sotDEBUG(15) << "kd = " << Kd << std::endl;
sotDEBUG(15) << "PD torque= " << tau << std::endl;
sotDEBUGOUT(15);
return tau;
}
/* --- COMMANDS ---------------------------------------------------------- */
/* --- MEMBERS ---------------------------------------------------------- */
void ControllerPD::
size(const int & dimension)
namespace dyninv
{
_dimension = dimension;
}
int ControllerPD::
size(void) const
{
return _dimension;
}
void ControllerPD::
setGainVelocityOnly( void )
{
ml::Vector zero(_dimension); zero.fill(0);
positionSIN = zero;
positionRefSIN = zero;
KpSIN = zero;
}
/** Give some specific values for the Kp and Kd gains. Possible
* configs are "low", "middle" and "high".
* Warning: middle and high only works for dim 30.
* TODO: properly throw errors when needed.
*/
void ControllerPD::
setStandardGains( const std::string & config )
{
if( config =="low" )
{
// Low gains
ml::Vector Kp(_dimension); Kp.fill(100);
ml::Vector Kd(_dimension); Kd.fill(20);
KpSIN = Kp;
KdSIN = Kd;
}
else if( config =="medium" )
{
// Medium gains
if( _dimension != 30 )
{ std::cerr << "Only working for dim=30!" << std::endl; return; }
ml::Vector Kp(_dimension),Kd(_dimension);
unsigned int i=0;
Kp(i++)=400; Kp(i++)=1000; Kp(i++)=2000; Kp(i++)=1800; Kp(i++)=2000;
Kp(i++)=1000; Kp(i++)=400; Kp(i++)=1000; Kp(i++)=2000; Kp(i++)=1800;
Kp(i++)=2000; Kp(i++)=1000; Kp(i++)=500; Kp(i++)=250; Kp(i++)=200;
Kp(i++)=200; Kp(i++)=300; Kp(i++)=300; Kp(i++)=200; Kp(i++)=400;
Kp(i++)=400; Kp(i++)=200; Kp(i++)=200; Kp(i++)=300; Kp(i++)= 300;
Kp(i++)=200; Kp(i++)=400; Kp(i++)=400; Kp(i++)=200; Kp(i++)=200;
i=0;
Kd(i++)=40; Kd(i++)=100; Kd(i++)=200; Kd(i++)=180; Kd(i++)=200;
Kd(i++)=100; Kd(i++)=40; Kd(i++)=100; Kd(i++)=200; Kd(i++)=180;
Kd(i++)=200; Kd(i++)=100; Kd(i++)=50; Kd(i++)=25; Kd(i++)=20;
Kd(i++)=20; Kd(i++)=30; Kd(i++)=30; Kd(i++)=20; Kd(i++)=40;
Kd(i++)=40; Kd(i++)=20; Kd(i++)=20; Kd(i++)=30; Kd(i++)= 30;
Kd(i++)=20; Kd(i++)=40; Kd(i++)=40; Kd(i++)=20; Kd(i++)=20;
KpSIN = Kp;
KdSIN = Kd;
}
else // high
{
// High gains
if( _dimension != 30 )
{ std::cerr << "Only working for dim=30!" << std::endl; return; }
ml::Vector Kp(_dimension),Kd(_dimension);
unsigned int i=0;
Kp(i++)=4000; Kp(i++)=10000; Kp(i++)=20000; Kp(i++)=18000; Kp(i++)=20000;
Kp(i++)=10000; Kp(i++)=4000; Kp(i++)=10000; Kp(i++)=20000; Kp(i++)=18000;
Kp(i++)=20000; Kp(i++)=10000; Kp(i++)=5000; Kp(i++)=2500; Kp(i++)=2000;
Kp(i++)=2000; Kp(i++)=3000; Kp(i++)=3000; Kp(i++)=2000; Kp(i++)=4000;
Kp(i++)=4000; Kp(i++)=2000; Kp(i++)=2000; Kp(i++)=3000; Kp(i++)= 3000;
Kp(i++)=2000; Kp(i++)=4000; Kp(i++)=4000; Kp(i++)=2000; Kp(i++)=2000;
i=0;
Kd(i++)=120; Kd(i++)=300; Kd(i++)=600; Kd(i++)=500; Kd(i++)=600;
Kd(i++)=300; Kd(i++)=120; Kd(i++)=300; Kd(i++)=600; Kd(i++)=500;
Kd(i++)=600; Kd(i++)=300; Kd(i++)=150; Kd(i++)=75; Kd(i++)=60;
Kd(i++)=60; Kd(i++)=90; Kd(i++)=90; Kd(i++)=60; Kd(i++)=120;
Kd(i++)=120; Kd(i++)=60; Kd(i++)=60; Kd(i++)=90; Kd(i++)= 90;
Kd(i++)=60; Kd(i++)=120; Kd(i++)=120; Kd(i++)=60; Kd(i++)=60;
KpSIN = Kp;
KdSIN = Kd;
}
}
/* --- ENTITY ----------------------------------------------------------- */
/* --- ENTITY ----------------------------------------------------------- */
/* --- ENTITY ----------------------------------------------------------- */
void ControllerPD::
display( std::ostream& os ) const
{
os << "ControllerPD "<<getName();
try
{
os <<"control = "<<controlSOUT;
}
catch (ExceptionSignal e) {}
}
void ControllerPD::
commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os )
{
if( cmdLine == "help" )
{
os << "sotControlPD:\n"
<< " - size <arg>\t\tset the size of the vector.\n"
<< " - stdGain \t\tset the input vector gains according to the size for HRP2.\n"
<< " - velocityonly <arg>\t\tset Kp = 0.\n"
<< std::endl;
}
else if( cmdLine == "size" )
{
cmdArgs >> std::ws;
if( cmdArgs.good() )
{
unsigned int i; cmdArgs >> i ;
size(i);
}
else
{
os << "size = " << size() << std::endl;
}
}
else if( cmdLine == "velocityonly" )
{ setGainVelocityOnly(); }
else if( cmdLine == "stdGain" )
{
std::string config = "high";
cmdArgs >> std::ws; if( cmdArgs.good() ) cmdArgs >> config;
setStandardGains( config );
}
else
{
Entity::commandLine(cmdLine,cmdArgs,os);
}
}
} // namespace dyninv
} // namespace sot
namespace dg = ::dynamicgraph;
using namespace dg;
/* --- DG FACTORY ------------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControllerPD,"ControllerPD");
/* --- CONSTRUCTION ----------------------------------------------------- */
/* --- CONSTRUCTION ----------------------------------------------------- */
/* --- CONSTRUCTION ----------------------------------------------------- */
ControllerPD::
ControllerPD( const std::string & name )
: Entity(name)
,CONSTRUCT_SIGNAL_IN(Kp,ml::Vector)
,CONSTRUCT_SIGNAL_IN(Kd,ml::Vector)
,CONSTRUCT_SIGNAL_IN(position,ml::Vector)
,CONSTRUCT_SIGNAL_IN(positionRef,ml::Vector)
,CONSTRUCT_SIGNAL_IN(velocity,ml::Vector)
,CONSTRUCT_SIGNAL_IN(velocityRef,ml::Vector)
,CONSTRUCT_SIGNAL_OUT(control,ml::Vector,
KpSIN << KdSIN << positionSIN << positionRefSIN
<< velocitySIN << velocityRefSIN )
{
Entity::signalRegistration( KpSIN << KdSIN << positionSIN << positionRefSIN
<< velocitySIN << velocityRefSIN
<< controlSOUT );
/* Commands. */
addCommand("getSize",
makeDirectGetter(*this,&_dimension,
docDirectGetter("dimension","int")));
addCommand("setSize",
makeDirectSetter(*this, &_dimension,
docDirectSetter("dimension","int")));
addCommand("velocityOnly",
makeCommandVoid0(*this,&ControllerPD::setGainVelocityOnly,
docCommandVoid0("Set a friction-style controller.")));
addCommand("stdGain",
makeCommandVoid1(*this,&ControllerPD::setStandardGains,
docCommandVoid1("Set spefic gains.",
"string (in low|medium|high)")));
}
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
ml::Vector& ControllerPD::
controlSOUT_function( ml::Vector &tau, int iter )
{
sotDEBUGIN(15);
const ml::Vector& Kp = KpSIN(iter);
const ml::Vector& Kd = KdSIN(iter);
const ml::Vector& position = positionSIN(iter);
const ml::Vector& desiredposition = positionRefSIN(iter);
const unsigned int size = Kp.size();
assert( _dimension == (int)size );
assert( size==Kp.size() ); assert( size==Kd.size() );
assert( size==position.size() ); assert( size==desiredposition.size() );
bool useVelocity = velocitySIN;
ml::Vector velocity;
bool useVelocityDesired = false;
ml::Vector desiredvelocity;
if( useVelocity ) // TODO: there is a useless copy here. Use a pointer?
{
velocity = velocitySIN(iter);
assert( size==velocity.size() );
useVelocityDesired = velocityRefSIN;
if( useVelocityDesired )
{
desiredvelocity = velocityRefSIN(iter);
assert( size==desiredvelocity.size() );
}
}
tau.resize(size); double dv; // TODO: use dv from start to avoid the copy.
for(unsigned i = 0u; i < size; ++i)
{
tau(i) = Kp(i)*(desiredposition(i)-position(i));
if( useVelocity )
{
dv= velocity(i);
if( useVelocityDesired )
dv-=desiredvelocity(i);
tau(i) -= Kd(i)*dv;
}
}
sotDEBUG(15) << "p = " << position << std::endl;
sotDEBUG(15) << "pd = " << desiredposition << std::endl;
if( useVelocity ) {sotDEBUG(15) << "v= " << velocity << std::endl;}
if( useVelocityDesired ) { sotDEBUG(15) << "vd= " << velocity << std::endl; }
sotDEBUG(15) << "kp = " << Kp << std::endl;
sotDEBUG(15) << "kd = " << Kd << std::endl;
sotDEBUG(15) << "PD torque= " << tau << std::endl;
sotDEBUGOUT(15);
return tau;
}
/* --- COMMANDS ---------------------------------------------------------- */
/* --- MEMBERS ---------------------------------------------------------- */
void ControllerPD::
size(const int & dimension)
{
_dimension = dimension;
}
int ControllerPD::
size(void) const
{
return _dimension;
}
void ControllerPD::
setGainVelocityOnly( void )
{
ml::Vector zero(_dimension); zero.fill(0);
positionSIN = zero;
positionRefSIN = zero;
KpSIN = zero;
}
/** Give some specific values for the Kp and Kd gains. Possible
* configs are "low", "middle" and "high".
* Warning: middle and high only works for dim 30.
* TODO: properly throw errors when needed.
*/
void ControllerPD::
setStandardGains( const std::string & config )
{
if( config =="low" )
{
// Low gains
ml::Vector Kp(_dimension); Kp.fill(100);
ml::Vector Kd(_dimension); Kd.fill(20);
KpSIN = Kp;
KdSIN = Kd;
}
else if( config =="medium" )
{
// Medium gains
if( _dimension != 30 )
{ std::cerr << "Only working for dim=30!" << std::endl; return; }
ml::Vector Kp(_dimension),Kd(_dimension);
unsigned int i=0;
Kp(i++)=400; Kp(i++)=1000; Kp(i++)=2000; Kp(i++)=1800; Kp(i++)=2000;
Kp(i++)=1000; Kp(i++)=400; Kp(i++)=1000; Kp(i++)=2000; Kp(i++)=1800;
Kp(i++)=2000; Kp(i++)=1000; Kp(i++)=500; Kp(i++)=250; Kp(i++)=200;
Kp(i++)=200; Kp(i++)=300; Kp(i++)=300; Kp(i++)=200; Kp(i++)=400;
Kp(i++)=400; Kp(i++)=200; Kp(i++)=200; Kp(i++)=300; Kp(i++)= 300;
Kp(i++)=200; Kp(i++)=400; Kp(i++)=400; Kp(i++)=200; Kp(i++)=200;
i=0;
Kd(i++)=40; Kd(i++)=100; Kd(i++)=200; Kd(i++)=180; Kd(i++)=200;
Kd(i++)=100; Kd(i++)=40; Kd(i++)=100; Kd(i++)=200; Kd(i++)=180;
Kd(i++)=200; Kd(i++)=100; Kd(i++)=50; Kd(i++)=25; Kd(i++)=20;
Kd(i++)=20; Kd(i++)=30; Kd(i++)=30; Kd(i++)=20; Kd(i++)=40;
Kd(i++)=40; Kd(i++)=20; Kd(i++)=20; Kd(i++)=30; Kd(i++)= 30;
Kd(i++)=20; Kd(i++)=40; Kd(i++)=40; Kd(i++)=20; Kd(i++)=20;
KpSIN = Kp;
KdSIN = Kd;
}
else // high
{
// High gains
if( _dimension != 30 )
{ std::cerr << "Only working for dim=30!" << std::endl; return; }
ml::Vector Kp(_dimension),Kd(_dimension);
unsigned int i=0;
Kp(i++)=4000; Kp(i++)=10000; Kp(i++)=20000; Kp(i++)=18000; Kp(i++)=20000;
Kp(i++)=10000; Kp(i++)=4000; Kp(i++)=10000; Kp(i++)=20000; Kp(i++)=18000;
Kp(i++)=20000; Kp(i++)=10000; Kp(i++)=5000; Kp(i++)=2500; Kp(i++)=2000;
Kp(i++)=2000; Kp(i++)=3000; Kp(i++)=3000; Kp(i++)=2000; Kp(i++)=4000;
Kp(i++)=4000; Kp(i++)=2000; Kp(i++)=2000; Kp(i++)=3000; Kp(i++)= 3000;
Kp(i++)=2000; Kp(i++)=4000; Kp(i++)=4000; Kp(i++)=2000; Kp(i++)=2000;
i=0;
Kd(i++)=120; Kd(i++)=300; Kd(i++)=600; Kd(i++)=500; Kd(i++)=600;
Kd(i++)=300; Kd(i++)=120; Kd(i++)=300; Kd(i++)=600; Kd(i++)=500;
Kd(i++)=600; Kd(i++)=300; Kd(i++)=150; Kd(i++)=75; Kd(i++)=60;
Kd(i++)=60; Kd(i++)=90; Kd(i++)=90; Kd(i++)=60; Kd(i++)=120;
Kd(i++)=120; Kd(i++)=60; Kd(i++)=60; Kd(i++)=90; Kd(i++)= 90;
Kd(i++)=60; Kd(i++)=120; Kd(i++)=120; Kd(i++)=60; Kd(i++)=60;
KpSIN = Kp;
KdSIN = Kd;
}
}
/* --- ENTITY ----------------------------------------------------------- */
/* --- ENTITY ----------------------------------------------------------- */
/* --- ENTITY ----------------------------------------------------------- */
void ControllerPD::
display( std::ostream& os ) const
{
os << "ControllerPD "<<getName();
try
{
os <<"control = "<<controlSOUT;
}
catch (ExceptionSignal e) {}
}
void ControllerPD::
commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os )
{
if( cmdLine == "help" )
{
os << "sotControlPD:\n"
<< " - size <arg>\t\tset the size of the vector.\n"
<< " - stdGain \t\tset the input vector gains according to the size for HRP2.\n"
<< " - velocityonly <arg>\t\tset Kp = 0.\n"
<< std::endl;
}
else if( cmdLine == "size" )
{
cmdArgs >> std::ws;
if( cmdArgs.good() )
{
unsigned int i; cmdArgs >> i ;
size(i);
}
else
{
os << "size = " << size() << std::endl;
}
}
else if( cmdLine == "velocityonly" )
{ setGainVelocityOnly(); }
else if( cmdLine == "stdGain" )
{
std::string config = "high";
cmdArgs >> std::ws; if( cmdArgs.good() ) cmdArgs >> config;
setStandardGains( config );
}
else
{
Entity::commandLine(cmdLine,cmdArgs,os);
}
}
} // namespace dyninv
} // namespace sot
} // namespace dynamicgraph
include/sot-dyninv/controller-pd.h → src/controller-pd.h
+ 40
− 38
View file @ 6c156dca
......@@ -41,59 +41,61 @@
namespace sot {
namespace dyninv {
namespace dynamicgraph {
namespace sot {
namespace dyninv {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTCONTROLLERPD_EXPORT ControllerPD
:public ::dynamicgraph::Entity
,public ::dynamicgraph::EntityHelper<ControllerPD>
{
class SOTCONTROLLERPD_EXPORT ControllerPD
:public ::dynamicgraph::Entity
,public ::dynamicgraph::EntityHelper<ControllerPD>
{
public: /* --- CONSTRUCTOR ---- */
public: /* --- CONSTRUCTOR ---- */
ControllerPD( const std::string & name );
ControllerPD( const std::string & name );
protected:
/* Parameters of the torque-control function:
* tau = kp * (qd-q) + kd* (dqd-dq) */
int _dimension;
protected:
/* Parameters of the torque-control function:
* tau = kp * (qd-q) + kd* (dqd-dq) */
int _dimension;
public:
void size(const int & dimension);
int size(void) const;
void setStandardGains( const std::string & config );
void setGainVelocityOnly( void );
public:
void size(const int & dimension);
int size(void) const;
void setStandardGains( const std::string & config );
void setGainVelocityOnly( void );
public: /* --- ENTITY INHERITANCE --- */
public: /* --- ENTITY INHERITANCE --- */
static const std::string CLASS_NAME;
virtual void display( std::ostream& os ) const;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
static const std::string CLASS_NAME;
virtual void display( std::ostream& os ) const;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
public: /* --- SIGNALS --- */
public: /* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp,ml::Vector);
DECLARE_SIGNAL_IN(Kd,ml::Vector);
DECLARE_SIGNAL_IN(position,ml::Vector);
DECLARE_SIGNAL_IN(positionRef,ml::Vector);
DECLARE_SIGNAL_IN(velocity,ml::Vector);
DECLARE_SIGNAL_IN(velocityRef,ml::Vector);
DECLARE_SIGNAL_IN(Kp,ml::Vector);
DECLARE_SIGNAL_IN(Kd,ml::Vector);
DECLARE_SIGNAL_IN(position,ml::Vector);
DECLARE_SIGNAL_IN(positionRef,ml::Vector);
DECLARE_SIGNAL_IN(velocity,ml::Vector);
DECLARE_SIGNAL_IN(velocityRef,ml::Vector);
DECLARE_SIGNAL_OUT(control,ml::Vector);
DECLARE_SIGNAL_OUT(control,ml::Vector);
}; // class ControllerPD
}; // class ControllerPD
} // namespace dyninv
} // namespace sot
} // namespace dyninv
} // namespace sot
} // namespace dynamicgraph
......
src/dynamic-integrator.cpp
+ 386
− 383
View file @ 6c156dca
This diff is collapsed.
include/sot-dyninv/dynamic-integrator.h → src/dynamic-integrator.h
+ 38
− 37
View file @ 6c156dca
......@@ -40,58 +40,59 @@
#include <sot-dyninv/entity-helper.h>
namespace sot {
namespace dyninv {
namespace dynamicgraph {
namespace sot {
namespace dyninv {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTDYNAMICINTEGRATOR_EXPORT DynamicIntegrator
:public ::dynamicgraph::Entity
,public ::dynamicgraph::EntityHelper<DynamicIntegrator>
{
class SOTDYNAMICINTEGRATOR_EXPORT DynamicIntegrator
:public ::dynamicgraph::Entity
,public ::dynamicgraph::EntityHelper<DynamicIntegrator>
{
public: /* --- CONSTRUCTOR ---- */
public: /* --- CONSTRUCTOR ---- */
DynamicIntegrator( const std::string & name );
DynamicIntegrator( const std::string & name );
public: /* --- ENTITY INHERITANCE --- */
public: /* --- ENTITY INHERITANCE --- */
static const std::string CLASS_NAME;
virtual void display( std::ostream& os ) const;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
static const std::string CLASS_NAME;
virtual void display( std::ostream& os ) const;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
public: /* --- SIGNALS --- */
public: /* --- SIGNALS --- */
DECLARE_SIGNAL_IN( acceleration,ml::Vector );
DECLARE_SIGNAL_IN( dt,double );
DECLARE_SIGNAL_IN( acceleration,ml::Vector );
DECLARE_SIGNAL_IN( dt,double );
DECLARE_SIGNAL_OUT( velocity,ml::Vector );
DECLARE_SIGNAL_OUT( position,ml::Vector );
DECLARE_SIGNAL_OUT( velocity,ml::Vector );
DECLARE_SIGNAL_OUT( position,ml::Vector );
public: /* --- MODIFIORS --- */
void integrate( const ml::Vector& acceleration, const double& dt,
ml::Vector & velocity, ml::Vector & position );
void integrateFromSignals( const int & time );
void integrateFromSignals( void );
public: /* --- MODIFIORS --- */
void integrate( const ml::Vector& acceleration, const double& dt,
ml::Vector & velocity, ml::Vector & position );
void integrateFromSignals( const int & time );
void integrateFromSignals( void );
void setPosition( const ml::Vector& p );
void setVelocity( const ml::Vector& v );
void setState( const ml::Vector& p,const ml::Vector& v );
void setPosition( const ml::Vector& p );
void setVelocity( const ml::Vector& v );
void setState( const ml::Vector& p,const ml::Vector& v );
protected:
ml::Vector position,velocity;
protected:
ml::Vector position,velocity;
}; // class DynamicIntegrator
} // namespace dyninv
} // namespace sot
}; // class DynamicIntegrator
} // namespace dyninv
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_dyninv_DynamicIntegrator_H__
include/sot-dyninv/entity-helper.h → src/entity-helper.h
+ 0
− 0
View file @ 6c156dca
File moved
include/sot-dyninv/mal-to-eigen.h → src/mal-to-eigen.h
+ 0
− 0
View file @ 6c156dca
File moved
src/pseudo-robot-dynamic.cpp
+ 293
− 290
View file @ 6c156dca
This diff is collapsed.
include/sot-dyninv/pseudo-robot-dynamic.h → src/pseudo-robot-dynamic.h
+ 116
− 0
View file @ 6c156dca
......@@ -40,74 +40,76 @@
#include <sot-dyninv/entity-helper.h>
#include <sot-dyninv/dynamic-integrator.h>
namespace sot {
namespace dyninv {
namespace dynamicgraph {
namespace sot {
namespace dyninv {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Inside this entity is an integrator, taking position, velocity and
* acceleration at time t, and computing pos and vel at time t+1. Around
* this integrator, a wrapper is made to make the stuff take the place of
* the OpenHRP kinematic entity. it thus take a "control" signal, to triger
* the computations, and possesses all the functionnalities of the OpenHRP
* entity. Plus, it has the signals to feed the OpenHRP entity that
* wrappes the simulator. All the computations are triggered by computing
* the "qdot" signal.
*/
/* Inside this entity is an integrator, taking position, velocity and
* acceleration at time t, and computing pos and vel at time t+1. Around
* this integrator, a wrapper is made to make the stuff take the place of
* the OpenHRP kinematic entity. it thus take a "control" signal, to triger
* the computations, and possesses all the functionnalities of the OpenHRP
* entity. Plus, it has the signals to feed the OpenHRP entity that
* wrappes the simulator. All the computations are triggered by computing
* the "qdot" signal.
*/
class SOTPSEUDOROBOTDYNAMIC_EXPORT PseudoRobotDynamic
:public DynamicIntegrator
,public ::dynamicgraph::EntityHelper<PseudoRobotDynamic>
{
class SOTPSEUDOROBOTDYNAMIC_EXPORT PseudoRobotDynamic
:public DynamicIntegrator
,public ::dynamicgraph::EntityHelper<PseudoRobotDynamic>
{
public: /* --- CONSTRUCTOR ---- */
public: /* --- CONSTRUCTOR ---- */
PseudoRobotDynamic( const std::string & name );
PseudoRobotDynamic( const std::string & name );
public: /* --- ENTITY INHERITANCE --- */
public: /* --- ENTITY INHERITANCE --- */
static const std::string CLASS_NAME;
virtual void display( std::ostream& os ) const;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
static const std::string CLASS_NAME;
virtual void display( std::ostream& os ) const;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
typedef ::dynamicgraph::EntityHelper<PseudoRobotDynamic>::EntityClassName
EntityClassName;
typedef ::dynamicgraph::EntityHelper<PseudoRobotDynamic>::EntityClassName
EntityClassName;
public: /* --- SIGNALS --- */
public: /* --- SIGNALS --- */
DECLARE_SIGNAL_IN( control,ml::Vector );
DECLARE_SIGNAL_OUT( qdot,ml::Vector );
DECLARE_SIGNAL_IN( control,ml::Vector );
DECLARE_SIGNAL_OUT( qdot,ml::Vector );
DECLARE_SIGNAL(rotation,OUT,ml::Vector);
DECLARE_SIGNAL(translation,OUT,ml::Vector);
//sotSignal< ml::Vector,int > rotationSOUT;
//sotSignal< ml::Vector,int > translationSOUT;
::dynamicgraph::SignalPtr< ml::Vector,int > stateSOUT;
DECLARE_SIGNAL(rotation,OUT,ml::Vector);
DECLARE_SIGNAL(translation,OUT,ml::Vector);
//sotSignal< ml::Vector,int > rotationSOUT;
//sotSignal< ml::Vector,int > translationSOUT;
::dynamicgraph::SignalPtr< ml::Vector,int > stateSOUT;
public: /* --- SIGNALS --- */
public: /* --- SIGNALS --- */
void replaceSimulatorEntity( const std::string& formerName,
const bool& plug = false );
void setRoot( const ml::Matrix & M );
void replaceSimulatorEntity( const std::string& formerName,
const bool& plug = false );
void setRoot( const ml::Matrix & M );
public: /* --- COMMAND --- */
template< typename T1 >
void forwardVoidCommandToSimu( const std::string& cmdName,
const T1& arg1 );
void addForward( const std::string& cmdName );
public: /* --- COMMAND --- */
template< typename T1 >
void forwardVoidCommandToSimu( const std::string& cmdName,
const T1& arg1 );
void addForward( const std::string& cmdName );
private:
::dynamicgraph::Entity* formerOpenHRP;
private:
::dynamicgraph::Entity* formerOpenHRP;
}; // class PseudoRobotDynamic
} // namespace dyninv
} // namespace sot
}; // class PseudoRobotDynamic
} // namespace dyninv
} // namespace sot
} // namespace dynamicgraph
......
include/sot-dyninv/signal-helper.h → src/signal-helper.h
+ 0
− 0
View file @ 6c156dca
File moved
src/solver-op-space.cpp
+ 675
− 662
View file @ 6c156dca
This diff is collapsed.
include/sot-dyninv/solver-op-space.h → src/solver-op-space.h
+ 160
− 0
View file @ 6c156dca
......@@ -42,116 +42,118 @@
#include <sot-dyninv/task-dyn-pd.h>
#include <soth/HCOD.hpp>
namespace sot {
namespace dyninv {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTSOLVEROPSPACE_EXPORT SolverOpSpace
:public ::dynamicgraph::Entity
,public ::dynamicgraph::EntityHelper<SolverOpSpace>
,public sot::Stack< TaskDynPD >
{
namespace dynamicgraph {
namespace sot {
namespace dyninv {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTSOLVEROPSPACE_EXPORT SolverOpSpace
:public ::dynamicgraph::Entity
,public ::dynamicgraph::EntityHelper<SolverOpSpace>
,public sot::Stack< TaskDynPD >
{
public: /* --- CONSTRUCTOR ---- */
public: /* --- CONSTRUCTOR ---- */
SolverOpSpace( const std::string & name );
SolverOpSpace( const std::string & name );
public: /* --- STACK INHERITANCE --- */
public: /* --- STACK INHERITANCE --- */
typedef sot::Stack<TaskDynPD> stack_t;
using stack_t::TaskDependancyList_t;
using stack_t::StackIterator_t;
using stack_t::StackConstIterator_t;
using stack_t::stack;
typedef sot::Stack<TaskDynPD> stack_t;
using stack_t::TaskDependancyList_t;
using stack_t::StackIterator_t;
using stack_t::StackConstIterator_t;
using stack_t::stack;
virtual TaskDependancyList_t getTaskDependancyList( const TaskDynPD& task );
virtual void addDependancy( const TaskDependancyList_t& depList );
virtual void removeDependancy( const TaskDependancyList_t& depList );
virtual void resetReady( void );
virtual TaskDependancyList_t getTaskDependancyList( const TaskDynPD& task );
virtual void addDependancy( const TaskDependancyList_t& depList );
virtual void removeDependancy( const TaskDependancyList_t& depList );
virtual void resetReady( void );
public: /* --- ENTITY INHERITANCE --- */
public: /* --- ENTITY INHERITANCE --- */
static const std::string CLASS_NAME;
virtual void display( std::ostream& os ) const;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
static const std::string CLASS_NAME;
virtual void display( std::ostream& os ) const;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
public: /* --- SIGNALS --- */
public: /* --- SIGNALS --- */
DECLARE_SIGNAL_IN(matrixInertia,ml::Matrix);
DECLARE_SIGNAL_IN(velocity,ml::Vector);
DECLARE_SIGNAL_IN(dyndrift,ml::Vector);
DECLARE_SIGNAL_IN(damping,double);
DECLARE_SIGNAL_IN(breakFactor,double);
DECLARE_SIGNAL_IN(matrixInertia,ml::Matrix);
DECLARE_SIGNAL_IN(velocity,ml::Vector);
DECLARE_SIGNAL_IN(dyndrift,ml::Vector);
DECLARE_SIGNAL_IN(damping,double);
DECLARE_SIGNAL_IN(breakFactor,double);
DECLARE_SIGNAL_OUT(control,ml::Vector);
DECLARE_SIGNAL_OUT(control,ml::Vector);
DECLARE_SIGNAL(zmp,OUT,ml::Vector);
DECLARE_SIGNAL(acceleration,OUT,ml::Vector);
DECLARE_SIGNAL(zmp,OUT,ml::Vector);
DECLARE_SIGNAL(acceleration,OUT,ml::Vector);
private: /* --- CONTACT POINTS --- */
private: /* --- CONTACT POINTS --- */
typedef boost::shared_ptr<dynamicgraph::SignalPtr<ml::Matrix,int> > matrixSINPtr;
typedef boost::shared_ptr<dynamicgraph::SignalPtr<ml::Vector,int> > vectorSINPtr;
typedef boost::shared_ptr<dynamicgraph::Signal<ml::Vector,int> > vectorSOUTPtr;
struct Contact
{
matrixSINPtr jacobianSIN;
matrixSINPtr JdotSIN;
matrixSINPtr supportSIN;
vectorSINPtr correctorSIN;
vectorSOUTPtr forceSOUT,fnSOUT;
std::pair<int,int> range;
};
typedef std::map< std::string,Contact > contacts_t;
contacts_t contactMap;
typedef boost::shared_ptr<dynamicgraph::SignalPtr<ml::Matrix,int> > matrixSINPtr;
typedef boost::shared_ptr<dynamicgraph::SignalPtr<ml::Vector,int> > vectorSINPtr;
typedef boost::shared_ptr<dynamicgraph::Signal<ml::Vector,int> > vectorSOUTPtr;
struct Contact
{
matrixSINPtr jacobianSIN;
matrixSINPtr JdotSIN;
matrixSINPtr supportSIN;
vectorSINPtr correctorSIN;
vectorSOUTPtr forceSOUT,fnSOUT;
std::pair<int,int> range;
};
typedef std::map< std::string,Contact > contacts_t;
contacts_t contactMap;
public:
void addContact( const std::string & name,
dynamicgraph::Signal<ml::Matrix,int> * jacobianSignal,
dynamicgraph::Signal<ml::Matrix,int> * JdotSignal,
dynamicgraph::Signal<ml::Vector,int> * corrSignal,
dynamicgraph::Signal<ml::Matrix,int> * contactPointsSignal );
void addContactFromTask( const std::string & taskName, const std::string & contactName );
void removeContact( const std::string & name );
void dispContacts( std::ostream& os ) const;
public:
void addContact( const std::string & name,
dynamicgraph::Signal<ml::Matrix,int> * jacobianSignal,
dynamicgraph::Signal<ml::Matrix,int> * JdotSignal,
dynamicgraph::Signal<ml::Vector,int> * corrSignal,
dynamicgraph::Signal<ml::Matrix,int> * contactPointsSignal );
void addContactFromTask( const std::string & taskName, const std::string & contactName );
void removeContact( const std::string & name );
void dispContacts( std::ostream& os ) const;
public: /* --- COMMANDS --- */
void debugOnce( void );
public: /* --- COMMANDS --- */
void debugOnce( void );
private: /* --- INTERNAL COMPUTATIONS --- */
void refreshTaskTime( int time );
void initialResizeSolver( void );
void resizeSolver( void );
private: /* --- INTERNAL COMPUTATIONS --- */
void refreshTaskTime( int time );
void initialResizeSolver( void );
void resizeSolver( void );
private:
int nbParam, nq,ntau;
int nbContactBodies,nbContactPoints,nfs;
private:
int nbParam, nq,ntau;
int nbContactBodies,nbContactPoints,nfs;
typedef boost::shared_ptr<soth::HCOD> hcod_ptr_t;
hcod_ptr_t hsolver;
typedef boost::shared_ptr<soth::HCOD> hcod_ptr_t;
hcod_ptr_t hsolver;
Eigen::MatrixXd Cdyn,Ccontact,Czmp,Czero;
soth::VectorBound bdyn,bcontact,bzmp,bzero;
std::vector< Eigen::MatrixXd > Ctasks;
std::vector< soth::VectorBound > btasks;
Eigen::MatrixXd Cdyn,Ccontact,Czmp,Czero;
soth::VectorBound bdyn,bcontact,bzmp,bzero;
std::vector< Eigen::MatrixXd > Ctasks;
std::vector< soth::VectorBound > btasks;
Eigen::VectorXd solution;
Eigen::VectorXd solution;
static const int nbPoint = 4;
static const int nbPoint = 4;
}; // class SolverOpSpace
}; // class SolverOpSpace
} // namespace dyninv
} // namespace sot
} // namespace dyninv
} // namespace sot
} // namespace dynamicgraph
......
src/stack-template.h 0 → 100644
+ 194
− 0
View file @ 6c156dca
/*
* 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_StackTemplate_H__
#define __sot_dyninv_StackTemplate_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (dynamic_interpretor_EXPORTS)
# define SOTSTACKTEMPLATE_EXPORT __declspec(dllexport)
# else
# define SOTSTACKTEMPLATE_EXPORT __declspec(dllimport)
# endif
#else
# define SOTSTACKTEMPLATE_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <sot-dyninv/signal-helper.h>
namespace dynamicgraph {
namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dg = dynamicgraph;
template< typename TaskGeneric >
class SOTSTACKTEMPLATE_EXPORT Stack
{
protected:
typedef std::vector<TaskGeneric*> Stack_t;
typedef typename Stack_t::iterator StackIterator_t;
typedef typename Stack_t::const_iterator StackConstIterator_t;
/*! \brief List of task (controllers) managed by the stack of tasks. */
Stack_t stack;
/*! \brief Number of joints to be used to compute the control law. */
int nbDofs;
public:
/*! \brief Number of joints by default. */
static const unsigned int NB_JOINTS_DEFAULT; // = 48;
public: /* --- Constructors --- */
Stack( void );
~Stack( void ) { /* TODO!! */ }
public: /* --- Modifiors --- */
/*! \name Methods to handle the stack.
@{
*/
/*! \brief Push the task in the stack.
It has a lowest priority than the previous ones.
If this is the first task, then it has the highest
priority. */
virtual void push( TaskGeneric& task );
/*! \brief Pop the task from the stack.
This method removes the task with the smallest
priority in the task. The other are projected
in the null-space of their predecessors. */
virtual TaskGeneric& pop( void );
/*! \brief This method allows to know if a task exists or not */
virtual bool exist( const TaskGeneric& task );
/*! \brief Remove a task regardless to its position in the stack.
It removes also the signals connected to the output signal of this stack.*/
virtual void remove( const TaskGeneric& task );
/*! \brief This method makes the task to swap with the task having the
immediate superior priority. */
virtual void up( const TaskGeneric& task );
/*! \brief This method makes the task to swap with the task having the
immediate inferior priority. */
virtual void down( const TaskGeneric& task );
/*! \brief Remove all the tasks from the stack. */
virtual void clear( void );
/*! Change the number of DOF, ie the field nbDofs. */
virtual void defineNbDof( const int& nbDof );
/*! @} */
dg::SignalBase<int>* tatata;
int titit;
typedef std::list< const dg::SignalBase<int>* > TaskDependancyList_t;
int totol;
/*! \brief Return the signal to be added/removed from the dependancy
* list of the control signal. */
virtual TaskDependancyList_t getTaskDependancyList( const TaskGeneric& task ) = 0;
virtual void addDependancy( const TaskDependancyList_t& depList ) = 0;
virtual void removeDependancy( const TaskDependancyList_t& depList ) = 0;
virtual void resetReady( void ) = 0;
public: /* --- DISPLAY --- */
/*! \name Methods to display the stack of tasks.
@{
*/
/*! Display the stack of tasks in text mode as a tree. */
virtual void display( std::ostream& os ) const;
/*! \brief Write the priority between tasks in the outstream os. */
virtual std::ostream &
writeGraph(const std::string & name, std::ostream & os) const;
/*! @} */
public: /* --- COMMANDS --- */
/*! @{ */
/* To use the macro, the typedef sot::Stack<TaskSpec> stack_t has to be set,
* as well as the EntityClassName. */
//#define ADD_COMMANDS_FOR_THE_STACK /// Code in the .t.cpp
/*! \brief This method deals with the command line.
The command given in argument is send to the stack of tasks by the shell.
The command understood by sot are:
<ul>
<li> Tasks
<ul>
<li> push <task> : Push a task in the stack (FILO).
<li> pop : Remove the task push in the stack.
<li> down <task> : Make the task have a higher priority, i.e.
swap with the task immediatly superior in priority.
<li> up <task> : Make the task have a lowest priority, i.e.
swap with the task immediatly inferior in priority.
<li> rm <task> : Remove the task from the stack.
</ul>
*/
virtual bool stackCommandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
void pushByTaskName( const std::string& taskName );
void removeByTaskName( const std::string& taskName );
void upByTaskName( const std::string& taskName );
void downByTaskName( const std::string& taskName );
/*! @} */
}; // class StackTemplate
template< typename TaskGeneric >
std::ostream&
operator<< ( std::ostream& os,const Stack<TaskGeneric>& sot )
{
sot.display(os);
return os;
}
} // namespace sot
} // namespace dynamicgraph
#include <sot-dyninv/stack-template.t.cpp>
#endif // #ifndef __sot_dyninv_StackTemplate_H__
src/stack-template.t.cpp 0 → 100644
+ 371
− 0
View file @ 6c156dca
This diff is collapsed.
src/task-dyn-pd.cpp
+ 150
− 148
View file @ 6c156dca
This diff is collapsed.
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