ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
2

Straight line in moveit

asked 2023-06-07 06:22:06 -0500

angcorcue2 gravatar image

updated 2023-06-14 08:55:33 -0500

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: r_trajec = std::make_shared<robot_trajectory::robottrajectory>(move_group_.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.

edit retag flag offensive close merge delete

2 Answers

Sort by » oldest newest most voted
0

answered 2023-06-07 10:15:44 -0500

updated 2023-06-19 07:54:03 -0500

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:

  1. Current State Stage
  2. 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 ...
(more)
edit flag offensive delete link more

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?

angcorcue2 gravatar image angcorcue2  ( 2023-06-08 03:11:46 -0500 )edit
1

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.

130s gravatar image 130s  ( 2023-06-09 10:05:22 -0500 )edit
1

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

Gaurav Gupta gravatar image Gaurav Gupta  ( 2023-06-19 04:27:00 -0500 )edit
-1

answered 2023-06-09 10:03:38 -0500

130s gravatar image

updated 2023-06-09 10:03:55 -0500

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:

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

Comments

1

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.

gvdhoorn gravatar image gvdhoorn  ( 2023-06-09 13:28:24 -0500 )edit
1

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

angcorcue2 gravatar image angcorcue2  ( 2023-06-12 09:30:50 -0500 )edit

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.

130s gravatar image 130s  ( 2023-06-12 09:41:06 -0500 )edit

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

angcorcue2 gravatar image angcorcue2  ( 2023-06-14 08:56:19 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2023-06-07 06:22:06 -0500

Seen: 520 times

Last updated: Jun 19 '23