Trajectories being recorded with extremely small values

[Abhishek8857]

I am trying to record the trajectories the robot takes when I exectue joint position based movements. However, the recorded trajectories have extremely minute values that the robot maybe is not able to recognise when I try to replay.

Executing Motion + Trajctory Recording (listens to published coordinates and generated and executes motion plan)

# include <memory>
# include <rclcpp/rclcpp.hpp>
# include <geometry_msgs/msg/point.hpp>
# include <moveit/move_group_interface/move_group_interface.h>
# include <std_msgs/msg/float64_multi_array.hpp>
# include <std_msgs/msg/bool.hpp>
# include <thread>
# include <chrono>
# include <yaml-cpp/yaml.h>
# include <fstream>
# include <iomanip>
# include <filesystem>


using std::placeholders::_1;

class AgentSubscriber : public rclcpp::Node
{
  public:
    // Initialise the Node
    AgentSubscriber () : Node ("agent_subscriber")
    {
      // Subscribe to recieve Coordinates
      RCLCPP_INFO(this->get_logger(), "Agent subscriber node initialised. Waiting for coordinates...");
      subscription = this->create_subscription<std_msgs::msg::Float64MultiArray>(
        "published_coordinates", 10, std::bind(&AgentSubscriber::agent_callback, this, _1));
    }

  private:  
    void agent_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) 
    {
      // DEBUG: Print out the recieved Coordinates
      RCLCPP_INFO(this->get_logger(), "Recieved Cartesian Coordinates..");
      for (size_t i {0}; i < msg->data.size(); i++)
      {
        RCLCPP_INFO(this->get_logger(), " -[%zu]: %f", i, msg->data[i]);
      }

      // Lazy initialization of MoveGroupInterface
      if (!move_group_manipulator && !move_group_gripper)
      {
        move_group_manipulator = std::make_unique<moveit::planning_interface::MoveGroupInterface>(shared_from_this(), "manipulator");
        move_group_gripper = std::make_unique<moveit::planning_interface::MoveGroupInterface>(shared_from_this(), "gripper");
      } 

      // Check if we need to execute Joint Target or Pose Target
      if (msg->data[0] == 0.0)
      {
        // Parse the Coordinates
        std::map<std::string, double> joint_values = 
        {
          {"joint_1", msg->data[1]},
          {"joint_2", msg->data[2]},
          {"joint_3", msg->data[3]},
          {"joint_4", msg->data[4]},
          {"joint_5", msg->data[5]},
          {"joint_6", msg->data[6]},
          {"joint_7", msg->data[7]}
        };

        // Plan and execute
        RCLCPP_INFO(this->get_logger(), "Planning the target pose ...");
        move_group_manipulator->setJointValueTarget(joint_values);

        // Verify if the motion was successful
        // bool success = (move_group_manipulator->move() == moveit::core::MoveItErrorCode::SUCCESS);
        moveit::planning_interface::MoveGroupInterface::Plan plan;
        bool success = (move_group_manipulator->plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);

        if (success)
        {
          move_group_manipulator->execute(plan);
          std::string file_name = "trajectory_" + getTimestamp() + ".yaml";
          saveTrajectoryToFile(plan.trajectory_, file_name); 
        }
        succeed(success);
      }
      else if (msg->data[0] == 1.0)
      {
        // Parse the Coordinates
        geometry_msgs::msg::Pose target_pose;
        target_pose.position.x = msg->data[1];
        target_pose.position.y = msg->data[2];
        target_pose.position.z = msg->data[3];
  
        // Parse the orientation
        target_pose.orientation.w = msg->data[4];
        target_pose.orientation.x = msg->data[5];
        target_pose.orientation.y = msg->data[6];
        target_pose.orientation.z = msg->data[7];

        // Plan and execute
        RCLCPP_INFO(this->get_logger(), "Planning the target pose ...");
        move_group_manipulator->setPoseTarget(target_pose);

        // Verify if the motion was successful
        // bool success = (move_group_manipulator->move() == moveit::core::MoveItErrorCode::SUCCESS);
        moveit::planning_interface::MoveGroupInterface::Plan plan;

        bool success = (move_group_manipulator->plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
        if (success)
        {
          move_group_manipulator->execute(plan);     
          std::string file_name = "trajectory_" + getTimestamp() + ".yaml";
          saveTrajectoryToFile(plan.trajectory_, file_name);    
        }
        succeed(success);
      }
      else if (msg->data[0] == 2.0)
      {
        // Parse the coordinates
        std::map<std::string, double> gripper_values = 
        {
          {"robotiq_85_left_knuckle_joint", msg->data[1]},
          {"robotiq_85_right_knuckle_joint", msg->data[2]}
        };

        // Planning the Gripper motion
        RCLCPP_INFO(this->get_logger(), "Planning the Gripper motion");
        move_group_gripper->setJointValueTarget(gripper_values);

        // Verify if the motion was successful
        // bool success = (move_group_gripper->move() == moveit::core::MoveItErrorCode::SUCCESS);
        moveit::planning_interface::MoveGroupInterface::Plan plan;

        bool success = (move_group_gripper->plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
        if (success)
        {
          move_group_gripper->execute(plan);
          std::string file_name = "trajectory_" + getTimestamp() + ".yaml";
          saveTrajectoryToFile(plan.trajectory_, file_name);
        }

        succeed(success);
      }
      else
      {
        RCLCPP_INFO(this->get_logger(), "Error. Mismatched Coordinates..");
        return;
      }
    }


    void succeed (bool status)
    {
      if (status)
      {
        RCLCPP_INFO(this->get_logger(), "Motion executed successfully.");
      }
      else
      {
        RCLCPP_ERROR(this->get_logger(), "Failed to execute motion.");
      }
    }
  
  std::string getTimestamp()
    {
      auto now = std::chrono::system_clock::now();
      auto now_c = std::chrono::system_clock::to_time_t(now);
      std::stringstream ss;
      ss << std::put_time(std::localtime(&now_c), "%Y%m%d_%H%M%S");
      return ss.str();
    }

    void saveTrajectoryToFile(const moveit_msgs::msg::RobotTrajectory& traj, const std::string& filename)
    {
      std::string folder_path = "/kinova-ros2/trajectories/";
      static bool first_call = true;
      std::filesystem::path dir(folder_path);
      
      // Create Directory if it is not present
      if (first_call)
      {
        if(std::filesystem::exists(dir))
        {
          for (const auto& entry : std::filesystem::directory_iterator(dir))
          {
            std::error_code ec;
            std::filesystem::remove_all(entry.path(), ec);
            if (ec)
            {
              RCLCPP_WARN(this->get_logger(), "Could not delete %s: %s",
                          entry.path().c_str(), ec.message().c_str());
            }
          }
          RCLCPP_INFO(this->get_logger(), "Cleared contents of folder: %s", folder_path.c_str());
        }
        else
        {
          std::filesystem::create_directories(dir);
          RCLCPP_INFO(this->get_logger(), "Created directory: %s", folder_path.c_str());
        }
        first_call = false;
      }
     

      YAML::Emitter out;
      out << YAML::BeginMap;
      out << YAML::Key << "joint_names" << YAML::Value << traj.joint_trajectory.joint_names;
      out << YAML::Key << "points" << YAML::Value << YAML::BeginSeq;

      for (const auto& pt : traj.joint_trajectory.points)
      {
        out << YAML::BeginMap;
        out << YAML::Key << "positions" << YAML::Value << YAML::Flow << pt.positions;
        out << YAML::Key << "velocities" << YAML::Value << YAML::Flow << pt.velocities;
        out << YAML::Key << "accelerations" << YAML::Value << YAML::Flow << pt.accelerations;
        out << YAML::Key << "time_from_start" << YAML::Value << (pt.time_from_start.sec + pt.time_from_start.nanosec / 1e9);
        out << YAML::EndMap;
      }

      out << YAML::EndSeq;
      out << YAML::EndMap;

      std::ofstream fout(folder_path + "/" + filename);
      fout << out.c_str();
      RCLCPP_INFO(this->get_logger(), "Trajectory saved to %s", filename.c_str());
    }

  
  rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr subscription;
  
  std::unique_ptr<moveit::planning_interface::MoveGroupInterface> move_group_manipulator; // Lazy-initialized
  std::unique_ptr<moveit::planning_interface::MoveGroupInterface> move_group_gripper; // Lazy-initialized
 
};

int main (int argc, char *argv []) 
{

  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<AgentSubscriber>());
  rclcpp::shutdown();
  return 0;
}

trajectory_player.cpp

# include <rclcpp/rclcpp.hpp>
# include <moveit/move_group_interface/move_group_interface.h>
# include <moveit/planning_interface/planning_interface.h>
# include <moveit_msgs/msg/robot_trajectory.hpp>
# include <trajectory_msgs/msg/joint_trajectory_point.hpp>

# include <yaml-cpp/yaml.h>
# include <fstream>
# include <string>
# include <filesystem>
# include <algorithm>
# include <vector>

namespace fs = std::filesystem;


moveit_msgs::msg::RobotTrajectory loadTrajectoryFromFile (const std::string& filepath)
{
    YAML::Node root = YAML::LoadFile(filepath);

    moveit_msgs::msg::RobotTrajectory trajectory_msg;
    auto jt_node = root["joint_trajectory"];
    auto joint_names = jt_node["joint_names"];

    for (const auto& name: joint_names)
    {
        trajectory_msg.joint_trajectory.joint_names.push_back(name.as<std::string>());
    }

    for (const auto& pt: jt_node["points"])
    {
        trajectory_msgs::msg::JointTrajectoryPoint point;
        for (const auto& val: pt["positions"])
        {
            point.positions.push_back(val.as<double>());
        }
        for (const auto& val: pt["velocities"])
        {
            point.velocities.push_back(val.as<double>());
        }
        for (const auto& val: pt["accelerations"])
        {
            point.accelerations.push_back(val.as<double>());
        }
        for (const auto& val: pt["effort"])
        {
            point.effort.push_back(val.as<double>());
        }
        point.time_from_start = rclcpp::Duration::from_seconds(pt["time_from_start"].as<double>());
        trajectory_msg.joint_trajectory.points.push_back(point);
    }
    return trajectory_msg;
}

class TrajectoryReplayAllNode : public rclcpp::Node
{
public:
    TrajectoryReplayAllNode()
        : Node("trajectory_replay_all_node")
    {
        // Constructor only sets up the node
    }

    void run()
    {
        move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(shared_from_this(), "manipulator");

        const std::string folder_path = "/kinova-ros2/trajectories/";
        std::vector<fs::directory_entry> yaml_files;

        for (const auto& entry : fs::directory_iterator(folder_path))
        {
            if (entry.path().extension() == ".yaml")
            {
                yaml_files.push_back(entry);
            }
        }

        std::sort(
            yaml_files.begin(),
            yaml_files.end(),
            [](const fs::directory_entry& a, const fs::directory_entry& b)
            {
                return a.last_write_time() < b.last_write_time();
            });

        if (yaml_files.empty())
        {
            RCLCPP_WARN(this->get_logger(), "No YAML files found in directory: %s", folder_path.c_str());
            return;
        }

        RCLCPP_INFO(this->get_logger(), "Found %zu trajectory files. Starting replay...", yaml_files.size());

        for (const auto& file : yaml_files)
        {
            std::string filepath = file.path().string();
            RCLCPP_INFO(this->get_logger(), "Loading %s", filepath.c_str());

            try
            {
                auto trajectory = loadTrajectoryFromFile(filepath);
                moveit::planning_interface::MoveGroupInterface::Plan plan;
                plan.trajectory_ = trajectory;

                RCLCPP_INFO(this->get_logger(), "Executing...");
                bool success = (move_group_->execute(plan) == moveit::core::MoveItErrorCode::SUCCESS);
                if (!success)
                {
                    RCLCPP_ERROR(this->get_logger(), "Execution failed for: %s", filepath.c_str());
                }
                rclcpp::sleep_for(std::chrono::seconds(1));
            }
            catch (const std::exception &e)
            {
                RCLCPP_ERROR(this->get_logger(), "Error loading/executing file %s: %s", filepath.c_str(), e.what());
            }
        }

        RCLCPP_INFO(this->get_logger(), "Finished executing all saved trajectories");
    }

private:
    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
};



int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TrajectoryReplayAllNode>();
    node->run();
    rclcpp::shutdown();
    return 0;
}

snippet of recorded Trajectory

joint_names:
  - joint_1
  - joint_2
  - joint_3
  - joint_4
  - joint_5
  - joint_6
  - joint_7
points:
  - positions: [0, 0, 0, 0, 0, 0, 0]
    velocities: [0, -0, 0, -0, 0, -0, 0]
    accelerations: [0, 0, 0, 0, 0, 0, 0]
    time_from_start: 0
  - positions: [1.0468996280379466e-07, -0.0010499350771287453, 0.0042999999999999957, -0.0029232191520149912, 8.1882340753862901e-06, -0.0016467942015641176, 0.0021271803757051707]
    velocities: [2.0937992560758908e-06, -0.020998701542574882, 0.085999999999999813, -0.058464383040299758, 0.0001637646815077256, -0.032935884031282309, 0.042543607514103361]
    accelerations: [2.0937992560753175e-05, -0.20998701542569076, 0.85999999999976717, -0.58464383040283796, 0.0016376468150768155, -0.32935884031273266, 0.42543607514091697]
    time_from_start: 0.10000000000000001
  - positions: [4.0392564124288337e-07, -0.004050968096028903, 0.016590704694389679, -0.011278666443736042, 3.159269151471465e-05, -0.0063538316954845431, 0.0082073073127966068]
    velocities: [3.3995022107660189e-06, -0.034093589492903961, 0.13963000000000017, -0.094923044231593992, 0.00026588909859213728, -0.053474854503348428, 0.069073999037142703]
    accelerations: [4.773824509027495e-18, -4.692860445354389e-14, 1.8771441781417556e-13, -1.4078581336063165e-13, 4.2773467600886356e-16, -7.8214340755906478e-14, 9.3857208907087781e-14]
    time_from_start: 0.20000000000000001
  - positions: [7.4387586231948705e-07, -0.007460327045319317, 0.030553704694389772, -0.020770970866895493, 5.8181601373928524e-05, -0.011701317145819415, 0.015114707216510915]
    velocities: [3.3995022107660557e-06, -0.034093589492904329, 0.13963000000000167, -0.094923044231595019, 0.00026588909859214015, -0.053474854503349004, 0.069073999037143452]
    accelerations: [7.092643439205653e-17, -7.0681920993492581e-13, 2.8751967861759693e-12, -1.9886777771050453e-12, 5.61561872299994e-15, -1.1141387546431882e-12, 1.4375983930879846e-12]
    time_from_start: 0.29999999999999999
  - positions: [1.0838260833960918e-06, -0.010869685994609742, 0.044516704694389911, -0.030263275290054972, 8.4770511233142479e-05, -0.017048802596154305, 0.022022107120225246]
    velocities: [3.3995022107660337e-06, -0.034093589492904107, 0.13963000000000078, -0.094923044231594395, 0.00026588909859213842, -0.053474854503348657, 0.069073999037143008]
    accelerations: [7.0260691931940922e-17, -6.9069070596775204e-13, 2.8650873729032676e-12, -1.9441664316129315e-12, 5.5958737752016944e-15, -1.0999889020967902e-12, 1.4325436864516338e-12]
    time_from_start: 0.40000000000000002
  - positions: [1.4237763044726967e-06, -0.014279044943900169, 0.058479704694390046, -0.039755579713214452, 0.00011135942109235643, -0.022396288046489195, 0.028929507023939576]
    velocities: [3.3995022107660833e-06, -0.034093589492904607, 0.13963000000000284, -0.094923044231595796, 0.00026588909859214232, -0.053474854503349441, 0.069073999037144007]
    accelerations: [7.0997070751188882e-17, -7.1085422661457028e-13, 2.9295810551388348e-12, -1.9961361110995002e-12, 5.6096450963902325e-15, -1.1201339328472016e-12, 1.4360691446758995e-12]
    time_from_start: 0.5
  - positions: [1.7637265255493021e-06, -0.0176884038931906, 0.072442704694390209, -0.049247884136373946, 0.00013794833095157042, -0.027743773496824092, 0.035836906927653914]
    velocities: [3.3995022107660612e-06, -0.034093589492904391, 0.13963000000000192, -0.094923044231595172, 0.00026588909859214058, -0.053474854503349094, 0.069073999037143563]
    accelerations: [7.0755840506994161e-17, -7.1663573617207525e-13, 2.9086979879925408e-12, -1.9812870352992668e-12, 5.5987166888443379e-15, -1.1171086475623526e-12, 1.4332714723441505e-12]
    time_from_start: 0.59999999999999998
  - positions: [2.103676746625907e-06, -0.021097762842481025, 0.086405704694390351, -0.058740188559533432, 0.0001645372408107844, -0.033091258947158982, 0.042744306831368248]
    velocities: [3.3995022107660396e-06, -0.034093589492904169, 0.13963000000000103, -0.094923044231594561, 0.00026588909859213885, -0.053474854503348754, 0.069073999037143119]
    accelerations: [7.1299121225637897e-17, -7.1277852335238393e-13, 2.930311707115356e-12, -1.9799403426455109e-12, 5.5685822136904994e-15, -1.1285659953079412e-12, 1.4255570467047679e-12]
    time_from_start: 0.69999999999999996
  - positions: [2.4436269677025023e-06, -0.02450712179177136, 0.1003687046943901, -0.068232492982692655, 0.00019112615066999762, -0.038438744397493729, 0.049651706735082395]
    velocities: [3.3995022107660062e-06, -0.034093589492903836, 0.13962999999999964, -0.094923044231593631, 0.00026588909859213625, -0.053474854503348226, 0.069073999037142439]
    accelerations: [-1.9928778576172264e-16, 1.9590786491520383e-12, -8.4893408129921659e-12, 5.5507228392641085e-12, -1.5305301946500299e-14, 3.1018745278240606e-12, -4.244670406496083e-12]
    time_from_start: 0.80000000000000004
  - positions: [2.7835771887790939e-06, -0.027916480741061653, 0.1143317046943897, -0.077724797405851767, 0.00021771506052921055, -0.043786229847828408, 0.056559106638796451]
    velocities: [3.3995022107658719e-06, -0.03409358949290249, 0.13962999999999415, -0.094923044231589884, 0.00026588909859212578, -0.053474854503346117, 0.069073999037139719]
    accelerations: [-1.9349248109138003e-16, 1.937855332051754e-12, -7.9395626225809725e-12, 5.3996551485325566e-12, -1.5066002088539501e-14, 3.0384819041394005e-12, -3.9321530524156947e-12]
    time_from_start: 0.90000000000000002
  - positions: [3.1235274098556859e-06, -0.031325839690351943, 0.12829470469438931, -0.087217101829010879, 0.0002443039703884235, -0.049133715298163093, 0.063466506542510515]
    velocities: [3.3995022107662539e-06, -0.03409358949290632, 0.13963000000000983, -0.094923044231600556, 0.00026588909859215565, -0.053474854503352126, 0.069073999037147477]
    accelerations: [5.5690952818201531e-16, -5.5876891602973662e-12, 2.2867537141563788e-11, -1.5568012573776479e-11, 4.3653821564823173e-14, -8.769119115726792e-12, 1.1304573445688314e-11]
    time_from_start: 1
  - positions: [3.4634776309322778e-06, -0.034735198639642244, 0.14225770469438892, -0.096709406252170005, 0.00027089288024763645, -0.054481200748497778, 0.070373906446224585]
    velocities: [3.3995022107659973e-06, -0.034093589492903746, 0.13962999999999928, -0.094923044231593395, 0.0002658890985921356, -0.053474854503348088, 0.069073999037142272]
    accelerations: [-1.980513411952634e-16, 1.9785812037458508e-12, -8.2308978075827388e-12, 5.3817408741887145e-12, -1.4839359028093882e-14, 3.0865866778435272e-12, -3.9571624074917015e-12]
    time_from_start: 1.1000000000000001
  - positions: [3.8034278520088694e-06, -0.038144557588932537, 0.15622070469438853, -0.10620171067532912, 0.00029748179010684941, -0.05982868619883247, 0.077281306349938655]
    velocities: [3.399502210765863e-06, -0.0340935894929024, 0.13962999999999379, -0.094923044231589634, 0.00026588909859212508, -0.053474854503345978, 0.069073999037139538]
    accelerations: [-1.9368842023433876e-16, 1.9412643739920391e-12, -7.9423419136843237e-12, 5.4071747403431225e-12, -1.5096876196142427e-14, 3.0404277638322803e-12, -3.935714073298928e-12]
    time_from_start: 1.2
  - positions: [4.1433780730854609e-06, -0.041553916538222824, 0.17018370469438809, -0.11569401509848823, 0.0003240706999660623, -0.065176171649167142, 0.084188706253652712]
    velocities: [3.3995022107659228e-06, -0.034093589492902997, 0.13962999999999623, -0.0949230442315913, 0.00026588909859212974, -0.053474854503346915, 0.069073999037140746]
    accelerations: [-1.935448611626285e-16, 1.9435400354994064e-12, -7.9495171376817828e-12, 5.406840700261507e-12, -1.506974181660724e-14, 3.0395212585253877e-12, -3.9455324028935321e-12]
    time_from_start: 1.3

[aalmrad]

@Abhishek8857 After quickly going over your code, i can see that you are registering the waypoints generated by the moveit planner from the current robot position to a target position published on the "published_coordinates" topic. While I do not have visibility on the publishing node, I assume that the target positions are close to the current robot position and as such your registered waypoints are way too small to be considered as anything but noise. You should only save meaningful joint changes to replay the registered trajectory. I do not know your final objective here but I recommended recording actual joint feedback from the robot instead of the planner's output.

[Abhishek8857]

@aalmrad Thanks for your reply and a proposed solution.

Here is a context for the published_coordinated topic. The robot always starts at Zero position where all joint coordinates are at [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]. The published_coordinated topic publishes joint positions for the robot to reach, one such example of published_coordinates is home_pose_coordinates = [0.0, 0.0, -0.7650, -3.15, -2.13, 0.006, -1.2, 1.55], where the first value is a flag variable for the code to difeerentiate between the grippper joints or arm joints. The end goal is to record multiple such poses and then record and play them all together

[github-actions]

As part of Kinova's beta testing: Claude is analyzing this issue and will attempt an automated fix. A pull request will be opened if changes are made.

[github-actions]

Claude has submitted a fix attempt: #398

Please review the proposed modifications and test them on your end, then provide feedback.
A member of Kinova's team will follow up on this issue and take the appropriate next steps.