Ask Your Question
0

Check collision between CollisonObject and robot with moveit

asked 2017-03-24 02:43:03 -0500

Xin Zhao gravatar image

updated 2017-03-28 20:56:25 -0500

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/movei... , 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<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(cylinder);

current_scene.addCollisionObjects(collision_objects);

Then, I check the collision with

std::vector<double> 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.

------------------------------------updated 2017-03-29------------------------------------------------ my entire codes

#include <moveit/move_group_interface/move_group.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <dual_arm_robot.hpp>
#include <sensor_msgs/JointState.h>
#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <moveit_msgs/PlanningScene.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/GetStateValidity.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/ApplyPlanningScene.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/conversions.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

int main(int argc, char **argv) {

ros::init(argc, argv, "planning_scene_ros_api_tutorial");
ros::AsyncSpinner spinner(1);
spinner.start();

ros::NodeHandle node_handle;
ros::Duration sleep_time(10.0);
sleep_time.sleep();
sleep_time.sleep();


ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);

ros::Publisher pub = node_handle.advertise<sensor_msgs::JointState>("/joint_states", 10);

while (planning_scene_diff_publisher.getNumSubscribers() < 1)
{
    ros::WallDuration sleep_t(0.5);
    sleep_t.sleep();
}


moveit_msgs::CollisionObject collsion_object;
collsion_object.header.frame_id = "world";
collsion_object.id = "box_1";

shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.15;
primitive.dimensions[1] = 0.15;
primitive.dimensions[2] = 0.4;

geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.3;
box_pose.position.y = 0;
box_pose.position.z = -0.35;

collsion_object.primitives.push_back(primitive);
collsion_object.primitive_poses.push_back(box_pose);
collsion_object.operation = collsion_object.ADD;

ROS_INFO("Adding the object into the world at the location of the right wrist.");
moveit_msgs::PlanningScene planning_scene;
planning_scene.world.collision_objects.push_back(collsion_object);
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);
sleep_time.sleep();

ros::ServiceClient planning_scene_diff_client =
        node_handle.serviceClient<moveit_msgs::ApplyPlanningScene>("apply_planning_scene");
planning_scene_diff_client.waitForExistence();

moveit_msgs::ApplyPlanningScene srv;
srv.request.scene = planning_scene;
planning_scene_diff_client.call(srv);

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();

planning_scene::PlanningScene planning_sence(kinematic_model);

robot_state::RobotState &check_collision_state = planning_sence.getCurrentStateNonConst();

collision_detection::CollisionRequest collision_request_right;   
collision_detection::CollisionRequest collision_request_left;    

collision_detection::CollisionResult collision_result_left;      
collision_detection::CollisionResult collision_result_right;      

collision_detection::AllowedCollisionMatrix ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2017-03-27 04:03:32 -0500

angeltop gravatar image

checkSelfCollision function checks if the links of the given joint group are in collision. Try to use checkCollision instead, or isStateColliding

Make sure that you update the planning scene with the collision object and that the AllowedCollisionMatrix of the planning scene that you use for collision checking is correctly updated.

In the first part of your code the planning scene update commands are missing.

edit flag offensive delete link more

Comments

Thanks for your answer.

I tried checkCollision, but it does not work. I also doubt the planning scene is not updated.

I find some tutorial, such as link text But it seems like not for indigo. Error occur when I compile the code.

So how should I update the planning scene

Xin Zhao gravatar imageXin Zhao ( 2017-03-28 02:43:33 -0500 )edit

Both of the links that you have provided are for kinetic, if you are using indigo you should always search for indigo tutorials like this one

angeltop gravatar imageangeltop ( 2017-03-28 03:04:20 -0500 )edit

Thank you again. I made a mistake before, I confused the moveit_msg::planning_scene with planning_scene. I tried this turorial, but it still doesn't work. I also checked the AllowedCollisionMatrix, the collision object is not in it. My codes is listed.

Xin Zhao gravatar imageXin Zhao ( 2017-03-28 20:50:07 -0500 )edit

Before planning_scene_diff_publisher.publish(planning_scene); and moveit_msgs::ApplyPlanningScene srv; srv.request.scene = planning_scene; planning_scene_diff_client.call(srv); You have to add the collision object in the allowedcollisionmatrix of the scene and then update it.

angeltop gravatar imageangeltop ( 2017-03-29 01:32:05 -0500 )edit

Check this comment as well

angeltop gravatar imageangeltop ( 2017-03-29 01:32:40 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-03-24 02:43:03 -0500

Seen: 1,178 times

Last updated: Mar 28 '17