when I use descartes to move a specf cartesian path ,I found that the robot 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_model_ptr_.reset(new ur3_demo_descartes::UR3RobotModel());

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

if(robot_model_ptr_->initialize(ROBOT_DESCRIPTION_PARAM,
config_.group_name,
config_.world_frame,
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_model_ptr_);
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");

}

and my question is：
the terminal show:
[ INFO] [1628060841.585946958]: Sparse planner succeeded with 102 planned point and 698 interpolated points in 3.747662 seconds
[ INFO] [1628060841.586019375]: Valid path was found
[ INFO] [1628060841.588509838]: Task 'planPath' completed
[ INFO] [1628060842.421728051]: Ready to take commands for planning group manipulator.
[ INFO] [1628060844.693831500]: Robot path sent for execution
[ INFO] [1628060957.975317054]: Robot path execution completed
[ INFO] [1628060957.975341219]: Task 'runPath' completed

it means the descartes has found Valid path ,but the path caused robot-collison with the environment, please tell me where I went wrong

the phenomenon ocurrs when I use descartes-melodic version from https://github.com/ros-industrial-consortium , but that does not appear when I use
welding_tutorial version from https://github.com/JeroenDM/descartes
the welding_tutorial version can successfully point out it cannot produce appropraite trajectory ,

it apper tips:

[ WARN] [1628066231.982286052]: GetAllIK has not solutions
[ WARN] [1628066231.982852456]: GetAllIK has not solutions
[ WARN] [1628066231.982870585]: Failed for find ANY joint poses, returning
[ERROR] [1628066231.982887910]: calculateJointSolutions: IK failed for input trajectory point with ID = ID251
[ERROR] [1628066231.982899951]: unable to calculate joint trajectories for input points

but descartes-melodic version from https://github.com/ros-industrial-consortium/descartes produced a trajectory which cause robot-collison with environment ;

so ,can you tell me how can we avoid that when I use descartes-melodic version from https://github.com/ros-industrial-consortium/descartes.

[gauvindigital]

I seem to have the same problem, using ROS Noetic, along with collision mesh and primitive... Descartes detects no collision and the robot passes right through the objects.

[jrgnicho]

I would look into the UR3RobotModel implementation and see if the check_collisions_ flag is being honored

[gauvindigital]

Hi @jrgnicho , and thanks for your answer! Actually, I'm working on a different robot than the UR3. I tried checking out the branch from #237, but I still can't make runtime defined scene object be considered for collision. I read (here #238) that yet it is only possible if objects are defined in the URDF. However, for my application, it wouldn't be practical. Have any of you guys managed to make in work? Is there a branch somewhere or instructions I could follow to enable collision checking for the live planning scene objects (meshes and primitives)?

[jrgnicho]

You may have to override the various isValid methods to do the kind of collision checking that your application needs

[dbdxnuliba]

@gauvindigital , have you solved the problem? I have tried the method box defined in the urdf,but it still produce the path which collision with the box defined in the urdf , like
https://github.com/ros-industrial-consortium/descartes/issues/260#:~:text=and%20the%20I%20test%20another%20scene%3A%0Abox_link%20and%20joint%20was%20defined%20in%20the%20irb2400.urdf

[gauvindigital]

Hi @dbdxnuliba!
I'm sorry, I have stopped working on my project since it has been cancelled, so I didn't have the chance to look it up any further. Good luck!