API Reference

Contents

API Reference#

This page contains the API documentation of sdu_controllers. The following classes are available:

Controllers:

Math:

Models:

HAL:

Controller#

class Controller#

This class provides a base class for the different controllers. This is useful when dealing with multiple controllers.

Subclassed by sdu_controllers::controllers::AdmittanceControllerPosition, sdu_controllers::controllers::ForceControlInnerVelocityLoop, sdu_controllers::controllers::OperationalSpaceController, sdu_controllers::controllers::PIDController

Public Functions

virtual Eigen::VectorXd get_output() = 0#

Get the output of the controller. Should be updated when the controller step function is called.

virtual void reset() = 0#

Reset internal controller variables.

PDController#

Warning

doxygenclass: Cannot find class “sdu_controllers::controllers::PDController” in doxygen xml output for project “sdu_controllers” from directory: /home/docs/checkouts/readthedocs.org/user_builds/sdu-controllers/checkouts/latest/doc/build-cmake/doc/xml

AdmittanceControllerPosition#

class AdmittanceControllerPosition : public sdu_controllers::controllers::Controller#

The admittance controller is implemented as described in: Robotics Modelling, Planning and Control, Chapter 9, Eq. 9.37:

\( \mathbf{M}_{t}\ddot{\tilde{z}} + \mathbf{K}_{Dt}\dot{\tilde{z}} + \mathbf{K}_{Pt}\tilde{z} = h_{e}^{d} \)

The position and orientation are treated separately. For the position Eq. 9.37 is rearranged to solve for the acceleration \(\ddot{x}\), yielding:

\( \ddot{x} = \mathbf{M}^{-1}f_{base}^{d} - \mathbf{K}x - \mathbf{D}\dot{x} \)

which is integrated twice to give a position.

Similarly, for the orientation we rearrange Eq. 9.37, to solve for the angular acceleration \(\ddot{\omega}\), yielding:

\( \ddot{\omega} = \mathbf{M}_{O}^{-1}\mu^{d} - \mathbf{K}^{'}_{O} \mathbf{q}_{e} - \mathbf{D}_{O}\dot{\omega} \)

The rotational stiffness \( \mathbf{K}^{'}_{O} \) is defined by Eq. 60 in F. Caccavale et al.: The Role of Euler Parameters in Robot Control as: \(\mathbf{K}^{'}_{O} = 2\mathbf{E}^{T}\mathbf{K}_{O}\) with as \(\mathbf{E} = \eta \mathbf{I} – \mathbf{S}(\epsilon) \).

We then integrate the acceleration to calculate the angular velocity. To integrate the velocity to the orientation in quaternions, we use Eq. 61 in F. Caccavale et al.: The Role of Euler Parameters in Robot Control: \( d\epsilon_{cd}^{d} = \frac{1}{2}\Delta\omega_{cd}^{d}dt \)

The output of the controller is a cartesian pose with the orientation described as a quaternion (scalar-first).

The admittance parameters default to:

\(\mathbf{M} = 22.5\), \(\mathbf{M}_{O} = 0.25\)

\(\mathbf{K} = 0\), \(\mathbf{K}_{O} = 0\)

\(\mathbf{D} = 70\), \(\mathbf{D}_{O} = 3\)

Public Functions

explicit AdmittanceControllerPosition(double frequency = 500.0)#

Initialize the admittance controller.

Parameters:

frequency – controller frequency, defaults to 500 Hz

void set_mass_matrix_position(const Eigen::Matrix3d &mass)#

Set the positional mass matrix \( \mathbf{M} \).

void set_stiffness_matrix_position(const Eigen::Matrix3d &stiffness)#

Set the positional stiffness matrix \( \mathbf{K} \).

void set_damping_matrix_position(const Eigen::Matrix3d &damping)#

Set the positional damping matrix \( \mathbf{D} \).

void set_mass_matrix_orientation(const Eigen::Matrix3d &mass)#

Set the orientational mass matrix \( \mathbf{M}_{O} \).

void set_stiffness_matrix_orientation(const Eigen::Matrix3d &stiffness)#

Set the orientational stiffness matrix \( \mathbf{K}_{O} \).

void set_damping_matrix_orientation(const Eigen::Matrix3d &damping)#

Set the orientational damping matrix \( \mathbf{D}_{O} \).

void step(const Eigen::Vector3d &input_force, const Eigen::Vector3d &input_torque, const Eigen::Vector3d &x_desired, const Eigen::Vector4d &quat_desired)#

Step the execution of the controller.

Parameters:

  • input_force – given as \( [f_{x}, f_{y}, f_{z}] \)

  • input_torque – given as \( [\mu_{x}, \mu_{y}, \mu_{z}] \)

  • x_desired – desired position given as \( [x, y, z] \)

  • quat_desired – desired orientation in quaternion \( [q_{w}, q_{x}, q_{y}, q_{z}]\) (scalar first)

virtual Eigen::VectorXd get_output() override#

Get the output of the controller. Updates when the step() function is called.

Returns:

the new pose as \([x, y, z, q_{w}, q_{x}, q_{y}, q_{z}]\)

virtual void reset() override#

Reset internal controller variables.

OperationalSpaceController#

class OperationalSpaceController : public sdu_controllers::controllers::Controller#

Operational (or cartesian) space controller with inverse dynamics control. The controller is implemented according to Eq. (8.114) from page 348, Robotics: Modelling, Planning and Control:

\( \mathbf{y} = \mathbf{J}_{A}^{-1}(q)\left(\ddot{x}_{d} + \mathbf{K}_{D}\dot{\tilde{x}} + \mathbf{K}_{P}\tilde{x}-\mathbf{\dot{J}}_{A}(q, \dot{q})\dot{q}\right) \)

Public Functions

explicit OperationalSpaceController(Eigen::MatrixXd Kp, Eigen::MatrixXd Kd, std::shared_ptr<models::RobotModel> robot_model)#

Step the execution of the controller.

void step(const Eigen::VectorXd &x_d, const Eigen::VectorXd &dx_d, const Eigen::VectorXd &ddx_d, const Eigen::VectorXd &q, const Eigen::VectorXd &dq)#

Step the execution of the controller.

virtual Eigen::VectorXd get_output() override#

Get the output of the controller. Updates when the step() function is called.

virtual void reset() override#

Reset internal controller variables.

ForwardDynamics#

class ForwardDynamics#

This class implements forward dynamics computing the acceleration due to applied torque. It requires a robot model defined by

See also

RobotModel class.

Public Functions

Eigen::VectorXd forward_dynamics(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, const Eigen::VectorXd &tau)#

Calculate the forward dynamics.

The forward dynamics are calculated according to Eq. (7.115), from page 293, Robotics: Modelling, Planning and Control, Chapter 7:

\( \ddot{q} = \mathbf{B}^{-1}(q) \left(\tau - \mathbf{C}(q)\dot{q} -\mathbf{\tau}_{g}\right) \)

Parameters:

  • q – robot joint positions.

  • dq – robot joint velocities.

  • tau – joint torques of the robot

Returns:

the acceleration \( \ddot{q} \)

InverseDynamics#

class InverseDynamics#

This class provides a base class for the inverse dynamics that can be calculated in joint-space or cartesian space, this class contains the robot model defined by

See also

RobotModel class.

Subclassed by sdu_controllers::math::InverseDynamicsCartesianSpace, sdu_controllers::math::InverseDynamicsJointSpace

InverseDynamicsJointSpace#

class InverseDynamicsJointSpace : public sdu_controllers::math::InverseDynamics#

This class implements inverse dynamics calculation for joint-space.

Public Functions

Eigen::VectorXd inverse_dynamics(const Eigen::VectorXd &y, const Eigen::VectorXd &q, const Eigen::VectorXd &dq)#

Calculate the inverse dynamics.

The inverse dynamics are calculated according to Eq. (6.27), from page 141, Springer Handbook of Robotics 2008:

\( \mathbf{\tau} = \mathbf{B}(q)y + \mathbf{C}(q, \dot{q})\dot{q} + \mathbf{\tau}_{g} \)

Parameters:

  • y – auxiliary control input.

  • q – robot joint positions.

  • dq – robot joint velocities.

Returns:

the computed torques for the joint actuators \( \tau \)

RobotModel#

class RobotModel#

This class provides a base class for the RobotModel.

Subclassed by sdu_controllers::models::ParameterRobotModel, sdu_controllers::models::RegressorRobotModel

Public Functions

virtual Eigen::VectorXd inverse_dynamics(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, const Eigen::VectorXd &ddq, const Eigen::VectorXd &he) = 0#

Calculate the inverse dynamics.

Parameters:

  • q – robot joint positions.

  • dq – robot joint velocities.

  • ddq – robot joint accelerations.

  • he – end-effector wrench.

Returns:

the computed torques for the joint actuators \( \tau \)

virtual Eigen::VectorXd forward_dynamics(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, const Eigen::VectorXd &tau) = 0#

Calculate the forward dynamics.

Parameters:

  • q – robot joint positions.

  • dq – robot joint velocities.

  • tau – joint torques of the robot

Returns:

the acceleration \( \ddot{q} \)

virtual Eigen::MatrixXd get_inertia_matrix(const Eigen::VectorXd &q) = 0#

Get inertia matrix \( \mathbf{B}(q) \).

Parameters:

q – the robot joint configuration.

Returns:

the inertia matrix.

virtual Eigen::MatrixXd get_coriolis(const Eigen::VectorXd &q, const Eigen::VectorXd &qd) = 0#

Get coriolis matrix \( \mathbf{C}(q, \dot{q}) \).

Parameters:

  • q – the robot joint configuration.

  • qd – the robot joint configuration.

Returns:

the coriolis matrix.

virtual Eigen::MatrixXd get_gravity(const Eigen::VectorXd &q) = 0#

Get gravity term \( \tau_{g} \).

Returns:

the gravity vector

virtual Eigen::MatrixXd get_jacobian(const Eigen::VectorXd &q) = 0#

Get the jacobian \( \mathbf{J(q)} \).

Returns:

the jacobian

virtual Eigen::MatrixXd get_jacobian_dot(const Eigen::VectorXd &q, const Eigen::VectorXd &dq) = 0#

Get jacobian dot \( \mathbf{\dot{J(q, dq)}} \).

Returns:

the jacobian dot

virtual std::pair<Eigen::VectorXd, Eigen::VectorXd> get_joint_pos_bounds() = 0#

Get joint position bounds.

Returns:

the joint position bounds

virtual Eigen::VectorXd get_joint_max_vel() = 0#

Get maximum joint velocity.

Returns:

the maximum joint velocity

virtual Eigen::VectorXd get_joint_max_acc() = 0#

Get maximum joint acceleration.

Returns:

the maximum joint acceleration

virtual Eigen::VectorXd get_joint_max_torque() = 0#

Get maximum joint torque.

Returns:

the maximum joint torque

virtual uint16_t get_dof() const = 0#

Get the degrees of freedom of the robot.

Returns:

the number of degrees of freedom

virtual std::vector<double> get_m() = 0#

Get the masses of the robot links.

Returns:

vector containing the mass of each link

virtual Eigen::Vector3d get_g0() = 0#

Get the gravity vector in base frame.

Returns:

the 3D gravity vector

virtual Eigen::Matrix<double, Eigen::Dynamic, 3> get_CoM() = 0#

Get the center of mass positions for each link.

Returns:

matrix where each row contains the 3D center of mass position for a link

virtual std::vector<Eigen::Matrix3d> get_link_inertia() = 0#

Get the inertia tensors for each link.

Returns:

vector of 3x3 inertia matrices for each link

virtual const kinematics::ForwardKinematics &get_fk_solver() const = 0#

Get the forward kinematics solver instance.

Returns:

reference to the forward kinematics solver

URRobotModel#

class URRobotModel : public sdu_controllers::models::ParameterRobotModel#

This class provides a robot model for a UR robot.

BreedingBlanketHandlingRobotModel#

class BreedingBlanketHandlingRobotModel : public sdu_controllers::models::ParameterRobotModel#

This class provides a robot model for the EUROfusion breeding blanket handling robot.

Robot#

class Robot#

This class provides a Robot base class for the different robot interfaces implemented in the (HAL) Hardware Abstraction Layer of sdu_controllers.

Subclassed by sdu_controllers::hal::FrankaRobot, sdu_controllers::hal::URRobot

URRobot#

class URRobot : private sdu_controllers::hal::Robot#

This class provides an interface for controlling Universal Robots. The interface is using the ur_rtde library to control and receive data from the robot.

The interface provides different online control modes:

  • JOINT_POSITION

  • CARTESIAN

  • VELOCITY

  • TORQUE

where a target reference for the given control mode is set using set_joint_pos_ref(), set_cartesian_pose_ref(), set_joint_vel_ref() or set_cartesian_vel_ref(). To perform the actual control, the step() function of the interface must be called from the real-time thread that should be controlling robot.

Besides the online real-time control modes one can also move the robot using the move_joints() or move_cartesian(), which uses the builtin robot controller functions to perform the movements (typically using a trapezoidal velocity interpolation).

Public Types

enum class ControlStates#

The ControlStates enum defines the available control states for the robot control.

Values:

enumerator NONE#
enumerator INIT#
enumerator STOPPED#
enumerator RUNNING#
enumerator ERROR#
enum class ControlMode#

The ControlMode enum defines the available control modes for this particular robot.

Values:

enumerator UNDEFINED#
enumerator JOINT_POSITION#
enumerator CARTESIAN_POSE#
enumerator JOINT_VELOCITY#
enumerator CARTESIAN_VELOCITY#
enumerator TORQUE#

Public Functions

explicit URRobot(const std::string &ip, double control_frequency = 500.0)#

URRobot constructor - creates a UR RTDE Interface for the robot at the specified IP-address.

Parameters:

  • ip – The IP-address of the robot.

  • control_frequency – The robot control frequency, defaults to 500Hz.

void step()#

the step() function should be called regularly at a fixed rate by the application control thread to ensure the state machine is updated and that the target references is sent to the physical robot.

bool set_control_mode(ControlMode control_mode)#

Set the robot control mode.

Allows you to set the control mode of the given robot. See ControlMode

Parameters:

control_mode – A control mode defined in ControlMode.

bool start_control()#

Start control - signals the control state machine to transition from STOPPED to the RUNNING state.

bool stop_control()#

Stop control - signals the control state machine to transition from RUNNING to the STOPPED state.

std::chrono::steady_clock::time_point init_period()#

This function is used in combination with wait_period() and is used to get the start of a control period / cycle.

void wait_period(const std::chrono::steady_clock::time_point &t_cycle_start)#

Used for waiting the rest of the control period, set implicitly as dt = 1 / frequency. A combination of sleeping and spinning are used to achieve the lowest possible jitter. The function is especially useful for a realtime control loop. NOTE: the function is to be used in combination with the init_period().

Parameters:

t_cycle_start – the start of the control period. Typically given as dt = 1 / frequency.

bool set_joint_pos_ref(const Eigen::Vector<double, ROBOT_DOF> &q)#

Set a joint position reference target

Parameters:

q – specifies joint positions of the robot axes [radians].

bool set_cartesian_pose_ref(const sdu_controllers::math::Pose &pose)#

Set a cartesian pose reference target (performs inverse kinematics on the robot to produce a target in joint-space).

Parameters:

pose – specifies the target pose with position in meters and orientation as a quaternion with scalar-first (WXYZ).

bool set_joint_vel_ref(const Eigen::Vector<double, ROBOT_DOF> &dq, double acceleration = 0.5)#

Set a joint velocity reference target.

Accelerate linearly in joint space and continue with constant joint speed.

Parameters:

  • dq – specifies the target joint velocities \(\dot{q}\).

  • acceleration – joint acceleration [rad/s^2] (of leading axis)

bool set_joint_torque_ref(const Eigen::Vector<double, ROBOT_DOF> &torques)#

Set a joint torque reference target.

Commands joint torques directly.

Parameters:

  • dq – specifies the target joint velocities \(\dot{q}\).

  • acceleration – joint acceleration [rad/s^2] (of leading axis)

bool set_cartesian_vel_ref(const Eigen::Vector<double, ROBOT_DOF> &xd, double acceleration = 0.25)#

Set a cartesian velocity reference target.

Accelerate linearly in Cartesian space and continue with constant tool speed.

Parameters:

  • xd – specifies the target tcp speed in [m/s].

  • acceleration – tool position acceleration [m/s^2].

void zero_ft_sensor()#

Zeroes the TCP force/torque measurement from the builtin force/torque sensor by subtracting the current measurement from the subsequent.

bool move_joints(const Eigen::Vector<double, ROBOT_DOF> &q, double velocity = 1.05, double acceleration = 1.4, bool asynchronous = false)#

Move to a given joint position in joint-space

Parameters:

  • q – specifies joint positions of the robot axes [radians].

  • velocity – joint velocity [rad/s]

  • acceleration – joint acceleration [rad/s^2]

  • asynchronous – a bool specifying if the move command should be asynchronous. Default is false, this means the function will block until the movement has completed.

bool move_cartesian(const math::Pose &pose, double velocity = 0.25, double acceleration = 1.2, bool asynchronous = false)#

Move to a given position in cartesian space

Parameters:

  • pose – specifies the target pose with position in meters and orientation as a quaternion with scalar-first (WXYZ).

  • velocity – tool velocity [m/s]

  • acceleration – tool acceleration [m/s^2]

  • asynchronous – a bool specifying if the move command should be asynchronous. Default is false, this means the function will block until the movement has completed.

Eigen::VectorXd get_joint_torques()#

Get the joint torques

Eigen::VectorXd get_joint_positions()#

Get the joint positions

Eigen::VectorXd get_joint_velocities()#

Get the joint velocities

math::Pose get_cartesian_tcp_pose()#

Get the cartesian pose with position in meters and orientation as Quaterniond (see the Pose class).

std::vector<double> get_tcp_forces()#
Returns:

Generalized forces in the TCP.

Public Static Attributes

static const uint16_t ROBOT_DOF = 6#

A constant that defines the number of degrees of freedom (DOF) the robot have.

Contents