Question

I have been working for several months with a 6gdl arm in Moveit2 in ROS" Foxy. I have not been able to accomplish a seemingly simple task. I need to make a straight line between two known points (both with the same orientation). I have tried OMPL Constrained Planning( boxes and orientation) and generate cartesain paths. All to no avail. I am desperate, has anyone worked with moveit before and would know how to tackle this, seemingly, simple problem?

I have a simple function that interpolates points between two given points:

std::vector<geometry_msgs::msg::Pose> RobotArmNode::interpolatePoses(const geometry_msgs::msg::PoseStamped& pose1,
                                                                     const geometry_msgs::msg::PoseStamped& pose2,
                                                                     double separation_distance)
{
    std::vector<geometry_msgs::msg::Pose> interpolated_poses;

    // Calculate the linear interpolation between pose1 and pose2
    // based on the separation_distance

    // Calculate the direction vector from pose1 to pose2
    double dx = pose2.pose.position.x - pose1.pose.position.x;
    double dy = pose2.pose.position.y - pose1.pose.position.y;
    double dz = pose2.pose.position.z - pose1.pose.position.z;

    // Calculate the distance between pose1 and pose2
    double distance = std::sqrt(dx*dx + dy*dy + dz*dz);

    // Calculate the number of interpolation points
    int num_points = static_cast<int>(std::ceil(distance / separation_distance));

    // Calculate the step size for each interpolation point
    double step_size = distance / num_points;

    // Interpolate the points and add them to the vector
    for (int i = 0; i <= num_points; ++i)
    {
      double t = static_cast<double>(i) / num_points;

      geometry_msgs::msg::Pose interpolated_pose;
      interpolated_pose.position.x = pose1.pose.position.x + t * dx;
      interpolated_pose.position.y = pose1.pose.position.y + t * dy;
      interpolated_pose.position.z = pose1.pose.position.z + t * dz;

      // Add the interpolated pose to the vector
      interpolated_poses.push_back(interpolated_pose);
    }

    return interpolated_poses;
}

I use these points in the computecartesianpath method:

waypoints = interpolatePoses(pose_40_, pose_20_, 0.05);
          fraction_ = move_group_.computeCartesianPath(waypoints, 10.0, 0.01, trajectory);
          RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction_ * 100.0);

Finally, I scale the speed of the trajectory and execute it: rtrajec = std::makeshared(movegroup.getRobotModel(), "handlerbot_arm");

          r_trajec->setRobotTrajectoryMsg(*move_group_.getCurrentState(), trajectory);
          iptp.computeTimeStamps(*r_trajec, 1, 1);
          r_trajec->getRobotTrajectoryMsg(trajectory);
          iptp.computeTimeStamps(*r_trajec, 0.03, 0.03);
          r_trajec->getRobotTrajectoryMsg(trajectory_slow);

          RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "VOY A SLOW");
          move_group_.execute(trajectory_slow);
          RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "SALDO DE SLOW");

La función de interpolación ha sido un añadido para probar a ver si se solucionaba el problema, inicialmente utilicé 2 waypoints directamente en ComputeCartesianPath y no funcionaba. I have also tested various Jump Threshold values.

The problem is that ComputeCartesianPath returns a fraction of 0. When I enter the interpolated points (plus points) it returns values of 20%. The path is perfectly achievable.

Asked by angcorcue2 on 2023-06-07 06:22:06 UTC

Answer 1

The way how you go about it is by using MoveIt Task Constructor. In this tool, you use the concept of stages to achieve your end-to-end solution. You need to add the below stages to make your arm follow a straight line while maintaining orientation constraints:

Current State Stage
Move Relative Stage In addition, you would need to figure out which axes in the MoveRelative Stage would correspond to your line axis. The official tutorials mention a detailed setup on how to use stages for Pick and Place.

Edit:

@130s I have previously faced the issue of not achieving a sizeable amount of fraction using computeCartesianPath. Hence, we used MTC to tackle this, as we couldn't find the solution as to what was limiting the range of motion.
@angcorcue2 The following can still be achieved using MTC by combining the mentioned stages.

I have attached a minor demo with ROS1 below, indicating the structure of the stages you require for the use case you mentioned. However, these stages can easily be ported to ROS2 syntax following the tutorial.

Prerequisites

We begin the pipeline by using the Current State stage. We assume that the arm is expected to not be in an attached state:

// Applicability Stage
 moveit::task_constructor::Stage* current_state_ptr = nullptr;
{
  auto current_state =
    std::make_unique<moveit::task_constructor::stages::CurrentState>(
      "current state");

  auto applicability_filter =
      std::make_unique<moveit::task_constructor::stages::PredicateFilter>(
        "applicability test", std::move(current_state));
  applicability_filter->setPredicate([object](
    const moveit::task_constructor::SolutionBase& s, std::string& comment) {
    if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
      comment = "object with id '" + object +
        "' is already attached and cannot be picked";
      return false;
    }
    return true;
  });
  current_state_ptr = applicability_filter.get();
  t.add(std::move(applicability_filter));
}

We then configure a Connect stage that is connected to a SerialContainer that consists of additional substages.

// Move to Pick Connect Stage - non-cartesian
{
  auto stage = std::make_unique<moveit::task_constructor::stages::Connect>(
    "move to pick",
      moveit::task_constructor::stages::Connect::GroupPlannerVector{
        { arm_group_name_, sampling_planner } });
  stage->setTimeout(60.0);
  stage->properties().configureInitFrom(
    moveit::task_constructor::Stage::PARENT);
  t.add(std::move(stage));
}

Our SerialContainer Stage would consist of a MoveRelative Stage to enable us to reach the first pose in a straight line and a GenerateCustomPose stage to generate a grasp of the said pose.

// Pick object connect the pipeline
moveit::task_constructor::Stage* attach_object_stage = nullptr;
{
  auto grasp =
    std::make_unique<moveit::task_constructor::SerialContainer>(
      "pick object");
  t.properties().exposeTo(grasp->properties(),
    { "eef", "hand", "group", "ik_frame" });
  grasp->properties().configureInitFrom(
    moveit::task_constructor::Stage::PARENT,
      { "eef", "hand", "group", "ik_frame" });

  // Approach object - substage
  {
    auto stage =
      std::make_unique<moveit::task_constructor::stages::MoveRelative>(
          "Approach object", cartesian_planner);
    stage->properties().set("marker_ns", "approach_object");
    stage->properties().set("link", hand_frame_);
    stage->properties().configureInitFrom(
        moveit::task_constructor::Stage::PARENT, { "group" });
    stage->setMinMaxDistance(
        approach_object_min_dist_, approach_object_max_dist_);

    // Set hand forward direction
    geometry_msgs::Vector3Stamped vec;
    vec.header.frame_id = hand_frame_;
    vec.vector.z = 1.0;
    stage->setDirection(vec);
    grasp->insert(std::move(stage));
  }

  // Generate grasp pose - substage
  {
    std::vector<geometry_msgs::PoseStamped> CustomPoses;
    geometry_msgs::PoseStamped p;
    p.header.frame_id = planning_frame_;
    p.pose = object_pose_;
    CustomPoses.push_back(p);

    auto stage =
      std::make_unique<
        moveit::task_constructor::stages::GenerateCustomPose>(
        "generate grasp pose");
    stage->setCustomPoses(CustomPoses);
    stage->properties().configureInitFrom(
        moveit::task_constructor::Stage::PARENT);
    stage->setMonitoredStage(current_state_ptr);

    // Compute IK - substage
    auto wrapper =
      std::make_unique<moveit::task_constructor::stages::ComputeIK>(
        "grasp pose IK", std::move(stage));
    wrapper->setMaxIKSolutions(1);
    wrapper->setMinSolutionDistance(1.0);
    wrapper->setIKFrame(grasp_frame_transform_, hand_frame_);

    wrapper->properties().configureInitFrom(
        moveit::task_constructor::Stage::PARENT, { "eef", "group" });
    wrapper->properties().configureInitFrom(
        moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
    grasp->insert(std::move(wrapper));
  }
  t.add(std::move(grasp));
}

This results in the following motion:

Final Pose Parametric Adjustment

To achieve the second pose, we would need the calculate the unit vector in question.
For instance, let our first point be at (0.35, -0.15, 0.635), and our second point at (0.586, 0.096, 0.880):
```
unit_vector = (dx/mag, dy/mag, dz/mag)

where:
   - dx = (x2 - x1) 
   - dy = (y2 - y1) 
   - dz = (z2 - z1)
   - mag = sqrt(dx^2 + dy^2 + dz^2)
```

Then by substituting these values into the above equation, we get:

    unit_vector =  (0.562, 0.587, 0.584)  # (ux, uy, uz)

Secondly, we need to calculate the distance between these two points:
```
distance = sqrt(dx^2 + dy^2 + dz^2) = 0.420
```

Finally, we substitute these values in our stage:

// Move 3D - Stage
{
  auto stage =
    std::make_unique<moveit::task_constructor::stages::MoveRelative>(
        "move 3d", cartesian_planner);
  stage->properties().set("marker_ns", "move_3d");
  stage->properties().set("link", hand_frame_);
  stage->properties().configureInitFrom(
      moveit::task_constructor::Stage::PARENT, { "group" });
  // setting minimum distance slightly lower to ensure max distance is reached
  stage->setMinMaxDistance(0.4, 0.42);

  // Set hand forward direction
  geometry_msgs::Vector3Stamped vec;
  vec.header.frame_id = hand_frame_;
  vec.vector.x = 0.562;
  vec.vector.y = 0.587;
  vec.vector.z = 0.584;
  stage->setDirection(vec);
  attach_object_stage = stage.get();
  t.add(std::move(stage));
}

This results in the following motion:

Things to Note

Cartesian motions across three axes simultaneously give a very low range of motion. It is recommended that if you do not need to follow a straight line doing so, use OMPL-constrained planning instead.
Integration of orientational constraints with OMPL works very reliably in stages using MTC.

Asked by Gaurav Gupta on 2023-06-07 10:15:44 UTC

Comments

The objective is to manipulate objects in unstructured environments using a 6dof arm on a mobile robot. The detection and estimation of the pose of the objects is already solved. So far I have been using the move group interface, would what you propose work in this case?

Asked by angcorcue2 on 2023-06-08 03:11:46 UTC

Although 'MoveIt Task Constructor' is a useful tool of general purpose with MoveIt, it has nothing to do with straight line path planning the OP is asking about.

Asked by 130s on 2023-06-09 10:05:22 UTC

Just edited to show how it can be done. Check it out

Asked by Gaurav Gupta on 2023-06-19 04:27:00 UTC

Answer 2

I honestly don't know if there's a motion planner that supports straight line path generation (there may exist, I just don't know). I would do:

Get poses of way points by linear interpolation of the path b/w origin and goal poses e.g. #q256456
Set waypoints to MoveIt's Cartesian API (Ref. Move Group C++ Interface).

Asked by 130s on 2023-06-09 10:03:38 UTC

Comments

The "Pilz planner" can do this.

But you'll lose collision avoidance, as there is no way to "plan a straight line between two points" while also "avoiding collisions". There's only collision detection, but planning will fail if a potential collision is detected.

Asked by gvdhoorn on 2023-06-09 13:28:24 UTC

computeCartesianPath always return me a very low fraction and cant complete a cartesian move

Asked by angcorcue2 on 2023-06-12 09:30:50 UTC

computeCartesianPath always return me a very low fraction

Can you update OP to share the relevant pieces of codes? I've seen multiple codebases that were doing straight lines with moveit's cartesian method, telling me it is capable.

Asked by 130s on 2023-06-12 09:41:06 UTC

I have updated the OP with more information. The problem is that computecartesianpath returns very low fraction values.

Asked by angcorcue2 on 2023-06-14 08:56:19 UTC

Straight line in moveit

Comments

Answers

Edit:

Prerequisites

Final Pose Parametric Adjustment

Things to Note

Comments

Comments