def __init__(self):
        rospy.logwarn("Initializing ")
        self.rate_publisher = rospy.Publisher('robot/joint_state_publish_rate',
                                         UInt16, queue_size=10)
        self._left_arm = baxter_interface.limb.Limb("left")
        self._left_joint_names = self._left_arm.joint_names()
        self._left_kin = baxter_kinematics('left')
        rospy.sleep(2)
        rospy.loginfo("Stuff")     

        # control parameters
        self.pub_rate = 500.0  # Hz

        rospy.loginfo("Getting robot state... ")
        self.interface = baxter_interface.RobotEnable(CHECK_VERSION)
        self._init_state = self.interface.state().enabled
        rospy.loginfo("Enabling robot... ")
        self.interface.enable()
        
        self.lower_limits, self.upper_limits = hf.get_limits()

        # set joint state publishing to 500Hz
        self.rate_publisher.publish(self.pub_rate)

        rospy.wait_for_service('check_collision')

        self.input_received = False
Пример #2
0
    def __init__(self):
        rospy.logwarn("Initializing ")
        self.rate_publisher = rospy.Publisher('robot/joint_state_publish_rate',
                                              UInt16,
                                              queue_size=10)
        self._left_arm = baxter_interface.limb.Limb("left")
        self._left_joint_names = self._left_arm.joint_names()
        self._left_kin = baxter_kinematics('left')
        rospy.sleep(2)
        rospy.loginfo("Stuff")

        # control parameters
        self.pub_rate = 500.0  # Hz

        rospy.loginfo("Getting robot state... ")
        self.interface = baxter_interface.RobotEnable(CHECK_VERSION)
        self._init_state = self.interface.state().enabled
        rospy.loginfo("Enabling robot... ")
        self.interface.enable()

        self.lower_limits, self.upper_limits = hf.get_limits()

        # set joint state publishing to 500Hz
        self.rate_publisher.publish(self.pub_rate)

        rospy.wait_for_service('check_collision')

        self.input_received = False
Пример #3
0
def sample_c_space(num_samples):
    '''
    Returns num_samples of random samples uniformly distributed in c-space
    '''
    lower, upper = hf.get_limits()
    q = np.zeros((len(hf.frame_dict), num_samples))
    for i, joint in enumerate(hf.frame_dict.keys()):
        q[i,:] = (np.random.uniform(lower[joint], upper[joint], num_samples))
    return q.T # n x 7
Пример #4
0
def sample_c_space(num_samples):
    """
    Returns num_samples of random samples uniformly distributed in c-space
    """
    lower, upper = hf.get_limits()
    q = np.zeros((len(hf.frame_dict), num_samples))
    for i, joint in enumerate(hf.frame_dict.keys()):
        q[i, :] = np.random.uniform(lower[joint], upper[joint], num_samples)
    return q.T  # n x 7
Пример #5
0
def sample_c_space_2d(num_samples):
    '''
    Deprecated - used for testing
    '''
    lower, upper = hf.get_limits()
    q = np.zeros((2, num_samples))
    for i, joint in enumerate(hf.frame_dict.keys()):
        q[i,:] = (np.random.uniform(lower[joint], upper[joint], num_samples))
        if i == 1:
            break
    return q.T # n x 2
Пример #6
0
def sample_c_space_2d(num_samples):
    """
    Deprecated - used for testing
    """
    lower, upper = hf.get_limits()
    q = np.zeros((2, num_samples))
    for i, joint in enumerate(hf.frame_dict.keys()):
        q[i, :] = np.random.uniform(lower[joint], upper[joint], num_samples)
        if i == 1:
            break
    return q.T  # n x 2
Пример #7
0
    def __init__(self, baxter, arm):

        self.arm = arm # arm string
        self._arm_obj = baxter.arm_obj # arm object
        self._gripper_obj = baxter.gripper

        self._joint_names = self._arm_obj.joint_names()
        self._kin = baxter_kinematics(arm)

        self.lower_limits, self.upper_limits = hf.get_limits(self.arm)
        print self.lower_limits
        print self.upper_limits
        print {joint:(self.lower_limits[joint] + self.upper_limits[joint])/2 for joint in self.lower_limits}
        print  self._arm_obj.joint_angles()

         # set joint state publishing to 500Hz
        self.pub_rate = 500.0  # Hz
        self.rate_publisher = rospy.Publisher('/robot/joint_state_publish_rate',
                                         UInt16, queue_size=10)
        self.rate_publisher.publish(self.pub_rate)