ChppGikGenericTask Class Reference

This object produces whole body joint motion based on entered "elements".These elements are cartesian motion planners of class ChppGikPrioritizedMotion or class ChppGikLocomotionElement. More...

#include <hpp/gik/task/generic-task.hh>

Inheritance diagram for ChppGikGenericTask:
Collaboration diagram for ChppGikGenericTask:

Public Member Functions

 ChppGikGenericTask (ChppGikStandingRobot *inStandingRobot, double inSamplingPeriod)
 constructor More...
 
bool addElement (ChppGikPrioritizedMotion *inElement)
 add a prioritized motion (like ChppGikInterpolatedElement or ChppGikReadyElement) More...
 
bool addElement (ChppGikLocomotionElement *inElement)
 add a locomotion More...
 
void clearElements ()
 Clear all the entered elements. More...
 
void extraEndTime (double inDuration)
 modify extra end time (use with caution, make sure there is at least one second of planned motion after the endtime of the last planned stepElement) More...
 
void dynamicWeights (bool inSwitch)
 Activate/disactivate different inverse kinematics weights according to support polygon type (single or double). More...
 
void neutralUpperBody (bool inSwitch)
 True = privelege to lighter joints in upper body. More...
 
void automaticJointsMask (bool inSwitch, const vectorN *inMask=0)
 True = use the given joints mask. More...
 
void bringBackZMP (bool inChoice, double inStartTime, double inDuration)
 inherited More...
 
 ~ChppGikGenericTask ()
 Destructor. More...
 
bool filterZmpError ()
 Filter zmp error. More...
 
- Public Member Functions inherited from ChppGikRobotTask
 ChppGikRobotTask (ChppGikStandingRobot *inStandingRobot, double inSamplingPeriod, const char *inTaskName)
 Constructor. More...
 
bool solve ()
 Solve for joint motion. More...
 
void showResolutionTime (bool inSwitch)
 show the resolution time More...
 
ChppRobotMotionsolutionMotion () const
 Get the computed solution motion. More...
 
virtual ~ChppGikRobotTask ()
 destructor More...
 

Protected Member Functions

void computeGikWeights (double inTime, const vectorN &inActiveJoints, vectorN &outGikWeights)
 
- Protected Member Functions inherited from ChppGikRobotTask
void cleanUp ()
 Delete objects created in this instance. More...
 
void backupRobot ()
 Backup current robot configuration (q, dot q, ddot q) More...
 
void restoreRobot ()
 Restore saved robot configuration (q, dot q, ddot q) and compute forward kinematics. More...
 
void cropMotion (ChppGikRobotTask *inRobotTask)
 concatanate a task solution motion to the current attSolutionMotion and apply the last solution configuration to the robot. More...
 

Protected Attributes

CjrlHumanoidDynamicRobot * attRobot
 
ChppGikMotionPlanattMotionPlan
 
ChppGikLocomotionPlanattLocomotionPlan
 
ChppGikSolverattGikSolver
 
- Protected Attributes inherited from ChppGikRobotTask
ChppGikStandingRobotattStandingRobot
 Associated robot. More...
 
double attSamplingPeriod
 Motion sampling period and its half. More...
 
double attEps
 
double attStartTime
 Start time. More...
 
bool attShowTime
 option to show resolution time More...
 
ChppRobotMotionattSolutionMotion
 The solution joint motion. More...
 
std::string attTaskName
 This task's name. More...
 
CjrlRobotConfiguration attInitialConfiguration
 Backup of robot configuration before task solving {. More...
 
struct timeval * Tps
 timimg {@ More...
 
struct timeval * Tpf
 
struct timezone * Tzp
 

Detailed Description

This object produces whole body joint motion based on entered "elements".These elements are cartesian motion planners of class ChppGikPrioritizedMotion or class ChppGikLocomotionElement.

Special attention must be paid when adding ChppGikLocomotionElement objects:
- Their time frames should not be overlapping, otherwise the overlapping Element is discarded
- The first ChppGikLocomotionElement will generate 1.6 seconds of motion before its actual start time, the last ChppGikLocomotionElement will generate 1.0 seconds of motion after its end. This is due to the preview controller. For example: the user enters a ChppGikStepElement that starts at time 0.0s and finishes at time 2.0s. Then the motion time scale will begin at time -1.6s to end at time 3.0s.
The first 1.6 second duration cannot be changed, but the latter 1.0s can be modified (with caution) through method ChppGikGenericTask::extraEndTime(). This extra 1.0 second is used to allow the center of mass of the robot to finish its motion.

Example of situation where forgetting these time extensions lead to unsolved problems (due to ZMP out of Support Polygon notifications):
Initial configuration is HalfSitting, the waist of the robot is horizontal, parallel to the flat ground. The user adds a ChppGikPrioritizedMotion to maintain the waist horizontality between time 0.0 and time 2.0. he also adds a ChppGikStepElement to make a step between time 0.0 and time 2.0.
Calling method ChppGikGenericTask::solve() will likely provoke "ZMP out" at time 0.0. This is because the center of mass starts moving due to the Preview Controller between times -1.6s and 0.0 such that at time 0.0, a discontinous constraint on the waist horizontality is suddenly added in the stack of tasks, and provokes large accelerations, and "ZMP out". Moreover, after time 2.0s, the constraint on the waist is no longer included in the stack of tasks, and while the center of mass continues to move during the extra end time of 1.0second, the constraint to be maintained for the waist does not hold anymore at the end the motion (time 3.0s).
The correct way to program such a scenario would be to set a waist ChppGikPrioritizedMotion between time 0.0 and time 4.6s, and to set the ChppGikStepElement to begin at time 1.6s and end at time 1.6+2.0=3.6s.

Constructor & Destructor Documentation

◆ ChppGikGenericTask()

ChppGikGenericTask::ChppGikGenericTask ( ChppGikStandingRobot inStandingRobot,
double  inSamplingPeriod 
)

constructor

◆ ~ChppGikGenericTask()

ChppGikGenericTask::~ChppGikGenericTask ( )

Destructor.

Member Function Documentation

◆ addElement() [1/2]

bool ChppGikGenericTask::addElement ( ChppGikPrioritizedMotion inElement)

add a prioritized motion (like ChppGikInterpolatedElement or ChppGikReadyElement)

◆ addElement() [2/2]

bool ChppGikGenericTask::addElement ( ChppGikLocomotionElement inElement)

add a locomotion

◆ automaticJointsMask()

void ChppGikGenericTask::automaticJointsMask ( bool  inSwitch,
const vectorN inMask = 0 
)

True = use the given joints mask.

False = automatically computed joints mask.

Note
Default is False

◆ bringBackZMP()

void ChppGikGenericTask::bringBackZMP ( bool  inChoice,
double  inStartTime,
double  inDuration 
)
virtual

◆ clearElements()

void ChppGikGenericTask::clearElements ( )

Clear all the entered elements.

◆ computeGikWeights()

void ChppGikGenericTask::computeGikWeights ( double  inTime,
const vectorN inActiveJoints,
vectorN outGikWeights 
)
protected

◆ dynamicWeights()

void ChppGikGenericTask::dynamicWeights ( bool  inSwitch)

Activate/disactivate different inverse kinematics weights according to support polygon type (single or double).

Default is Enabled.

◆ extraEndTime()

void ChppGikGenericTask::extraEndTime ( double  inDuration)

modify extra end time (use with caution, make sure there is at least one second of planned motion after the endtime of the last planned stepElement)

◆ filterZmpError()

bool ChppGikGenericTask::filterZmpError ( )

Filter zmp error.

◆ neutralUpperBody()

void ChppGikGenericTask::neutralUpperBody ( bool  inSwitch)

True = privelege to lighter joints in upper body.

False = no privelege.

Note
Default value is False

Member Data Documentation

◆ attGikSolver

ChppGikSolver* ChppGikGenericTask::attGikSolver
protected

◆ attLocomotionPlan

ChppGikLocomotionPlan* ChppGikGenericTask::attLocomotionPlan
protected

◆ attMotionPlan

ChppGikMotionPlan* ChppGikGenericTask::attMotionPlan
protected

◆ attRobot

CjrlHumanoidDynamicRobot* ChppGikGenericTask::attRobot
protected