Lab 1: Cartesian trajectory planning

1. Setup ROS 2

For this lab session we will use ROS 2 Humble.

This is optional: You may want to install the uma_environment_tools as it will install ROS2 Humble, and some important packages and libraries that may be useful later in the course. In that repo, you'll find the required steps to install it.

If you already have a native version of Ubuntu 22.04 installed, you can skip steps 1 and 2.

A video of the installation, including the troubleshooting (if you don't find the errors, you don't need to run that part!) is shown below. Note that the video shows the installation with WSL. If you're using a native Ubuntu 22.04, you can skip the first instruction.

1.1. Testing the UMA environment

If you have installed the UMA environment, you should see that everything is working correctly.

Try the following;

create_catkin_ws

Put the name advanced_robotics_ws to your workspace.

If, after installing it, you go to your catkin workspace folder and when you run this alias

cb

you find an error like 'ROS colcon build is not installed', then you'll need to uninstall ros and install the environment again:

sudo apt remove ~nros-humble-* && sudo apt autoremove
sudo rm /etc/apt/sources.list.d/ros2.list
sudo apt update
sudo apt autoremove
# Consider upgrading for packages previously shadowed.
sudo apt upgrade
cd ~/uma_environment_tools/scripts
./install_uma_environment.sh

Then you'll ned to run

update_uma_environment

---

2. Introduction

This exercise illustrates the generation of Cartesian trajectories using one of the methodologies studied in this course. For this purpose, you'll use the the Cartesian Trajectory Planning package.

2.1. Install the dependencies

First, you'll need to install a series of dependencies:

sudo apt install ros-${ROS_DISTRO}-xacro
sudo apt install ros-${ROS_DISTRO}-gazebo-ros-pkgs
sudo  apt install ros-${ROS_DISTRO}-ros2-control ros-${ROS_DISTRO}-ros2-controllers ros-${ROS_DISTRO}-gazebo-ros2-control
sudo apt-get install -y ros-${ROS_DISTRO}-joint-state-publisher-gui ros-${ROS_DISTRO}-rviz2

If you find an error trying to install these dependencies, most probably you'll need to update the packages repositories and upgrade them to the last version. Once upgraded, you can install the dependecies using the commands above.

sudo apt update
sudo apt upgrade

2.2. Clone the package

Open a new terminal, go to the src folder in yout worskpace and clone the repo

git clone https://github.com/jmgandarias/cartesian_trajectory_planning

Once this is done, complie the workspace (if you have installed the UMA environment you can just use cb).

You should see something like this (don't worry about the warnings, this is because the code is incompleted - you'll need to complete it in this lab session)

2.3. Package content

It consists of the following:

• bringup: launch files and ros2_controller configuration You don't have to modify it.
• config: poses.yaml file with the definition of the EE poses for the lab session You need to modify it in this lab.
• controller: definition of the robot controller using ros2 control You don't have to modify it.
• description: the 6-DOF robot description (URDF and visualizer launch file) You don't have to modify it.
• experiment_data: you can use this folder to save and plot the data of the experiment You need to modify it in this lab.
• hardware: ros2_control hardware interface You don't have to modify it.
• reference_generator: It has 3 cpp files
  • send_circular_trajectory.cpp: Example using a velocity controller with a circular trajectory. You don't have to modify it.
  • send_linear_trajectory.cpp:Example using a velocity controller with a linear trajectory. You don't have to modify it.
  • send_trajectory.cpp: This is the main file where you have to implement the Cartesian trajectory. You need to modify it in this lab.

The content of this package is inspired by and built on the ROS2 control example 7.

2.4. Test the demo

In one terminal, run:

ros2 launch cartesian_trajectory_planning r6bot_controller.launch.py

In another terminal, run:

ros2 launch cartesian_trajectory_planning send_linear_trajectory.launch.py 

To show the EE trail in Rviz: * Go to RobotModel>Links>tool0 (or the link that refers to the EE). * Habilitate Show Trail.

You should see the following:

You can also try the circular trajectory if you want:

ros2 launch cartesian_trajectory_planning send_circular_trajectory.launch.py 

These trajectories are generated because the robot is under a joint velocity controller with an Inverse Kinematics solver (implemented using the ros2 control package - Described in Example 7).

Snippet of the velocity-based circular trajecotry:

for (int i = 0; i < trajectory_len; i++)
  {
    // set endpoint twist
    double t = i;

    // circular trajectory in xy plane
    double vx = 2.0 * 0.3 * cos(2 * M_PI * t / trajectory_len);
    double vy = -0.3 * sin(2 * M_PI * t / trajectory_len);
    twist.vel.x(vx);
    twist.vel.y(vy);

    // convert cart to joint velocities
    ik_vel_solver_->CartToJnt(joint_positions, twist, joint_velocities);

    ...
  }

Snippet of the velocity-based linear trajecotry:

for (int i = 0; i < trajectory_len; i++)
  {
    // set endpoint twist
    double t = i;
    // compute the desired linear x velocity once and set both x and y components
    double vx = 2.0 * 0.3 * cos(2 * M_PI * t / trajectory_len);
    twist.vel.x(vx);
    twist.vel.y(vx);

    // convert cart to joint velocities
    ik_vel_solver_->CartToJnt(joint_positions, twist, joint_velocities);

    ...
  }

However, in this lab you'll implement a Cartesian interpolation using the robot under a joint position controller (using the same Inverse Kinematics solver that is already implemented with the Kinemcatics and Dynamics Library (KDL) — if you're interested, there are other libraries that can also do this (and much more), like Pinocchio).

---

3. Understanding the code

3.1. send_trajectory.cpp

This script performs the complete interpolation during all proposed segments. Let's see the code (each part of the code is explained in detail below):

Show send_trajectory.cpp

send_trajectory.cpp

// Author: Juan M. Gandarias
// email: jmgandarias@uma.es
// web: https://jmgandarias.com
//
// Inspired from the ros2_control Development Team demos

#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainiksolverpos_nr.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/tree.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <rclcpp/rclcpp.hpp>
#include <trajectory_msgs/msg/joint_trajectory.hpp>
#include <trajectory_msgs/msg/joint_trajectory_point.hpp>
#include <Eigen/Dense>
#include <tf2/LinearMath/Transform.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>
#include <yaml-cpp/yaml.h>

#include <fstream>
#include <iomanip>
#include <string>
#include <chrono>
#include <ctime>
#include <sstream>
#include <cerrno>
#include <cstring>
#include <sys/stat.h>
#include <sys/types.h>

Eigen::Matrix4d ParsePoseMatrix(const YAML::Node &root, const std::string &key)
{
    // This function parses a 4x4 matrix from a YAML node given a specific key word.
    if (!root[key] || !root[key].IsSequence() || root[key].size() != 4)
    {
        throw std::runtime_error("YAML key '" + key + "' must be a 4x4 matrix sequence");
    }

    Eigen::Matrix4d pose;
    for (int row = 0; row < 4; ++row)
    {
        const YAML::Node row_node = root[key][row];
        if (!row_node.IsSequence() || row_node.size() != 4)
        {
            throw std::runtime_error("YAML key '" + key + "' must contain rows of length 4");
        }
        for (int col = 0; col < 4; ++col)
        {
            pose(row, col) = row_node[col].as<double>();
        }
    }

    return pose;
}

tf2::Quaternion MuliplyQuaternions(const tf2::Quaternion &q1, const tf2::Quaternion &q2)
{
    // This function multiplies two quaternions q1 and q2 and returns the resulting quaternion.
    // The multiplication is defined as:
    // q_result = q1 * q2
    // where q1 and q2 are represented as (x, y, z, w) and the multiplication is performed using the Hamilton product.

    double x1 = q1.x();
    double y1 = q1.y();
    double z1 = q1.z();
    double w1 = q1.w();

    double x2 = q2.x();
    double y2 = q2.y();
    double z2 = q2.z();
    double w2 = q2.w();

    double x_result = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2;
    double y_result = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2;
    double z_result = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2;
    double w_result = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2;

    return tf2::Quaternion(x_result, y_result, z_result, w_result);
}

tf2::Quaternion InverseQuaternion(const tf2::Quaternion &q)
{
    // This function computes the inverse of a quaternion q and returns the resulting quaternion.
    // The inverse of a quaternion q = (x, y, z, w) is given by:
    // q_inv = (-x, -y, -z, w) / (x^2 + y^2 + z^2 + w^2)
    // where the numerator is the conjugate of the quaternion and the denominator is the norm squared of the quaternion.

    double x = q.x();
    double y = q.y();
    double z = q.z();
    double w = q.w();

    double norm = sqrt(x * x + y * y + z * z + w * w);

    if (norm == 0.0)
    {
        throw std::runtime_error("Cannot compute inverse of a zero-norm quaternion");
    }

    double x_inv = -x / norm;
    double y_inv = -y / norm;
    double z_inv = -z / norm;
    double w_inv = w / norm;

    return tf2::Quaternion(x_inv, y_inv, z_inv, w_inv);
}

tf2::Quaternion rot2Quat(const Eigen::Matrix3d &R, int m = 1)
{
    // This function converts a rotation matrix R to a quaternion representation.
    int M = (m >= 0) ? 1 : -1;
    double w = M * std::sqrt(R(0, 0) + R(1, 1) + R(2, 2) + 1.0) / 2.0;

    double x, y, z;

    if (std::abs(w) > 1e-3)
    {
        x = (R(2, 1) - R(1, 2)) / (4.0 * w);
        y = (R(0, 2) - R(2, 0)) / (4.0 * w);
        z = (R(1, 0) - R(0, 1)) / (4.0 * w);
    }
    else
    {
        w = 0.0;
        x = M * std::sqrt((R(0, 0) + 1.0) / 2.0);
        y = M * std::sqrt((R(1, 1) + 1.0) / 2.0);
        z = M * std::sqrt((R(2, 2) + 1.0) / 2.0);
    }

    return tf2::Quaternion(x, y, z, w);
}

// To be completed in Exercise 1
std::pair<tf2::Vector3, tf2::Quaternion> PoseInterpolation(
    const Eigen::Matrix4d &start_pose,
    const Eigen::Matrix4d &end_pose,
    double lambda)
{
    tf2::Vector3 p_interp;    // Placeholder for the interpolated position
    tf2::Quaternion q_interp; // Placeholder for the interpolated quaternion

    return {p_interp, q_interp};
}

// To be completed in Exercise 2
std::pair<tf2::Vector3, tf2::Quaternion> ComputeNextCartesianPose(
    const Eigen::Matrix4d &pose_0,
    const Eigen::Matrix4d &pose_1,
    const Eigen::Matrix4d &pose_2,
    double tau,
    double T,
    double t)
{
    // Check if t is within the valid range of [-T, T]
    if (t < -T || t > T)
    {
        throw std::out_of_range("Parameter t is outside [-T, T]");
    }

    const tf2::Vector3 p_interp; // Placeholder for the interpolated position
    tf2::Quaternion q_interp;    // Placeholder for the interpolated quaternion
    return {p_interp, q_interp};
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<rclcpp::Node>("send_trajectory");
    auto pub = node->create_publisher<trajectory_msgs::msg::JointTrajectory>(
        "/r6bot_controller/joint_trajectory", 10);

    // get robot description
    auto robot_param = rclcpp::Parameter();
    node->declare_parameter("robot_description", rclcpp::ParameterType::PARAMETER_STRING);
    node->get_parameter("robot_description", robot_param);
    auto robot_description = robot_param.as_string();

    // create kinematic chain
    KDL::Tree robot_tree;
    KDL::Chain chain;
    kdl_parser::treeFromString(robot_description, robot_tree);
    robot_tree.getChain("base_link", "tool0", chain);

    auto joint_positions = KDL::JntArray(chain.getNrOfJoints());
    // create KDL solvers
    auto fk_solver_ = std::make_shared<KDL::ChainFkSolverPos_recursive>(chain);
    auto ik_vel_solver_ = std::make_shared<KDL::ChainIkSolverVel_pinv>(chain, 0.0000001);
    auto ik_pos_solver_ = std::make_shared<KDL::ChainIkSolverPos_NR>(
        chain, *fk_solver_, *ik_vel_solver_, 100, 1e-6);

    trajectory_msgs::msg::JointTrajectory trajectory_msg;
    trajectory_msg.header.stamp = node->now();
    for (size_t i = 0; i < chain.getNrOfSegments(); i++)
    {
        auto joint = chain.getSegment(i).getJoint();
        if (joint.getType() != KDL::Joint::Fixed)
        {
            trajectory_msg.joint_names.push_back(joint.getName());
        }
    }

    trajectory_msgs::msg::JointTrajectoryPoint trajectory_point_msg;
    trajectory_point_msg.positions.resize(chain.getNrOfJoints());
    trajectory_point_msg.velocities.resize(chain.getNrOfJoints());

    std::string poses_yaml_path;
    node->declare_parameter<std::string>("poses_yaml", "");
    node->get_parameter("poses_yaml", poses_yaml_path);

    if (poses_yaml_path.empty())
    {
        poses_yaml_path =
            ament_index_cpp::get_package_share_directory("cartesian_trajectory_planning") +
            "/config/poses.yaml";
    }

    Eigen::Matrix4d pose0;
    Eigen::Matrix4d pose1;
    Eigen::Matrix4d pose2;
    try
    {
        const YAML::Node poses_root = YAML::LoadFile(poses_yaml_path);
        pose0 = ParsePoseMatrix(poses_root, "pose0");
        pose1 = ParsePoseMatrix(poses_root, "pose1");
        pose2 = ParsePoseMatrix(poses_root, "pose2");
    }
    catch (const std::exception &e)
    {
        std::fprintf(stderr, "Failed to load poses YAML '%s': %s\n", poses_yaml_path.c_str(), e.what());
        return 1;
    }

    // Exercise 1 : Cartesian interpolation
    const auto [p0, q0] = PoseInterpolation(pose0, pose1, 0.0);
    const auto [p1, q1] = PoseInterpolation(pose0, pose1, 1.0);
    const auto [p2, q2] = PoseInterpolation(pose1, pose2, 0.0);
    const auto [p3, q3] = PoseInterpolation(pose1, pose2, 1.0);

    // Check the results of the interpolation
    printf("p0: %f, %f, %f\n", p0.x(), p0.y(), p0.z());
    printf("q0: %f, %f, %f, %f\n",
           q0.x(), q0.y(), q0.z(), q0.w());
    printf("p1: %f, %f, %f\n", p1.x(), p1.y(), p1.z());
    printf("q1: %f, %f, %f, %f\n",
           q1.x(), q1.y(), q1.z(), q1.w());
    printf("p2: %f, %f, %f\n", p2.x(), p2.y(), p2.z());
    printf("q2: %f, %f, %f, %f\n",
           q2.x(), q2.y(), q2.z(), q2.w());
    printf("p3: %f, %f, %f\n", p3.x(), p3.y(), p3.z());
    printf("q3: %f, %f, %f, %f\n",
           q3.x(), q3.y(), q3.z(), q3.w());

    // Exercise 2 : Cartesian trajectory generation
    int tau = 1;
    int T = 10;
    bool exercise_2 = false; // Set to true to execute Exercise 2

    if (exercise_2)
    {
        trajectory_msg.points.clear();
        const double sample_time = 0.1;
        int point_index = 0;

        const std::string package_name = "cartesian_trajectory_planning";
        const std::string package_share_dir =
            ament_index_cpp::get_package_share_directory(package_name);

        std::string output_base_dir = package_share_dir;
        const std::string install_suffix =
            "/install/" + package_name + "/share/" + package_name;
        const std::size_t suffix_pos = package_share_dir.rfind(install_suffix);
        if (suffix_pos != std::string::npos)
        {
            const std::string workspace_root = package_share_dir.substr(0, suffix_pos);
            const std::string source_candidate = workspace_root + "/src/" + package_name;

            struct stat source_candidate_stat;
            if (::stat(source_candidate.c_str(), &source_candidate_stat) == 0 &&
                S_ISDIR(source_candidate_stat.st_mode))
            {
                output_base_dir = source_candidate;
            }
        }

        const std::string output_dir = output_base_dir + "/experiment_data";

        if (::mkdir(output_dir.c_str(), 0755) != 0 && errno != EEXIST)
        {
            std::fprintf(stderr, "Failed to create output directory '%s': %s.\n", output_dir.c_str(), std::strerror(errno));
            return 1;
        }

        const auto now = std::chrono::system_clock::now();
        const std::time_t now_time_t = std::chrono::system_clock::to_time_t(now);
        std::tm local_tm;
        localtime_r(&now_time_t, &local_tm);

        std::ostringstream filename_builder;
        filename_builder << "data_" << std::put_time(&local_tm, "%Y%m%d_%H%M%S") << ".csv";
        const std::string output_csv = output_dir + "/" + filename_builder.str();
        std::ofstream csv_file(output_csv, std::ios::out | std::ios::trunc);
        if (!csv_file.is_open())
        {
            std::fprintf(stderr, "Failed to open CSV output file '%s'.\n", output_csv.c_str());
            return 1;
        }

        csv_file << "t,X,Y,Z,roll,pitch,yaw\n";
        csv_file << std::fixed << std::setprecision(9);

        // Loop over the time range from -T to T with a step of sample_time and compute the interpolated Cartesian pose at each time step
        for (double t = -T; t <= T + 1e-9; t += sample_time)
        {
            const auto [p_interp, q_interp] = ComputeNextCartesianPose(pose0, pose1, pose2, tau, T, t);

            double roll = 0.0;
            double pitch = 0.0;
            double yaw = 0.0;
            tf2::Matrix3x3(q_interp).getRPY(roll, pitch, yaw);

            csv_file << t << ","
                     << p_interp.x() << "," << p_interp.y() << "," << p_interp.z() << ","
                     << roll << "," << pitch << "," << yaw << "\n";

            const KDL::Frame desired_ee_pose(
                KDL::Rotation::Quaternion(q_interp.x(), q_interp.y(), q_interp.z(), q_interp.w()),
                KDL::Vector(p_interp.x(), p_interp.y(), p_interp.z()));

            KDL::JntArray next_joint_positions(chain.getNrOfJoints());
            const int ik_status = ik_pos_solver_->CartToJnt(joint_positions, desired_ee_pose, next_joint_positions);
            if (ik_status < 0)
            {
                std::fprintf(
                    stderr,
                    "IK failed at t=%.3f with error code %d. Skipping point.\n",
                    t, ik_status);
                continue;
            }

            std::memcpy(
                trajectory_point_msg.positions.data(), next_joint_positions.data.data(),
                trajectory_point_msg.positions.size() * sizeof(double));

            std::fill(trajectory_point_msg.velocities.begin(), trajectory_point_msg.velocities.end(), 0.0);

            const double elapsed = static_cast<double>(point_index + 1) * sample_time;
            trajectory_point_msg.time_from_start.sec = static_cast<int32_t>(elapsed);
            trajectory_point_msg.time_from_start.nanosec = static_cast<uint32_t>(
                (elapsed - static_cast<double>(trajectory_point_msg.time_from_start.sec)) * 1e9);

            trajectory_msg.points.push_back(trajectory_point_msg);
            joint_positions = next_joint_positions;
            point_index++;
        }

        if (trajectory_msg.points.empty())
        {
            std::fprintf(stderr, "No valid trajectory points were generated. Nothing will be published.\n");
            return 1;
        }

        while (pub->get_subscription_count() == 0 && rclcpp::ok())
        {
            std::fprintf(stderr, "Waiting for /r6bot_controller/joint_trajectory subscriber...\n");
            rclcpp::sleep_for(std::chrono::milliseconds(200));
        }

        trajectory_msg.header.stamp = node->now() + rclcpp::Duration::from_seconds(0.2);
        std::fprintf(stderr, "Publishing %zu trajectory points.\n", trajectory_msg.points.size());
        pub->publish(trajectory_msg);
        while (rclcpp::ok())
        {
            rclcpp::spin_some(node);
            rclcpp::sleep_for(std::chrono::milliseconds(50));
        }
    }

    return 0;
}

Warning

This repo is under development and keeps improving. To check the latest version, visit the github repo.

3.2. Code parts explained

• Eigen::Matrix4d ParsePoseMatrix(const YAML::Node &root, const std::string &key): This function parses a 4x4 matrix from a YAML file given a specific key word. It's used in the code to parse the pose matrices defined in config/poses.yaml.

  Show ParsePoseMatrix

  ParsePoseMatrix.cpp

  Eigen::Matrix4d ParsePoseMatrix(const YAML::Node &root, const std::string &key)
  {
      // This function parses a 4x4 matrix from a YAML node given a specific key word.
      if (!root[key] || !root[key].IsSequence() || root[key].size() != 4)
      {
          throw std::runtime_error("YAML key '" + key + "' must be a 4x4 matrix sequence");
      }
  
      Eigen::Matrix4d pose;
      for (int row = 0; row < 4; ++row)
      {
          const YAML::Node row_node = root[key][row];
          if (!row_node.IsSequence() || row_node.size() != 4)
          {
              throw std::runtime_error("YAML key '" + key + "' must contain rows of length 4");
          }
          for (int col = 0; col < 4; ++col)
          {
              pose(row, col) = row_node[col].as<double>();
          }
      }
  
      return pose;
  }
  

  Info

  In the course you'll use the Eigen library to work with linear algebra (matrices, vectors, etc) in C++. In this part of the code you are defining the object pose from the Matrix class.

  Info

  In the course you'll use YAML files.

• tf2::Quaternion MuliplyQuaternions(const tf2::Quaternion &q1, const tf2::Quaternion &q2) : This function multiplies two quaternions q1 and q2 and returns the resulting quaternion. The multiplication is defined as: \(q_{result} = q_1 * q_2\), where \(q_1\) and \(q_2\) are represented as \((x, y, z, w)\) and the product is performed using the Hamilton product.

  Show MuliplyQuaternions

  MuliplyQuaternions.cpp

  tf2::Quaternion MuliplyQuaternions(const tf2::Quaternion &q1, const tf2::Quaternion &q2)
  {
      // This function multiplies two quaternions q1 and q2 and returns the resulting quaternion.
      // The multiplication is defined as:
      // q_result = q1 * q2
      // where q1 and q2 are represented as (x, y, z, w) and the multiplication is performed using the Hamilton product.
  
      double x1 = q1.x();
      double y1 = q1.y();
      double z1 = q1.z();
      double w1 = q1.w();
  
      double x2 = q2.x();
      double y2 = q2.y();
      double z2 = q2.z();
      double w2 = q2.w();
  
      double x_result = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2;
      double y_result = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2;
      double z_result = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2;
      double w_result = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2;
  
      return tf2::Quaternion(x_result, y_result, z_result, w_result);
  }
  

  Info

  In the course you'll use the tf2 package to work with coordinate frames and take advantage of useful classes and methods. In this part of the code you are using the class Quaternion.

• tf2::Quaternion InverseQuaternion(const tf2::Quaternion &q) : This function computes the inverse of a quaternion \(q\) and returns the resulting quaternion.

  Show InverseQuaternion

  InverseQuaternion.cpp

  tf2::Quaternion InverseQuaternion(const tf2::Quaternion &q)
  {
      // This function computes the inverse of a quaternion q and returns the resulting quaternion.
      // The inverse of a quaternion q = (x, y, z, w) is given by:
      // q_inv = (-x, -y, -z, w) / (x^2 + y^2 + z^2 + w^2)
      // where the numerator is the conjugate of the quaternion and the denominator is the norm squared of the quaternion.
  
      double x = q.x();
      double y = q.y();
      double z = q.z();
      double w = q.w();
  
      double norm = sqrt(x * x + y * y + z * z + w * w);
  
      if (norm == 0.0)
      {
          throw std::runtime_error("Cannot compute inverse of a zero-norm quaternion");
      }
  
      double x_inv = -x / norm;
      double y_inv = -y / norm;
      double z_inv = -z / norm;
      double w_inv = w / norm;
  
      return tf2::Quaternion(x_inv, y_inv, z_inv, w_inv);
  }
  

• tf2::Quaternion rot2Quat(const Eigen::Matrix3d &R, int m = 1) : This function converts a rotation matrix R to a quaternion representation.

  Show rot2Quat

  rot2Quat.cpp

  tf2::Quaternion rot2Quat(const Eigen::Matrix3d &R, int m = 1)
  {
      // This function converts a rotation matrix R to a quaternion representation.
      int M = (m >= 0) ? 1 : -1;
      double w = M * std::sqrt(R(0, 0) + R(1, 1) + R(2, 2) + 1.0) / 2.0;
  
      double x, y, z;
  
      if (std::abs(w) > 1e-3)
      {
          x = (R(2, 1) - R(1, 2)) / (4.0 * w);
          y = (R(0, 2) - R(2, 0)) / (4.0 * w);
          z = (R(1, 0) - R(0, 1)) / (4.0 * w);
      }
      else
      {
          w = 0.0;
          x = M * std::sqrt((R(0, 0) + 1.0) / 2.0);
          y = M * std::sqrt((R(1, 1) + 1.0) / 2.0);
          z = M * std::sqrt((R(2, 2) + 1.0) / 2.0);
      }
  
      return tf2::Quaternion(x, y, z, w);
  }
  

• std::pair<tf2::Vector3, tf2::Quaternion> PoseInterpolation(const Eigen::Matrix4d &start_pose, const Eigen::Matrix4d &end_pose, double lambda) : You must implement this function in the Exercise 1

  PoseInterpolation.cpp

  // To be completed in Exercise 1
  std::pair<tf2::Vector3, tf2::Quaternion> PoseInterpolation(
      const Eigen::Matrix4d &start_pose,
      const Eigen::Matrix4d &end_pose,
      double lambda)
  {
      tf2::Vector3 p_interp;    // Placeholder for the interpolated position
      tf2::Quaternion q_interp; // Placeholder for the interpolated quaternion
  
      return {p_interp, q_interp};
  }
  

• std::pair<tf2::Vector3, tf2::Quaternion> ComputeNextCartesianPose(const Eigen::Matrix4d &pose_0, const Eigen::Matrix4d &pose_1, const Eigen::Matrix4d &pose_2, double tau, double T, double t) : You must implement this function in the Exercise 2

  ComputeNextCartesianPose.cpp

  // To be completed in Exercise 2
  std::pair<tf2::Vector3, tf2::Quaternion> ComputeNextCartesianPose(
      const Eigen::Matrix4d &pose_0,
      const Eigen::Matrix4d &pose_1,
      const Eigen::Matrix4d &pose_2,
      double tau,
      double T,
      double t)
  {
      // Check if t is within the valid range of [-T, T]
      if (t < -T || t > T)
      {
          throw std::out_of_range("Parameter t is outside [-T, T]");
      }
  
      const tf2::Vector3 p_interp; // Placeholder for the interpolated position
      tf2::Quaternion q_interp;    // Placeholder for the interpolated quaternion
      return {p_interp, q_interp};
  }
  

• int main(int argc, char **argv) - First part : The first part of the main include code needed to run the application that you don't need to change. The first part of the main initialices the node and publisher, gets the robot description, creates the kinematic chain and solvers using KDL, creates the ROS 2 joint trajectory message that will be later filled in with the points you'll calculate, and gets the poses (pose0, pose1, pose2) from the config/poses.yaml.

  Show int main(int argc, char **argv)

  main.cpp

  int main(int argc, char **argv)
  {
      rclcpp::init(argc, argv);
      auto node = std::make_shared<rclcpp::Node>("send_trajectory");
      auto pub = node->create_publisher<trajectory_msgs::msg::JointTrajectory>(
          "/r6bot_controller/joint_trajectory", 10);
  
      // get robot description
      auto robot_param = rclcpp::Parameter();
      node->declare_parameter("robot_description", rclcpp::ParameterType::PARAMETER_STRING);
      node->get_parameter("robot_description", robot_param);
      auto robot_description = robot_param.as_string();
  
      // create kinematic chain
      KDL::Tree robot_tree;
      KDL::Chain chain;
      kdl_parser::treeFromString(robot_description, robot_tree);
      robot_tree.getChain("base_link", "tool0", chain);
  
      auto joint_positions = KDL::JntArray(chain.getNrOfJoints());
      // create KDL solvers
      auto fk_solver_ = std::make_shared<KDL::ChainFkSolverPos_recursive>(chain);
      auto ik_vel_solver_ = std::make_shared<KDL::ChainIkSolverVel_pinv>(chain, 0.0000001);
      auto ik_pos_solver_ = std::make_shared<KDL::ChainIkSolverPos_NR>(
          chain, *fk_solver_, *ik_vel_solver_, 100, 1e-6);
  
      trajectory_msgs::msg::JointTrajectory trajectory_msg;
      trajectory_msg.header.stamp = node->now();
      for (size_t i = 0; i < chain.getNrOfSegments(); i++)
      {
          auto joint = chain.getSegment(i).getJoint();
          if (joint.getType() != KDL::Joint::Fixed)
          {
              trajectory_msg.joint_names.push_back(joint.getName());
          }
      }
  
      trajectory_msgs::msg::JointTrajectoryPoint trajectory_point_msg;
      trajectory_point_msg.positions.resize(chain.getNrOfJoints());
      trajectory_point_msg.velocities.resize(chain.getNrOfJoints());
  
      std::string poses_yaml_path;
      node->declare_parameter<std::string>("poses_yaml", "");
      node->get_parameter("poses_yaml", poses_yaml_path);
  
      if (poses_yaml_path.empty())
      {
          poses_yaml_path =
              ament_index_cpp::get_package_share_directory("cartesian_trajectory_planning") +
              "/config/poses.yaml";
      }
  
      Eigen::Matrix4d pose0;
      Eigen::Matrix4d pose1;
      Eigen::Matrix4d pose2;
      try
      {
          const YAML::Node poses_root = YAML::LoadFile(poses_yaml_path);
          pose0 = ParsePoseMatrix(poses_root, "pose0");
          pose1 = ParsePoseMatrix(poses_root, "pose1");
          pose2 = ParsePoseMatrix(poses_root, "pose2");
      }
      catch (const std::exception &e)
      {
          std::fprintf(stderr, "Failed to load poses YAML '%s': %s\n", poses_yaml_path.c_str(), e.what());
          return 1;
      }
  

• Cartesian interpolation : This block runs the Exercise 1 code and checks your interpolation function. It calls PoseInterpolation at the endpoints of two segments:

  • pose0 -> pose1 with \(\lambda = 0\) and \(\lambda = 1\)
  • pose1 -> pose2 with \(\lambda = 0\) and \(\lambda = 1\)

  It stores each result as position (p) and orientation quaternion (q) and prints all four positions and quaternions to verify correctness. What you should expect if PoseInterpolation is implemented properly:

  • p0, q0 should match pose0
  • p1, q1 should match pose1
  • p2, q2 should also match pose1 (start of second segment)
  • p3, q3 should match pose2

  So this code does not generate the trajectory yet; it just validates that interpolation gives the correct boundary poses.

  exercise1.cpp

  // Exercise 1 : Cartesian interpolation
  const auto [p0, q0] = PoseInterpolation(pose0, pose1, 0.0);
  const auto [p1, q1] = PoseInterpolation(pose0, pose1, 1.0);
  const auto [p2, q2] = PoseInterpolation(pose1, pose2, 0.0);
  const auto [p3, q3] = PoseInterpolation(pose1, pose2, 1.0);
  
  // Check the results of the interpolation
  printf("p0: %f, %f, %f\n", p0.x(), p0.y(), p0.z());
  printf("q0: %f, %f, %f, %f\n",
         q0.x(), q0.y(), q0.z(), q0.w());
  printf("p1: %f, %f, %f\n", p1.x(), p1.y(), p1.z());
  printf("q1: %f, %f, %f, %f\n",
         q1.x(), q1.y(), q1.z(), q1.w());
  printf("p2: %f, %f, %f\n", p2.x(), p2.y(), p2.z());
  printf("q2: %f, %f, %f, %f\n",
         q2.x(), q2.y(), q2.z(), q2.w());
  printf("p3: %f, %f, %f\n", p3.x(), p3.y(), p3.z());
  printf("q3: %f, %f, %f, %f\n",
         q3.x(), q3.y(), q3.z(), q3.w());
  

• Smooth trajectory generation. Configuration : This block is the first part of the code needed for the Exercise 2. It is disabled by default. you have to set the variable exercise_2 = true once you've finished the Exercise 1. It defines: \(\tau = 1\) and \(T = 10\) as trajectory timing parameters. If enabled, it enables the trajectory generation.

  exercise2_part1.cpp

  // Exercise 2 : Cartesian trajectory generation
  int tau = 1;
  int T = 10;
  bool exercise_2 = false; // Set to true to execute Exercise 2
  

• Exercise 2: Smooth trajectory generation. Initialization : This block is the second part of the code needed for the Exercise 2. If enabled, it initializes trajectory generation state:

  • Clears previous points from trajectory_msg.
  • Sets sampling period sample_time = 0.1 s.
  • Initializes point_index for time stamps.
  • It resolves where to save experiment data: a .csv file in the folder experiment_data.

  This part is mostly preparation: configure run parameters, choose output location, and prepare a CSV logger before the actual trajectory loop starts. Important detail: with exercise_2 = false, none of this executes.

  Show Exercise 2, part 2

  exercise2_part2.cpp

  if (exercise_2)
  {
      trajectory_msg.points.clear();
      const double sample_time = 0.1;
      int point_index = 0;
  
      const std::string package_name = "cartesian_trajectory_planning";
      const std::string package_share_dir =
          ament_index_cpp::get_package_share_directory(package_name);
  
      std::string output_base_dir = package_share_dir;
      const std::string install_suffix =
          "/install/" + package_name + "/share/" + package_name;
      const std::size_t suffix_pos = package_share_dir.rfind(install_suffix);
      if (suffix_pos != std::string::npos)
      {
          const std::string workspace_root = package_share_dir.substr(0, suffix_pos);
          const std::string source_candidate = workspace_root + "/src/" + package_name;
  
          struct stat source_candidate_stat;
          if (::stat(source_candidate.c_str(), &source_candidate_stat) == 0 &&
              S_ISDIR(source_candidate_stat.st_mode))
          {
              output_base_dir = source_candidate;
          }
      }
  
      const std::string output_dir = output_base_dir + "/experiment_data";
  
      if (::mkdir(output_dir.c_str(), 0755) != 0 && errno != EEXIST)
      {
          std::fprintf(stderr, "Failed to create output directory '%s': %s.\n", output_dir.c_str(), std::strerror(errno));
          return 1;
      }
  
      const auto now = std::chrono::system_clock::now();
      const std::time_t now_time_t = std::chrono::system_clock::to_time_t(now);
      std::tm local_tm;
      localtime_r(&now_time_t, &local_tm);
  
      std::ostringstream filename_builder;
      filename_builder << "data_" << std::put_time(&local_tm, "%Y%m%d_%H%M%S") << ".csv";
      const std::string output_csv = output_dir + "/" + filename_builder.str();
      std::ofstream csv_file(output_csv, std::ios::out | std::ios::trunc);
      if (!csv_file.is_open())
      {
          std::fprintf(stderr, "Failed to open CSV output file '%s'.\n", output_csv.c_str());
          return 1;
      }
  
      csv_file << "t,X,Y,Z,roll,pitch,yaw\n";
      csv_file << std::fixed << std::setprecision(9);
  

• Exercise 2: Smooth trajectory generation. Time loop : This block is the third part of the code needed for the Exercise 2. This loop is the core trajectory sampler: for each time step, it computes one Cartesian pose, converts it to joint space, and appends one trajectory point.

  • The loop iterates from \(t= −T\) to \(t=T\) in steps of sample_time.
  • In every step, it calls ComputeNextCartesianPose (that you need to implement in Exercise 2) to get the interpolated position p_interp and orientation quaternion q_interp at time t.
  • It also logs the data for analysis: Converts quaternion to roll/pitch/yaw, and writes t, X, Y, Z, roll, pitch, yaw of every trajectory point to the CSV file.
  • Packs p_interp and q_interp into a KDL::Frame (desired_ee_pose) for the end-effector. Then, it solves IK from current joint_positions to desired_ee_pose. If IK fails, prints an error and skips that time sample (continue).
  • It copies solved joint angles into trajectory_point_msg.positions and pushes the point into trajectory_msg.points.
  • Updates joint_positions with the solved values so next IK step starts near the previous one (improves continuity/convergence).

  So in short: Cartesian path sample -> IK -> trajectory point, repeated until the whole ROS joint trajectory is built.

  Show Exercise 2, part 3

  exercise2_part3.cpp

  // Loop over the time range from -T to T with a step of sample_time and compute the interpolated Cartesian pose at each time step
  for (double t = -T; t <= T + 1e-9; t += sample_time)
  {
      const auto [p_interp, q_interp] = ComputeNextCartesianPose(pose0, pose1, pose2, tau, T, t);
  
      double roll = 0.0;
      double pitch = 0.0;
      double yaw = 0.0;
      tf2::Matrix3x3(q_interp).getRPY(roll, pitch, yaw);
  
      csv_file << t << ","
               << p_interp.x() << "," << p_interp.y() << "," << p_interp.z() << ","
               << roll << "," << pitch << "," << yaw << "\n";
  
      const KDL::Frame desired_ee_pose(
          KDL::Rotation::Quaternion(q_interp.x(), q_interp.y(), q_interp.z(), q_interp.w()),
          KDL::Vector(p_interp.x(), p_interp.y(), p_interp.z()));
  
      KDL::JntArray next_joint_positions(chain.getNrOfJoints());
      const int ik_status = ik_pos_solver_->CartToJnt(joint_positions, desired_ee_pose, next_joint_positions);
      if (ik_status < 0)
      {
          std::fprintf(
              stderr,
              "IK failed at t=%.3f with error code %d. Skipping point.\n",
              t, ik_status);
          continue;
      }
  
      std::memcpy(
          trajectory_point_msg.positions.data(), next_joint_positions.data.data(),
          trajectory_point_msg.positions.size() * sizeof(double));
  
      std::fill(trajectory_point_msg.velocities.begin(), trajectory_point_msg.velocities.end(), 0.0);
  
      const double elapsed = static_cast<double>(point_index + 1) * sample_time;
      trajectory_point_msg.time_from_start.sec = static_cast<int32_t>(elapsed);
      trajectory_point_msg.time_from_start.nanosec = static_cast<uint32_t>(
          (elapsed - static_cast<double>(trajectory_point_msg.time_from_start.sec)) * 1e9);
  
      trajectory_msg.points.push_back(trajectory_point_msg);
      joint_positions = next_joint_positions;
      point_index++;
  }
  

• End of the program : This is the publish-and-wait tail of the node.

  • Safety check: If no trajectory points were generated, it prints an error and exits with code 1.
  • Wait for a subscriber: It loops until something subscribes to /r6bot_controller/joint_trajectory. While waiting, it logs a message and sleeps 200 ms to avoid busy-waiting.
  • It sets the trajectory header stamp to “now + 0.2 s”. That small delay gives the controller a little lead time before execution starts.
  • It logs how many points will be sent, then publishes the full trajectory message.
  • It enters a loop while ROS is running. This keeps the process alive (instead of exiting immediately after publishing).

  So this section ensures the message is valid to send, waits for a listener, publishes once with a future start time, and then keeps the node running.

  Show End of the code

  end_of_code.cpp

  if (trajectory_msg.points.empty())
  {
      std::fprintf(stderr, "No valid trajectory points were generated. Nothing will be published.\n");
      return 1;
  }
  
  while (pub->get_subscription_count() == 0 && rclcpp::ok())
  {
      std::fprintf(stderr, "Waiting for /r6bot_controller/joint_trajectory subscriber...\n");
      rclcpp::sleep_for(std::chrono::milliseconds(200));
  }
  
  trajectory_msg.header.stamp = node->now() + rclcpp::Duration::from_seconds(0.2);
  std::fprintf(stderr, "Publishing %zu trajectory points.\n", trajectory_msg.points.size());
  pub->publish(trajectory_msg);
  while (rclcpp::ok())
  {
      rclcpp::spin_some(node);
      rclcpp::sleep_for(std::chrono::milliseconds(50));
  }
  }
  
  return 0;
  }
  

---

4. Smooth Cartesian interpolation

Cartesian interpolation is characterized by achieving a linear variation of position and orientation. While when interpolating the position we can conduct a linear interpolation in the Cartesian space, for the orientation, the interpolation depends on how the orientation is represented. As described in the course, the most appropriate way to do this is by using quaternions.

Info

Hence, you're performing a Spherical Linear Interpolation (slerp). Original paper here. The slerp method is actually implemented in tf2 (you can check the code of the implementation ros2 humble here) but you're going to implement it by yourself in this lab.

Also, when linking two rectilinear displacements, a velocity discontinuity occurs at the transition point. Figure 1 shows the described situation, using the example of concatenating a displacement from location \(P_0\) to \(P_1\) with another from \(P_1\) to \(P_2\). To avoid the velocity discontinuity that would occur at \(P_1\), a constant acceleration is used to adapt the velocity variation of vector \(X\) from the first segment to the second, resulting in a smooth transition across \(P_1\).

Figure 1. Diagram of the variation of position and velocity in the movement from \(P_0\) to \(P_2\) via \(P_1\).

This way, \(-\tau\) units of time before reaching \(P_1\) (time 0), the velocity will be linearly changed from \(\Delta P_1/T_1\) to \(\Delta P_2/T_2\), to accommodate the velocity \(\tau\) units of time after passing \(P_1\). Thus, the problem is defined as the calculation of a quadratic function \(X(t)\) that starts at point \(P_A\) and ends at \(P_B\) (start and end points of the smoothing) defined in the time range \([-\tau, \tau]\).

Applying the boundary conditions at both ends of the segment and defining the acceleration in the area, the position is obtained as:

$$ \mathbf{p}(t) = \mathbf{p}_1 - \frac{(\tau - t)^2}{4\tau T_1} \Delta \mathbf{p}_1 + \frac{(\tau + t)^2}{4\tau T_2} \Delta \mathbf{p}_2 $$

And the orientation as:

$$ \mathbf{q}(t) = \mathbf{q}_1 \cdot \mathbf{q} \left[\frac{-(\tau - t)^2}{4\tau T_1} \theta_1, \mathbf{n}_1 \right] \cdot \mathbf{q} \left[ \frac{(\tau + t)^2}{4\tau T_2} \theta_2, \mathbf{n}_2 \right] $$

5. Exercises

Considering all the above, and the following values for \(\mathbf{P}_0, \mathbf{P}_1, \mathbf{P}_2\), the following exercises are requested:

\[ \mathbf{P}_0 = \begin{bmatrix} 0 & 1 & 0 & -0.187\\ 1 & 0 & 0 & 1.038\\ 0 & 0 & -1 & 0.307\\ 0 & 0 & 0 & 1\\ \end{bmatrix}, \quad \mathbf{P}_1 = \begin{bmatrix} 0 & 1 & 0 & 0.150\\ 0 & 0 & -1 & 0.638\\ 1 & 0 & 0 & 0.607\\ 0 & 0 & 0 & 1\\ \end{bmatrix}, \quad \mathbf{P}_2 = \begin{bmatrix} 0 & 0 & -1 & -0.950\\ 0 & 1 & 0 & 0.638\\ 1 & 0 & 0 & 0.607\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \]

5.1. Exercise 1: Cartesian interpolation

Define the quaternion interpolation function based on the Taylor method [pr, qr]=qpinter(P1, P2, lambda) that calculates the intermediate quaternion between \(q_1\) (initial) and \(q_2\) (final). The value \(\lambda\) must satisfy \(0\leq \lambda \leq 1\), so that [p1, q1]=qpinter(P1, P2, 0) and [p2, q2]=qpinter(P1, P2, 1).

5.2. Exercise 2: Smooth trajectory generation

Create a MATLAB function in the format P=generate_smooth_path(P0, P1, P2, tau, T, t) that calculates the transformation \(P\) corresponding to the movement from \(P_0\) to \(P_2\) via \(P_1\) smoothed by the Taylor method. The parameters \(\tau\) and \(T\) correspond respectively to the transition interval and total time used to traverse the path as shown in Figure 1, and \(T\) indicates the time at which the location of the calculated path \(P\) is reached.

6. Graphical representation

Plot the evolution of position and orientation (in ZYZ Euler angles) throughout the trajectory.

7. Expected results

The expected result is illustrated in the following video and figures:

Video 1. Expected result of the lab session.

Figure 2. Final configuration and smooth path.

Figure 3. Position trajectories.

Figure 4. Orientation trajectories.

7. Extra (optional)

Create a node that implements the PD controller presented in Fig. 4 (stabilizing linear control block) of the lecture slides. Specify a desired joint position \(\mathbf{q}_d\) (inside the joint workspace) and set \(\dot{\mathbf{q}}_d = \boldsymbol{0}\), \(\ddot{\mathbf{q}}_d = \boldsymbol{0}\).

This node must subscribe to the current joint state topic /joint_states to get the current joint positions (\(\mathbf{q}\)) and velocities (\(\dot{\mathbf{q}}\)); and it must then publish the desired joint accelerations (\(\ddot{\mathbf{q}}_d\)) in the topic /desired_joint_accelerations to which the dynamics cancellation node will subscribe.

Select and report the values chosen for \(\mathbf{K}_P\) and \(\mathbf{K}_D\). You can use matlab to simulate the expected dynamic behavior of the overall system.