diff --git a/src/grasping/environment_util.cpp b/src/grasping/environment_util.cpp index 54766834b5e4fb2a625ddabc44a907940babeea2..6d8af3d85f7d18691629029c1c537c3379662ceb 100644 --- a/src/grasping/environment_util.cpp +++ b/src/grasping/environment_util.cpp @@ -24,10 +24,11 @@ void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> & plate.primitive_poses[0].position.x = 0; plate.primitive_poses[0].position.y = 0; plate.primitive_poses[0].position.z = -0.1; + plate.primitive_poses[0].orientation.w = 1; plate.operation = plate.ADD; - collision_objects.push_back(plate); +// collision_objects.push_back(plate); } void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id, @@ -50,6 +51,7 @@ void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject> pick_support.primitive_poses[0].position.x = object_to_pick_pose.position.x; pick_support.primitive_poses[0].position.y = object_to_pick_pose.position.y; pick_support.primitive_poses[0].position.z = (object_to_pick_pose.position.z - (object_z_dimension / 2) - 0.02); + pick_support.primitive_poses[0].orientation.w = 1; // TODO align to object pick_support.operation = pick_support.ADD;