Ask Your Question
0

Attaching collision object to robot gets crooked

asked 2015-07-15 02:33:24 -0500

JonasPallis gravatar image

updated 2015-07-15 10:54:19 -0500

gvdhoorn gravatar image

I'm attaching a tool as a collision object to the robot (UR10), so far so good. But when it's attached the tool gets crooked relative to the robot. Please take a look at the pictures and code below. Do anyone else have this problem, can't find a similar problem. I'm using Ubuntu 14.04, ROS-indigo and C++ for programming.

Picture 1

Picture2

      //MESH
  moveit_msgs::CollisionObject co;
 co.header.frame_id = group.getPlanningFrame();
 co.id = "Mounter";

shapes::Mesh* m = shapes::createMeshFromResource("package://ur_description/meshes/ur10/visual/mounter.stl");
shape_msgs::Mesh co_mesh;
shapes::ShapeMsg co_mesh_msg;
shapes::constructMsgFromShape(m,co_mesh_msg);
co_mesh = boost::get<shape_msgs::Mesh>(co_mesh_msg);
co.meshes.resize(1);
co.meshes[0] = co_mesh;
co.mesh_poses.resize(1);
co.mesh_poses[0].position.x = -0.39;
co.mesh_poses[0].position.y = -0.777;
co.mesh_poses[0].position.z = 0.3115;
co.mesh_poses[0].orientation.w= 0.0;
co.mesh_poses[0].orientation.x= 0.0;
co.mesh_poses[0].orientation.y= 3.14;
co.mesh_poses[0].orientation.z= -3.1415;
//pub_co.publish(co);

co.meshes.push_back(co_mesh);
co.mesh_poses.push_back(co.mesh_poses[0]);
co.operation = co.ADD;
std::vector<moveit_msgs::CollisionObject> collision_objects3; 
collision_objects3.push_back(co);  

// Now, let's add the collision object into the world
ROS_INFO("Adding Mounter to world");  
planning_scene_interface.addCollisionObjects(collision_objects3);
sleep(5);


ROS_INFO("Attach the tool to the robot");
group.attachObject("Mounter", "ee_link");

We have done the same test with basicshape cylinders instead of a mesh as collision object without this problem.

moveit_msgs::CollisionObject collision_object1;
collision_object1.header.frame_id = group.getPlanningFrame();
collision_object1.id = "cylinder2";

/* Define a box to add to the world. */
shape_msgs::SolidPrimitive primitive1;
primitive1.type = primitive1.CYLINDER;
primitive1.dimensions.resize(2);
primitive1.dimensions[0] = 0.13;
primitive1.dimensions[1] = 0.06;

/* A pose for the box (specified relative to frame_id) */
geometry_msgs::Pose cylinder_pose1;
cylinder_pose1.orientation.w = 0;
cylinder_pose1.position.x = -0.39;
cylinder_pose1.position.y = -0.78;
cylinder_pose1.position.z = -0.025;

collision_object1.primitives.push_back(primitive1);
collision_object1.primitive_poses.push_back(cylinder_pose1);
collision_object1.operation = collision_object1.ADD;

std::vector<moveit_msgs::CollisionObject> collision_objects1;
collision_objects1.push_back(collision_object1);

ROS_INFO("Add cylinder into the world");
planning_scene_interface.addCollisionObjects(collision_objects);
sleep(2);

ROS_INFO("Add cylinder into the world");
planning_scene_interface.addCollisionObjects(collision_objects1);
sleep(2);
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2015-07-15 03:19:21 -0500

gvdhoorn gravatar image

updated 2015-07-15 03:29:03 -0500

co.mesh_poses[0].orientation.w= 0.0;
co.mesh_poses[0].orientation.x= 0.0;
co.mesh_poses[0].orientation.y= 3.14;
co.mesh_poses[0].orientation.z= -3.1415;

Poses in ROS are preferably encoded as quaternions, not Euler angles. The moveit_msgs/CollisionObject message uses arrays of geometry_msgs/Pose messages to store those poses.

From your code snippet, I get the idea that you tried to assign Euler angle rotations around the Y and Z-axis. That won't work (as you noticed). If you want to continue using Euler angles, you can convert them to a quaternion before assigning them to the co.mesh_poses array elements.


Edit:

group.attachObject("Mounter", "ee_link");

It might also be an idea to attach your object to the tool0 link of the UR10 model, instead of ee_link. The tool0 link was recently introduced (universal_robot/pull#200) and is oriented in such a way that it corresponds to the default (all-zeros) toolframe of the UR10.


Edit2: see Transform Quaternion fi for some hints on how to perform the conversion.

edit flag offensive delete link more

Comments

Using the new universal_robot folder with tool0 instead of ee_link did unfortunately not solve the problem. We will use tool0 in the future anyway.

I guess that our problem lies with the co.mesh_poses. When we did this test with two basic cylinders instead of a .stl we did not have this problem.

JonasPallis gravatar image JonasPallis  ( 2015-07-15 04:16:10 -0500 )edit

So I'm confused: are you or are you not using a quaternion for the pose in your 'stl mesh' code excerpt? Cause those values in the .y and .z fields of the orientation field seem rather strange for a quaternion.

gvdhoorn gravatar image gvdhoorn  ( 2015-07-15 04:25:05 -0500 )edit

Using the new universal_robot folder with tool0 instead of ee_link did unfortunately not solve the problem.

I wasn't suggesting it would. I just thought I'd mention the new frames, as they are oriented somewhat more intuitively than ee_link and the base_link.

gvdhoorn gravatar image gvdhoorn  ( 2015-07-15 04:27:03 -0500 )edit

We are using Euler. We tried different .y and .z values until the tool was placed correctly. Isn't this just the initial position? Does these coordinates matter when the robot has picked the tool up?

JonasPallis gravatar image JonasPallis  ( 2015-07-15 05:04:44 -0500 )edit

We got the code from this answer.

JonasPallis gravatar image JonasPallis  ( 2015-07-15 05:05:40 -0500 )edit
1

You cannot treat the pose.orientation field as a set of Euler angles. It just doesn't work. Please read the answer to the question I linked to earlier, it should show you how to convert from a roll-pitch-yaw triplet to a quaternion, which you can then use for the orientation field.

gvdhoorn gravatar image gvdhoorn  ( 2015-07-15 05:26:41 -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

1 follower

Stats

Asked: 2015-07-15 02:33:24 -0500

Seen: 404 times

Last updated: Jul 15 '15