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 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 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 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'
