<commit_before>import numpy as np
from numpy import inf, zeros, dot, r_, pi
from numpy.linalg import norm, inv
from threading import Thread

from pr2 import retiming, math_utils as mu, conversions as conv, func_utils, resampling

import roslib

roslib.load_manifest("pr2_controllers_msgs")
roslib.load_manifest("move_base_msgs")
import pr2_controllers_msgs.msg as pcm
import trajectory_msgs.msg as tm
import sensor_msgs.msg as sm
import actionlib
import rospy
import geometry_msgs.msg as gm
import move_base_msgs.msg as mbm

VEL_RATIO = .2
ACC_RATIO = .3


class IKFail(Exception):
    pass


class TopicListener:
    "stores last message"
    last_msg = None

    def __init__(self, topic_name, msg_type):
        self.sub = rospy.Subscriber(topic_name, msg_type, self.callback)

        rospy.loginfo('waiting for the first message: %s' % topic_name)
        while self.last_msg is None: rospy.sleep(.01)
        rospy.loginfo('ok: %s' % topic_name)

    def callback(self, msg):
        self.last_msg = msg


class JustWaitThread(Thread):
    def __init__(self, duration):
        Thread.__init__(self)
        self.wants_exit = False
        self.duration = duration

    def run(self):
        t_done = rospy.Time.now() + rospy.Duration(self.duration)
        while not self.wants_exit and rospy.Time.now() < t_done and not rospy.is_shutdown():
            rospy.sleep(.01)


class GripperTrajectoryThread(Thread):
    def __init__(self, gripper, times, angles):
        Thread.__init__(self)
        self.wants_exit = False
        self.gripper = gripper
        self.times = times
        self.angles = angles

    def run(self):
        t_start = rospy.Time.now()
        self.gripper.set_angle_target(self.angles[0])
        for i in range(1, len(self.times)):
            if rospy.is_shutdown() or self.wants_exit: break
            duration_elapsed = rospy.Time.now() - t_start
            self.gripper.set_angle_target(self.angles[i])
            rospy.sleep(rospy.Duration(self.times[i]) - duration_elapsed)


class PR2:
    pending_threads = []

    @func_utils.once
    def create(rave_only=False):
        return PR2(rave_only)

    def __init__(self):
        self.joint_listener = TopicListener("/joint_states", sm.JointState)

        self.larm = Arm(self, "l")
        self.rarm = Arm(self, "r")
        self.lgrip = Gripper(self, "l")
        self.rgrip = Gripper(self, "r")
        self.head = Head(self)
        self.torso = Torso(self)
        self.base = Base(self)

        rospy.on_shutdown(self.stop_all)

    def start_thread(self, thread):
        self.pending_threads.append(thread)
        thread.start()

    def get_last_joint_message(self):
        return self.joint_listener.last_msg

    def join_all(self):
        for thread in self.pending_threads:
            thread.join()
        self.pending_threads = []

    def is_moving(self):
        return any(thread.is_alive() for thread in self.pending_threads)

    def stop_all(self):
        self.larm.stop()
        self.rarm.stop()
        self.head.stop()
        self.torso.stop()
        for thread in self.pending_threads:<commit_msg>Add back openrave stuff to pr2 script.<commit_after>import numpy as np
import openravepy as rave
from numpy import inf, zeros, dot, r_, pi
from numpy.linalg import norm, inv
from threading import Thread

from pr2 import retiming, math_utils as mu, conversions as conv, func_utils, resampling

import roslib

roslib.load_manifest("pr2_controllers_msgs")
roslib.load_manifest("move_base_msgs")
import pr2_controllers_msgs.msg as pcm
import trajectory_msgs.msg as tm
import sensor_msgs.msg as sm
import actionlib
import rospy
import geometry_msgs.msg as gm
import move_base_msgs.msg as mbm

VEL_RATIO = .2
ACC_RATIO = .3


class IKFail(Exception):
    pass


class TopicListener(object):
    "stores last message"
    last_msg = None

    def __init__(self, topic_name, msg_type):
        self.sub = rospy.Subscriber(topic_name, msg_type, self.callback)

        rospy.loginfo('waiting for the first message: %s' % topic_name)
        while self.last_msg is None: rospy.sleep(.01)
        rospy.loginfo('ok: %s' % topic_name)

    def callback(self, msg):
        self.last_msg = msg


class JustWaitThread(Thread):
    def __init__(self, duration):
        Thread.__init__(self)
        self.wants_exit = False
        self.duration = duration

    def run(self):
        t_done = rospy.Time.now() + rospy.Duration(self.duration)
        while not self.wants_exit and rospy.Time.now() < t_done and not rospy.is_shutdown():
            rospy.sleep(.01)


class GripperTrajectoryThread(Thread):
    def __init__(self, gripper, times, angles):
        Thread.__init__(self)
        self.wants_exit = False
        self.gripper = gripper
        self.times = times
        self.angles = angles

    def run(self):
        t_start = rospy.Time.now()
        self.gripper.set_angle_target(self.angles[0])
        for i in xrange(1, len(self.times)):
            if rospy.is_shutdown() or self.wants_exit: break
            duration_elapsed = rospy.Time.now() - t_start
            self.gripper.set_angle_target(self.angles[i])
            rospy.sleep(rospy.Duration(self.times[i]) - duration_elapsed)


class PR2(object):
    pending_threads = []
    wait = True  # deprecated way of blocking / not blocking

    @func_utils.once
    def create(rave_only=False):
        return PR2(rave_only)

    def __init__(self):

        # set up openrave
        self.env = rave.Environment()
        self.env.StopSimulation()
        self.env.Load("robots/pr2-beta-static.zae")  # todo: use up-to-date urdf
        self.robot = self.env.GetRobots()[0]

        self.joint_listener = TopicListener("/joint_states", sm.JointState)

        # rave to ros conversions
        joint_msg = self.get_last_joint_message()
        ros_names = joint_msg.name
        inds_ros2rave = np.array([self.robot.GetJointIndex(name) for name in ros_names])
        self.good_ros_inds = np.flatnonzero(inds_ros2rave != -1)  # ros joints inds with matching rave joint
        self.rave_inds = inds_ros2rave[self.good_ros_inds]  # openrave indices corresponding to those joints
        self.update_rave()

        self.larm = Arm(self, "l")
        self.rarm = Arm(self, "r")
        self.lgrip = Gripper(self, "l")
        self.rgrip = Gripper(self, "r")
        self.head = Head(self)
        self.torso = Torso(self)
        self.base = Base(self)

        rospy.on_shutdown(self.stop_all)

    def _set_rave_limits_to_soft_joint_limits(self):
        # make the joint limits match the PR2 soft limits
        low_limits, high_limits = self.robot.GetDOFLimits()
        rarm_low_limits = [-2.1353981634, -0.3536, -3.75, -2.1213, None, -2.0, None]
        rarm_high_limits = [0.564601836603, 1.2963, 0.65, -0.15, None, -0.1, None]
        for rarm_index, low, high in zip(self.robot.GetManipulator("rightarm").GetArmIndices(), rarm_low_limits,
                                         rarm_high_limits):
            if low is not None and high is not None:
                low_limits[rarm_index] = low
                high_limits[rarm_index] = high
        larm_low_limits = [-0.564601836603, -0.3536, -0.65, -2.1213, None, -2.0, None]
        larm_high_limits = [2.1353981634, 1.2963, 3.75, -0.15, None, -0.1, None]
        for larm_index, low, high in zip(self.robot.GetManipulator("leftarm").GetArmIndices(), larm_low_limits,
                                         larm_high_limits):
            if low is not None and high is not None:
                low_limits[larm_index] = low
                high_limits[larm_index] = high
        self.robot.SetDOFLimits(low_limits, high_limits)

    def start_thread(self, thread):
        self.pending_threads.append(thread)
        thread.start()

    def get_last_joint_message(self):
        return self.joint_listener.last_msg

    def update_rave(self):
        ros_values = self.get_last_joint_message().position
        rave_values = [ros_values[i_ros] for i_ros in self.good_ros_inds]
        self.robot.SetJointValues(rave_values[:20], self.rave_inds[:20])
        self.robot.SetJointValues(rave_values[20:], self.rave_inds[20:])

        # if use_map:
        #     (trans,rot) = self.tf_listener.lookupTransform('/map', '/base_link', rospy.Time(0))            
        #     self.robot.SetTransform(conv.trans_rot_to_hmat(trans, rot))

    def update_rave_without_ros(self, joint_vals):
        self.robot.SetJointValues(joint_vals)

    def join_all(self):
        for thread in self.pending_threads:
            thread.join()
        self.pending_threads = []

    def is_moving(self):
        return any(thread.is_alive() for thread in self.pending_threads)

    def stop_all(self):
        self.larm.stop()
        self.rarm.stop()
        self.head.stop()
        self.torso.stop()
        for thread in self.pending_threads:
{'fraction_non_alphanumeric': 0.10326576576576577, 'fraction_numerical': 0.016554054054054055, 'mean_word_length': 2.665290961617829, 'pattern_counts': {'":': 0, '<': 5, '<?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': '1003668', 'n_tokens_mistral': 3117, 'n_tokens_neox': 2977, 'n_words': 645}