Skip to content

Manipulator Joint Limits

cbfpy.examples.joint_limits_demo

Manipulator Joint Limit Avoidance Demo

We will command a joint position trajectory that exceeds the joint limits, and the CBF will ensure that we stay within the limits (+ some margin)

This uses a single-integrator reduced model of the manipulator dynamics. We define the state as the joint positions and assume that we can directly control the joint velocities i.e. z = [q1, q2, q3] and u = [q1_dot, q2_dot, q3_dot]

JointLimitsConfig

Bases: CBFConfig

Config for the 3-DOF arm, avoiding its joint limits using velocity control

Source code in cbfpy/examples/joint_limits_demo.py 
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
class JointLimitsConfig(CBFConfig):
    """Config for the 3-DOF arm, avoiding its joint limits using velocity control"""

    def __init__(self):
        self.num_joints = 3
        # Joint limit values from the URDF
        self.q_min = -np.pi / 2 * np.ones(self.num_joints)
        self.q_max = np.pi / 2 * np.ones(self.num_joints)
        # Pad joint limts (to better evauate CBF performance)
        self.padding = 0.3
        super().__init__(n=self.num_joints, m=self.num_joints)

    def f(self, z):
        return jnp.zeros(self.num_joints)

    def g(self, z):
        return jnp.eye(self.num_joints)

    def h_1(self, z):
        q = z
        return jnp.concatenate(
            [self.q_max - q - self.padding, q - self.q_min - self.padding]
        )

nominal_controller(q, q_des)

Very simple proportional controller: Commands joint velocities to reduce a position error

Parameters:

Name Type Description Default
q Array

Joint positions, shape (num_joints,)

 required
q_des Array

Desired joint positions, shape (num_joints,)

 required

Returns:

Name Type Description
Array Array

Joint velocity command, shape (num_joints,)
Source code in cbfpy/examples/joint_limits_demo.py 
51
52
53
54
55
56
57
58
59
60
61
62
def nominal_controller(q: Array, q_des: Array) -> Array:
    """Very simple proportional controller: Commands joint velocities to reduce a position error

    Args:
        q (Array): Joint positions, shape (num_joints,)
        q_des (Array): Desired joint positions, shape (num_joints,)

    Returns:
        Array: Joint velocity command, shape (num_joints,)
    """
    k = 1.0
    return k * (q_des - q)