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

# Straight line in moveit

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 close merge delete

Sort by » oldest newest most voted

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
{
auto current_state =
"current state");

auto applicability_filter =
"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();
}

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

// Move to Pick Connect Stage - non-cartesian
{
"move to pick",
{ arm_group_name_, sampling_planner } });
stage->setTimeout(60.0);
stage->properties().configureInitFrom(
}

• 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
{
auto grasp =
"pick object");
t.properties().exposeTo(grasp->properties(),
{ "eef", "hand", "group", "ik_frame" });
grasp->properties().configureInitFrom(
{ "eef", "hand", "group", "ik_frame" });

// Approach object - substage
{
auto stage =
"Approach object", cartesian_planner);
stage->properties().set("marker_ns", "approach_object");
stage->properties().configureInitFrom(
stage->setMinMaxDistance(
approach_object_min_dist_, approach_object_max_dist_);

// Set hand forward direction
geometry_msgs::Vector3Stamped vec;
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.pose = object_pose_;
CustomPoses.push_back(p);

auto stage =
std::make_unique<
"generate grasp pose");
stage->setCustomPoses(CustomPoses);
stage->properties().configureInitFrom(
stage->setMonitoredStage(current_state_ptr);

// Compute IK - substage
auto wrapper =
std::make_unique<moveit::task_constructor::stages ...
more

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?

( 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.

( 2023-06-09 10:05:22 -0500 )edit
1

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

( 2023-06-19 04:27:00 -0500 )edit

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).
more

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.

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

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

( 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.

( 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.

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