avoid obstacles while using computeCartesianPath

edit

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

rt.getRobotTrajectoryMsg(trajectory);
// 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();
waypoints.clear();

} };

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.