Overview

In this library, the user must call the function plot(). It will dispatch the setting defined by the user using kwargs to the underlying methods that deal with each type of plot.

Warning

Never use internal methods, e.g., with a trailing _ directly. The API for those can change at any time.

Importing dqrobotics-pyplot

Warning

This import directive is to be used while dqrobotics-pyplot is not an integral part of the dqrobotics library.

# Adding the prefix `dqp` to help users differentiate from `plt`
import dqrobotics_extensions.pyplot as dqp

Setting up a plot with plt

Note

If you don’t create an plt.axes with projection=’3d’ the figure will be incompatible with dqrobotics-pyplot and an error will be raised.

A suitable figure must be created using matplotlib.pyplot, given that this library integrally rely on it.

    # Set up the plot
    plt.figure()
    plot_size = 1
    ax = plt.axes(projection='3d')
    ax.set_xlabel('$x$')
    ax.set_xlim((-plot_size, plot_size))
    ax.set_ylabel('$y$')
    ax.set_ylim((-plot_size, plot_size))
    ax.set_zlabel('$z$')
    ax.set_zlim((-plot_size, plot_size))

Plot a pose

Note

See its API pyplot._pyplot._plot_pose().

The most basic version of the call is.

    # Draw a pose
    x_phi = pi / 3
    r = cos(x_phi) + i_ * sin(x_phi)
    x = r + 0.5 * E_ * (0.5 * j_ + 0.45 * k_) * r
    dqp.plot(x)

Plot a line

Note

See its API pyplot._pyplot._plot_line().

The most basic version of the call is.

    # Draw a line
    l = k_
    m = cross(-0.3 * j_, l)
    l_dq = l + E_ * m
    dqp.plot(l_dq, line=True, scale=1)

Plot a plane

Note

See its API pyplot._pyplot._plot_plane().

The most basic version of the call is.

    # Draw a plane
    n_pi = i_
    d_pi = 0.1
    pi_dq = n_pi + E_ * d_pi
    dqp.plot(pi_dq, plane=True, scale=1)

Plot a DQ_SerialManipulator

Note

See its API pyplot._pyplot._plot_serial_manipulator().

The most basic version of the call is.

    # Draw a manipulator
    q = deg2rad([0, 45, 0, -90, 0, -45, 0])
    robot = KukaLw4Robot.kinematics()
    dqp.plot(robot, q=q)

Animating a DQ_SerialManipulator

Note

This is fully reliant on https://matplotlib.org/stable/api/animation_api.html.

The important aspect of making animations is to properly use matplotlib.animation which is straightforward but does not allow for “execution-time” plots. The idea is to store the desired motion aspects and then animate that afterwards.

We start importing the relevant libraries. We use matplotlib.animation and, to support more intricate animation functions we use functools.partial.

# Adding the prefix `dqp` to help users differentiate from `plt`
import dqrobotics_extensions.pyplot as dqp

import matplotlib.pyplot as plt
import matplotlib.animation as anm # Matplotlib animation
from functools import partial # Need to call functions correctly for matplotlib animations

One example animation function for the robot is

# Animation function
def animate_robot(n, robot, stored_q, stored_time):
    """
    Create an animation function compatible with `plt`.
    Adapted from https://marinholab.github.io/OpenExecutableBooksRobotics//lesson-dq8-optimization-based-robot-control.
    :param n: The frame number, necessary for pyplot.
    :param robot: The DQ_SerialManipulator instance to plot.
    :param stored_q: The sequence of joint configurations.
    :param stored_time: The sequence of timepoints to plot in the title.
    """
    plt.cla()
    plt.xlabel('x [m]')
    plt.xlim([-1.0, 0.0])
    plt.ylabel('y [m]')
    plt.ylim([-0.5, 0.5])
    plt.gca().set_zlabel('z [m]')
    plt.gca().set_zlim([0, 0.5])
    plt.title(f'Joint control time={stored_time[n]:.2f} s out of {stored_time[-1]:.2f} s')

    dqp.plot(robot, q=stored_q[n])

where only the last line is related to dqrobotics-pyplot. Further reinforcing the need of a proper understanding of how matplotlib.animation works.

Then, we have a main function that moves the robot as desired.

def main():

    # Define the robot
    robot = KukaLw4Robot.kinematics()

    # Sampling time [s]
    tau = 0.01
    # Simulation time [s]
    time_final = 1
    # Initial joint values [rad]
    q = deg2rad([0, 45, 0, -45, 0, 45, 0])
    # Store the control signals
    stored_q = []
    stored_time = []

    # Translation controller loop.
    for time in np.arange(0, time_final + tau, tau):
        # Output to console
        print(f"Simulation at time = {time}")

        # Store data for posterior animation
        stored_q.append(q)
        stored_time.append(time)

        # A joint-space velocity
        u = np.ones(7)

        # Move the robot
        q = q + u * tau

    # Set up the plot
    fig = plt.figure()
    plt.axes(projection='3d')

    anim = anm.FuncAnimation(fig,
                      partial(animate_robot,
                              robot=robot,
                              stored_q=stored_q,
                              stored_time=stored_time),
                      frames=len(stored_q))

    plt.show()

Where we have some additional commands to store the control behavior, but, in general, only a few extra lines at the end are needed to generate the animation.