Skip to article frontmatterSkip to article content

This is an example implementation of the adaptive controller described in Marinho & Adorno (2022).

Installation

%%capture
%pip uninstall dqrobotics --y
%pip install dqrobotics --pre
%pip install dqrobotics --pre --break-system-packages
%%capture
%pip install matplotlib dqrobotics-pyplot marinholab-papers-tro2022-adaptivecontrol
%pip install matplotlib dqrobotics-pyplot marinholab-papers-tro2022-adaptivecontrol --break-system-packages

Preamble

The implementation of the techniques described in Marinho & Adorno (2022) and shown herein is an example. This should be unequivocally understood from the prefix Example_ added to all classes in this module.

It is expected that this module will be an integral part of dqrobotics soon™. When that does happen, it might be modified both in obvious ways, such as the swap to the prefix DQ_, and more subtle ways, such as different ways to retrieve parameters, variable names, and so on.

Importing

example.py
import numpy as np
from numpy import pi

import matplotlib.pyplot as plt

from dqrobotics import *
from dqrobotics.utils.DQ_Math import deg2rad
import dqrobotics_extensions.pyplot as dqp

from marinholab.papers.tro2022.adaptive_control import Example_SerialManipulatorEDH
from marinholab.papers.tro2022.adaptive_control.Example_ParameterSpaceEDH import Example_Parameter, Example_ParameterType

Kinematic Model

In this implementation we rely on the object called Example_SerialManipulatorEDH. This object inherits from DQ_SerialManipulator. By EDH, it was meant Extended DH, from which the parametric Jacobians can also be obtained.

This initialization is similar to what is done in DQ_SerialManipulatorDH.

example.py
def vs050_raw_kinematics() -> Example_SerialManipulatorEDH:
    """
    Gets the ideal kinematics of the VS050 robot.
    :return: A Suitable Example_SerialManipulatorEDH with VS050 kinematics.
    """
    VS050_dh_matrix = np.array([
        [-pi, pi / 2, -pi / 2, 0, pi, 0],
        [0.345, 0, 0, 0.255, 0, 0.07],
        [0, 0.250, 0.01, 0, 0, 0],
        [pi / 2, 0, -pi / 2, pi / 2, pi / 2, 0],
        [0, 0, 0, 0, 0, 0]
    ])

    lower_joint_limits = np.array([-170, -100, -60, -265, -119, -355]) * pi / 180
    upper_joint_limits = np.array([170, 100, 124, 265, 89.9, 355]) * pi / 180

    robot = Example_SerialManipulatorEDH(VS050_dh_matrix)
    robot.set_lower_q_limit(lower_joint_limits)
    robot.set_upper_q_limit(upper_joint_limits)

    robot.set_base_frame(DQ([1]))
    robot.set_effector_frame(DQ([1]))

    return robot

Parameter Space

The dimension of the configuration space Q\mathcal{Q} of a robot is its degrees of freedom. Hence, for a nn-DoF robot its Jacobians will have nn rows.

Differently from that, the parameter space A\mathcal{A} has a variable dimension depending on the parameters that can be adapted. When using the DH convention, they can be, for instance, one of the four parameters for each of the joints. Each of which defined as part of the enumeration Example_ParameterType.

Each parameter must be specified as an instance of Example_Parameter. This object will store the information about which link index it refers to, what Example_ParameterType it is, the current value of the parameter, and its boundaries.

A list of Example_Parameter is used as argument for Example_SerialManipulatorEDH.set_parameter_space() so that the instance of Example_SerialManipulatorEDH can properly calculate the parametric Jacobians for the parameters we chose.

example.py
def set_parameter_space_boundaries(r: Example_SerialManipulatorEDH,
                                   other_parameters_linear_confidence_meters:float = 0.001,
                                   other_parameters_angular_confidence_degrees:float = 1):

    # Helpful shorthand
    opa = other_parameters_angular_confidence_degrees
    opl = other_parameters_linear_confidence_meters

    theta = Example_ParameterType.theta
    d = Example_ParameterType.d
    a = Example_ParameterType.a
    alpha = Example_ParameterType.alpha

    # Let us suppose we have inaccuracies at the first joint (0 indexed)
    parameter_space: list[Example_Parameter] = [
        Example_Parameter(0, theta, r.get_theta(0), r.get_theta(0) - deg2rad(opa), r.get_theta(0) + deg2rad(opa)),
        Example_Parameter(0, d, r.get_d(0), r.get_d(0) - opl, r.get_d(0) + opl),
        Example_Parameter(0, a, r.get_a(0), r.get_a(0) - opl, r.get_a(0) + opl),
        Example_Parameter(0, alpha, r.get_alpha(0), r.get_alpha(0) - deg2rad(opa), r.get_alpha(0) + deg2rad(opa))
    ]

    r.set_parameter_space(parameter_space)

Main function

The controller adapts the parameters of estimated_robot, which is our instance of Example_SerialManipulatorEDH.

In a physical or simulation environment, the measurements y\myvec{y} will come from a sensor.

Because in this worked example we won’t have access to one, we use another instance called real_robot to provide mock measurements. This is also the only way to study the behavior of the estimated parameters with respect to the real ones, given that the real parameters are not measurable physically.

example.py
def main():
    fig, ax = setup_plot()

    estimated_robot = vs050_raw_kinematics()
    q_init = np.array([0, pi/4, pi/2, 0, pi/4, 0])

    set_parameter_space_boundaries(estimated_robot)

    dqp.plot(estimated_robot,
             q = q_init,
             line_color = 'r',
             cylinder_color = "r",
             cylinder_alpha = 0.3,
             ax = ax)

    return fig, ax

Running the example

import matplotlib.pyplot as plt
%matplotlib inline
from adaptive_control.example import main
fig, ax = main()
plt.show()
<Figure size 640x480 with 1 Axes>

Troubleshooting

References
  1. Marinho, M. M., & Adorno, B. V. (2022). Adaptive Constrained Kinematic Control Using Partial or Complete Task-Space Measurements. IEEE Transactions on Robotics, 38(6), 3498–3513. 10.1109/tro.2022.3181047
[Incomplete] Working chapters
Working chapters