def clear(self):
octomap_opt = OctomapOptions(OctomapOptions.CLEAR)
self.octomap(octomap_opt)
def start(self):
octomap_opt = OctomapOptions(OctomapOptions.START)
self.octomap(octomap_opt)
def update(self):
octomap_opt = OctomapOptions(OctomapOptions.UPDATE)
self.octomap(octomap_opt)
def stop(self):
octomap_opt = OctomapOptions(OctomapOptions.STOP)
self.octomap(octomap_opt)
def main():
rospy.wait_for_service('/l_arm_planning/attach_object')
#attach_object = rospy.ServiceProxy('/l_arm_planning/attach_object', AttachObject)
rospy.ServiceProxy('/l_arm_planning/attach_object', AttachObject)
rospy.wait_for_service('/manage_octomap')
octomap = rospy.ServiceProxy('/manage_octomap', ManageOctomap)
# Table
# table_pose = get_pose(0.65, 0.0, 0.70)
# table = get_collision_box(table_pose, 'table', [0.5, 1.0, 0.08])
# attach_object(table)
# Grasp ob
pringles_pose = get_pose(0.70, 0.25, 0.75)
pringles = get_collision_cylinder(pringles_pose, 'pringles', [0.10, 0.04])
#attach_object(pringles)
# Actualizar y congelar octomap
octomap_opt = OctomapOptions(OctomapOptions.UPDATE)
octomap(octomap_opt)
l_limb = Limb('l')
l_limb.arm.generate_grasp(pringles, axial_res=6, angle_res=10)
l_limb.arm.set_position_named('home')
rospy.sleep(2.0)
l_limb.arm.set_position_named('premanip_1')
rospy.sleep(3.0)
possible_grasp = l_limb.arm.get_grasp()
if len(possible_grasp.ik_solutions) == 0:
rospy.logerr('NO SE GENERARON GRASPS :(')
return
pregrasp_joints = possible_grasp.ik_solutions[
2 * possible_grasp.order[0]].positions
grasp_joints = possible_grasp.ik_solutions[2 * possible_grasp.order[0] +
1].positions
result = l_limb.arm.set_joint(
pregrasp_joints) # Movimiento con planificador
if (result.error_code.val == MoveItErrorCodes.SUCCESS):
l_limb.gripper.open(effort=300)
rospy.sleep(1.0)
l_limb.arm.move_joint(
grasp_joints,
interval=2.5) # Movimiento con collisiones permitidas
l_limb.arm.wait()
rospy.sleep(0.5)
l_limb.gripper.close(effort=300)
l_limb.gripper.open(effort=300)
l_limb.gripper.close(effort=300)
l_limb.gripper.open(effort=300)
l_limb.gripper.close(effort=300)
rospy.sleep(1.0)
else:
rospy.sleep(2.0)
l_limb.arm.set_position_named('premanip_1')
rospy.sleep(2.0)