<commit_before>import herbpy
import openravepy
import numpy
import planner
import time
import util

HerbMethod = util.CreateMethodListDecorator()

@HerbMethod
def LookAt(robot, target, execute=True):
    """
    Look at a point in the world frame. This creates and, optionally executes,
    a two-waypoint trajectory that starts at the current configuration and
    moves to a goal configuration found through the neck's inverse kinematics.
    @param target point in the world frame.
    @param execute immediately execute the trajectory
    @return trajectory head trajectory
    """
    # Find an IK solution to look at the point.
    ik_params = openravepy.IkParameterization(target, openravepy.IkParameterization.Type.Lookat3D)
    target_dof_values = robot.head.ik_database.manip.FindIKSolution(ik_params, 0)
    if target_dof_values == None:
        return None

    # Create a two waypoint trajectory for the head.
    current_dof_values = robot.GetDOFValues(robot.head.GetArmIndices())
    config_spec = robot.head.GetArmConfigurationSpecification()
    traj = openravepy.RaveCreateTrajectory(robot.GetEnv(), '')
    traj.Init(config_spec)
    traj.Insert(0, current_dof_values, config_spec)
    traj.Insert(1, target_dof_values, config_spec)

    # Retiming the trajectory is necessary for it play on the IdealController.
    openravepy.planningutils.RetimeTrajectory(traj)

    # Optionally exeucute the trajectory.
    if execute:
        return robot.ExecuteTrajectory(traj, retime=True)
    else:
        return traj

@HerbMethod
def PlanGeneric(robot, command_name, args, execute=True, **kw_args):
    traj = None

    # Sequentially try each planner until one succeeds.
    with robot.GetEnv():
        with robot.CreateRobotStateSaver():
            for delegate_planner in robot.planners:
                try:
                    traj = getattr(delegate_planner, command_name)(*args, **kw_args)
                    break
                except planner.UnsupportedPlanningError, e:
                    herbpy.logger.debug('Unable to plan with {0:s}: {1:s}'.format(delegate_planner.GetName(), e))
                except planner.PlanningError, e:
                    herbpy.logger.warning('Planning with {0:s} failed: {1:s}'.format(delegate_planner.GetName(), e))
                    # TODO: Log the scene and planner parameters to a file.<commit_msg>Added FindHeadDofs <commit_after>import herbpy
import openravepy
import numpy
import planner
import time
import util

HerbMethod = util.CreateMethodListDecorator()

@HerbMethod
def LookAt(robot, target, execute=True):
    """
    Look at a point in the world frame. This creates and, optionally executes,
    a two-waypoint trajectory that starts at the current configuration and
    moves to a goal configuration found through the neck's inverse kinematics.
    @param target point in the world frame.
    @param execute immediately execute the trajectory
    @return trajectory head trajectory
    """
    # Find an IK solution to look at the point.
    ik_params = openravepy.IkParameterization(target, openravepy.IkParameterization.Type.Lookat3D)
    target_dof_values = robot.head.ik_database.manip.FindIKSolution(ik_params, 0)
    if target_dof_values == None:
        return None

    # Create a two waypoint trajectory for the head.
    current_dof_values = robot.GetDOFValues(robot.head.GetArmIndices())
    config_spec = robot.head.GetArmConfigurationSpecification()
    traj = openravepy.RaveCreateTrajectory(robot.GetEnv(), '')
    traj.Init(config_spec)
    traj.Insert(0, current_dof_values, config_spec)
    traj.Insert(1, target_dof_values, config_spec)

    # Retiming the trajectory is necessary for it play on the IdealController.
    openravepy.planningutils.RetimeTrajectory(traj)

    # Optionally exeucute the trajectory.
    if execute:
        return robot.ExecuteTrajectory(traj, retime=True)
    else:
        return traj

@HerbMethod
def FindHeadDofs(robot, target):
    # Find an IK solution to look at the point.
    ik_params = openravepy.IkParameterization(target, openravepy.IkParameterization.Type.Lookat3D)
    target_dof_values = robot.head.ik_database.manip.FindIKSolution(ik_params, 0)
    if target_dof_values == None:
        return None
    return target_dof_values


@HerbMethod
def PlanGeneric(robot, command_name, args, execute=True, **kw_args):
    traj = None

    # Sequentially try each planner until one succeeds.
    with robot.GetEnv():
        with robot.CreateRobotStateSaver():
            for delegate_planner in robot.planners:
                try:
                    traj = getattr(delegate_planner, command_name)(*args, **kw_args)
                    break
                except planner.UnsupportedPlanningError, e:
                    herbpy.logger.debug('Unable to plan with {0:s}: {1:s}'.format(delegate_planner.GetName(), e))
                except planner.PlanningError, e:
                    herbpy.logger.warning('Planning with {0:s} failed: {1:s}'.format(delegate_planner.GetName(), e))
                    # TODO: Log the scene and planner parameters to a file.
{'fraction_non_alphanumeric': 0.0788699234844026, 'fraction_numerical': 0.003531489111241907, 'mean_word_length': 3.2554257095158596, 'pattern_counts': {'":': 0, '<': 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': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4182601', 'n_tokens_mistral': 1551, 'n_tokens_neox': 1494, 'n_words': 415}