_rate = 500.0
#rospy.sleep(rospy.Duration(5,0))
rs = baxter_interface.RobotEnable(CHECK_VERSION)
rs.enable()
right_gripper = Gripper('right');
right_limb = Limb('right');
right_gripper.calibrate()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
print "opening gripper"
right_gripper.open()
current_angles = [right_limb.joint_angle(joint) for joint in right_limb.joint_names()]
orient_quaternion_components = quaternion_from_euler(math.pi, 0,math.pi/2)
orient_down = Quaternion()
orient_down.x = orient_quaternion_components[0]
orient_down.y = orient_quaternion_components[1]
orient_down.z = orient_quaternion_components[2]
orient_down.w = orient_quaternion_components[3]
twist_quaternion_components = quaternion_from_euler(math.pi, 0, -math.pi/4)
twist_down = Quaternion()
twist_down.x = twist_quaternion_components[0]
twist_down.y = twist_quaternion_components[1]
twist_down.z = twist_quaternion_components[2]
twist_down.w = twist_quaternion_components[3]
#rospy.sleep(rospy.Duration(5,0))
rs = baxter_interface.RobotEnable(CHECK_VERSION)
rs.enable()
right_gripper = Gripper('right')
right_limb = Limb('right')
right_gripper.calibrate()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
print "opening gripper"
right_gripper.open()
current_angles = [
right_limb.joint_angle(joint) for joint in right_limb.joint_names()
]
orient_quaternion_components = quaternion_from_euler(
math.pi, 0, math.pi / 2)
orient_down = Quaternion()
orient_down.x = orient_quaternion_components[0]
orient_down.y = orient_quaternion_components[1]
orient_down.z = orient_quaternion_components[2]
orient_down.w = orient_quaternion_components[3]
highPose = PoseStamped(header=hdr,
pose=Pose(position=Point(0, -0.428, -0.57),
orientation=orient_down))
gripPose = PoseStamped(header=hdr,
pose=Pose(position=Point(0, -0.428, -0.71),
class Baxter(object):
def __init__(self, calibrate_grippers=True):
self._baxter_state = RobotEnable()
self._left = Limb(LEFT)
self._right = Limb(RIGHT)
self._limbs = {LEFT: self._left, RIGHT: self._right}
self._head = Head()
self._left_gripper, self._right_gripper = Gripper(LEFT), Gripper(RIGHT)
if calibrate_grippers:
self.calibrate()
self._left_ikservice = IKService(LEFT)
self._right_ikservice = IKService(RIGHT)
def calibrate(self):
self._left_gripper.calibrate()
self._left_gripper_max = self._left_gripper.position()
self._right_gripper.calibrate()
self._right_gripper_max = self._right_gripper.position()
@property
def left_gripper_max(self):
return self._left_gripper_max
@property
def right_gripper_max(self):
return self._right_gripper_max
@property
def left_gripper(self):
return self._left_gripper.position()
@left_gripper.setter
def left_gripper(self, position):
self._left_gripper.command_position(position)
@property
def right_gripper(self):
return self._right_gripper.position()
@right_gripper.setter
def right_gripper(self, position):
self._right_gripper.command_position(position)
def set_left_joints(self, angles):
joints = self._left.joint_angles()
for joint, angle in angles.iteritems():
if angle:
joints[joint] = angle
self.enable_check()
self._left.set_joint_positions(joints)
def set_right_joints(self, angles):
joints = self._right.joint_angles()
for joint, angle in angles.iteritems():
if angle:
joints[joint] = angle
self.enable_check()
self._right.set_joint_positions(joints)
def reset_limb(self, side):
angles = {joint: 0.0 for joint in self._limbs[side].joint_angles()}
self.enable_check()
self._limbs[side].move_to_joint_positions(angles)
def enable_check(self):
# Sometimes robot is disabled due to another program resetting state
if not self._baxter_state.state().enabled:
self._baxter_state.enable()
@property
def joints(self):
joints = {
limb: joint.joint_angles()
for limb, joint in self._limbs.iteritems()
}
return joints
@property
def enabled(self):
return self._baxter_state.state().enabled
@property
def left_s0(self):
return self._left.joint_angle('left_s0')
@left_s0.setter
def left_s0(self, angle):
self.set_left_joints({'left_s0': angle})
@property
def left_s1(self):
return self._left.joint_angle('left_s1')
@left_s1.setter
def left_s1(self, angle):
self.set_left_joints({'left_s1': angle})
@property
def left_e0(self):
return self._left.joint_angle('left_e0')
@left_e0.setter
def left_e0(self, angle):
self.set_left_joints({'left_e0': angle})
@property
def left_e1(self):
return self._left.joint_angle('left_e1')
@left_e1.setter
def left_e1(self, angle):
self.set_left_joints({'left_e1': angle})
@property
def left_w0(self):
return self._left.joint_angle('left_w0')
@left_w0.setter
def left_w0(self, angle):
self.set_left_joints({'left_w0': angle})
@property
def left_w1(self):
return self._left.joint_angle('left_w1')
@left_w1.setter
def left_w1(self, angle):
self.set_left_joints({'left_w1': angle})
@property
def left_w2(self):
return self._left.joint_angle('left_w2')
@left_w2.setter
def left_w2(self, angle):
self.set_left_joints({'left_w2': angle})
@property
def right_s0(self):
return self._right.joint_angle('right_s0')
@right_s0.setter
def right_s0(self, angle):
self.set_right_joints({'right_s0': angle})
@property
def right_s1(self):
return self._right.joint_angle('right_s1')
@right_s1.setter
def right_s1(self, angle):
self.set_right_joints({'right_s1': angle})
@property
def right_e0(self):
return self._right.joint_angle('right_e0')
@right_e0.setter
def right_e0(self, angle):
self.set_right_joints({'right_e0': angle})
@property
def right_e1(self):
return self._right.joint_angle('right_e1')
@right_e1.setter
def right_e1(self, angle):
self.set_right_joints({'right_e1': angle})
@property
def right_w0(self):
return self._right.joint_angle('right_w0')
@right_w0.setter
def right_w0(self, angle):
self.set_right_joints({'right_w0': angle})
@property
def right_w1(self):
return self._right.joint_angle('right_w1')
@right_w1.setter
def right_w1(self, angle):
self.set_right_joints({'right_w1': angle})
@property
def right_w2(self):
return self._right.joint_angle('right_w2')
@right_w2.setter
def right_w2(self, angle):
self.set_right_joints({'right_w2': angle})
@property
def left_position(self):
return self._left.endpoint_pose()['position']
@property
def left_position_x(self):
return self.left_position.x
@left_position_x.setter
def left_position_x(self, point):
self.set_left_pose(position={'x': point})
@property
def left_position_y(self):
return self.left_position.y
@left_position_y.setter
def left_position_y(self, point):
self.set_left_pose(position={'y': point})
@property
def left_position_z(self):
return self.left_position.z
@left_position_z.setter
def left_position_z(self, point):
self.set_left_pose(position={'z': point})
@property
def left_orientation(self):
return self._left.endpoint_pose()['orientation']
@property
def left_orientation_x(self):
return self.left_orientation.x
@left_orientation_x.setter
def left_orientation_x(self, point):
self.set_left_pose(orientation={'x': point})
@property
def left_orientation_y(self):
return self.left_orientation.y
@left_orientation_y.setter
def left_orientation_y(self, point):
self.set_left_pose(orientation={'y': point})
@property
def left_orientation_z(self):
return self.left_orientation.z
@left_orientation_z.setter
def left_orientation_z(self, point):
self.set_left_pose(orientation={'z': point})
@property
def left_orientation_w(self):
return self.left_orientation.w
@left_orientation_w.setter
def left_orientation_w(self, point):
self.set_left_pose(orientation={'w': point})
@property
def right_position(self):
return self._right.endpoint_pose()['position']
@property
def right_orientation(self):
return self._right.endpoint_pose()['orientation']
def set_left_pose(self, position={}, orientation={}):
pos = {
'x': self.left_position_x,
'y': self.left_position_y,
'z': self.left_position_z,
}
for key, value in position.iteritems():
pos[key] = value
orient = {
'x': self.left_orientation_x,
'y': self.left_orientation_y,
'z': self.left_orientation_z,
'w': self.left_orientation_w,
}
for key, value in orientation.iteritems():
orient[key] = value
pos = self._left_ikservice.solve_position(
Pose(position=Point(**pos), orientation=Quaternion(**orient)))
if pos:
self.set_left_joints(pos)
else:
print 'nothing'
#print self.joints
@property
def right_position(self):
return self._right.endpoint_pose()['position']
@property
def right_position_x(self):
return self.right_position.x
@right_position_x.setter
def right_position_x(self, point):
self.set_right_pose(position={'x': point})
@property
def right_position_y(self):
return self.right_position.y
@right_position_y.setter
def right_position_y(self, point):
self.set_right_pose(position={'y': point})
@property
def right_position_z(self):
return self.right_position.z
@right_position_z.setter
def right_position_z(self, point):
self.set_right_pose(position={'z': point})
@property
def right_orientation(self):
return self._right.endpoint_pose()['orientation']
@property
def right_orientation_x(self):
return self.right_orientation.x
@right_orientation_x.setter
def right_orientation_x(self, point):
self.set_right_pose(orientation={'x': point})
@property
def right_orientation_y(self):
return self.right_orientation.y
@right_orientation_y.setter
def right_orientation_y(self, point):
self.set_right_pose(orientation={'y': point})
@property
def right_orientation_z(self):
return self.right_orientation.z
@right_orientation_z.setter
def right_orientation_z(self, point):
self.set_right_pose(orientation={'z': point})
@property
def right_orientation_w(self):
return self.right_orientation.w
@right_orientation_w.setter
def right_orientation_w(self, point):
self.set_right_pose(orientation={'w': point})
@property
def right_position(self):
return self._right.endpoint_pose()['position']
@property
def right_orientation(self):
return self._right.endpoint_pose()['orientation']
def set_right_pose(self, position={}, orientation={}):
pos = {
'x': self.right_position_x,
'y': self.right_position_y,
'z': self.right_position_z,
}
for key, value in position.iteritems():
pos[key] = value
orient = {
'x': self.right_orientation_x,
'y': self.right_orientation_y,
'z': self.right_orientation_z,
'w': self.right_orientation_w,
}
for key, value in orientation.iteritems():
orient[key] = value
pos = self._right_ikservice.solve_position(
Pose(position=Point(**pos), orientation=Quaternion(**orient)))
if pos:
self.set_right_joints(pos)
@property
def head_position(self):
return self._head.pan()
@head_position.setter
def head_position(self, position):
self._head.set_pan(position)
