SCL
1.0
Standard Control Library : Control, dynamics, physics, and simulation
|
#include <STaskBase.hpp>
Public Member Functions | |
STaskBase () | |
virtual | ~STaskBase () |
bool | init (const std::string &arg_name, const std::string &arg_type, const sUInt arg_priority, const scl::sUInt arg_task_dof, const SRobotParsed *arg_robot_ds, const SGcModel *arg_gc_model, const Eigen::VectorXd &arg_kp, const Eigen::VectorXd &arg_kv, const Eigen::VectorXd &arg_ka, const Eigen::VectorXd &arg_ki, const Eigen::VectorXd &arg_ftask_max, const Eigen::VectorXd &arg_ftask_min, const std::vector< scl::sString2 > &arg_nonstd_params) |
bool | setParentController (const SControllerMultiTask *arg_parent) |
virtual bool | initTaskParams ()=0 |
virtual const std::string & | getType () const |
virtual const std::string & | getName () const |
virtual bool | hasBeenInit () const |
Public Attributes | |
const SControllerMultiTask * | parent_controller_ |
std::string | type_task_ |
scl::sBool | has_been_activated_ |
scl::sBool | has_control_null_space_ |
sUInt | priority_ |
scl::sUInt | dof_task_ |
const SRobotParsed * | robot_ |
const SGcModel * | gc_model_ |
Eigen::MatrixXd | J_ |
Eigen::MatrixXd | J_6_ |
Eigen::MatrixXd | J_dyn_inv_ |
Eigen::MatrixXd | null_space_ |
Eigen::MatrixXd | M_task_ |
Eigen::MatrixXd | M_task_inv_ |
Eigen::MatrixXd | force_task_cc_ |
Eigen::MatrixXd | force_task_grav_ |
Eigen::VectorXd | force_task_ |
Eigen::VectorXd | force_task_max_ |
Eigen::VectorXd | force_task_min_ |
Eigen::VectorXd | force_gc_ |
Eigen::MatrixXd | range_space_ |
Eigen::VectorXd | kp_ |
Eigen::VectorXd | kv_ |
Eigen::VectorXd | ka_ |
Eigen::VectorXd | ki_ |
std::vector< sString2 > | task_nonstd_params_ |
Eigen::VectorXd | shared_data_ |
std::string | name_ |
sBool | has_been_init_ |
Protected Attributes | |
std::string | type_ |
Contains all the data required to compute a task
A task computes forces/torques in any arbitrary space.
For instance an Euclidean task would do so in Euclidean space. A joint space task would do so in joint space. Any arbitrary space works as long as it can be mapped to the output joint space..
NOTE : YOU CAN NOT USE THIS TASK DATA STRUCTURE DIRECTLY. You must subclass it, re-implement the function: virtual bool initTaskParams() to parse all the custom parameters (from the xml file) stored in task_nonstd_params_. The function must then return true
scl::STaskBase::STaskBase | ( | ) |
Constructor
|
inlinevirtual |
Destructor : Does nothing
|
inlinevirtualinherited |
Get the object's type
|
inlinevirtualinherited |
Get the object's type
|
inlinevirtualinherited |
Get the object's type
bool scl::STaskBase::init | ( | const std::string & | arg_name, |
const std::string & | arg_type, | ||
const sUInt | arg_priority, | ||
const scl::sUInt | arg_task_dof, | ||
const SRobotParsed * | arg_robot_ds, | ||
const SGcModel * | arg_gc_model, | ||
const Eigen::VectorXd & | arg_kp, | ||
const Eigen::VectorXd & | arg_kv, | ||
const Eigen::VectorXd & | arg_ka, | ||
const Eigen::VectorXd & | arg_ki, | ||
const Eigen::VectorXd & | arg_ftask_max, | ||
const Eigen::VectorXd & | arg_ftask_min, | ||
const std::vector< scl::sString2 > & | arg_nonstd_params | ||
) |
Initialization function
NOTE : This function also activates the task.
arg_task_dof | 0 task dof means a gc task. Ie. full dofs |
arg_nonstd_params | These are ignored during STaskBase initialization. However, subclasses may choose to use them and/or require various values |
|
pure virtual |
Processes the task's non standard parameters, which the init() function stores in the task_nonstd_params_.
This function is called by init() and must be implemented by all subclasses. Else it will be impossible to initialize the task. Ie. init() will always return false.
Implemented in scl::STaskOpPosPIDA1OrderInfTime, scl::STaskOpPos, scl::STaskOpPosContactForce, scl::STaskComPos, scl::STaskParsedData, scl::STaskGc, scl::STaskGcLimitCentering, scl::STaskGcSet, and scl::STaskNullSpaceDamping.
bool scl::STaskBase::setParentController | ( | const SControllerMultiTask * | arg_parent | ) |
Sets the parent controller
scl::sUInt scl::STaskBase::dof_task_ |
Task space degrees of freedom
Eigen::VectorXd scl::STaskBase::force_gc_ |
Generalized coordinate forces (usually joint torques) : Computed by : the task-servo = J' * f Used by : the robot-servo to calculate overall generalized coordinate force
Eigen::VectorXd scl::STaskBase::force_task_ |
Task space forces Computed by : the task-servo Used by : the task-servo to compute the force_gc_
Eigen::MatrixXd scl::STaskBase::force_task_cc_ |
Task-space centrifugal/coriolis force vector Controls cc forces in a subspace of the entire gc space so that the operational point experience no cc forces. Other points, however, do.
Eigen::MatrixXd scl::STaskBase::force_task_grav_ |
Task-space gravity force vector. Controls gravity in a subspace of the entire gc space so that the operational point is massless. Other points experience gravity.
Eigen::VectorXd scl::STaskBase::force_task_max_ |
Upper and lower limits of task-space forces (Why separate : Consider muscles which can only pull, not push)
const SGcModel* scl::STaskBase::gc_model_ |
Robot generalized coordinate model
scl::sBool scl::STaskBase::has_been_activated_ |
Whether the task is active or inactive Default = false. True after init()
|
inherited |
Whether the object is ready for use
scl::sBool scl::STaskBase::has_control_null_space_ |
Is control task. This variable controls whether the controller computes remaining null spaces for lower level tasks or not. If it is true, the lower level task null spaces are not computed, potentially improving performance substantially.
Settings: True : Task dof < Robot dof [Default] False : Task dof == Robot dof [Must enable manually in init]
Eigen::MatrixXd scl::STaskBase::J_ |
The computed task space Jacobian matrices. Used for velocities. Note that we compute the generalized inverse (the Jacobian is usually not full rank) without inverting the jacobian itself.
Eigen::VectorXd scl::STaskBase::kp_ |
Gains (scalar if same for different dimensions; vector if different for different dimensions) kp = position, kv = velocity, ka = acceleration ki = integrated position error (to fix steady-state errors).
Computed by : None. These are constants. Used by : The task-servo to calculate task forces
Eigen::MatrixXd scl::STaskBase::M_task_ |
Task-space mass matrix
|
inherited |
The object's name
Eigen::MatrixXd scl::STaskBase::null_space_ |
The task's null space. Used to compute the range space of lower priority tasks.
const SControllerMultiTask* scl::STaskBase::parent_controller_ |
The parent controller
sUInt scl::STaskBase::priority_ |
The task's priority. Important to determine its range space. Lower is better. 0 is best. -1 is NULL.
Eigen::MatrixXd scl::STaskBase::range_space_ |
Range space of the task : Computed by : the robot-servo to calculate overall generalized coordinate force Used by : the robot-servo to calculate overall generalized coordinate force
const SRobotParsed* scl::STaskBase::robot_ |
Robot model. Not a const because it could use the branching representation's iterator
Eigen::VectorXd scl::STaskBase::shared_data_ |
This may be used by any shared module in scl that wants to communicate arbitrary information to a STaskBase subclass
std::vector<sString2> scl::STaskBase::task_nonstd_params_ |
A set of additional options that may be used by users to initialize this specific task. These typically go above and beyond the standard options.
Eg.The parent link and the pos in parent, which are required by op point tasks but not by gc tasks. These will be stored like: task_options_[0].data_[0] = "parent_link"; task_options_[0].data_[1] = "base"; task_options_[1].data_[0] = "pos_in_parent"; task_options_[1].data_[1] = "0.0 0.0 0.0"; task_options_[2].data_[0] = "my_new_arbitrary_option"; task_options_[2].data_[1] = "8080";
|
protectedinherited |
The object's type. Should only be set by the constructor
std::string scl::STaskBase::type_task_ |
The type of the task