def to_collision_object(self): """ :return: the map as a collision object :type: CollisionObject """ co = CollisionObject() co.header.frame_id = "/odom_combined" co.id = "map" primitive = SolidPrimitive() primitive.type = SolidPrimitive.BOX primitive.dimensions.append(self.cell_size) primitive.dimensions.append(self.cell_size) primitive.dimensions.append(2) for x in range(0, len(self.field)): for y in range(0, len(self.field[x])): if self.field[x][y].is_free() or self.field[x][y].is_object(): continue if self.field[x][y].is_obstacle(): primitive.dimensions[primitive.BOX_Z] = self.get_cell_by_index(x, y).highest_z * 2 else: primitive.dimensions[primitive.BOX_Z] = self.get_average_z_of_surrounded_obstacles(x, y) * 2 primitive.dimensions[primitive.BOX_Z] += 0.02 co.primitives.append(deepcopy(primitive)) pose = Pose() (pose.position.x, pose.position.y) = self.index_to_coordinates(x, y) pose.orientation.w = 1 co.primitive_poses.append(pose) return co
def calculate_grasp_position_list(collision_object, transform_func): """ Calculates grasp positions for a composition of collision objects. :param collision_object: object composition to be grapsed :type: CollisionObject :param transform_func(object, frame_id): transform function :return: list of graps positions :type: [PoseStamped] """ grasp_positions = [] for i in range(0, len(collision_object.primitives)): co = CollisionObject() co.id = collision_object.id co.primitives.append(collision_object.primitives[i]) co.header = collision_object.header co.primitive_poses.append(collision_object.primitive_poses[i]) temp = calculate_grasp_position(co, transform_func, False, 4) for j in range(0, len(temp)): tmp_pose = transform_func(co, "/" + collision_object.id).primitive_poses[0].position temp[j].pose.position = add_point(temp[j].pose.position, tmp_pose) grasp_positions.extend(temp) return grasp_positions
def test2_1(self): co = CollisionObject() co.operation = CollisionObject.ADD co.id = "muh" co.header.frame_id = "/odom_combined" cylinder = SolidPrimitive() cylinder.type = SolidPrimitive.CYLINDER cylinder.dimensions.append(0.3) cylinder.dimensions.append(0.03) co.primitives = [cylinder] co.primitive_poses = [Pose()] co.primitive_poses[0].position = Point(1.2185, 0, 0) co.primitive_poses[0].orientation = Quaternion(0, 0, 0, 1) box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions.append(0.1) box.dimensions.append(0.1) box.dimensions.append(0.1) co.primitives.append(box) co.primitive_poses.append(Pose()) co.primitive_poses[1].position = Point(1.1185, 0, 0) co.primitive_poses[1].orientation = Quaternion(0, 0, 0, 1) co.primitives.append(box) co.primitive_poses.append(Pose()) co.primitive_poses[2].position = Point(0, 0, 0) co.primitive_poses[2].orientation = Quaternion(0, 0, 0, 1) p = PoseStamped() p.header.frame_id = "/odom_combined" p.pose.position = Point(1, 0, 0) p.pose.orientation = euler_to_quaternion(0, 0, 0) self.assertEqual(get_grasped_part(co, get_fingertip(p))[1], 0)
def to_collision_object(self): """ :return: the map as a collision object :type: CollisionObject """ co = CollisionObject() co.header.frame_id = "/odom_combined" co.id = "map" primitive = SolidPrimitive() primitive.type = SolidPrimitive.BOX primitive.dimensions.append(self.cell_size) primitive.dimensions.append(self.cell_size) primitive.dimensions.append(2) for x in range(0, len(self.field)): for y in range(0, len(self.field[x])): if self.field[x][y].is_free() or self.field[x][y].is_object(): continue if self.field[x][y].is_obstacle(): primitive.dimensions[ primitive.BOX_Z] = self.get_cell_by_index( x, y).highest_z * 2 else: primitive.dimensions[ primitive. BOX_Z] = self.get_average_z_of_surrounded_obstacles( x, y) * 2 primitive.dimensions[primitive.BOX_Z] += 0.02 co.primitives.append(deepcopy(primitive)) pose = Pose() (pose.position.x, pose.position.y) = self.index_to_coordinates(x, y) pose.orientation.w = 1 co.primitive_poses.append(pose) return co
def make_cylinder(name, pose, size): """ Creates a cylinder collision object. :param name: name of the cylinder :type: str :param pose: position of the cylinder :type: PoseStamped :param size: cylinder size :type: [float(height), float(radius)] :return: cylinder collisionobject :type: CollisionObject """ co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header cylinder = SolidPrimitive() cylinder.type = SolidPrimitive.CYLINDER cylinder.dimensions = list(size) co.primitives = [cylinder] co.primitive_poses = [pose.pose] return co
def make_box(name, pose, size): """ Creates a box collision object. :param name: name of the box :type: str :param pose: position of the box :type: PoseStamped :param size: box size :type: [float(x), float(y), float(z)] :return: box collision object :type: CollisionObject """ co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = list(size) co.primitives = [box] co.primitive_poses = [pose.pose] return co
def test2_1(self): co = CollisionObject() co.operation = CollisionObject.ADD co.id = "muh" co.header.frame_id = "/odom_combined" cylinder = SolidPrimitive() cylinder.type = SolidPrimitive.CYLINDER cylinder.dimensions.append(0.3) cylinder.dimensions.append(0.03) co.primitives = [cylinder] co.primitive_poses = [Pose()] co.primitive_poses[0].position = Point(1.2185, 0,0) co.primitive_poses[0].orientation = Quaternion(0,0,0,1) box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions.append(0.1) box.dimensions.append(0.1) box.dimensions.append(0.1) co.primitives.append(box) co.primitive_poses.append(Pose()) co.primitive_poses[1].position = Point(1.1185, 0,0) co.primitive_poses[1].orientation = Quaternion(0,0,0,1) co.primitives.append(box) co.primitive_poses.append(Pose()) co.primitive_poses[2].position = Point(0, 0,0) co.primitive_poses[2].orientation = Quaternion(0,0,0,1) p = PoseStamped() p.header.frame_id = "/odom_combined" p.pose.position = Point(1,0,0) p.pose.orientation = euler_to_quaternion(0,0,0) self.assertEqual(get_grasped_part(co, get_fingertip(p))[1], 0)
def execute(self, taskdata): rospy.loginfo('Executing state PlaceObject') destination = taskdata.place_position if self._retry == 2: rospy.logwarn('Failed to place two times. Dropping object.') self._retry = 0 if not utils.manipulation.open_gripper(): #cant happen taskdata.failed_object = EurocObject() return 'fail' if taskdata.enable_movement: move_to_func = utils.manipulation.move_arm_and_base_to else: move_to_func = utils.manipulation.move_to co = utils.manipulation.get_planning_scene().get_attached_object() if co is None: self._retry = 0 taskdata.failed_object = EurocObject() return 'noObjectAttached' co = co.object rospy.logdebug("Placing: " + str(co)) destination = utils.manipulation.transform_to(destination) rospy.logdebug("at::: " + str(destination)) if taskdata.yaml.task_type == Task.TASK_5: destination.point.z = 0.4 dest_orientation = deepcopy(taskdata.yaml.puzzle_fixture.orientation) place_poses = get_place_position_for_puzzle(destination, dest_orientation) else: place_poses = get_place_position(co, destination, utils.manipulation.transform_to, taskdata.dist_to_obj, taskdata.grasp) print place_poses # place_poses = utils.map.filter_invalid_poses3(destination.point.x, destination.point.y, place_poses) if not taskdata.enable_movement: place_poses = utils.manipulation.filter_close_poses(place_poses) for place_pose in place_poses: rospy.logdebug("Try to place at: " + str(place_pose)) if taskdata.yaml.task_type != Task.TASK_5: if not move_to_func(get_pre_place_position(place_pose), do_not_blow_up_list=(co.id)): rospy.logwarn("Can't reach preplaceposition.") continue else: rospy.logdebug("preplaceposition taken") time.sleep(0.5) rospy.sleep(1) if not move_to_func(place_pose, do_not_blow_up_list=(co.id, "map")): rospy.logwarn("Can't reach placeposition.") continue else: rospy.logdebug("placeposition taken") time.sleep(0.5) rospy.sleep(1) self._retry = 0 gripper_target = min(utils.manipulation.get_current_gripper_state()[1] + 0.002, gripper_max_pose) if not utils.manipulation.open_gripper(gripper_target): #cant happen taskdata.failed_object = EurocObject() return 'fail' rospy.sleep(3) if not utils.manipulation.open_gripper(): #cant happen taskdata.failed_object = EurocObject() return 'fail' if taskdata.yaml.task_type == Task.TASK_5: utils.manipulation.get_planning_scene().remove_object(co.id) time.sleep(0.5) rospy.sleep(1) # post_place_pose = utils.manipulation.transform_to(place_pose, co.id) if not move_to_func(get_pre_grasp(place_pose), do_not_blow_up_list=(co.id, "map")) and \ not move_to_func(get_pre_place_position(place_pose), do_not_blow_up_list=(co.id, "map")): rospy.logwarn("Can't reach postplaceposition. Continue anyway") taskdata.failed_object = EurocObject() self._retry = 0 return 'success' else: rospy.logdebug("postplaceposition taken") rospy.sleep(0.25) rospy.loginfo("placed " + co.id) taskdata.failed_object = EurocObject() self._retry = 0 return 'success' #try to place the object where it currently is rospy.logdebug("Placement failed, to to place where we are.") taskdata.place_position = self.new_place_position() o = EurocObject o.mpe_object = CollisionObject() o.object = CollisionObject() o.object.id = co.id o.mpe_object.id = co.id o.c_centroid = Point(0,0,0) taskdata.failed_object = o self._retry += 1 return 'noPlacePosition'