Attaching collision object to robot gets crooked

edit

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.

      //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);