dynamicgraph::sot::Sot Class Reference

This class implements the Stack of Task. It allows to deal with the priority of the controllers through the shell. The controllers can be either constraints either tasks. More...

#include <sot/core/sot.hh>

Inheritance diagram for dynamicgraph::sot::Sot:
[legend]

Public Member Functions

virtual const std::string & getClassName () const
 Returns the name of this class. More...
 
 Sot (const std::string &name)
 Default constructor. More...
 
 ~Sot (void)
 
virtual void defineFreeFloatingJoints (const unsigned int &jointIdFirst, const unsigned int &jointIdLast=-1)
 This method defines the part of the state vector which correspond to the free flyer of the robot. More...
 
virtual void defineNbDof (const unsigned int &nbDof)
 
virtual void commandLine (const std::string &cmdLine, std::istringstream &cmdArgs, std::ostream &os)
 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: More...
 
virtual std::ostream & writeGraph (std::ostream &os) const
 This method write the priority between tasks in the output stream os. More...
 
Methods to handle the stack.
virtual void push (TaskAbstract &task)
 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. More...
 
virtual TaskAbstractpop (void)
 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. More...
 
virtual bool exist (const TaskAbstract &task)
 This method allows to know if a task exists or not. More...
 
virtual void remove (const TaskAbstract &task)
 Remove a task regardless to its position in the stack. It removes also the signals connected to the output signal of this stack. More...
 
virtual void removeDependency (const TaskAbstract &key)
 This method removes the output signals depending on this task. More...
 
virtual void up (const TaskAbstract &task)
 This method makes the task to swap with the task having the immediate superior priority. More...
 
virtual void down (const TaskAbstract &task)
 This method makes the task to swap with the task having the immediate inferior priority. More...
 
virtual void clear (void)
 Remove all the tasks from the stack. More...
 
Methods to handle the constraints.
virtual void addConstraint (Constraint &constraint)
 Add a constraint to the stack with the current level of priority. More...
 
virtual void removeConstraint (const Constraint &constraint)
 Remove a constraint from the stack. More...
 
virtual void clearConstraint (void)
 Remove all the constraints from the stack. More...
 
Methods to compute the control law following the

recursive definition of the stack of tasks.

virtual ml::VectorcomputeControlLaw (ml::Vector &control, const int &time)
 Compute the control law. More...
 
virtual ml::MatrixcomputeConstraintProjector (ml::Matrix &Proj, const int &time)
 Compute the projector of the constraint. More...
 
- Public Member Functions inherited from dynamicgraph::Entity
 Entity (const std::string &name)
 
virtual ~Entity ()
 
const std::string & getName () const
 
virtual std::string getDocString () const
 
bool hasSignal (const std::string &signame) const
 
SignalBase< int > & getSignal (const std::string &signalName)
 
const SignalBase< int > & getSignal (const std::string &signalName) const
 
std::ostream & displaySignalList (std::ostream &os) const
 
virtual std::ostream & writeCompletionList (std::ostream &os) const
 
virtual SignalBase< int > * test ()
 
virtual void test2 (SignalBase< int > *)
 
const std::string & getCommandList () const
 
CommandMap_t getNewStyleCommandMap ()
 
command::CommandgetNewStyleCommand (const std::string &cmdName)
 
SignalMap getSignalMap () const
 

Static Public Member Functions

static ml::MatrixcomputeJacobianConstrained (const ml::Matrix &Jac, const ml::Matrix &K, ml::Matrix &JK, ml::Matrix &Jff, ml::Matrix &Jact)
 
static ml::MatrixcomputeJacobianConstrained (const TaskAbstract &task, const ml::Matrix &K)
 
static ml::Vector taskVectorToMlVector (const VectorMultiBound &taskVector)
 

Public Attributes

Methods to handle signals
SignalPtr< ml::Vector, int > q0SIN
 Intrinsec velocity of the robot, that is used to initialized the recurence of the SOT (e.g. velocity comming from the other OpenHRP plugins). More...
 
SignalPtr< double, int > inversionThresholdSIN
 This signal allow to change the threshold for the damped pseudo-inverse on-line. More...
 
SignalTimeDependent< ml::Matrix, int > constraintSOUT
 Allow to get the result of the Constraint projector. More...
 
SignalTimeDependent< ml::Vector, int > controlSOUT
 Allow to get the result of the computed control law. More...
 

Static Public Attributes

static const std::string CLASS_NAME
 Specify the name of the class entity. More...
 
static const double INVERSION_THRESHOLD_DEFAULT
 Threshold to compute the dumped pseudo inverse. More...
 
static const unsigned int NB_JOINTS_DEFAULT
 Number of joints by default. More...
 

Protected Types

typedef std::list< TaskAbstract * > StackType
 Defines a type for a list of tasks. More...
 
typedef std::list< Constraint * > ConstraintListType
 Defines a type for a list of constraints. More...
 

Protected Attributes

StackType stack
 This field is a list of controllers managed by the stack of tasks. More...
 
ConstraintListType constraintList
 This field is a list of constraints managed by the stack of tasks. More...
 
unsigned int ffJointIdFirst
 Defines an interval in the state vector of the robot which is the free flyer. More...
 
unsigned int ffJointIdLast
 
unsigned int nbJoints
 Store the number of joints to be used in the command computed by the stack of tasks. More...
 
TaskAbstracttaskGradient
 Store a pointer to compute the gradient. More...
 
ml::Matrix Proj
 
bool recomputeEachTime
 
- Protected Attributes inherited from dynamicgraph::Entity
std::string name
 
SignalMap signalMap
 
CommandMap_t commandMap
 

Static Protected Attributes

static const unsigned int FF_JOINT_ID_DEFAULT = 0
 Defines a default joint. More...
 

Methods to display the stack of tasks.

virtual void display (std::ostream &os) const
 
SOTSOT_CORE_EXPORT friend std::ostream & operator<< (std::ostream &os, const Sot &sot)
 

Additional Inherited Members

- Public Types inherited from dynamicgraph::Entity
typedef std::map< std::string, SignalBase< int > * > SignalMap
 
typedef std::map< const std::string, command::Command *> CommandMap_t
 
- Protected Member Functions inherited from dynamicgraph::Entity
void addCommand (const std::string &name, command::Command *command)
 
void entityRegistration ()
 
void entityDeregistration ()
 
void signalRegistration (const SignalArray< int > &signals)
 
void signalDeregistration (const std::string &name)
 

Detailed Description

This class implements the Stack of Task. It allows to deal with the priority of the controllers through the shell. The controllers can be either constraints either tasks.

Member Typedef Documentation

◆ ConstraintListType

Defines a type for a list of constraints.

◆ StackType

typedef std::list<TaskAbstract*> dynamicgraph::sot::Sot::StackType
protected

Defines a type for a list of tasks.

Constructor & Destructor Documentation

◆ Sot()

dynamicgraph::sot::Sot::Sot ( const std::string &  name)

Default constructor.

◆ ~Sot()

dynamicgraph::sot::Sot::~Sot ( void  )
inline

Member Function Documentation

◆ addConstraint()

virtual void dynamicgraph::sot::Sot::addConstraint ( Constraint constraint)
virtual

Add a constraint to the stack with the current level of priority.

◆ clear()

virtual void dynamicgraph::sot::Sot::clear ( void  )
virtual

Remove all the tasks from the stack.

◆ clearConstraint()

virtual void dynamicgraph::sot::Sot::clearConstraint ( void  )
virtual

Remove all the constraints from the stack.

◆ commandLine()

virtual void dynamicgraph::sot::Sot::commandLine ( const std::string &  cmdLine,
std::istringstream &  cmdArgs,
std::ostream &  os 
)
virtual

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:

  • Tasks
    • push <task> : Push a task in the stack (FILO).
    • pop : Remove the task push in the stack.
    • down <task> : Make the task have a higher priority, i.e. swap with the task immediatly superior in priority.
    • up <task> : Make the task have a lowest priority, i.e. swap with the task immediatly inferior in priority.
    • rm <task> : Remove the task from the stack.
  • Constraints
    • addConstraint <constraint> : Add the constraint in the stack (FILO).
    • rmConstraint <constraint> : Remove the constraint.
    • clearConstraint : Remove all the constraints.
    • printConstraint :

Reimplemented from dynamicgraph::Entity.

Reimplemented in dynamicgraph::sot::SotH.

◆ computeConstraintProjector()

virtual ml::Matrix& dynamicgraph::sot::Sot::computeConstraintProjector ( ml::Matrix Proj,
const int &  time 
)
virtual

Compute the projector of the constraint.

◆ computeControlLaw()

virtual ml::Vector& dynamicgraph::sot::Sot::computeControlLaw ( ml::Vector control,
const int &  time 
)
virtual

Compute the control law.

Reimplemented in dynamicgraph::sot::SotH.

◆ computeJacobianConstrained() [1/2]

static ml::Matrix& dynamicgraph::sot::Sot::computeJacobianConstrained ( const ml::Matrix Jac,
const ml::Matrix K,
ml::Matrix JK,
ml::Matrix Jff,
ml::Matrix Jact 
)
static

◆ computeJacobianConstrained() [2/2]

static ml::Matrix& dynamicgraph::sot::Sot::computeJacobianConstrained ( const TaskAbstract task,
const ml::Matrix K 
)
static

◆ defineFreeFloatingJoints()

virtual void dynamicgraph::sot::Sot::defineFreeFloatingJoints ( const unsigned int &  jointIdFirst,
const unsigned int &  jointIdLast = -1 
)
virtual

This method defines the part of the state vector which correspond to the free flyer of the robot.

◆ defineNbDof()

virtual void dynamicgraph::sot::Sot::defineNbDof ( const unsigned int &  nbDof)
virtual

Reimplemented in dynamicgraph::sot::SotH.

◆ display()

virtual void dynamicgraph::sot::Sot::display ( std::ostream &  os) const
virtual

Display the stack of tasks in text mode as a tree.

Reimplemented from dynamicgraph::Entity.

◆ down()

virtual void dynamicgraph::sot::Sot::down ( const TaskAbstract task)
virtual

This method makes the task to swap with the task having the immediate inferior priority.

◆ exist()

virtual bool dynamicgraph::sot::Sot::exist ( const TaskAbstract task)
virtual

This method allows to know if a task exists or not.

◆ getClassName()

virtual const std::string& dynamicgraph::sot::Sot::getClassName ( void  ) const
inlinevirtual

Returns the name of this class.

Implements dynamicgraph::Entity.

Reimplemented in dynamicgraph::sot::WeightedSot, and dynamicgraph::sot::SotH.

◆ pop()

virtual TaskAbstract& dynamicgraph::sot::Sot::pop ( void  )
virtual

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.

◆ push()

virtual void dynamicgraph::sot::Sot::push ( TaskAbstract task)
virtual

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.

◆ remove()

virtual void dynamicgraph::sot::Sot::remove ( const TaskAbstract task)
virtual

Remove a task regardless to its position in the stack. It removes also the signals connected to the output signal of this stack.

◆ removeConstraint()

virtual void dynamicgraph::sot::Sot::removeConstraint ( const Constraint constraint)
virtual

Remove a constraint from the stack.

◆ removeDependency()

virtual void dynamicgraph::sot::Sot::removeDependency ( const TaskAbstract key)
virtual

This method removes the output signals depending on this task.

◆ taskVectorToMlVector()

static ml::Vector dynamicgraph::sot::Sot::taskVectorToMlVector ( const VectorMultiBound taskVector)
static

◆ up()

virtual void dynamicgraph::sot::Sot::up ( const TaskAbstract task)
virtual

This method makes the task to swap with the task having the immediate superior priority.

◆ writeGraph()

virtual std::ostream& dynamicgraph::sot::Sot::writeGraph ( std::ostream &  os) const
virtual

This method write the priority between tasks in the output stream os.

Reimplemented from dynamicgraph::Entity.

Friends And Related Function Documentation

◆ operator<<

SOTSOT_CORE_EXPORT friend std::ostream& operator<< ( std::ostream &  os,
const Sot sot 
)
friend

Wrap the previous method around an operator.

Member Data Documentation

◆ CLASS_NAME

const std::string dynamicgraph::sot::Sot::CLASS_NAME
static

Specify the name of the class entity.

◆ constraintList

ConstraintListType dynamicgraph::sot::Sot::constraintList
protected

This field is a list of constraints managed by the stack of tasks.

◆ constraintSOUT

SignalTimeDependent<ml::Matrix,int> dynamicgraph::sot::Sot::constraintSOUT

Allow to get the result of the Constraint projector.

◆ controlSOUT

SignalTimeDependent<ml::Vector,int> dynamicgraph::sot::Sot::controlSOUT

Allow to get the result of the computed control law.

◆ FF_JOINT_ID_DEFAULT

const unsigned int dynamicgraph::sot::Sot::FF_JOINT_ID_DEFAULT = 0
staticprotected

Defines a default joint.

◆ ffJointIdFirst

unsigned int dynamicgraph::sot::Sot::ffJointIdFirst
protected

Defines an interval in the state vector of the robot which is the free flyer.

◆ ffJointIdLast

unsigned int dynamicgraph::sot::Sot::ffJointIdLast
protected

◆ INVERSION_THRESHOLD_DEFAULT

const double dynamicgraph::sot::Sot::INVERSION_THRESHOLD_DEFAULT
static

Threshold to compute the dumped pseudo inverse.

◆ inversionThresholdSIN

SignalPtr<double,int> dynamicgraph::sot::Sot::inversionThresholdSIN

This signal allow to change the threshold for the damped pseudo-inverse on-line.

◆ NB_JOINTS_DEFAULT

const unsigned int dynamicgraph::sot::Sot::NB_JOINTS_DEFAULT
static

Number of joints by default.

◆ nbJoints

unsigned int dynamicgraph::sot::Sot::nbJoints
protected

Store the number of joints to be used in the command computed by the stack of tasks.

◆ Proj

ml::Matrix dynamicgraph::sot::Sot::Proj
protected

Projection used to compute the control law.

◆ q0SIN

SignalPtr<ml::Vector,int> dynamicgraph::sot::Sot::q0SIN

Intrinsec velocity of the robot, that is used to initialized the recurence of the SOT (e.g. velocity comming from the other OpenHRP plugins).

◆ recomputeEachTime

bool dynamicgraph::sot::Sot::recomputeEachTime
protected

Force the recomputation at each step.

◆ stack

StackType dynamicgraph::sot::Sot::stack
protected

This field is a list of controllers managed by the stack of tasks.

◆ taskGradient

TaskAbstract* dynamicgraph::sot::Sot::taskGradient
protected

Store a pointer to compute the gradient.