unit of measurement for moveit plan

Question

Hi can someone help me to find out what the unit of measuremnt of the distance variable in this code is? I'm using it to control a ur10:

//-------------- Längenberechnung der Trajectorie-----------------------------------------
double calculateDistance(const std::vector<double>& p1, const std::vector<double>& p2)
{
   if (p1.size() != p2.size())
   {
       RCLCPP_ERROR(rclcpp::get_logger("EndEffectorIkSolverNode"), "Gelenkpunkt-Dimensionen stimmen nicht überein");
       return 0.0;
   }

   double distance = 0.0;
   for (size_t i = 0; i < p1.size(); ++i)
   {
       distance += std::abs(p1[i] - p2[i]);
   }
   return distance;
} 


//------------Callabkc wenn eine PSoe erhalten worden ist--------------------------------

void poseCallback(const geometry_msgs::msg::Point::SharedPtr msg,
                 moveit::planning_interface::MoveGroupInterface& move_group_interface,
                 rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr planningResultPub,
                 rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr executionResultPub,
                 rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr planningTimePub,
                 rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr executionTimePub,
                 rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr planningAttPub,
                 rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr planLenPub) {

   // Setzen der Zeilpose
   geometry_msgs::msg::Pose target_pose;
   target_pose.position.x = msg->x;
   target_pose.position.y = msg->y;
   target_pose.position.z = msg->z;

   // setzen der Orientierung für "wrist_3_link" mittels quaternion
   tf2::Quaternion quaternion;
   quaternion.setRPY(M_PI, 0.0, -M_PI_2); 
   target_pose.orientation.x = quaternion.getX();
   target_pose.orientation.y = quaternion.getY();
   target_pose.orientation.z = quaternion.getZ();
   target_pose.orientation.w = quaternion.getW();

   move_group_interface.setPoseTarget(target_pose);


   // planen einer trajectorie
   bool success = false;
   moveit::planning_interface::MoveGroupInterface::Plan plan;

   const int max_attempts = 10; // Maximalanzahl an Plaungsversuchen

   for (int attempt = 0; attempt < max_attempts && !success; ++attempt) {
     RCLCPP_INFO(rclcpp::get_logger("EndEffectorIkSolverNode"), "Planung gescheitert Versuch: %d", attempt);   
     std::tie(success, plan) = [&move_group_interface]() -> std::pair<bool, moveit::planning_interface::MoveGroupInterface::Plan> {
         moveit::planning_interface::MoveGroupInterface::Plan msg;
         auto const ok = static_cast<bool>(move_group_interface.plan(msg));
         return std::make_pair(ok, msg);
     }();

     if (!success) {
       RCLCPP_ERROR(rclcpp::get_logger("EndEffectorIkSolverNode"), "Planung gescheitert!");

     } else {
       // Extract planning time from the plan
       double planning_time = plan.planning_time_;

       // Publish planning time
       std_msgs::msg::Float64 planningTimeMsg;
       planningTimeMsg.data = planning_time;
       planningTimePub->publish(planningTimeMsg);

       std_msgs::msg::Float64 planningAttMsg;
       planningAttMsg.data = attempt+1;
       planningAttPub->publish(planningAttMsg);


       double total_distance = 0.0;
       for (size_t i = 1; i < plan.trajectory_.joint_trajectory.points.size(); ++i)
       {
           total_distance += calculateDistance(plan.trajectory_.joint_trajectory.points[i-1].positions, plan.trajectory_.joint_trajectory.points[i].positions);
       }
     std_msgs::msg::Float64 planLengthMsg;
     planLengthMsg.data = total_distance;
     planLenPub->publish(planLengthMsg);

     }
   }

   std_msgs::msg::Bool planningResultMsg;
   planningResultMsg.data = success;
   planningResultPub->publish(planningResultMsg);
  
          
   // Wenn es einen Plan gibt ausführen
   if (success){

     double execution_duration = 0.0;
     auto execute_start_time = std::chrono::high_resolution_clock::now();
     moveit::core::MoveItErrorCode result = move_group_interface.execute(plan);
     auto execute_end_time = std::chrono::high_resolution_clock::now();
     execution_duration = std::chrono::duration<double>(execute_end_time - execute_start_time).count();
           
     // Publish execution time
     std_msgs::msg::Float64 executionTimeMsg;
     executionTimeMsg.data = execution_duration;
     executionTimePub->publish(executionTimeMsg);

     if (result == moveit::core::MoveItErrorCode::SUCCESS){
       RCLCPP_INFO(rclcpp::get_logger("EndEffectorIkSolverNode"), "Geplante Bewegung ausgeführt");
     } else {
       RCLCPP_ERROR(rclcpp::get_logger("EndEffectorIkSolverNode"), "geplante Bewegung konnte nicht ausgeführt werden");
     }
     std_msgs::msg::Bool executionResultMsg;
     executionResultMsg.data = (result == moveit::core::MoveItErrorCode::SUCCESS);
     executionResultPub->publish(executionResultMsg);


   } else {
     RCLCPP_ERROR(rclcpp::get_logger("EndEffectorIkSolverNode"), "Planning failed!");
   }
}

Answer 1

In a ros trajectory_msgs/JointTrajectoryPoint message, the position field specifies a joint rotation position. The units are radians. It's not clear that the author of the code you show understood this.