class Baxter_Interactive(object):
def __init__(self, arm):
#Vector to record poses
self.recorded = []
self.arm = arm
#enable Baxter
self._en = RobotEnable()
self._en.enable()
#use DigitalIO to connect to gripper buttons
self._close_button = DigitalIO(self.arm + '_upper_button')
self._open_button = DigitalIO(self.arm + '_lower_button')
#set up gripper
self._gripper = Gripper(self.arm)
self._gripper.calibrate()
#set up navigator
self._navigator = Navigator(self.arm)
#set up limb
self._limb = Limb(self.arm)
self._limb.set_joint_position_speed(0.5)
self._limb.move_to_neutral()
print 'Ready to record...'
def recorder(self):
doneRecording = False
while not doneRecording:
if self._navigator.button0:
self.recorded.append(self._limb.joint_angles())
print 'Waypoint Added'
rospy.sleep(1)
if self._close_button.state:
self.recorded.append('CLOSE')
self._gripper.close()
print 'Gripper Closed'
rospy.sleep(1)
if self._open_button.state:
self.recorded.append('OPEN')
self._gripper.open()
print 'Gripper Opened'
rospy.sleep(1)
if self._navigator.button1:
print 'END RECORDING'
doneRecording = True
rospy.sleep(3)
while doneRecording:
for waypoint in self.recorded:
if waypoint == 'OPEN':
self._gripper.open()
rospy.sleep(1)
elif waypoint == 'CLOSE':
self._gripper.close()
rospy.sleep(1)
else:
self._limb.move_to_joint_positions(waypoint)
class Abbe_IK(object):
def __init__(self):
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.3)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.3)
def neutral(self):
self._left_arm.move_to_neutral()
self._right_arm.move_to_neutral()
def ik_calculate(self, limb, pos, rot):
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
#rotation -0.5 is perp and -1.0 is parallel for rot[2]
quat = transformations.quaternion_from_euler(rot[0], rot[1], rot[2])
pose = PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=pos[0], #depth
y=pos[1], #lateral
z=pos[2], #height
),
orientation=Quaternion(
quat[0],
quat[1],
quat[2],
quat[3],
),
),
)
ikreq.pose_stamp.append(pose)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return 1
# Check if result valid, and type of seed ultimately used to get solution
# convert rospy's string representation of uint8[]'s to int's
resp_seeds = struct.unpack('<%dB' % len(resp.result_type),
resp.result_type)
if (resp_seeds[0] != resp.RESULT_INVALID):
seed_str = {
ikreq.SEED_USER: '******',
ikreq.SEED_CURRENT: 'Current Joint Angles',
ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
}.get(resp_seeds[0], 'None')
#print("SUCCESS - Valid Joint Solution Found from Seed Type: %s" %
# (seed_str,))
# Format solution into Limb API-compatible dictionary
limb_joints = dict(
zip(resp.joints[0].name, resp.joints[0].position))
return limb_joints
else:
pass
#print pose
#print("INVALID POSE - No Valid Joint Solution Found.")
return 0
class Mimic(object):
IKSVC_LEFT_URI = 'ExternalTools/left/PositionKinematicsNode/IKService'
IKSVC_RIGHT_URI = 'ExternalTools/right/PositionKinematicsNode/IKService'
def __init__(self):
self._tf = tf.TransformListener()
self._rs = RobotEnable()
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.5)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.5)
self._left_arm_neutral = None
self._right_arm_neutral = None
self._left_iksvc = rospy.ServiceProxy(Mimic.IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(Mimic.IKSVC_RIGHT_URI,
SolvePositionIK)
self._left_camera = CameraController('left_hand_camera')
self._right_camera = CameraController('right_hand_camera')
self._head_camera = CameraController('head_camera')
self._left_camera.close()
self._right_camera.close()
self._head_camera.close()
self._head_camera.resolution = CameraController.MODES[0]
self._head_camera.open()
self._head_camera_sub = rospy.Subscriber(
'/cameras/head_camera/image', Image,
self._head_camera_sub_callback)
self._xdisplay_pub = rospy.Publisher('/robot/xdisplay',
Image,
latch=True)
self._out_of_range = False
self._ik_smooth = 4
self._ik_rate = 200
self._avg_window = 1000
self._r_trans_prev = []
self._l_trans_prev = []
self._r_ik_prev = []
self._l_ik_prev = []
self._joint_update_pub = rospy.Publisher(
'/robot/joint_state_publish_rate', UInt16)
self._joint_update_pub.publish(self._ik_rate)
self._mimic_timer = None
############################################################################
def start(self):
self._rs.enable()
self._reset()
self._mimic_timer = rospy.Timer(rospy.Duration(1.0 / self._ik_rate),
self._mimic_callback)
rate = rospy.Rate(self._avg_window)
while not rospy.is_shutdown():
try:
(r_trans, r_rot) = self._tf.lookupTransform(
'/skeleton/user_1/right_hand', '/skeleton/user_1/torso',
rospy.Time(0))
(l_trans, l_rot) = self._tf.lookupTransform(
'/skeleton/user_1/left_hand', '/skeleton/user_1/torso',
rospy.Time(0))
self._l_trans_prev.append(l_trans)
if (len(self._l_trans_prev) > self._avg_window):
self._l_trans_prev.pop(0)
self._r_trans_prev.append(r_trans)
if (len(self._r_trans_prev) > self._avg_window):
self._r_trans_prev.pop(0)
except:
self._out_of_range = True
rate.sleep()
continue
rate.sleep()
def _reset(self):
if self._mimic_timer:
self._mimic_timer.shutdown()
self._left_arm.move_to_neutral()
self._left_arm_neutral = self._left_arm.joint_angles()
self._right_arm.move_to_neutral()
self._right_arm_neutral = self._right_arm.joint_angles()
print(self._left_arm_neutral)
print(self._right_arm_neutral)
def shutdown(self):
self._reset()
############################################################################
def _mimic_callback(self, event):
try:
l_trans = (sum(map(lambda x: x[0], self._l_trans_prev)) /
self._avg_window,
sum(map(lambda x: x[1], self._l_trans_prev)) /
self._avg_window,
sum(map(lambda x: x[2], self._l_trans_prev)) /
self._avg_window)
r_trans = (sum(map(lambda x: x[0], self._r_trans_prev)) /
self._avg_window,
sum(map(lambda x: x[1], self._r_trans_prev)) /
self._avg_window,
sum(map(lambda x: x[2], self._r_trans_prev)) /
self._avg_window)
rh_x = clamp(0.65 + (l_trans[2] / 1.5), 0.5, 0.9)
rh_y = clamp(-l_trans[0], -0.8, 0)
rh_z = clamp(-l_trans[1], -0.1, 0.8)
lh_x = clamp(0.65 + (r_trans[2] / 1.5), 0.5, 0.9)
lh_y = clamp(-r_trans[0], 0, 0.8)
lh_z = clamp(-r_trans[1], -0.1, 0.8)
self.set_left_coords(lh_x, lh_y, lh_z)
self.set_right_coords(rh_x, rh_y, rh_z)
except:
return
def _head_camera_sub_callback(self, data):
orig_img = cvbr.imgmsg_to_cv2(data, 'rgb8')
img = cv2.cvtColor(orig_img, cv2.COLOR_RGB2GRAY)
img = cv2.equalizeHist(img)
img = cv2.GaussianBlur(img, (5, 5), 0)
img = cv2.Canny(img, 100, 200)
img = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB)
img = cv2.addWeighted(img, 0.75, orig_img, 0.25, 0)
img = cv2.resize(img, (1024, 768))
if self._out_of_range:
cv2.putText(img, 'Out of range!', (50, 768 - 50),
cv2.FONT_HERSHEY_SIMPLEX, 2, (255, 0, 0), 4)
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
img = cvbr.cv2_to_imgmsg(img)
self._xdisplay_pub.publish(img)
############################################################################
def set_left_coords(self,
x,
y,
z,
er=math.pi * -1,
ep=math.pi * 0.5,
ey=math.pi * -1):
self._set_arm_coords(
self._left_iksvc, self._left_arm, x, y, z, er, ep, ey,
self._get_neutral_joint_state(self._left_arm_neutral),
self._l_ik_prev)
def set_right_coords(self,
x,
y,
z,
er=math.pi * -1,
ep=math.pi * 0.5,
ey=math.pi * -1):
self._set_arm_coords(
self._right_iksvc, self._right_arm, x, y, z, er, ep, ey,
self._get_neutral_joint_state(self._right_arm_neutral),
self._r_ik_prev)
############################################################################
def _set_arm_coords(self, iksvc, limb, x, y, z, er, ep, ey, njs, prev):
resp = self._get_ik(iksvc, x, y, z, er, ep, ey, njs)
positions = resp[0]
isValid = resp[1]
self._out_of_range = not isValid
prev.append(positions)
if (len(prev) > self._ik_smooth):
prev.pop(0)
smooth_positions = {}
for joint_name in positions:
smooth_positions[joint_name] = \
sum(map(lambda x: x[joint_name], prev)) / self._ik_smooth
limb.set_joint_positions(smooth_positions)
def _get_ik(self, iksvc, x, y, z, er, ep, ey, njs):
q = quaternion_from_euler(er, ep, ey)
pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(position=Point(
x=x,
y=y,
z=z,
),
orientation=Quaternion(q[0], q[1], q[2], q[3])),
)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
ikreq.seed_angles.append(njs)
iksvc.wait_for_service(1.0)
resp = iksvc(ikreq)
positions = dict(zip(resp.joints[0].name, resp.joints[0].position))
return (positions, resp.isValid[0])
def _get_neutral_joint_state(self, njs):
js = JointState()
js.header = Header(stamp=rospy.Time.now(), frame_id='base')
for j in njs:
js.name.append(j)
js.position.append(njs[j])
return js
class IK(object):
def __init__(self):
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.6)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.6)
def neutral(self):
self._left_arm.move_to_neutral()
self._right_arm.move_to_neutral()
def ik_calculate(self,limb,pos,rot):
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
#rotation -0.5 is perp and -1.0 is parallel for rot[2]
quat = transformations.quaternion_from_euler(rot[0], rot[1], rot[2])
pose = PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=pos[0], #depth
y=pos[1], #lateral
z=pos[2], #height
),
orientation=Quaternion(
quat[0],
quat[1],
quat[2],
quat[3],
),
),
)
ikreq.pose_stamp.append(pose)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return 1
# Check if result valid, and type of seed ultimately used to get solution
# convert rospy's string representation of uint8[]'s to int's
resp_seeds = struct.unpack('<%dB' % len(resp.result_type),
resp.result_type)
if (resp_seeds[0] != resp.RESULT_INVALID):
seed_str = {
ikreq.SEED_USER: '******',
ikreq.SEED_CURRENT: 'Current Joint Angles',
ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
}.get(resp_seeds[0], 'None')
#print("SUCCESS - Valid Joint Solution Found from Seed Type: %s" %
# (seed_str,))
# Format solution into Limb API-compatible dictionary
limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
return limb_joints
else:
pass
#print pose
#print("INVALID POSE - No Valid Joint Solution Found.")
return 0
class Mimic(object):
IKSVC_LEFT_URI = 'ExternalTools/left/PositionKinematicsNode/IKService'
IKSVC_RIGHT_URI = 'ExternalTools/right/PositionKinematicsNode/IKService'
def __init__(self):
self._tf = tf.TransformListener()
self._rs = RobotEnable()
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.5)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.5)
self._left_arm_neutral = None
self._right_arm_neutral = None
self._left_iksvc = rospy.ServiceProxy(
Mimic.IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(
Mimic.IKSVC_RIGHT_URI,
SolvePositionIK)
self._left_camera = CameraController('left_hand_camera')
self._right_camera = CameraController('right_hand_camera')
self._head_camera = CameraController('head_camera')
self._left_camera.close()
self._right_camera.close()
self._head_camera.close()
self._head_camera.resolution = CameraController.MODES[0]
self._head_camera.open()
self._head_camera_sub = rospy.Subscriber('/cameras/head_camera/image', Image,
self._head_camera_sub_callback)
self._xdisplay_pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self._out_of_range = False
self._ik_smooth = 4
self._ik_rate = 200
self._avg_window = 1000
self._r_trans_prev = []
self._l_trans_prev = []
self._r_ik_prev = []
self._l_ik_prev = []
self._joint_update_pub = rospy.Publisher('/robot/joint_state_publish_rate', UInt16)
self._joint_update_pub.publish(self._ik_rate)
self._mimic_timer = None
############################################################################
def start(self):
self._rs.enable()
self._reset()
self._mimic_timer = rospy.Timer(rospy.Duration(1.0 / self._ik_rate), self._mimic_callback)
rate = rospy.Rate(self._avg_window)
while not rospy.is_shutdown():
try:
(r_trans,r_rot) = self._tf.lookupTransform(
'/skeleton/user_1/right_hand',
'/skeleton/user_1/torso',
rospy.Time(0))
(l_trans,l_rot) = self._tf.lookupTransform(
'/skeleton/user_1/left_hand',
'/skeleton/user_1/torso',
rospy.Time(0))
self._l_trans_prev.append(l_trans)
if (len(self._l_trans_prev) > self._avg_window):
self._l_trans_prev.pop(0)
self._r_trans_prev.append(r_trans)
if (len(self._r_trans_prev) > self._avg_window):
self._r_trans_prev.pop(0)
except:
self._out_of_range = True
rate.sleep()
continue
rate.sleep()
def _reset(self):
if self._mimic_timer:
self._mimic_timer.shutdown()
self._left_arm.move_to_neutral()
self._left_arm_neutral = self._left_arm.joint_angles()
self._right_arm.move_to_neutral()
self._right_arm_neutral = self._right_arm.joint_angles()
print(self._left_arm_neutral)
print(self._right_arm_neutral)
def shutdown(self):
self._reset()
############################################################################
def _mimic_callback(self, event):
try:
l_trans = (
sum(map(lambda x: x[0], self._l_trans_prev)) / self._avg_window,
sum(map(lambda x: x[1], self._l_trans_prev)) / self._avg_window,
sum(map(lambda x: x[2], self._l_trans_prev)) / self._avg_window)
r_trans = (
sum(map(lambda x: x[0], self._r_trans_prev)) / self._avg_window,
sum(map(lambda x: x[1], self._r_trans_prev)) / self._avg_window,
sum(map(lambda x: x[2], self._r_trans_prev)) / self._avg_window)
rh_x = clamp(0.65 + (l_trans[2] / 1.5), 0.5, 0.9)
rh_y = clamp(-l_trans[0], -0.8, 0)
rh_z = clamp(-l_trans[1], -0.1, 0.8)
lh_x = clamp(0.65 + (r_trans[2] / 1.5), 0.5, 0.9)
lh_y = clamp(-r_trans[0], 0, 0.8)
lh_z = clamp(-r_trans[1], -0.1, 0.8)
self.set_left_coords(lh_x, lh_y, lh_z)
self.set_right_coords(rh_x, rh_y, rh_z)
except:
return
def _head_camera_sub_callback(self, data):
orig_img = cvbr.imgmsg_to_cv2(data, 'rgb8')
img = cv2.cvtColor(orig_img, cv2.COLOR_RGB2GRAY)
img = cv2.equalizeHist(img)
img = cv2.GaussianBlur(img, (5, 5), 0)
img = cv2.Canny(img, 100, 200)
img = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB)
img = cv2.addWeighted(img, 0.75, orig_img, 0.25, 0)
img = cv2.resize(img, (1024, 768))
if self._out_of_range:
cv2.putText(img, 'Out of range!', (50, 768-50), cv2.FONT_HERSHEY_SIMPLEX, 2, (255, 0, 0), 4)
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
img = cvbr.cv2_to_imgmsg(img)
self._xdisplay_pub.publish(img)
############################################################################
def set_left_coords(self,
x, y, z,
er=math.pi * -1, ep=math.pi * 0.5, ey=math.pi * -1):
self._set_arm_coords(self._left_iksvc, self._left_arm,
x, y, z,
er, ep, ey,
self._get_neutral_joint_state(self._left_arm_neutral),
self._l_ik_prev)
def set_right_coords(self,
x, y, z,
er=math.pi * -1, ep=math.pi * 0.5, ey=math.pi * -1):
self._set_arm_coords(self._right_iksvc, self._right_arm,
x, y, z,
er, ep, ey,
self._get_neutral_joint_state(self._right_arm_neutral),
self._r_ik_prev)
############################################################################
def _set_arm_coords(self, iksvc, limb, x, y, z, er, ep, ey, njs, prev):
resp = self._get_ik(iksvc, x, y, z, er, ep, ey, njs)
positions = resp[0]
isValid = resp[1]
self._out_of_range = not isValid
prev.append(positions)
if (len(prev) > self._ik_smooth):
prev.pop(0)
smooth_positions = {}
for joint_name in positions:
smooth_positions[joint_name] = \
sum(map(lambda x: x[joint_name], prev)) / self._ik_smooth
limb.set_joint_positions(smooth_positions)
def _get_ik(self, iksvc, x, y, z, er, ep, ey, njs):
q = quaternion_from_euler(er, ep, ey)
pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(
position=Point(
x=x,
y=y,
z=z,
),
orientation=Quaternion(q[0], q[1], q[2], q[3])
),
)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
ikreq.seed_angles.append(njs)
iksvc.wait_for_service(1.0)
resp = iksvc(ikreq)
positions = dict(zip(resp.joints[0].name, resp.joints[0].position))
return (positions, resp.isValid[0])
def _get_neutral_joint_state(self, njs):
js = JointState()
js.header = Header(stamp=rospy.Time.now(), frame_id='base')
for j in njs:
js.name.append(j)
js.position.append(njs[j])
return js
class IKHelper(object):
"""An abstraction layer for using Baxter's built in IK service."""
############################################################################
def __init__(self):
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.3)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.3)
self._left_iksvc = rospy.ServiceProxy(_IKSVC_LEFT_URI, SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(_IKSVC_RIGHT_URI,
SolvePositionIK)
self._joint_update_pub = rospy.Publisher(
'/robot/joint_state_publish_rate', UInt16)
self._joint_update_pub.publish(250)
############################################################################
def reset(self):
"""Reset both arms to their neutral positions."""
self._left_arm.move_to_neutral()
self._right_arm.move_to_neutral()
def set_left(self, pos, rot=(0, math.pi, math.pi * 0.5), wait=False):
"""Set the endpoint of the left arm to the supplied coordinates.
Arguments:
pos -- Position in space in (x, y, z) format.
rot -- Rotation in space in (r, p, y) format. (defaults to pointing
downwards.)
Keyword arguments:
wait -- If True, method will block until in position. (default
False)
"""
self._set_arm(self._left_iksvc, self._left_arm, pos, rot, wait)
def set_left(self, pos, rot=(0, math.pi, math.pi * 0.5), wait=False):
"""Set the endpoint of the right arm to the supplied coordinates.
Arguments:
pos -- Position in space in (x, y, z) format.
rot -- Rotation in space in (r, p, y) format. (defaults to pointing
downwards.)
Keyword arguments:
wait -- If True, method will block until in position. (default
False)
"""
self._set_arm(self._right_iksvc, self._right_arm, pos, rot, wait)
def get_left(self):
"""Return the current endpoint pose of the left arm."""
return self._left_arm.endpoint_pose()['position']
def get_right(self):
"""Return the current endpoint pose of the left arm."""
return self._right_arm.endpoint_pose()['position']
def get_left_velocity(self):
"""Return the current endpoint velocity of the left arm."""
return self._left_arm.endpoint_velocity()['linear']
def get_right_velocity(self):
"""Return the current endpoint velocity of the right arm."""
return self._right_arm.endpoint_velocity()['linear']
def get_left_force(self):
"""Return the current endpoint force on the left arm."""
return self._left_arm.endpoint_effort()['force']
def get_right_force(self):
"""Return the current endpoint force on the right arm."""
return self._right_arm.endpoint_effort()['force']
############################################################################
def _set_arm(self, iksvc, limb, pos, rot, wait):
resp = self._get_ik(iksvc, pos, rot)
positions = resp[0]
isValid = resp[1]
if not isValid:
print('invalid: {0} {1} {2}'.format(x, y, z))
if not wait:
limb.set_joint_positions(positions)
else:
limb.move_to_joint_positions(positions)
def _get_ik(self, iksvc, pos, rot):
q = quaternion_from_euler(rot[0], rot[1], rot[2])
pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(position=Point(
x=pos[0],
y=pos[1],
z=pos[2],
),
orientation=Quaternion(q[0], q[1], q[2], q[3])),
)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
iksvc.wait_for_service(5.0)
resp = iksvc(ikreq)
positions = dict(zip(resp.joints[0].name, resp.joints[0].position))
return (positions, resp.isValid[0])
class IKHelper(object):
"""An abstraction layer for using Baxter's built in IK service."""
############################################################################
def __init__(self):
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.3)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.3)
self._left_iksvc = rospy.ServiceProxy(
_IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(
_IKSVC_RIGHT_URI,
SolvePositionIK)
self._joint_update_pub = rospy.Publisher(
'/robot/joint_state_publish_rate',
UInt16)
self._joint_update_pub.publish(250)
############################################################################
def reset(self):
"""Reset both arms to their neutral positions."""
self._left_arm.move_to_neutral()
self._right_arm.move_to_neutral()
def set_left(self, pos, rot=(0, math.pi, math.pi *0.5), wait=False):
"""Set the endpoint of the left arm to the supplied coordinates.
Arguments:
pos -- Position in space in (x, y, z) format.
rot -- Rotation in space in (r, p, y) format. (defaults to pointing
downwards.)
Keyword arguments:
wait -- If True, method will block until in position. (default
False)
"""
self._set_arm(self._left_iksvc, self._left_arm, pos, rot, wait)
def set_left(self, pos, rot=(0, math.pi, math.pi *0.5), wait=False):
"""Set the endpoint of the right arm to the supplied coordinates.
Arguments:
pos -- Position in space in (x, y, z) format.
rot -- Rotation in space in (r, p, y) format. (defaults to pointing
downwards.)
Keyword arguments:
wait -- If True, method will block until in position. (default
False)
"""
self._set_arm(self._right_iksvc, self._right_arm, pos, rot, wait)
def get_left(self):
"""Return the current endpoint pose of the left arm."""
return self._left_arm.endpoint_pose()['position']
def get_right(self):
"""Return the current endpoint pose of the left arm."""
return self._right_arm.endpoint_pose()['position']
def get_left_velocity(self):
"""Return the current endpoint velocity of the left arm."""
return self._left_arm.endpoint_velocity()['linear']
def get_right_velocity(self):
"""Return the current endpoint velocity of the right arm."""
return self._right_arm.endpoint_velocity()['linear']
def get_left_force(self):
"""Return the current endpoint force on the left arm."""
return self._left_arm.endpoint_effort()['force']
def get_right_force(self):
"""Return the current endpoint force on the right arm."""
return self._right_arm.endpoint_effort()['force']
############################################################################
def _set_arm(self, iksvc, limb, pos, rot, wait):
resp = self._get_ik(iksvc, pos, rot)
positions = resp[0]
isValid = resp[1]
if not isValid:
print('invalid: {0} {1} {2}'.format(x, y, z))
if not wait:
limb.set_joint_positions(positions)
else:
limb.move_to_joint_positions(positions)
def _get_ik(self, iksvc, pos, rot):
q = quaternion_from_euler(rot[0], rot[1], rot[2])
pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(
position=Point(
x=pos[0],
y=pos[1],
z=pos[2],
),
orientation=Quaternion(q[0], q[1], q[2], q[3])
),
)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
iksvc.wait_for_service(5.0)
resp = iksvc(ikreq)
positions = dict(zip(resp.joints[0].name, resp.joints[0].position))
return (positions, resp.isValid[0])
