0
$\begingroup$

Rosanswers logo

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<moveit_msgs::CollisionObject> 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.

------------------------------------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 acm = planning_sence.getAllowedCollisionMatrix();

std::vector<std::string> obj_names;
acm.getAllEntryNames(obj_names);
for(int i = 0; i < obj_names.size(); i++)
    std::cout<< obj_names[i] << std::endl;

collision_request_right.group_name = "right_arm";
collision_request_left.group_name = "left_arm";
JntArray jnt_init_left = JntArray(6);
JntArray jnt_init_right = JntArray(6);
sensor_msgs::JointState joint = InitJointState(JntArray(16));
setJointState(jnt_init_left, jnt_init_right, joint);    
joint.header.stamp = ros::Time::now();
pub.publish(joint);

std::vector<double> joint_pos_left(6, 0.0);
std::vector<double> joint_pos_right(6, 0.0);

static const std::string PLANNING_GROUP_LEFT = "left_arm";
static const std::string PLANNING_GROUP_RIGHT = "right_arm";
moveit::planning_interface::MoveGroup move_group_left(PLANNING_GROUP_LEFT);
moveit::planning_interface::MoveGroup move_group_right(PLANNING_GROUP_RIGHT);


const robot_state::JointModelGroup *joint_model_group_left =
        move_group_left.getCurrentState()->getJointModelGroup(PLANNING_GROUP_LEFT);

const robot_state::JointModelGroup *joint_model_group_right =
        move_group_right.getCurrentState()->getJointModelGroup(PLANNING_GROUP_RIGHT);


jntArray2jntVector(jnt_init_right, joint_pos_right);
check_collision_state.setJointGroupPositions(joint_model_group_right, joint_pos_right);

jntArray2jntVector(jnt_init_left, joint_pos_left);   
check_collision_state.setJointGroupPositions(joint_model_group_left, joint_pos_left);


collision_result_right.clear();   

planning_sence.checkCollision(collision_request_right, collision_result_right); 
collision_result_left.clear();   

planning_sence.checkCollision(collision_request_left, collision_result_left); 

std::cout << "left arm: " << collision_result_left.collision << std::endl;
std::cout << "right arm: " << collision_result_right.collision << std::endl;


return 0;

}


Originally posted by Xin Zhao on ROS Answers with karma: 1 on 2017-03-24

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by angeltop with karma: 351 on 2017-03-27

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by Xin Zhao on 2017-03-28:
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

Comment by angeltop on 2017-03-28:
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

Comment by Xin Zhao on 2017-03-28:
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.

Comment by angeltop on 2017-03-29:
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.

Comment by angeltop on 2017-03-29:
Check this comment as well

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.