class
#include <BipedalLocomotion/TSID/SE3Task.h>
SE3Task SE3Task is a concrete implementation of the OptimalControlElement.
Please use this element if you want to control the position and the orientation of a given frame rigidly attached to the robot. The task assumes perfect control of the robot acceleration that contains the base linear and angular acceleration expressed in mixed representation and the joints acceleration. The task represents the following equation
where is the robot jacobian and is the desired acceleration. The desired acceleration is chosen such that the frame will asymptotically converge to the desired trajectory. The linear component of is computed with a standard PD controller in while the angular acceleration is computed by a PD controller in .
Base classes
- struct TSIDLinearTask
- TSIDLinearTask specializes a LinearTask in the case of Task based Inverse Dynamics.
- struct BipedalLocomotion::System::ITaskControllerManager
- ITaskControllerManager is an interface that can help you to handle tasks containing controllers.
Public types
Public functions
-
auto initialize(std::weak_ptr<const ParametersHandler::
IParametersHandler> paramHandler) -> bool override - Initialize the planner.
- auto setKinDyn(std::shared_ptr<iDynTree::KinDynComputations> kinDyn) -> bool
- Set the kinDynComputations object.
-
auto setVariablesHandler(const System::
VariablesHandler& variablesHandler) -> bool override - Set the set of variables required by the task.
- auto update() -> bool override
- Update the content of the element.
- auto setSetPoint(const manif::SE3d& I_H_F, const manif::SE3d::Tangent& mixedVelocity, const manif::SE3d::Tangent& mixedAcceleration) -> bool
- Set the desired reference trajectory.
- auto size() const -> std::size_t override
- Get the size of the task.
- auto type() const -> Type override
- The SE3Task is an equality task.
- auto isValid() const -> bool override
- Determines the validity of the objects retrieved with getA() and getB()
- void setTaskControllerMode(Mode mode) override
- Set the task controller mode.
- auto getTaskControllerMode() const -> Mode override
- Get the task controller mode.
- auto getControllerOutput() const -> Eigen::Ref<const Eigen::VectorXd>
- Get the controller output.
Function documentation
bool BipedalLocomotion:: TSID:: SE3Task:: initialize(std::weak_ptr<const ParametersHandler:: IParametersHandler> paramHandler) override
Initialize the planner.
Parameters | |
---|---|
paramHandler | pointer to the parameters handler. |
Returns | True in case of success, false otherwise. |
bool BipedalLocomotion:: TSID:: SE3Task:: setKinDyn(std::shared_ptr<iDynTree::KinDynComputations> kinDyn)
Set the kinDynComputations object.
Parameters | |
---|---|
kinDyn | pointer to a kinDynComputations object. |
Returns | True in case of success, false otherwise. |
bool BipedalLocomotion:: TSID:: SE3Task:: setVariablesHandler(const System:: VariablesHandler& variablesHandler) override
Set the set of variables required by the task.
Parameters | |
---|---|
variablesHandler | reference to a variables handler. |
Returns | True in case of success, false otherwise. |
The variables are stored in the System::
bool BipedalLocomotion:: TSID:: SE3Task:: update() override
Update the content of the element.
Returns | True in case of success, false otherwise. |
---|
bool BipedalLocomotion:: TSID:: SE3Task:: setSetPoint(const manif::SE3d& I_H_F,
const manif::SE3d::Tangent& mixedVelocity,
const manif::SE3d::Tangent& mixedAcceleration)
Set the desired reference trajectory.
Parameters | |
---|---|
I_H_F | Homogeneous transform between the link and the inertial frame. |
mixedVelocity | 6D-velocity written in mixed representation. |
mixedAcceleration | 6D-acceleration written in mixed representation. |
Returns | True in case of success, false otherwise. |
std::size_t BipedalLocomotion:: TSID:: SE3Task:: size() const override
Get the size of the task.
Returns | the size of the task. |
---|
(I.e the number of rows of the vector b)
void BipedalLocomotion:: TSID:: SE3Task:: setTaskControllerMode(Mode mode) override
Set the task controller mode.
Please use this method to disable/enable the Proportional Derivative controller implemented in this task.
Mode BipedalLocomotion:: TSID:: SE3Task:: getTaskControllerMode() const override
Get the task controller mode.
Returns | the state of the controller |
---|
Eigen::Ref<const Eigen::VectorXd> BipedalLocomotion:: TSID:: SE3Task:: getControllerOutput() const
Get the controller output.
Returns | a const reference to the controller output. |
---|