'''
Created on 2016-01-20
@author: Andrew H. Fagg and Bonnie Pope
'''
import rospy
import baxter_interface
from std_msgs.msg import Header
from geometry_msgs.msg import (
PoseStamped,
Pose,
Point,
Quaternion,
)
from baxter_core_msgs.srv import SolvePositionIK, SolvePositionIKRequest
from baxter_interface import AnalogIO
import time
import math
import random
from tf import transformations as t
from sensor_msgs.msg import (
Image,
)
import numpy as np
import cv2
import cv_bridge
global origin, xaxis, yaxis, zaxis
origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1)
[docs]class Karpal:
'''
**Karpal control tools.**
:ivar robotEnable: Interface for enabling/disabling the robot. See: http://api.rethinkrobotics.com/baxter_interface/html/baxter_interface.robot_enable.RobotEnable-class.html
:ivar leftArm: Interface for the left arm. See: http://api.rethinkrobotics.com/baxter_interface/html/baxter_interface.limb.Limb-class.html
:ivar rightArm: Interface for the right arm. See: http://api.rethinkrobotics.com/baxter_interface/html/baxter_interface.limb.Limb-class.html
:ivar leftGripper: Interface for the left gripper. See: http://api.rethinkrobotics.com/baxter_interface/html/baxter_interface.gripper.Gripper-class.html
:ivar rightGripper: Interface for the right gripper. See: http://api.rethinkrobotics.com/baxter_interface/html/baxter_interface.gripper.Gripper-class.html
:ivar rightArmNavigator: Interface for the right arm navigator (user interface). See: http://api.rethinkrobotics.com/baxter_interface/html/baxter_interface.navigator.Navigator-class.html
:ivar leftArmNavigator: Interface for the left arm navigator (user interface). See: http://api.rethinkrobotics.com/baxter_interface/html/baxter_interface.navigator.Navigator-class.html
:ivar rightTorsoNavigator: Interface for the right torso navigator (user interface). See: http://api.rethinkrobotics.com/baxter_interface/html/baxter_interface.navigator.Navigator-class.html
:ivar leftTorsoNavigator: Interface for the left torso navigator (user interface). See: http://api.rethinkrobotics.com/baxter_interface/html/baxter_interface.navigator.Navigator-class.html
:ivar head: Interface to the head. See: http://api.rethinkrobotics.com/baxter_interface/html/baxter_interface.head.Head-class.html
:ivar leftIkService: Interface to the inverse kinematics service. See: TBD
:ivar rightIkService: Interface to the inverse kinematics service. See: TBD
:ivar ikService: Dict containing both the left and right IK services.
:ivar displayPublisher: Publisher for images being routed to the head display.
'''
JPOSITION_ZERO = {'s0': 0,
's1': 0,
'e0': 0,
'e1': 0,
'w0': 0,
'w1': 0,
'w2': 0
}
''' Zero joint angle position'''
JPOSITION_CRANE = {'s0': 0,
's1': -math.pi/4,
'e0': 0,
'e1': math.pi/2,
'w0': 0,
'w1': math.pi/4,
'w2': 0
}
''' Crane joint angle position. Elbow up and hand oriented downward.'''
JPOSITION_GRASP_READY = {'s0': 0,
's1': -math.pi/4,
'e0': 0,
'e1': math.pi/2,
'w0': 0,
'w1': -math.pi/4,
'w2': 0
}
''' Grasp ready angle position. Elbow up and forearm and hand oriented horizontally (45 degrees off center).'''
JPOSITION_PARK = {'s0': 0,
's1': math.pi/8,
'e0': 0,
'e1': math.pi/4,
'w0': 0,
'w1': math.pi/8,
'w2': 0
}
''' Park angle position. Arm is in a low-energy state where it is safe to be disabled.'''
def __init__(self):
'''
Initialize the connections to the baxter control services, and initialize internal structures with links to the key underlying baxter components.
'''
self.robotEnable = baxter_interface.RobotEnable()
self.leftArm = baxter_interface.Limb('left')
self.rightArm = baxter_interface.Limb('right')
self.leftGripper = baxter_interface.Gripper('left')
self.rightGripper = baxter_interface.Gripper('right')
self.rightArmNavigator = baxter_interface.Navigator('right')
self.leftArmNavigator = baxter_interface.Navigator('left')
self.rightTorsoNavigator = baxter_interface.Navigator('torso_right')
self.leftTorsoNavigator = baxter_interface.Navigator('torso_left')
self.head = baxter_interface.Head();
#self.leftHandCamera = CameraController('left_hand_camera')
#self.rightHandCamera = CameraController('right_hand_camera')
#self.headCamera = CameraController('head_camera')
# Inverse kinematics services
ns = "ExternalTools/left/PositionKinematicsNode/IKService"
rospy.wait_for_service(ns)
self.leftIkService = rospy.ServiceProxy(ns, SolvePositionIK)
# Inverse kinematics services
ns = "ExternalTools/right/PositionKinematicsNode/IKService"
rospy.wait_for_service(ns)
self.rightIkService = rospy.ServiceProxy(ns, SolvePositionIK)
# Both services in one dict
self.ikService = {'left': self.leftIkService, 'right': self.rightIkService}
self.displayPublisher = rospy.Publisher('/robot/xdisplay', Image, latch=True)
###########################################################################
# Robot
[docs] def enable(self):
"""
Enable the robot.
.. note:: The robot's E-Stop button must be disabled in order for this function to be successful (this is the big red button).
"""
self.robotEnable.enable()
[docs] def disable(self):
'''
Disable the robot.
'''
self.robotEnable.disable()
##########################################################################################
# Gripper Methods:
# Left Gripper
[docs] def calibrateLeftGripper(self):
'''
Calibrate the positional offset of the left gripper.
'''
self.leftGripper.calibrate()
[docs] def closeLeftGripper(self):
'''
Close the left gripper until a force threshold.
'''
self.leftGripper.close()
[docs] def openLeftGripper(self):
'''
Open the left gripper to its widest extent.
'''
self.leftGripper.open()
#Right Gripper
[docs] def calibrateRightGripper(self):
'''
Calibrate the positional offset of the right gripper.
'''
self.rightGripper.calibrate()
[docs] def closeRightGripper(self):
'''
Close the right gripper until a force threshold.
'''
self.rightGripper.close()
[docs] def openRightGripper(self):
'''
Open the right gripper to its widest extent.
'''
self.rightGripper.open()
#Either Gripper
[docs] def calibrateGripper(self, limb):
'''
Calibrate the specified gripper
:param limb: Side to calibrate. Either 'left' or 'right'
'''
baxter_interface.Gripper(limb).calibrate()
[docs] def closeGripper(self, limb):
'''
Close the specified gripper until a force threshold.
:param limb: Side to close. Either 'left' or 'right'
'''
baxter_interface.Gripper(limb).close()
[docs] def openGripper(self, limb):
'''
Open the specified gripper to its widest extent
:param limb: Side to open. Either 'left' or 'right'
'''
baxter_interface.Gripper(limb).open()
###########################################################################################
#Position Methods:
[docs] def getLeftArmPosition(self):
'''
Get the Cartesian position of the left hand (base of the hand) in the Baxter's main coordinate frame.
:returns: A vector containing the position (m)
'''
position = self.leftArm.endpoint_pose()
x = position['position'].x
y = position['position'].y
z = position['position'].z
return [x,y,z]
[docs] def getRightArmPosition(self):
'''
Get the Cartesian position of the right hand (base of the hand) in the Baxter's main coordinate frame.
:returns: A vector containing the position (m)
'''
position = self.rightArm.endpoint_pose()
x = position['position'].x
y = position['position'].y
z = position['position'].z
return [x,y,z]
[docs] def getArmPosition(self, limb):
"""
Get the Cartesian position of the specified hand (base of the hand) in the Baxter's main coordinate frame.
:param limb: 'left' or 'right'
:returns: A vector containing the position (m)
"""
position = baxter_interface.Limb(limb).endpoint_pose()
x = position['position'].x
y = position['position'].y
z = position['position'].z
return [x,y,z]
[docs] def getJointAngles(self, limb):
'''
Get the joint configuration of the specified limb.
:param limb: 'left' or 'right'
:returns: A dict of joint angles (rad).
'''
angles = baxter_interface.Limb(limb).joint_angles()
return angles
#Orientataion Methods:
[docs] def getLeftArmOrientation(self):
"""
Get the orientation of the left hand (base of the hand) in the Baxter's main coordinate frame.
:returns: A vector containing the unit quaternion
"""
orientation = self.leftArm.endpoint_pose()
x = orientation['orientation'].x
y = orientation['orientation'].y
z = orientation['orientation'].z
w = orientation['orientation'].w
return [x,y,z,w]
[docs] def getRightArmOrientation(self):
"""
Get the orientation of the right hand (base of the hand) in the Baxter's main coordinate frame.
:returns: A vector containing the unit quaternion
"""
orientation = self.rightArm.endpoint_pose()
x = orientation['orientation'].x
y = orientation['orientation'].y
z = orientation['orientation'].z
w = orientation['orientation'].w
return [x,y,z,w]
[docs] def getArmOrientation(self, limb):
"""
Get the orientation of the specified hand (base of the hand) in the Baxter's main coordinate frame.
:param limb: 'left' or 'right'
:returns: A vector containing the unit quaternion
"""
orientation = baxter_interface.Limb(limb).endpoint_pose()
x = orientation['orientation'].x
y = orientation['orientation'].y
z = orientation['orientation'].z
w = orientation['orientation'].w
return [x,y,z,w]
[docs] def getArmPose(self, limb):
'''
Get the full pose of the specified limb.
:param limb: 'left' or 'right'
:returns: A dict containing the full pose (keys: 'position' and 'orientation').
'''
# Not sure why we need to ask again, but we do not seem to be getting good poses all the time...
while(True):
pose = baxter_interface.Limb(limb).endpoint_pose()
if 'position' in pose.keys() and 'orientation' in pose.keys():
return pose
print "Retrying position sensing for arm..."
####################################################################################
# Joint control methods
[docs] def moveArmJoints(self, limb, angles, timeout=15.0):
'''
Move specified arm to a joint goal
:param limb: 'left' or 'right'
:param angles: A dict (?) containing the joint goal position
:param timeout: Number of seconds before aborting joint movement (default = 15.0 sec)
:returns: The joint angles
'''
baxter_interface.Limb(limb).move_to_joint_positions(angles, timeout=timeout)
return angles
[docs] def tiltWrist(self, limb):
"""
For the specified limb, flip the final wrist joint by pi radians
:param limb: 'left' or 'right'
:returns: **True** if successful; **False** if not
"""
# Get the Current joint angles
angles = self.getJointAngles(limb)
# Add pi
angles['left_w2'] += math.pi
# Is this a legal position?
if angles['left_w2'] > 3.059:
# No
print "tilt other way"
# Try to flip the other way
angles['left_w2'] = angles['left_w2'] - 2*math.pi
# Check again
if angles['left_w2'] < -3.059:
# No
print "tilt not completed"
return False
#move to position
self.moveArmJoints(limb, angles)
return True
[docs] def armMoveToJointPosition(self, limb='left', orientation={}, timeout=15.0):
'''
Move arm to a specified joint position.
:param limb: 'left' or 'right'
:param orientation: dict containing the joint angles of interest. The elements can be either specific joints (including limb) or generic joints. In the latter case, the specified limb is appended
:param timeout: Number of seconds to wait for movement to complete (default = 15.0 sec)
'''
# Make sure we have a valid input
if(orientation == None):
return False
# Get the limb's current joint angles
angles = self.getJointAngles(limb)
# Extract the new joint angles
for j in orientation:
# Already have the limb specified?
if j.find(limb) == 0:
# Yes - just copy
angles[j] = orientation[j]
else:
# No - add limb
angles[limb + '_' + j] = orientation[j]
# Move the arm
self.moveArmJoints(limb, angles, timeout=timeout)
return True
####################################################################################
# Cartesian control methods
[docs] def inverseKinematics(self, limb, position, orientation):
'''
Perform the inverse kinematics given a limb, position and orientation
:param limb: 'left' or 'right'
:param position: Vector containing the goal Cartesian position for the base of the hand
:param orientation: Vector containing the goal orientation for the hand (unit quaternion)
:returns: **None** if no solution found, or a dict containing the joint angles that correspond to the goal.
'''
# Create the request message
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
# Pose
pose = PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=position[0],
y=position[1],
z=position[2],
),
orientation=Quaternion(
x=orientation[0],
y=orientation[1],
z=orientation[2],
w=orientation[3],
)
)
);
# Add the pose to the request
ikreq.pose_stamp.append(pose)
try:
# Make the request
resp = self.ikService[limb](ikreq)
except rospy.ServiceException,e :
rospy.loginfo("Service call failed: %s" % (e,))
return None
# Check for a valid solution
if (resp.isValid[0]):
print("SUCCESS - Valid Joint Solution Found:")
# Format solution into Limb API-compatible dictionary
limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
return limb_joints
else:
print("INVALID POSE - No Valid Joint Solution Found.")
return None
[docs] def moveArm(self, limb, position, orientation):
'''
Move the arm to a Cartesian goal position/orientation
:param limb: 'left' or 'right'
:param position: Vector containing the goal Cartesian position for the base of the hand
:param orientation: Vector containing the goal orientation for the hand (unit quaternion)
:returns: **None** if no solution found, or a dict containing the joint angles that correspond to the goal.
'''
# Compute the inverse kinematics
angles = self.inverseKinematics(limb, position, orientation)
# Check the result
if not angles:
# No solution found
return None
# Execute the motion
self.moveArmJoints(limb, angles)
# Return joint position
return angles
##########################################################################
# Inverse kinematics functions
[docs] def computeHorizontalOrientation(self, orient, handFlip=0):
'''
Compute a hand orientation in which the hand is in the horizontal plane
:param orient: Orientation of the hand in the robots coordinate frame (about Z)
:param handFlip: Enables addition of a flipped hand (still horizontal, though) if set to PI
:returns: Quaternion describing the full orientation
'''
global xaxis, yaxis, zaxis
T1 = t.rotation_matrix(math.pi/2, yaxis) # Flip hand so it is horizontal
T2 = t.rotation_matrix(orient, xaxis) # Rotate hand about local x axis
T3 = t.rotation_matrix(handFlip, zaxis) # Flip hand over
T = t.concatenate_matrices(T1, T2, T3)
# Convert from quaternion
q = t.quaternion_from_matrix(T)
return q
[docs] def findHorizontalSolution(self, limb='left', position=[.6, .1, .3],
minOrient=math.pi/2, maxOrient=math.pi/2,
nAttempts=10):
'''
Find a viable joint solution for a horizontal reach. A range of
acceptable orientations in the horizontal plane is specified; one is
selected at random. Multiple attempts are tried at finding a valid
inverse kinematics solution (up to nAttemts)
:param limb: Determines which limb; 'left' or 'right'
:param position: Goal position of the gripper
:param minOrient: Minimum acceptable hand orientation (in the horiziontal plane)
:param maxOrient: Maximum acceptable hand orientation
:param nAttempts: Maximum number of attempts
:returns: None if no solution is found, or a dictionary that contains information about the chosen configuration: the solution (**joint_angles**), the full hand orientation (**q**), the hand orientation in the horizontal plane (**orient**); and the limb the solution was computed for (**limb**).
'''
iteration = 0
# Iterate over each attempt
while iteration < nAttempts:
# Randomly select an orientation within the allowed range
orient = minOrient + (maxOrient-minOrient) * random.random()
# Compute the quaternion
q = self.computeHorizontalOrientation(orient)
# Compute the inverse kinematics
angles = self.inverseKinematics(limb, position, q)
if angles:
# We are done: return the solution
return {'joint_angles': angles, 'q': q, 'orient': orient, 'limb': limb}
# Not found: try again?
iteration += 1
# No solution found over the attempts
print "No solution Found"
return None
[docs] def computeVerticalOrientation(self, orient):
'''
Compute a hand orientation in which the hand is pointed downwards
:param orient: Orientation of the hand in the robot coordinate frame (about Z)
:returns: Quaternion describing the full orientation
'''
global xaxis, yaxis, zaxis
T1 = t.rotation_matrix(math.pi, yaxis) # Flip hand so it is pointed down
T2 = t.rotation_matrix(orient, zaxis) # Rotate hand about global Z axis
T = t.concatenate_matrices(T1, T2)
# Convert from quaternion
q = t.quaternion_from_matrix(T)
return q;
[docs] def findVerticalSolution(self, limb='left', position=[.6, .1, .3], minOrient=0, maxOrient=2*math.pi,
flipFlag=False, nAttempts=10):
'''
Find a viable joint solution for a vertical reach (from above). A range of
acceptable orientations about the vertical axis is specified; one is
selected at random. Multiple attempts are tried at finding a valid
inverse kinematics solution (up to nAttemts)
:param limb: Determines which limb; 'left' or 'right'
:param position: Goal position of the gripper
:param minOrient: Minimum acceptable hand orientation (about the vertical axis)
:param maxOrient: Maximum acceptable hand orientation
:param nAttempts: Maximum number of attempts
:returns: None if no solution is found, or a dictionary that contains information about the chosen configuration: the solution (**joint_angles**), the full hand orientation (**q**), the hand orientation in the horizontal plane (**orient**); and the limb the solution was computed for (**limb**).
'''
iteration = 0
# Loop over attempts
while iteration < nAttempts:
# Pick an orientation within the specified range
orient = minOrient + (maxOrient-minOrient) * random.random()
# If allowed, we will flip the orientation by 180 degrees with 50% probability
if flipFlag & (random.random() < 0.5):
orient += math.pi;
# Compute quaternion
q = self.computeVerticalOrientation(orient)
# Inverse kinematics
angles = self.inverseKinematics(limb, position, q)
# Did we get a solution?
if angles:
# We are done
return {'joint_angles': angles, 'q': q, 'orient': orient, 'limb': limb}
# Count the number of failures
iteration += 1
# No solution found over the attempts
print "No solution Found"
return None
[docs] def findAbsoluteJogSolution(self, limb='left', deltaX=[0,0,0]):
'''
Compute the inverse kinematics solution for a small change in position. The change
is specified in the robot base coordinate frame.
:param limb: Which limb to compute solution for: left/right
:param deltaX: Three element array: [dX, dY, dZ]
:returns: None if no solution is found, or a dictionary that contains information about the chosen solution: the joint angles (**joint_angles**), the full hand orientation (**q**), position of the limb (**position**); and the limb that the solution was computed for (**limb**).
'''
# Current pose of the robot
pose = self.getArmPose(limb)
# Extract the position and q
position = pose['position']
q = pose['orientation']
# Construct a new position
position2 = [position.x + deltaX[0],
position.y + deltaX[1],
position.z + deltaX[2]];
# Inverse kinematics
angles = self.inverseKinematics(limb, position2, q)
# Do we have a solution?
if angles:
# We are done: return the solution
return {'joint_angles': angles, 'q': q, 'position': position2, 'limb': limb}
return None
[docs] def findRelativeJogSolution(self, limb='left', deltaX=[0,0,0]):
'''
Compute the inverse kinematics solution for a small change in position. The change
is specified in the hand coordinate frame.
:param limb: Which limb to compute solution for: left/right
:param deltaX: Three element array: [dX, dY, dZ]
:returns: None if no solution is found, or a dictionary that contains information about the chosen solution: the joint angles (**joint_angles**), the full hand orientation (**q**), position of the limb (**position**); and the limb that the solution was computed for (**limb**).
'''
# Current pose of the robot
pose = self.getArmPose(limb)
# Extract the position and q
position = pose['position']
q = pose['orientation']
T = t.quaternion_matrix(q)
# Compute the delta in absolute coordinates from the hand delta
delta = [deltaX[0], deltaX[1], deltaX[2], 1]
delta2 = np.dot(T,delta)
# Construct a new position
position2 = [position.x + delta2[0],
position.y + delta2[1],
position.z + delta2[2]];
# Inverse kinematics
angles = self.inverseKinematics(limb, position2, q)
# Do we have a solution?
if angles:
# We are done: return the solution
return {'joint_angles': angles, 'q': q, 'position': position2, 'limb': limb}
return None
###########################################################################
# Sensor-driven movement
[docs] def moveArmUntilDistance(self, limb='left', delta = 0.02, maxDistance=.3, handDistance = 70):
'''
Move the arm along the Z axis of the hand until an obstacle is sensed or a maximum distance is achieved.
.. warning:: Not all obstacles can be sensed with the distance sensor.
:param limb: Determines which limb; 'left' or 'right'
:param delta: Distance to move along the Z axis (m) between sensor checks. This parameter should be positive
:param maxDistance: Maximum distance to move (m)
:param handDistance: Distance to the obstacle to stop at (mm)
:returns: 2 if successfully reached the obstacle; 1 if the maximum distance has been reached; and 0 **False** if no solution
'''
# Accumulated distance moved
distance = 0;
# Get access to the distance sensor
sensorName = limb + "_hand_range"
sensor = AnalogIO(sensorName)
# Loop until obstacle or maximum reached
while sensor.state() > handDistance and distance <= maxDistance:
# Compute the jog
j = self.findRelativeJogSolution(limb, [0, 0, delta])
# Is there a solution?
if j != None:
# Yes: do the motion
self.armMoveToJointPosition(limb, j['joint_angles'])
else:
# No
return 0
# Update distance
distance += delta
print sensor.state()
# Did we make it to the obstacle
if sensor.state() <= handDistance:
# Yes
return 2
else:
# No
return 1
##########################################################################
# Screen
[docs] def printTextToScreen(self, text, scale=3):
'''
Print text to the Baxter screen
:param text: String containing the text to print. Newlines in the string are handled properly.
:param scale: Size of the text.
'''
# Create the buffer
img = np.zeros((2000,2000,3), np.uint8)
# Get the font
font = cv2.FONT_HERSHEY_SIMPLEX
# Split the text and place each on a separate line
for i, line in enumerate(text.split('\n')):
# TODO: the increment needs to change as a function of scale
y = 200*(i+1)
# Draw the text into the buffer
cv2.putText(img,line,(50,y), font, scale,(255,255,255),2)
# Create the message
msg = cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8")
# Publish the image
#pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self.displayPublisher.publish(msg)
# Sleep to allow for image to be published.
# TODO: do we need this?
rospy.sleep(1)
# main
[docs]def main():
"""Test function"""
rospy.init_node('Karpal_Class')
karpal = Karpal()
karpal.enable()
#testing
point = [.80,.10,0]
point2 = [.80,.04,-.06]
orientationAbove = [-1,0,0,0]
orientationSide = [0,math.sin(math.pi/4),0,math.cos(math.pi/4)]
limb = "left"
angles = {'left_w0': 0.06519418341064454, 'left_w1': 0.36930587426147465, 'left_w2': 1.4323545590515139, 'left_e0': -1.5405002044738771, 'left_e1': 1.3882526114501954, 'left_s0': 0.7853981625, 'left_s1': -0.11543205415649414}
"""
#Gripper Methods
print "Left Gripper"
karpal.calibrateLeftGripper()
karpal.closeLeftGripper()
karpal.openLeftGripper()
print "Right Gripper"
karpal.calibrateRightGripper()
karpal.closeRightGripper()
karpal.openRightGripper()
print "Either Gripper"
karpal.calibrateGripper(limb)
karpal.closeGripper(limb)
karpal.openGripper(limb)
#Position Methods:
print "Postion methods"
print karpal.getLeftArmPosition()
print karpal.getRightArmPosition()
print karpal.getArmPosition(limb)
print karpal.getJointAngles(limb)
#Orientataion Methods:
print "Orientation methods"
print karpal.getLeftArmOrientation()
print karpal.getRightArmOrientation()
print karpal.getArmOrientation(limb)
"""
#Moving Methods:
print "Moving Methods"
karpal.armMoveToZeroPosition(limb)
#karpal.moveArmJoints(limb, angles)
#karpal.moveArm(limb, point, orientation)
#arpal.tiltWrist(limb, point)
#Grabbing Methods:
print "Grabbing Methods"
karpal.grab_object(limb, point, orientation)
#karpal.grab_object_range_sensor(limb, point, orientation)
#Computing Methods:
#print "Computing Methods"
#karpal.findHorizontalSolution(limb, point, minOrient, maxOrient)
#karpal.findVerticalSolution(limb, point, minOrient, maxOrient)
karpal.armMoveToZeroPosition(limb)
karpal.disable()
if __name__ == '__main__':
main()