why using rosrun?

[devika-dudo]

in the documentation on creating ikfast plugin for custom robot, we use a docker image since openrave requires it so are we running the rosrun command inside the docker or inside humble if yes how is it possible ---- also will i have to move my urdf files including the meshes to the docker to create the plugin this documentation seems vague i am having difficulty in understanding this , please help i have been stuck in this for weeks---or are there any other good 5dof plugins available? cant use trac ik since i am in ros2 humble

[mikeferguson]

I would recommend adding a link to what documentation/tutorial you are even referencing

[gavanderhoorn]

cant use trac ik since i am in ros2 humble

Not sure it'll help, but you might be able to build it from source using this branch.

[devika-dudo]

But see

See it doesn't have a humble branch

[devika-dudo]

Also https://moveit.picknik.ai/humble/doc/examples/ikfast/ikfast_tutorial.html this is the documentation I am referring to

[gavanderhoorn]

No need for giant screenshots.

See it doesn't have a humble branch

no, that's why I linked to the jazzy branch. Until recently, I've built the rolling branch to use it with Humble. So there's a chance the jazzy branch works for you.

[mikeferguson]

Also https://moveit.picknik.ai/humble/doc/examples/ikfast/ikfast_tutorial.html this is the documentation I am referring to

That does not appear to have been ported to ROS 2 (hence the rosrun). If the docs aren't ported, I'm also not sure the state of the IKFast in Humble either. I will note that the docs have been ported in the latest branch - https://moveit.picknik.ai/main/doc/examples/ikfast/ikfast_tutorial.html - you might find some helpful information there

[devika-dudo]

No need for giant screenshots.

See it doesn't have a humble branch

no, that's why I linked to the jazzy branch. Until recently, I've built the rolling branch to use it with Humble. So there's a chance the jazzy branch works for you.

ok i will try and inform

[devika-dudo]

Also https://moveit.picknik.ai/humble/doc/examples/ikfast/ikfast_tutorial.html this is the documentation I am referring to

That does not appear to have been ported to ROS 2 (hence the rosrun). If the docs aren't ported, I'm also not sure the state of the IKFast in Humble either. I will note that the docs have been ported in the latest branch - https://moveit.picknik.ai/main/doc/examples/ikfast/ikfast_tutorial.html - you might find some helpful information there

thanks for the reply, i will try using the TranslationDirection5D and update the results

[devika-dudo]

HI, i tried using the trac_ik plugin but still my planning failed, i was using the movegroupinterface api for moving the end effector to a particular pose and orientation , since the arm is 5dof i set the yaw to a fixed 0 . so that i can give any pitch and yaw , is this a problem with the iksolver or am i feeding a 6dof pose to the solver ( so is the actual problem with my movegroupinterface's code? or am i trying to set a 6dof pose for a 5dof robotic arm ) if so how to set a 5dof pose so that planning wont get aborted this is my movegroupinterface node

 * @file hello_moveit_v2_5dof_constrained.cpp
 * @brief MoveIt 2 + ROS 2 code for a 5-DOF robot arm with proper constraints.
 *
 * For 5DOF robots, we use position targets + orientation constraints
 * instead of full 6DOF pose targets to avoid kinematic impossibilities.
 */
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/msg/constraints.hpp>
#include <moveit_msgs/msg/orientation_constraint.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <mutex>
class MoveIt5DOFController : public rclcpp::Node
{
public:
  MoveIt5DOFController() : Node("moveit_5dof_controller")
  {
    // Declare parameters
    this->declare_parameter("planning_pipeline", "ompl");
    this->declare_parameter("planner_id", "RRTConnectkConfigDefault");
    this->declare_parameter("planning_time", 10.0);
    this->declare_parameter("position_tolerance", 0.01);
    this->declare_parameter("orientation_tolerance", 0.2);  // More relaxed for 5DOF
    this->declare_parameter("max_velocity_scaling", 0.5);
    this->declare_parameter("max_acceleration_scaling", 0.5);
    this->declare_parameter("use_constraints", true);
    this->declare_parameter("constrain_orientation", true);
    RCLCPP_INFO(this->get_logger(), "MoveIt 5DOF Controller starting initialization...");
  }
  void initialize()
  {
    // Initialize MoveGroupInterface after construction
    arm_group_interface_ = std::make_unique<moveit::planning_interface::MoveGroupInterface>(
      shared_from_this(), "arm_group");
    // Configure planning parameters
    setupPlanningParameters();
    // Create subscribers for different input methods 
    // Method 1: Position-only target (recommended for 5DOF)
    position_subscriber_ = this->create_subscription<geometry_msgs::msg::PointStamped>(
      "/arm_position_command", 10,
      std::bind(&MoveIt5DOFController::positionCallback, this, std::placeholders::_1));
    // Method 2: Position + limited orientation (XYZ + Roll, Pitch)
    xyz_rp_subscriber_ = this->create_subscription<std_msgs::msg::Float64MultiArray>(
      "/arm_xyz_rp_command", 10,
      std::bind(&MoveIt5DOFController::xyzRpCallback, this, std::placeholders::_1));
    // Method 3: Full pose with constraints (for testing)
    pose_constrained_subscriber_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
      "/arm_pose_constrained_command", 10,
      std::bind(&MoveIt5DOFController::poseConstrainedCallback, this, std::placeholders::_1));
    // Method 4: Joint space control (always works)
    joint_subscriber_ = this->create_subscription<std_msgs::msg::Float64MultiArray>(
      "/arm_joint_command", 10,
      std::bind(&MoveIt5DOFController::jointCallback, this, std::placeholders::_1));
    RCLCPP_INFO(this->get_logger(), "MoveIt 5DOF Controller initialized");
    RCLCPP_INFO(this->get_logger(), "Available control methods:");
    RCLCPP_INFO(this->get_logger(), "  1. /arm_position_command (geometry_msgs/PointStamped) - Position only");
    RCLCPP_INFO(this->get_logger(), "  2. /arm_xyz_rp_command (Float64MultiArray) - [x,y,z,roll,pitch]");
    RCLCPP_INFO(this->get_logger(), "  3. /arm_pose_constrained_command (PoseStamped) - With orientation constraints");
    RCLCPP_INFO(this->get_logger(), "  4. /arm_joint_command (Float64MultiArray) - Joint angles");
  }
private:
  void setupPlanningParameters()
  {
    // Get parameters
    std::string planning_pipeline = this->get_parameter("planning_pipeline").as_string();
    std::string planner_id = this->get_parameter("planner_id").as_string();
    double planning_time = this->get_parameter("planning_time").as_double();
    double pos_tol = this->get_parameter("position_tolerance").as_double();
    double orient_tol = this->get_parameter("orientation_tolerance").as_double();
    double max_vel = this->get_parameter("max_velocity_scaling").as_double();
    double max_acc = this->get_parameter("max_acceleration_scaling").as_double();
    // Configure MoveGroupInterface
    arm_group_interface_->setPlanningPipelineId(planning_pipeline);
    arm_group_interface_->setPlannerId(planner_id);
    arm_group_interface_->setPlanningTime(planning_time);
    arm_group_interface_->setGoalPositionTolerance(pos_tol);
    arm_group_interface_->setGoalOrientationTolerance(orient_tol);
    arm_group_interface_->setMaxVelocityScalingFactor(max_vel);
    arm_group_interface_->setMaxAccelerationScalingFactor(max_acc);
    RCLCPP_INFO(this->get_logger(), "Planning configuration:");
    RCLCPP_INFO(this->get_logger(), "  Position tolerance: %.4f m", pos_tol);
    RCLCPP_INFO(this->get_logger(), "  Orientation tolerance: %.4f rad (%.1f°)", 
                orient_tol, orient_tol * 180.0 / M_PI);
    RCLCPP_INFO(this->get_logger(), "  Planning time: %.1f s", planning_time);
    // Clear any existing constraints
    arm_group_interface_->clearPathConstraints();
  }
  // Method 1: Position-only control (BEST for 5DOF)
  void positionCallback(const geometry_msgs::msg::PointStamped::SharedPtr msg)
  {
    std::lock_guard<std::mutex> lock(planning_mutex_);    
    RCLCPP_INFO(this->get_logger(), "Received position-only command:");
    RCLCPP_INFO(this->get_logger(), "  Target position: [%.3f, %.3f, %.3f]", 
                msg->point.x, msg->point.y, msg->point.z);
    // Clear constraints and use position target only
    arm_group_interface_->clearPathConstraints();
    arm_group_interface_->clearPoseTargets();
    // Set position target - let MoveIt choose orientation
    bool success = arm_group_interface_->setPositionTarget(
      msg->point.x, msg->point.y, msg->point.z);
    if (!success) {
      RCLCPP_ERROR(this->get_logger(), "Failed to set position target!");
      return;
    }
    executePlan("position target");
  }
  // Method 2: Position + Roll/Pitch (5DOF control)
  void xyzRpCallback(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
  {
    std::lock_guard<std::mutex> lock(planning_mutex_);
    
    if (msg->data.size() != 5) {
      RCLCPP_ERROR(this->get_logger(), "Expected 5 values [x,y,z,roll,pitch], got %zu", 
                   msg->data.size());
      return;
    }
    double x = msg->data[0];
    double y = msg->data[1];
    double z = msg->data[2];
    double roll = msg->data[3];
    double pitch = msg->data[4];
    double yaw = 0.0; // Fixed or let MoveIt choose
    RCLCPP_INFO(this->get_logger(), "Received XYZ + Roll/Pitch command:");
    RCLCPP_INFO(this->get_logger(), "  Position: [%.3f, %.3f, %.3f]", x, y, z);
    RCLCPP_INFO(this->get_logger(), "  Orientation: roll=%.3f, pitch=%.3f (yaw=free)", roll, pitch);
    // Method 2A: Use orientation constraints
    if (this->get_parameter("use_constraints").as_bool()) {
      executeWithOrientationConstraints(x, y, z, roll, pitch);
    } 
    // Method 2B: Use approximate pose with relaxed tolerances
    else {
      executeWithApproximatePose(x, y, z, roll, pitch, yaw);
    }
  }
  // Method 3: Full pose with constraints
  void poseConstrainedCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
  {
    std::lock_guard<std::mutex> lock(planning_mutex_);
    RCLCPP_INFO(this->get_logger(), "Received constrained pose command");
    // Extract position
    double x = msg->pose.position.x;
    double y = msg->pose.position.y;
    double z = msg->pose.position.z;
    // Extract orientation
    tf2::Quaternion q;
    tf2::fromMsg(msg->pose.orientation, q);
    double roll, pitch, yaw;
    tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
    RCLCPP_INFO(this->get_logger(), "  Position: [%.3f, %.3f, %.3f]", x, y, z);
    RCLCPP_INFO(this->get_logger(), "  Orientation: [%.3f, %.3f, %.3f] (yaw may be ignored)", roll, pitch, yaw);
    executeWithOrientationConstraints(x, y, z, roll, pitch);
  }
  // Method 4: Direct joint control (always works)
  void jointCallback(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
  {
    std::lock_guard<std::mutex> lock(planning_mutex_);
    if (msg->data.size() != 5) {
      RCLCPP_ERROR(this->get_logger(), "Expected 5 joint values, got %zu", msg->data.size());
      return;
    }
    RCLCPP_INFO(this->get_logger(), "Received joint command:");
    RCLCPP_INFO(this->get_logger(), "  Joints: [%.3f, %.3f, %.3f, %.3f, %.3f]", 
                msg->data[0], msg->data[1], msg->data[2], msg->data[3], msg->data[4]);
    arm_group_interface_->clearPathConstraints();
    arm_group_interface_->setJointValueTarget(msg->data);
    executePlan("joint target");
  }
  void executeWithOrientationConstraints(double x, double y, double z, double roll, double pitch)
  {
    // Clear previous constraints
    arm_group_interface_->clearPathConstraints();
    arm_group_interface_->clearPoseTargets();
    // Set position target
    arm_group_interface_->setPositionTarget(x, y, z);
    // Create orientation constraints for roll and pitch only
    moveit_msgs::msg::Constraints constraints;
    // Roll constraint
    moveit_msgs::msg::OrientationConstraint roll_constraint;
    roll_constraint.link_name = arm_group_interface_->getEndEffectorLink();
    roll_constraint.header.frame_id = arm_group_interface_->getPlanningFrame();
    tf2::Quaternion roll_q;
    roll_q.setRPY(roll, 0, 0);
    roll_constraint.orientation = tf2::toMsg(roll_q);
    roll_constraint.absolute_x_axis_tolerance = 0.1;  // Relaxed
    roll_constraint.absolute_y_axis_tolerance = 3.14; // Free
    roll_constraint.absolute_z_axis_tolerance = 3.14; // Free
    roll_constraint.weight = 1.0;
    // Pitch constraint
    moveit_msgs::msg::OrientationConstraint pitch_constraint;
    pitch_constraint.link_name = arm_group_interface_->getEndEffectorLink();
    pitch_constraint.header.frame_id = arm_group_interface_->getPlanningFrame();
    tf2::Quaternion pitch_q;
    pitch_q.setRPY(0, pitch, 0);
    pitch_constraint.orientation = tf2::toMsg(pitch_q);
    pitch_constraint.absolute_x_axis_tolerance = 3.14; // Free
    pitch_constraint.absolute_y_axis_tolerance = 0.1;  // Constrained
    pitch_constraint.absolute_z_axis_tolerance = 3.14; // Free
    pitch_constraint.weight = 1.0;
    constraints.orientation_constraints.push_back(roll_constraint);
    constraints.orientation_constraints.push_back(pitch_constraint);
    arm_group_interface_->setPathConstraints(constraints);
    executePlan("position + orientation constraints");
  }
  void executeWithApproximatePose(double x, double y, double z, double roll, double pitch, double yaw)
  {
    // Create pose with very relaxed orientation tolerance
    geometry_msgs::msg::PoseStamped target_pose;
    target_pose.header.frame_id = arm_group_interface_->getPlanningFrame();
    target_pose.header.stamp = this->now();
    target_pose.pose.position.x = x;
    target_pose.pose.position.y = y;
    target_pose.pose.position.z = z;
    tf2::Quaternion q;
    q.setRPY(roll, pitch, yaw);
    q.normalize();
    target_pose.pose.orientation = tf2::toMsg(q);
    arm_group_interface_->clearPathConstraints();
    // Set very relaxed orientation tolerance
    double original_orient_tol = this->get_parameter("orientation_tolerance").as_double();
    arm_group_interface_->setGoalOrientationTolerance(0.5); // Very relaxed
    bool success = arm_group_interface_->setPoseTarget(target_pose);
    if (!success) {
      RCLCPP_ERROR(this->get_logger(), "Failed to set pose target!");
      return;
    }
    executePlan("approximate pose");
    // Restore original tolerance
    arm_group_interface_->setGoalOrientationTolerance(original_orient_tol);
  }
  void executePlan(const std::string& method_name)
  {
    RCLCPP_INFO(this->get_logger(), "Planning motion using %s...", method_name.c_str());
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    bool success = static_cast<bool>(arm_group_interface_->plan(plan));
    if (success) {
      RCLCPP_INFO(this->get_logger(), "Planning successful! Executing motion...");     
      auto execute_result = arm_group_interface_->execute(plan);
      if (execute_result == moveit::core::MoveItErrorCode::SUCCESS) {
        RCLCPP_INFO(this->get_logger(), "✅ Motion executed successfully!");
      } else {
        RCLCPP_ERROR(this->get_logger(), "❌ Motion execution failed!");
      }
    } else {
      RCLCPP_ERROR(this->get_logger(), "❌ Planning failed with %s", method_name.c_str());
      RCLCPP_ERROR(this->get_logger(), "Try one of these solutions:");
      RCLCPP_ERROR(this->get_logger(), "  1. Use position-only control (/arm_position_command)");
      RCLCPP_ERROR(this->get_logger(), "  2. Use joint space control (/arm_joint_command)");
      RCLCPP_ERROR(this->get_logger(), "  3. Increase orientation tolerance");
      RCLCPP_ERROR(this->get_logger(), "  4. Check if target is within workspace");
    }
    RCLCPP_INFO(this->get_logger(), "Ready for next command...");
  }
  std::unique_ptr<moveit::planning_interface::MoveGroupInterface> arm_group_interface_;
  rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr position_subscriber_;
  rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr xyz_rp_subscriber_;
  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_constrained_subscriber_;
  rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr joint_subscriber_;
  std::mutex planning_mutex_;
};
int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<MoveIt5DOFController>();
  node->initialize();
  RCLCPP_INFO(node->get_logger(), "🤖 MoveIt 5DOF Controller ready!");
  RCLCPP_INFO(node->get_logger(), "");
  RCLCPP_INFO(node->get_logger(), "🎯 RECOMMENDED: Position-only control (works best for 5DOF):");
  RCLCPP_INFO(node->get_logger(), "  ros2 topic pub /arm_position_command geometry_msgs/PointStamped");
  RCLCPP_INFO(node->get_logger(), "  '{header: {frame_id: base_link}, point: {x: 0.3, y: 0.2, z: 0.4}}'");
  RCLCPP_INFO(node->get_logger(), "");
  RCLCPP_INFO(node->get_logger(), "🎯 Alternative: XYZ + Roll/Pitch (5DOF control):");
  RCLCPP_INFO(node->get_logger(), "  ros2 topic pub /arm_xyz_rp_command std_msgs/Float64MultiArray");
  RCLCPP_INFO(node->get_logger(), "  \"data: [0.3, 0.2, 0.4, 0.0, -0.785]\"");
  RCLCPP_INFO(node->get_logger(), "");
  RCLCPP_INFO(node->get_logger(), "🎯 Always works: Joint space control:");
  RCLCPP_INFO(node->get_logger(), "  ros2 topic pub /arm_joint_command std_msgs/Float64MultiArray");
  RCLCPP_INFO(node->get_logger(), "  \"data: [0.0, 0.5, -1.0, 0.5, 0.0]\"");
  RCLCPP_INFO(node->get_logger(), "");
  RCLCPP_INFO(node->get_logger(), "Press Ctrl+C to exit.");
  rclcpp::spin(node);
  rclcpp::shutdown();  
  return 0;
}

please help i searched online and found that some people are setting position_only_ik:True in the kinematics file, but doesnt that make the robot a 3dof one? but i want to set the roll and pitch too

please tell me if i have moved out of the scope of the issue --