I would like to compare/visualize the planned path and actual path of an industrial robot. I am using Moveit2 and I can access planned trajectory via /display_planned_path topic which is moveit_msgs/DisplayTrajectory. I can also read the joint position of the robot via /joint_states topic which is sensor_msgs/JointState.

I found a manual way of visualizing actual path by writing the transformation between base_frame and gripper_frame to a file with ros2 run tf2_ros tf2_echo base_frame gripper_frame > tf2.txt. Then extracting the Translation [X, Y, Z] data and plot by using Matplotlib. However, I don't think that it is the proper way of doing it.


I am open to your suggestions.

Thank you for your time.

UPDATE: I used the code from robotics-toolbox to import URDF and their Kinematic function to calculate the planned path from joint positions inside trajectory(available in .yaml file). Matplotlib is used for 3D data visualization.

#!/usr/bin/env python3
@author: Jesse Haviland

import re
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy.testing as nt
import numpy as np
import unittest
from roboticstoolbox import Robot
from spatialmath import SE3
from roboticstoolbox.tools.data import rtb_path_to_datafile
from distutils.dir_util import copy_tree
from os import mkdir, path
import tempfile as tf
import yaml

def fetch_positions(data):
    positions_list = []
    for trajectory in data['trajectory']:
        for point in trajectory['joint_trajectory']['points']:
    return positions_list

class TestCustomXacro(unittest.TestCase):

    def test_custom(self):

        class CustomPanda(Robot):
            def __init__(self, xacro_path):

                links, name, urdf_string, urdf_filepath = self.URDF_read(

                    name="Super Robot",
                    manufacturer="Super Manufacturer",

        temp_dir = tf.mkdtemp()

        xacro_dir = path.join(temp_dir, "custom_xacro_folder")

        robot = CustomPanda(xacro_dir)


        for link in robot.links:

        with open('<trajectory_yaml_file>', 'r') as file:
            yaml_data = list(yaml.safe_load_all(file))

        positions = fetch_positions(yaml_data[0])  # Assuming the data is in the first document

        # Convert positions_list to NumPy array
        positions_array = np.array(positions)

        translationList = []
        print("Joint Positions for all trajectory points:")
        for i, pos in enumerate(positions_array, start=1):
            print(f"Trajectory Point {i}: {pos}")
            T = robot.fkine(pos, end='end_point_link')

        x = [translation[0] for translation in translationList]
        y = [translation[1] for translation in translationList]
        z = [translation[2] for translation in translationList]

        fig = plt.figure()
        ax = fig.add_subplot(111, projection='3d')
        ax.scatter(x, y, z, label='Translation Points')
        #ax.plot(x, y, z, label='Connected Trajectory', color='red')

        # Add start and end labels
        ax.text(x[0], y[0], z[0], 'Start', fontsize=12, color='green')
        ax.text(x[-1], y[-1], z[-1], 'End', fontsize=12, color='red')

        ax.set_title('Planned Path Visualization')


if __name__ == "__main__":


Planned Path

  • $\begingroup$ Thank you for your answer. As you have suggested I will create a node for tf2 data acquisiton to visualize the actual path. Is there any tool that you can suggest for converting joint positions to waypoints (I guess a Kinematic tool) for planned path visualization? $\endgroup$ Feb 27 at 14:11
  • $\begingroup$ So, does this work as you expected it to? It looks to me like you are able to trace the plan points vs actual points. If it does then I'd suggest to close the question. $\endgroup$
    – ssarkar
    Feb 29 at 9:00
  • $\begingroup$ I have tested on Rviz and it did the job that I expected except getting sparse actual path compared to planned path. Yes we can close it. $\endgroup$ Feb 29 at 10:51

1 Answer 1


I do not see anything wrong in your approach.

I'd probably suggest you to write a dedicated node which subscribes to the trajectory as well as the TF tree, then you can use this information to perform the same computation you have done above, and maybe get a pointwise deviation metric in case you want to find out to what level the robot smooths out the provided trajectory. This saves you from having to ros2 run . . it every time.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.