wbc
Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
wbc::CartesianPosPDController Class Reference

The CartesianPosPDController class implements a PD Controller with feed forward on the RigidBodyStateSE3 type. The following control schemes are available: More...

#include <CartesianPosPDController.hpp>

Inheritance diagram for wbc::CartesianPosPDController:
wbc::PosPDController

Public Member Functions

 CartesianPosPDController ()
 
const base::samples::RigidBodyStateSE3 & update (const base::samples::RigidBodyStateSE3 &setpoint, const base::samples::RigidBodyStateSE3 &feedback)
 
- Public Member Functions inherited from wbc::PosPDController
 PosPDController (size_t dim_controller)
 
void update ()
 
void setPGain (const base::VectorXd &gain)
 
const base::VectorXd & pGain ()
 
void setDGain (const base::VectorXd &gain)
 
const base::VectorXd & dGain ()
 
void setFFGain (const base::VectorXd &gain)
 
const base::VectorXd & ffGain ()
 
void setMaxCtrlOutput (const base::VectorXd &max_ctrl_out)
 
const base::VectorXd & maxCtrlOutput ()
 
void setDeadZone (const base::VectorXd &dz)
 
const base::VectorXd & deadZone ()
 
base::VectorXd getControlError ()
 
void applySaturation (const base::VectorXd &in, const base::VectorXd &max, base::VectorXd &out)
 Apply Saturation on the control output. If one or more values of <in> are bigger than the Corrresponding entry of <max>, all values will be scaled down according to the biggest ratio eta = in_i / max,i.
 
void applyDeadZone (const base::VectorXd &in, const base::VectorXd &min, base::VectorXd &out)
 Apply dead zone, i.e. minimum position control error. If one of the input value falls below minimum, it will be set to zero. Otherwise dead zone will be subtracted from the input value, to get a smooth transition:
 

Protected Member Functions

void extractFeedback (const base::samples::RigidBodyStateSE3 &feedback)
 
void extractSetpoint (const base::samples::RigidBodyStateSE3 &setpoint, const base::samples::RigidBodyStateSE3 &feedback)
 

Protected Attributes

base::samples::RigidBodyStateSE3 control_output
 
base::Vector6d control_error
 
base::Twist pose_diff
 
base::Acceleration vel_diff
 
- Protected Attributes inherited from wbc::PosPDController
size_t dim_controller
 
base::VectorXd p_gain
 
base::VectorXd d_gain
 
base::VectorXd ff_gain
 
base::VectorXd dead_zone
 
base::VectorXd max_control_output
 
base::VectorXd ref_pos
 
base::VectorXd ref_vel
 
base::VectorXd ref_acc
 
base::VectorXd pos
 
base::VectorXd vel
 
base::VectorXd acc
 
base::VectorXd pos_diff
 
base::VectorXd vel_diff
 
base::VectorXd control_out_vel
 
base::VectorXd control_out_acc
 

Detailed Description

The CartesianPosPDController class implements a PD Controller with feed forward on the RigidBodyStateSE3 type. The following control schemes are available:

  1. Velocity Output: \(\mathbf{v}_d = \mathbf{K}_d\mathbf{v}_r + \mathbf{K}_pe\)
  2. Acceleration Output: \(\mathbf{a}_d = \mathbf{K}_{ff}\ddot{\mathbf{e}} + \mathbf{K}_d\dot{\mathbf{e}} + \mathbf{K}_p\mathbf{e}\),

where pose, twist and accelerationerror are computed as \( \renewcommand*{\arraystretch}{1.2} \mathbf{e} = \left( \begin{array}{c} \mathbf{p}_r - \mathbf{p}_a \\ \mathbf{R}_a\cdot \theta \mathbf{\hat \omega}^a_r \end{array} \right) \) , \( \dot{\mathbf{e}} = \mathbf{v}_r-\mathbf{v} \) and \( \ddot{\mathbf{e}} = \mathbf{a}_r-\mathbf{a} \)

\( \mathbf{p},\mathbf{p}_r \) - Actual and reference position
\( \theta\mathbf{\hat \omega}^a_r \) - Rotation error as angle-axis representation
\( \mathbf{R}_a \) - Rotation matrix, converts the rotation error to base frame
\( \mathbf{v},\mathbf{v}_r \) - Actual and reference twist
\( \mathbf{a}_r,\mathbf{a} \) - Reference and actual spatial acceleration
\( \mathbf{K}_d,\mathbf{K}_p \) - Derivative and proportial gain matrices
\( \mathbf{K}_{ff} \) - Feed forward gain matrix
\( \mathbf{a}_d,\mathbf{v}_d \) - Control output, desired acceleration or twist

Note: If an input is NaN, it might be ignored by the controller. E.g. if the reference or actual twist is none, the twist error will be set to zero in the controller

Constructor & Destructor Documentation

◆ CartesianPosPDController()

wbc::CartesianPosPDController::CartesianPosPDController ( )

Initializes members

Member Function Documentation

◆ extractFeedback()

void wbc::CartesianPosPDController::extractFeedback ( const base::samples::RigidBodyStateSE3 & feedback)
protected

◆ extractSetpoint()

void wbc::CartesianPosPDController::extractSetpoint ( const base::samples::RigidBodyStateSE3 & setpoint,
const base::samples::RigidBodyStateSE3 & feedback )
protected

◆ update()

const base::samples::RigidBodyStateSE3 & wbc::CartesianPosPDController::update ( const base::samples::RigidBodyStateSE3 & setpoint,
const base::samples::RigidBodyStateSE3 & feedback )

Convert typed to raw input data and call PosPDController::update()

Member Data Documentation

◆ control_error

base::Vector6d wbc::CartesianPosPDController::control_error
protected

◆ control_output

base::samples::RigidBodyStateSE3 wbc::CartesianPosPDController::control_output
protected

◆ pose_diff

base::Twist wbc::CartesianPosPDController::pose_diff
protected

◆ vel_diff

base::Acceleration wbc::CartesianPosPDController::vel_diff
protected
The documentation for this class was generated from the following files:
Generated by doxygen 1.12.0