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
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
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
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
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
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
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)
