checkCollision in MoveIt doesn't show collision between robot and scene
Hi everyone
I have problem - function checkCollision in MoveIt doesn't show collision between robot and scene.
The purpose is to move arm to random valid poses for testing planning algorithms.
My code:
int main( int argc, char** argv )
{
ros::init(argc, argv, "moveit");
ros::AsyncSpinner spinner(1);
spinner.start();
Planner planner("left_arm", "right_arm");
addBox(planner);
for (int i=0; i< 1000; ++i)
{
ros::Time begin = ros::Time::now();
if (planner.moveRandomPos(1))
{
cout << ros::Time::now()-begin << endl;
}
}
ros::shutdown();
return 0;
}
function addBox for left arm:
bool addBox(Planner& planner)
{
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = planner.move_group1->getPlanningFrame();
collision_object.id = "box";
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.050;
primitive.dimensions[1] = 0.50;
primitive.dimensions[2] = 2;
geometry_msgs::Pose stlPose;
stlPose.orientation.w = 1.0;
stlPose.position.x = 0.3; //left
stlPose.position.y = -0.2; //nazad
stlPose.position.z = 1.5;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(stlPose);
collision_object.operation = collision_object.ADD;
planner.node_handle.advertise<moveit_msgs::CollisionObject>("collision_object", 0);
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
ROS_INFO_NAMED("tutorial", "Add an object into the world");
planner.planning_scene_interface.addCollisionObjects(collision_objects);
ros::Duration(1.0).sleep();
moveit_msgs::PlanningScene planning_scene;
ros::Publisher planning_scene_diff_publisher = planner.node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
planning_scene.world.collision_objects.push_back(collision_object);
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);
sleep(2.0);
}
Structure Planner with constructor, moveRandomPos and isRobotStateCollision functions
struct Planner {
ros::NodeHandle node_handle;
string packPath, planGroupName1, planGroupName2, planner_plugin_name;
std::vector<std::string> joint_names1, joint_names2, link_names1, link_names2;
moveit::planning_interface::MoveGroupInterface *move_group1, *move_group2;
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
planning_scene::PlanningScenePtr planning_scene;
planning_scene_monitor::PlanningSceneMonitor *planning_scene_monitor;
robot_model_loader::RobotModelLoader *robot_model_loader;
robot_model::RobotModelPtr kinematic_model;
robot_state::RobotStatePtr kinematic_state;
robot_state::JointModelGroup *joint_model_group1, *joint_model_group2;
Planner(string planGroup1,string planGroup2)
{
planGroupName1 = planGroup1;
planGroupName2 = planGroup2;
packPath = ros::package::getPath("moveit")+"/";
move_group1 = new moveit::planning_interface::MoveGroupInterface(planGroupName1);
move_group2 = new moveit::planning_interface::MoveGroupInterface(planGroupName2);
robot_model_loader = new robot_model_loader::RobotModelLoader("robot_description");
//new
planning_scene_monitor = new planning_scene_monitor::PlanningSceneMonitor("robot_description");
planning_scene = planning_scene_monitor->getPlanningScene();
kinematic_model = robot_model_loader->getModel();
kinematic_state = robot_state::RobotStatePtr(new robot_state::RobotState(kinematic_model));
joint_model_group1 = kinematic_model->getJointModelGroup(planGroupName1);
joint_model_group2 = kinematic_model->getJointModelGroup(planGroupName2);
joint_names1 = joint_model_group1->getJointModelNames();
joint_names2 = joint_model_group2->getJointModelNames();
link_names1 = joint_model_group1->getLinkModelNames();
link_names2 = joint_model_group2->getLinkModelNames();
}
bool moveRandomPos(u_int armNumber)
{
moveit::planning_interface::MoveGroupInterface *move_group;
robot_state::JointModelGroup *joint_model_group;
switch (armNumber) {
case 1:
move_group = move_group1;
joint_model_group = joint_model_group1;
break;
case 2:
move_group = move_group2;
joint_model_group = joint_model_group2;
break;
default:
cout << "Bad armNumber" << endl;
return false;
}
move_group->setStartStateToCurrentState();
robot_state::RobotState goal_state = planning_scene->getCurrentState();
for (u_int i=0; i<100; ++i)
{
goal_state.setToRandomPositions(joint_model_group);
if (!isRobotStateCollision(goal_state))
{
break;
}
if (i==99)
{
cout << "Can't gen random pos" << endl;
return false;
}
}
Eigen::Affine3d end_effector_state = goal_state.getGlobalLinkTransform(joint_model_group->getLinkModelNames().back());
geometry_msgs::Pose idle_pose;
tf::poseEigenToMsg(end_effector_state, idle_pose);
move_group->setPoseTarget(idle_pose);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
move_group->setPlanningTime(10.0);
if (move_group->plan(my_plan))
return true;
else
return false;
}
bool isRobotStateCollision(robot_state::RobotState& robSt)
{
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
collision_detection::AllowedCollisionMatrix acm = planning_scene->getAllowedCollisionMatrix();
acm.print(cout);
planning_scene->checkCollision(req, res, robSt, acm);
return res.collision;
}
}
Planning is nice, robot avoids "box" without collisions. But I have troubles with moveRandomPos and checkCollision function. I want to generate valid poses without collisions but checkCollision doesn't show collision between robot and "box" and I often have error from planner:
[ERROR] [1493255278.226718474]: RRTConnect: Unable to sample any valid states for goal tree
acm.print(cout) shows only parts of robot. How can I update acm? I try different ways but don't have a result.
I tried function publishBox instead of addBox but result is the same:
void publishBox(Planner& planner)
{
geometry_msgs::Pose stlPose;
stlPose.orientation.w = 1.0;
stlPose.position.x = 0.3; //left
stlPose.position.y = -0.2; //nazad
stlPose.position.z = 1.5;
//publish collision object
moveit_msgs::CollisionObject object;
// object.header = handle_pose.header;
object.header.frame_id = planner.move_group1->getPlanningFrame();
object.id = "box";
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.050;
primitive.dimensions[1] = 0.50;
primitive.dimensions[2] = 2;
object.primitives.push_back(primitive);
object.primitive_poses.push_back(stlPose);
object.operation = object.ADD;
planner.node_handle.advertise<moveit_msgs::CollisionObject>("collision_object", 0);
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(object);
ROS_INFO_NAMED("tutorial", "Add an object into the world");
planner.planning_scene_interface.addCollisionObjects(collision_objects);
sleep(2.0);
moveit_msgs::PlanningScene planning_scene_add;
planning_scene_add.world.collision_objects.push_back(object);
planning_scene_add.is_diff = true;
ros::Publisher planning_scene_diff_publisher = planner.node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
planning_scene_diff_publisher.publish(planning_scene_add);
sleep(2.0);
moveit_msgs::GetPlanningScene srv;
srv.request.components.components = moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES;
//wait until box is published (I don't think that this is really needed)
bool object_in_world = false;
while(!object_in_world)
{
ROS_ERROR("waiting for box to appear");
if (planner.client_get_scene_.call(srv))
{
for (int i = 0; i < (int)srv.response.scene.world.collision_objects.size(); ++i)
{
if (srv.response.scene.world.collision_objects[i].id == object.id)
object_in_world = true;
}
}
}
moveit_msgs::PlanningScene currentScene;
moveit_msgs::PlanningScene newSceneDiff;
moveit_msgs::GetPlanningScene scene_srv;
scene_srv.request.components.components = scene_srv.request.components.ALLOWED_COLLISION_MATRIX;
if(!planner.client_get_scene_.call(scene_srv))
{
ROS_WARN("Failed to call service /get_planning_scene");
}
else
{
ROS_INFO_STREAM("Initial scene!");
currentScene = scene_srv.response.scene;
moveit_msgs::AllowedCollisionMatrix currentACM = currentScene.allowed_collision_matrix;
ROS_ERROR_STREAM("size of acm_entry_names before " << currentACM.entry_names.size());
ROS_ERROR_STREAM("size of acm_entry_values before " << currentACM.entry_values.size());
ROS_ERROR_STREAM("size of acm_entry_values[0].entries before " << currentACM.entry_values[0].enabled.size());
currentACM.entry_names.push_back(object.id);
moveit_msgs::AllowedCollisionEntry entry;
entry.enabled.resize(currentACM.entry_names.size());
for(int i = 0; i < entry.enabled.size(); i++)
entry.enabled[i] = false;
//add new row to allowed collsion matrix
currentACM.entry_values.push_back(entry);
for(int i = 0; i < currentACM.entry_values.size(); i++)
{
//extend the last column of the matrix
currentACM.entry_values[i].enabled.push_back(false);
}
newSceneDiff.is_diff = true;
newSceneDiff.allowed_collision_matrix = currentACM;
planning_scene_diff_publisher.publish(newSceneDiff);
}
if(!planner.client_get_scene_.call(scene_srv))
{
ROS_WARN("Failed to call service /get_planning_scene");
}
else
{
ROS_INFO_STREAM("Modified scene!");
currentScene = scene_srv.response.scene;
moveit_msgs::AllowedCollisionMatrix currentACM = currentScene.allowed_collision_matrix;
ROS_ERROR_STREAM("size of acm_entry_names after " << currentACM.entry_names.size());
ROS_ERROR_STREAM("size of acm_entry_values after " << currentACM.entry_values.size());
ROS_ERROR_STREAM("size of acm_entry_values[0].entries after " << currentACM.entry_values[0].enabled.size());
}
}
acmprint from isRobotStateCollision doesn't show "box" but ROSERROR_STREAM from publishBox shows it:
[ERROR] [1493257331.750786744]: size of acm_entry_names before 61
[ERROR] [1493257331.750805772]: size of acm_entry_values before 61
[ERROR] [1493257331.750815565]: size of acm_entry_values[0].entries before 61
[ERROR] [1493257331.757684566]: size of acm_entry_names after 62
[ERROR] [1493257331.757750064]: size of acm_entry_values after 62
[ERROR] [1493257331.757775842]: size of acm_entry_values[0].entries after 62
Asked by Vlad222 on 2017-04-21 16:57:09 UTC
Answers
The AllowedCollisionMatrix should be fine as it is.
how are you creating the planning_scene
pointer? Are you sure it is synchronized with the one you are adding the collision objects to? I would suspect a problem in that direction.
Asked by rbbg on 2017-04-22 04:57:26 UTC
Comments