<commit_before>#!/usr/bin/env python
import logging
from prpy.action import ActionMethod
from prpy.planning.base import PlanningError
logger = logging.getLogger('herbpy')

@ActionMethod
def PushToPoseOnTable(robot, obj, table, goal_position, goal_radius, 
                      manip=None, max_plan_duration=30.0, **kw_args):
    """
    @param robot The robot performing the push
    @param obj The object to push
    @param table The table kinbody the object is resting on
    @param goal_position The desired (x,y) position of the object in world coordinates
    @param goal_radius The max distance from goal_position for the object to be to 
      still consider the goal achieved
    @param manip The manipulator to use for the push - if None the active manipulator is used
    @param max_plan_duration The max time to run the planner
    """
    # Get a push planner
    from or_pushing.push_planner import PushPlanner
    planner = PushPlanner(robot.GetEnv())

    # Get the manipulator
    if manip is None:
        with robot.GetEnv():
            manip = robot.GetActiveManipulator()

    # Make the state bounds be at the edges of the table
    with robot.GetEnv():
        table_aabb = table.ComputeAABB()
    table_pos = table_aabb.pos()
    table_extents = table_aabb.extents()
    sbounds = {'high': [table_pos[0] + table_extents[0],
                        table_pos[1] + table_extents[1],
                        2.*numpy.pi],
               'low': [table_pos[0] - table_extents[0],
                       table_pos[1] - table_extents[1],
                       0]}
    
    # This should be the pose and the height of the manipulator
    # for pushing - parallel to table, 0.1m above surface  
    ee_pushing_transform = numpy.array([[ 0.,  0., 1., 0], 
                                        [-1.,  0., 0., 0], 
                                        [ 0., -1., 0., table_pos[2] + table_extents[2] + 0.1], 
                                        [ 0.,  0., 0., 1]])

    # Compute the goal pose
    with robot.GetEnv():
        goal_pose = obj.GetTransform()
    table_height = table_pos[2] + table_extents[2] + 0.01<commit_msg>Fixing the pushing action to use some newer tools<commit_after>#!/usr/bin/env python
import logging, numpy
from prpy.action import ActionMethod
from prpy.planning.base import PlanningError
logger = logging.getLogger('herbpy')

@ActionMethod
def PushToPoseOnTable(robot, obj, table, goal_position, goal_radius, 
                      manip=None, max_plan_duration=30.0, **kw_args):
    """
    @param robot The robot performing the push
    @param obj The object to push
    @param table The table kinbody the object is resting on
    @param goal_position The desired (x,y) position of the object in world coordinates
    @param goal_radius The max distance from goal_position for the object to be to 
      still consider the goal achieved
    @param manip The manipulator to use for the push - if None the active manipulator is used
    @param max_plan_duration The max time to run the planner
    """
    # Get a push planner
    from or_pushing.push_planner import PushPlanner
    planner = PushPlanner(robot.GetEnv())

    # Get the manipulator
    if manip is None:
        with robot.GetEnv():
            manip = robot.GetActiveManipulator()

    # Make the state bounds be at the edges of the table
    with robot.GetEnv():
        from prpy.rave import Disabled
        from prpy.util import ComputeEnabledAABB
        with Disabled(table, padding_only=True):
            table_aabb = ComputeEnabledAABB(table)
    table_pos = table_aabb.pos()
    table_extents = table_aabb.extents()

    sbounds = {'high': [table_pos[0] + table_extents[0],
                        table_pos[1] + table_extents[1],
                        2.*numpy.pi],
               'low': [table_pos[0] - table_extents[0],
                       table_pos[1] - table_extents[1],
                       0]}
    
    # Get the current pose of the manipulator and assume we want to keep that pose throughout
    #  the push
    with robot.GetEnv():
        ee_pushing_transform = manip.GetEndEffectorTransform()
    ee_pushing_transform[:2,3] = [0., 0.] #ignore x,y pose

    # Compute the goal pose
    with robot.GetEnv():
        goal_pose = obj.GetTransform()
    table_height = table_pos[2] + table_extents[2] + 0.01
{'fraction_non_alphanumeric': 0.08509659613615456, 'fraction_numerical': 0.014029438822447102, 'mean_word_length': 2.3844357976653696, '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': 5, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3717625', 'n_tokens_mistral': 1301, 'n_tokens_neox': 1244, 'n_words': 420}