Animate URDF Robot with trajectory of end effector

[Shaamsi]

I ma trying to animate robot from urdf with trajectory of end effector in the same time. How to do it in one animation callback? How to show the history of motion of end effector of robot and keeping trajectory while robot is moving in the same time?

[AlexanderFabisch]

If your question is about how to animate multiple "artists" in general and not about robots and lines specifically, here is an example that I am currently working with:

class AnimationCallback:  # animation callback as callable object (defines __call__ method)
    def __init__(self, fig, pipeline, show_object, show_robot):
        self.fig = fig
        self.show_object = show_object
        self.show_robot = show_robot

        ...

        if self.show_object:
            self.object_frame = pv.Frame(np.eye(4), s=0.1)
            self.object_frame.add_artist(self.fig)

        if self.show_robot:
            self.robot = pipeline.make_robot_artist()
            self.robot.add_artist(self.fig)

    def __call__(self, t, markers, dataset, pipeline):
        markers.set_data(dataset.get_markers(t))

        artists = [markers]

        if self.show_object:
            object_pose = ... # compute object_pose
            self.object_frame.set_data(object_pose)
            artists.append(self.object_frame)

        if self.show_robot:
            angle = ... # compute new angle, in my case the pipeline object does this
            self.robot.tm.set_joint("joint_name", angle)  # update robot configuration
            self.robot.set_data()
            artists.append(self.robot)

        return artists  # return a tuple or a list of artists


...

markers = ...  # an artist that represents several sphere meshes
pipeline = ...  # contains some algorithm that computes robot joint angles
dataset = ...  # data based on which the pipeline object operates
n_steps = ...  # number of steps

from pytransform3d import visualizer as pv
fig = pv.figure()
animation_callback = AnimationCallback(fig, pipeline, True, True)
fig.animate(
    animation_callback, n_steps, loop=True,
    fargs=(markers, dataset, pipeline))

You can also find further information here: https://rock-learning.github.io/pytransform3d/_apidoc/pytransform3d.visualizer.Figure.html#pytransform3d.visualizer.Figure.animate

[AlexanderFabisch]

My use case is moving the robot and see the end effector transforms. along end effector trajectory. I still can not achieve this by multiple artists, Any advice?

That sounds like you do not want to animate a trajectory artist. This sounds like you want to draw a line along the positions of the end-effector and animate a frame that moves along this line. Correct me if I am wrong.

[Shaamsi]

The trajectory (or more accurately path) of robot is just collection of frames/transforms from start pose to end pose. I want to visualize pose trajectory like this without a line but online in open3d visualizer while the robot is moving. Is it possible to have something like that?

Note: I tried even in matplotlib, but still did not manage to.

import os
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.urdf import UrdfTransformManager
import matplotlib
from pytransform3d.trajectories import plot_trajectory
from pytransform3d.rotations import quaternion_from_matrix
matplotlib.interactive(True)
# import sys
data_dir = "/urdf/ur5/"
tm = UrdfTransformManager()
filename = os.path.join(data_dir, "ur5.urdf")
with open(filename, "r") as f:
    robot_urdf = f.read()
    tm.load_urdf(robot_urdf, mesh_path=data_dir)


def signal_handler(sig, frame):
    matplotlib.pyplot.close("all")
    # sys.exit(0)


a = 0
fig = plt.figure()

n_steps = 100000
P = np.empty((n_steps, 7))
for step_idx in range(n_steps):
    a = a+0.01
    tm.set_joint("shoulder_pan_joint", a * np.pi)
    ee2world = tm.get_transform("ee_link", "base_link")
    P[step_idx, 0] = ee2world[0, 3]
    P[step_idx, 1] = ee2world[1, 3]
    P[step_idx, 2] = ee2world[2, 3]
    P[step_idx, 3:] = quaternion_from_matrix(ee2world[:3, :3])
    ax = tm.plot_visuals("ur5", ax_s=0.6, alpha=0.7)
    plot_trajectory(ax=ax,
                    P=P, s=0.3, n_frames=100, normalize_quaternions=False, lw=2, c="k")
    plt.draw()
    plt.pause(0.0001)

[AlexanderFabisch]

There is the plot function, which will create a Line3D artist. I'd suggest to use it to draw the positions of the end-effector in the trajectory before you start the animation and then you don't have to update it in the animation callback. The only things that you have to update in the animation callback are the robot's state and the frame object that represents the end-effector pose. The Frame object can be created by plot_transform. In the animation callback you have to call Frame.set_data to set the new end-effector pose.

[Shaamsi]

@AlexanderFabisch . It works now. Thanks very much for your time and efforts.

[AlexanderFabisch]

Awesome!