Hi,
I'm try to check the collision between collisionObject and robot.
I added the collision object following the tutorial http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/pr2_tutorials/planning/src/doc/move_group_interface_tutorial.html, and the collision object is shown in Rviz. However, when I try to check the collision, there is some problem.
moveit_msgs::CollisionObject cylinder;
cylinder.id = "seven_dof_arm_cylinder";
cylinder.header.frame_id = "base_link";
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.CYLINDER;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.6;
primitive.dimensions[1] = 0.2;
geometry_msgs::Pose pose;
pose.orientation.w = 1.0;
pose.position.x = 0.0;
pose.position.y = -0.4;
pose.position.z = 0.4;
cylinder.primitives.push_back(primitive);
cylinder.primitive_poses.push_back(pose);
cylinder.operation = cylinder.ADD;
std::vector collision_objects;
collision_objects.push_back(cylinder);
current_scene.addCollisionObjects(collision_objects);
Then, I check the collision with
std::vector joint_values;
const robot_model::JointModelGroup* joint_model_group =
current_state.getJointModelGroup("arm");
current_state.copyJointGroupPositions(joint_model_group, joint_values);
joint_values[0] = -1.57; //hard-coded since we know collisions will happen here
joint_values[1] = 0.5;
current_state.setJointGroupPositions(joint_model_group, joint_values);
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
ROS_INFO_STREAM("collision Test: "<< (collision_result.collision ? "in" : "not in")<< " self collision");
actually, the robot and the collision object is in collision at this position, but the result shows not in collision.
So, what's wrong with my codes. please help me out.
↧