<commit_before>import logging; logger = logging.getLogger("robot." + __name__)
logger.setLevel(logging.DEBUG)

import random

from robots.action import *

from robots.helpers import postures
from robots.helpers.cb import *

from robots.actions import configuration, nav, look_at

used_plan_id = []

@tested("22/02/2012")
@action
def release_gripper(robot, obj = "NONE", gripper = "RIGHT"):
    """
    Opens the gripper to release something.

    Like gripper_open, except it waits until it senses some effort on the gripper force sensors.

    :see: open_gripper

    :param gripper: "RIGHT" (default) or "LEFT"
    :param obj: the object graped (if no object, NONE will make the gripper open anyway)
    """
    
    if obj != "NONE" :
        actions =[
        genom_request("spark","SetGraspedObject", [obj, 0, 0]),
        genom_request("spark","SetInferrenceForObject", [obj, 0, robot.id, 0,
            "SPARK_PRECISE_ROBOT_HAND", 1.0])
        ]

    if gripper == "RIGHT":
        actions = [genom_request("pr2SoftMotion", "GripperGrabRelease", ["RRELEASE"])]
    else:
        actions = [genom_request("pr2SoftMotion", "GripperGrabRelease", ["LRELEASE"])]

    if obj != "NONE" :
        tmp = actions
        actions =[
        genom_request("spark","SetGraspedObject", [obj, 0, 0]),
        genom_request("spark","SetInferrenceForObject", [obj, 0, robot.id, 0,
            "SPARK_PRECISE_ROBOT_HAND", 1.0])
        ]
        actions += tmp

    return actions

@tested("22/02/2012")
@action
def grab_gripper(robot, gripper = "RIGHT"):
    """
    Closes the gripper to grab something.

    Like gripper_close, except it waits until it senses some effort on the gripper force sensors.

    :see: close_gripper
    :param gripper: "RIGHT" (default) or "LEFT"
    """
    if gripper == "RIGHT":
        return [genom_request("pr2SoftMotion", "GripperGrabRelease", ["RGRAB"])]
    else:
        return [genom_request("pr2SoftMotion", "GripperGrabRelease", ["LGRAB"])]

@tested("22/02/2012")
@action
def open_gripper(robot, gripper = "RIGHT", callback = None):
    """
    Opens the right gripper.

    :see: release_gripper
    :param gripper: "RIGHT" (default) or "LEFT"
    :param callback: if set, the action is non-blocking and the callback is invoked upon completion
    """
    if gripper == "RIGHT":
        return [genom_request("pr2SoftMotion", 
                "GripperGrabRelease", 
                ["ROPEN"],
                wait_for_completion = False if callback else True,
                callback = callback)]

    else:
        return [genom_request("pr2SoftMotion", 
                "GripperGrabRelease", 
                ["LOPEN"],
                wait_for_completion = False if callback else True,
                callback = callback)]

@tested("22/02/2012")
@action
def close_gripper(robot, gripper = "RIGHT", callback = None):
    """ Closes the right gripper.
    
    :see: grab_gripper
    :param gripper: "RIGHT" (default) or "LEFT"
    :param callback: if set, the action is non-blocking and the callback is invoked upon completion
    """
    if gripper == "RIGHT":
        return [genom_request("pr2SoftMotion", 
                "GripperGrabRelease", 
                ["RCLOSE"],
                wait_for_completion = False if callback else True,
                callback = callback)]
    else:
        return [genom_request("pr2SoftMotion", 
                "GripperGrabRelease", 
                ["LCLOSE"],
                wait_for_completion = False if callback else True,
                callback = callback)]

@tested("23/02/2012")
@action
def configure_grippers(robot, grab_acc = 8.0, grab_slip = 0.2, release_acc = 4.0, release_slip = 0.05, force = 25):
    """ Sets the grippers thresholds.
    
    :param grab_acc: threshold for grab detection
    :param grab_slip: threshold for grab detection
    :param release_acc: threshold for release detection
    :param release_slip: threshold for release detection
    :param force: hold force
    """
    return [genom_request("pr2SoftMotion", 
            "SetGripperTresholds", 
            [grab_acc, grab_slip, release_acc, release_slip, force])]


def getplanid():
    """ Returns a random plan id (for Amit planification routines) which is
    guaranteed to be 'fresh'.
    """
    plan_id = random.randint(1, 1000)
    while plan_id in used_plan_id:
        plan_id = random.randint(1, 1000)
    used_plan_id.append(plan_id)
    return plan_id

@action
def pick(robot, obj, use_cartesian = "GEN_FALSE"):
    """ Picks an object that is reachable by the robot.

    :param object: the object to pick.
    """

    # Open gripper
    actions = open_gripper(robot)

    # Plan trajectory to object and execute it
    actions += [
    genom_request("mhp", "ArmPlanTask",
            [0,
            'GEN_TRUE',
            'MHP_ARM_PICK_GOTO',
            0,
            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
            obj,
            'NO_NAME',
            'NO_NAME',
            use_cartesian,
            0,
            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
        genom_request("mhp", "ArmSelectTraj", [0]),
        genom_request("pr2SoftMotion", "TrackQ", ['mhpArmTraj', 'PR2SM_TRACK_POSTER', 'RARM'])
    ]

    # Close gripper
    actions += close_gripper(robot)

    # create link between the robot and the object
    actions += [
        genom_request("spark","SetGraspedObject", [obj, 1, 0]),
        genom_request("spark","SetInferrenceForObject", [obj, 1, robot.id, 0, 
            "SPARK_PRECISE_ROBOT_HAND", 1.0]) 
    ]
    return actions

@tested("22/02/2012")
@action
def basicgive(robot):
    """ The ultra stupid basic GIVE: simply hand the object in front of the
    robot.
    
    After handing the object, the robot waits for someone to take it, and
    stay in this posture. 
    """

    posture = postures.read()
    
    actions = configuration.setpose(robot, posture["GIVE"])
    actions += release_gripper(robot)
    actions += [wait(2)]
    actions += close_gripper(nop)
        
    return actions

@tested("22/02/2012")
@action
def basicgrab(robot):
    """ The ultra stupid basic GRAB: simply take the object in front of the
    robot.<commit_msg>Fixing the attach detach function<commit_after>import logging; logger = logging.getLogger("robot." + __name__)
logger.setLevel(logging.DEBUG)

import random

from robots.action import *

from robots.helpers import postures
from robots.helpers.cb import *

from robots.actions import configuration, nav, look_at

used_plan_id = []

@tested("22/02/2012")
@action
def release_gripper(robot, gripper = "RIGHT"):
    """
    Opens the gripper to release something.

    Like gripper_open, except it waits until it senses some effort on the gripper force sensors.

    :see: open_gripper

    :param gripper: "RIGHT" (default) or "LEFT"
    """
    
    if gripper == "RIGHT":
        return [genom_request("pr2SoftMotion", "GripperGrabRelease", ["RRELEASE"])]
    else:
        return [genom_request("pr2SoftMotion", "GripperGrabRelease", ["LRELEASE"])]


@tested("22/02/2012")
@action
def grab_gripper(robot, gripper = "RIGHT"):
    """
    Closes the gripper to grab something.

    Like gripper_close, except it waits until it senses some effort on the gripper force sensors.

    :see: close_gripper
    :param gripper: "RIGHT" (default) or "LEFT"
    """
    if gripper == "RIGHT":
        return [genom_request("pr2SoftMotion", "GripperGrabRelease", ["RGRAB"])]
    else:
        return [genom_request("pr2SoftMotion", "GripperGrabRelease", ["LGRAB"])]

@tested("22/02/2012")
@action
def open_gripper(robot, gripper = "RIGHT", callback = None):
    """
    Opens the right gripper.

    :see: release_gripper
    :param gripper: "RIGHT" (default) or "LEFT"
    :param callback: if set, the action is non-blocking and the callback is invoked upon completion
    """
    if gripper == "RIGHT":
        return [genom_request("pr2SoftMotion", 
                "GripperGrabRelease", 
                ["ROPEN"],
                wait_for_completion = False if callback else True,
                callback = callback)]

    else:
        return [genom_request("pr2SoftMotion", 
                "GripperGrabRelease", 
                ["LOPEN"],
                wait_for_completion = False if callback else True,
                callback = callback)]

@tested("22/02/2012")
@action
def close_gripper(robot, gripper = "RIGHT", callback = None):
    """ Closes the right gripper.
    
    :see: grab_gripper
    :param gripper: "RIGHT" (default) or "LEFT"
    :param callback: if set, the action is non-blocking and the callback is invoked upon completion
    """
    if gripper == "RIGHT":
        return [genom_request("pr2SoftMotion", 
                "GripperGrabRelease", 
                ["RCLOSE"],
                wait_for_completion = False if callback else True,
                callback = callback)]
    else:
        return [genom_request("pr2SoftMotion", 
                "GripperGrabRelease", 
                ["LCLOSE"],
                wait_for_completion = False if callback else True,
                callback = callback)]

@tested("23/02/2012")
@action
def configure_grippers(robot, grab_acc = 8.0, grab_slip = 0.2, release_acc = 4.0, release_slip = 0.05, force = 25):
    """ Sets the grippers thresholds.
    
    :param grab_acc: threshold for grab detection
    :param grab_slip: threshold for grab detection
    :param release_acc: threshold for release detection
    :param release_slip: threshold for release detection
    :param force: hold force
    """
    return [genom_request("pr2SoftMotion", 
            "SetGripperTresholds", 
            [grab_acc, grab_slip, release_acc, release_slip, force])]


def getplanid():
    """ Returns a random plan id (for Amit planification routines) which is
    guaranteed to be 'fresh'.
    """
    plan_id = random.randint(1, 1000)
    while plan_id in used_plan_id:
        plan_id = random.randint(1, 1000)
    used_plan_id.append(plan_id)
    return plan_id

@action
def pick(robot, obj, use_cartesian = "GEN_FALSE"):
    """ Picks an object that is reachable by the robot.

    :param object: the object to pick.
    """

    # Open gripper
    actions = open_gripper(robot)

    # Plan trajectory to object and execute it
    actions += [
    genom_request("mhp", "ArmPlanTask",
            [0,
            'GEN_TRUE',
            'MHP_ARM_PICK_GOTO',
            0,
            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
            obj,
            'NO_NAME',
            'NO_NAME',
            use_cartesian,
            0,
            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
        genom_request("mhp", "ArmSelectTraj", [0]),
        genom_request("pr2SoftMotion", "TrackQ", ['mhpArmTraj', 'PR2SM_TRACK_POSTER', 'RARM'])
    ]

    # Close gripper
    actions += close_gripper(robot)

    return actions


@tested("")
@action
def attachObject(robot, obj, attach):
    """ attach or detach the object to the robot hand
    
    This function should be called after a release or a grab
    :param obj: the object to attach/dettach.
    :param attach: true to attach an object
    """
    i = 0
    if (attach) :
      i = 1
    actions = [
        genom_request("spark","SetGraspedObject", [obj, i, 0]),
        genom_request("spark","SetInferrenceForObject", [obj, i, robot.id, 0,
            "SPARK_PRECISE_ROBOT_HAND", 1.0])
    ]
    return actions


@tested("22/02/2012")
@action
def basicgive(robot):
    """ The ultra stupid basic GIVE: simply hand the object in front of the
    robot.
    
    After handing the object, the robot waits for someone to take it, and
    stay in this posture. 
    """

    posture = postures.read()
    
    actions = configuration.setpose(robot, posture["GIVE"])
    actions += release_gripper(robot)
    actions += [wait(2)]
    actions += close_gripper(nop)
        
    return actions

@tested("22/02/2012")
@action
def basicgrab(robot):
    """ The ultra stupid basic GRAB: simply take the object in front of the
    robot.
{'fraction_non_alphanumeric': 0.11786038077969176, 'fraction_numerical': 0.026209511250309073, 'mean_word_length': 2.6070154577883473, 'pattern_counts': {'":': 8, '<': 3, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5506585', 'n_tokens_mistral': 4185, 'n_tokens_neox': 3849, 'n_words': 1097}