when I use descartes to move a specf cartesian path ,I found that the robot trajectory will cause collision to the environment

[dbdxnuliba]

I has list part of the code of application ：

int main(int argc,char** argv) { ros::init(argc,argv,"plan_and_run"); ros::AsyncSpinner spinner(2); spinner.start(); moveit::planning_interface::PlanningSceneInterface scene;

// creating application plan_and_run::DemoApplication application;

// loading parameters application.loadParameters(); application.loadEnv(scene);

// initializing ros components application.initRos();

// initializing descartes application.initDescartes();

// moving to home position application.moveHome();

// generating trajectory plan_and_run::DescartesTrajectory traj; application.generateTrajectory(traj);

// planning robot path plan_and_run::DescartesTrajectory output_path; application.planPath(traj,output_path);

// running robot path application.runPath(output_path);

// exiting ros node spinner.stop();

return 0; }

void DemoApplication::initDescartes() { robot_modelptr.reset(new ur3_demo_descartes::UR3RobotModel());

// //Enable collision checking robot_modelptr->setCheckCollisions(true);

if(robot_modelptr->initialize(ROBOT_DESCRIPTIONPARAM, config.groupname, config.worldframe, config.tip_link)) { ROS_INFO_STREAM("Descartes Robot Model initialized"); } else { ROS_ERROR_STREAM("Failed to initialize Robot Model"); exit(-1); }

bool succeeded = planner_.initialize(robot_modelptr); if(succeeded) { ROS_INFO_STREAM("Descartes Dense Planner initialized"); } else { ROS_ERROR_STREAM("Failed to initialize Dense Planner"); exit(-1); }

ROS_INFO_STREAM("Task '"<<FUNCTION<<"' completed"); }

}

void DemoApplication::loadEnv(moveit::planning_interface::PlanningSceneInterface& scene) { ros::NodeHandle pnh("~");

moveit::planning_interface::MoveGroupInterface move_group(config_.group_name);

std::vector<moveit_msgs::CollisionObject> objects;
 double high = 0.1;

moveit_msgs::CollisionObject obj;
obj.operation = obj.ADD;
obj.id = "box";
std::string frame_id = move_group.getPlanningFrame();
shape_msgs::SolidPrimitive shape;
shape.type = shape.BOX;
shape.dimensions.push_back(0.3);
shape.dimensions.push_back(0.3);
shape.dimensions.push_back(high);
obj.primitives.push_back(shape);
geometry_msgs::Pose pose;
pose.position.x = 0.3;
pose.position.y = 0;

//pose.position.z = -high / 2.0; pose.position.z = 0.2;

tf::Quaternion quat;
quat.setRPY(deg2rad(0), deg2rad(0), deg2rad(0));
pose.orientation.x = quat.x();
pose.orientation.y = quat.y();
pose.orientation.z = quat.z();
pose.orientation.w = quat.w();
obj.primitive_poses.push_back(pose);

objects.push_back(obj);

std::vector<moveit_msgs::ObjectColor> colors;
moveit_msgs::ObjectColor color;
color.id = "box";
// [0,255] int, float32 [0, 1]
color.color.r = 0;
color.color.g = 255 / 255.0;
color.color.b = 0;
color.color.a = 1;
colors.push_back(color);

scene.applyCollisionObjects(objects, colors);

ROS_INFO_STREAM("Task '"<<FUNCTION<<"' completed");

}

[dbdxnuliba]

I sincerely look forward to your reply