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 ...