README.md

🎛️ Control

🔙 Back to the Foyer

This sub-library contains control classes for robots. At present, it contains the SerialLinkBase class, and the SerialKinematicControl class for real-time velocity control. A SerialDynamicControl class is in development.

✨ Key Features:

• Joint feedback, and Cartesian feedback control.
• Seemless integration with the Trajectory sub-library.
• Optimisation using Quadratic Programming (QP) to satisfy joint limits.
• Automatic redundancy resolution for singularity avoidance, and energy minimisation.

🧭 Navigation:

• Data Structures
• Serial Link Base
• Serial Kinematic Control

Data Structures

🔝 Back to Top

Serial Link Base

This class provides a standardised structure to all serial link control classes, and polymorphism to implement different controllers for the same task.

Note

SerialLinkBase inherits the QPSolver which can be used to optimise the Cartesian control. It is automatically downloaded by RobotLibrary from this repository.

Construction:

1. A pointer to a RobotLibrary::Model::KinematicTree object,
2. The name of a valid reference frame (i.e. the endpoint of the robot) in said model for Cartesian control purposes, and
3. Optional RobotLibrary::Control::Parameters (feedback gains, etc.).

This is done deliberately so that:

1. Two serial link controllers can control different endpoints on the same robot (e.g. a controller for each arm on a humanoid robot), and
2. The model can update the kinematics & dynamics independently in a separate thread.

Key Methods:

Note

Many of these are virtual methods and must be defined in any derived class.

• update : Computes a new Jacobian matrix to the specified endpoint.
• resolve_endpoint_motion : Computes the joint control required to achieve a given motion (velocity or acceleration) of the endpoint.
• resolve_endpoint_twist : The same as above, but assumes the input is a twist (linear & angular velocity),
• track_endpoint_trajectory : Computes the joint control needed to follow a Cartesian trajectory.
• track_joint_trajectory : Computes the joint control needed to follow a joint trajectory.

Note

You should always call update_state() on the RobotLibrary::Model::KinematicTree object first, before calling update() on the controller.

Class Diagram:

🔝 Back to Top

Serial Kinematic Control

This builds upon the SerialLinkBase class to implement algorithms for real-time velocity control.

Tip

You can check out my ROS2 action server to see this class in action.

Construction:

The same as SerialLinkBase.

Key Methods:

The same as SerialLinkBase.

• track_joint_trajectory:

This method computes:

$$\dot{\mathbf{q}}={\dot{\mathbf{q}}}_d+k_p({\mathbf{q}}_d-\mathbf{q})$$

where:

• $\dot{\mathbf{q}}\in {\mathbb{R}}^n$ are the joint velocities to command the robot,
• ${\mathbf{q}}_d,{\dot{\mathbf{q}}}_d\in {\mathbb{R}}^n$ is the desired joint position and joint velocity,
• $\mathbf{q}\in {\mathbb{R}}^n$ is the current joint position, and
• $k_p\in {\mathbb{R}}^{+}$ is the joint position gain.

• track_cartesian_trajectory:

This method computes

$$\dot{\mathbf{x}}={\dot{\mathbf{x}}}_d+\mathbf{K}({\mathbf{x}}_d-\mathbf{x})$$

where:

• $\mathbf{x}\in {\mathbb{R}}^6$ is the command twist (Cartesian velocity) for the endpoint of the robot,
• ${\mathbf{x}}_d,{\dot{\mathbf{x}}}_d$ is the desired pose & twist,
• $\mathbf{x}\in \mathbb{SE}(3)$ is the current pose of the endpoint, and
• $\mathbf{K}\in {\mathbb{R}}^{6\times 6}$ is the Cartesian stiffness matrix.

then immediately calls the method below.

• resolve_endpoint_motion:

For a non-redundant robot, this solves:

$$\begin{array}{r}\underset{\dot{\mathbf{q}}}{min}\frac{1}{2}{(\dot{\mathbf{x}}-\mathbf{J}\dot{\mathbf{q}})}^T(\dot{\mathbf{x}}-\mathbf{J}\dot{\mathbf{q}})\\ \text{subject to: }{\dot{\mathbf{q}}}_{min}\le \dot{\mathbf{q}}\le {\dot{\mathbf{q}}}_{max}\\ \dot{\mu }\ge \gamma (\mu -{\mu }_{min})\end{array}$$

where:

• $\mathbf{J}\in {\mathbb{R}}^{6\times n}$ is the Jacobian matrix,
• $\dot{\mathbf{q}}min$ and ${\dot{\mathbf{q}}}_{max}\in {\mathbb{R}}^n$ are lower and upper bounds on the joint speed based on joint limits (both position and speed),
• $\mu$ is the measure of manipulability (proximity to a singularity), and
• ${\mu }_{min}$ is the minimum manipulability threshold.

If the robot is redundant it solves,

$$\begin{array}{r}\underset{\dot{\mathbf{q}}}{min}\frac{1}{2}{({\dot{\mathbf{q}}}_{\varnothing }-\dot{\mathbf{q}})}^T\mathbf{M}({\dot{\mathbf{q}}}_{\varnothing }-\dot{\mathbf{q}})\\ \text{subject to: }\dot{\mathbf{x}}=\mathbf{J}\dot{\mathbf{q}}\\ {\dot{\mathbf{q}}}_{min}\le \dot{\mathbf{q}}\le {\dot{\mathbf{q}}}_{max}\\ \dot{\mu }\ge -\gamma (\mu -{\mu }_{min})\end{array}$$

where:

• ${\dot{\mathbf{q}}}_{\varnothing }\in {\mathbb{R}}^n$ is a redundant task, and
• $\mathbf{M}\in {\mathbb{R}}^{n\times n}$ is the joint inertia matrix.

Note

Weighting the joint velocities by the inertia matrix instantaneously minimises the kinetic energy needed to move the robot.

Tip

You can set the redundant task with the set_redundant_task() method. If none is provided, it automatically uses the gradient projection method to move away from singularities (where possible).

Note

If the singularity avoidance fails and the manipulability $\mu$ is below ${\mu }_{min}$, then the algorithm automatically reverts to a damped-least-squares method to avoid numerical instability.

Class Diagram:

🔝 Back to Top