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

avoid obstacles while using computeCartesianPath

asked 2015-11-19 01:34:43 -0600

hecatezxx gravatar image

updated 2015-11-19 02:39:21 -0600

Hi everyone!

I got a problem while using computeCartesianPath to avoid obstacles in moveit! I define an object in the xacro file, and indeed the object could be detected and avoided. but all i want is when the object is detected along the path, the planner could re-plan the path in order to avoid the object. But obviously, it fails. It just follows the original path and stops when it's adjacent to the object. Could anyone has some ideas about this or do I really did it the right way?

Here are the relevant code:

void movetopose(const geometry_msgs::Pose& pose) { move_group_interface::MoveGroup::Plan my_plan; moveit_msgs::RobotTrajectory trajectory; std::vector<geometry_msgs::pose> waypoints; group_ptr->setStartStateToCurrentState(); waypoints.push_back(this->group_ptr->getCurrentPose().pose); waypoints.push_back(pose);

double fraction = group_ptr->computeCartesianPath(waypoints,
                        0.01,   //eef_step
                        0.0,    //jump_thredshold
                        trajectory, true);

ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",
    fraction * 100.0);

robot_trajectory::RobotTrajectory rt(this->kinematic_state->getRobotModel(),"manipulator");
rt.setRobotTrajectoryMsg(*(this->kinematic_state), trajectory);

// Thrid create a IterativeParabolicTimeParameterization object
trajectory_processing::IterativeParabolicTimeParameterization iptp;
bool success = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");

// Get RobotTrajectory_msg from RobotTrajectory

// Finally plan and execute the trajectory
my_plan.trajectory_ = trajectory;
success = this->group_ptr->execute(my_plan);
ROS_INFO("Executing the plan %s",success?"SUCCEDED":"FAILED");

this->kinematic_state = this->group_ptr->getCurrentState();

} };

xacro: <link name="box_frame" &gt;="" <visual=""> <geometry> <box size="0.21 0.295 0.05"/> </geometry> </visual> <collision> <geometry> <box size="0.21 0.295 0.05"/> </geometry> </collision> </link>

<joint name="world_to_box" type="fixed"> <parent link="world"/> <child link="box_frame"/> <origin xyz="0.2 0.4 0.6" rpy="0 0 0"/> </joint>

Thanks a lot.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2015-11-19 03:06:37 -0600

The cartesian path functionality is solely intended for computing cartesian paths and the behavior you request is highly application-dependent. You could either implement your own move group plugin that performs specialized treatment on failure or add your own logic for treating failures in the code where you call computeCartesianPath() (for instance checking if the goal has been reached, and triggering normal planning if this is not the case).

edit flag offensive delete link more

Question Tools



Asked: 2015-11-19 01:34:43 -0600

Seen: 876 times

Last updated: Nov 19 '15