def __init__(self):
#All of the robot's logic is decoupled and found in ./robot/[nameofrobot]/
self._robot = Robot()
#enable move it
self.robot = moveit_commander.RobotCommander()
self._planningscene = PlanningScene()
self.scene = moveit_commander.PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
#if you don't sleep for a couple seconds you'll get errors that the scene or robot doesn't exist
print "Sleeping for 2 seconds to load MoveIt scene."
rospy.sleep(2)
#Add the table to MoveIt
self.draw_table_in_rviz()
#_liatris_three_points_matrix allows this script to transform any touchscreen's point to the robot's coordinate system.
self._liatris_three_points_matrix = Liatris_Three_Points_To_Rot_Matrix(
)
#check if threepoints.config exists. If no, prompt the user to calibrate the screen's pose.
self._require_configuration()
def __init__(self):
#All of the robot's logic is decoupled and found in ./robot/[nameofrobot]/
self._robot = Robot()
#enable move it
self.robot = moveit_commander.RobotCommander()
self._planningscene = PlanningScene()
self.scene = moveit_commander.PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene',PlanningScene)
#if you don't sleep for a couple seconds you'll get errors that the scene or robot doesn't exist
print "Sleeping for 2 seconds to load MoveIt scene."
rospy.sleep(2)
#Add the table to MoveIt
self.draw_table_in_rviz()
#_liatris_three_points_matrix allows this script to transform any touchscreen's point to the robot's coordinate system.
self._liatris_three_points_matrix = Liatris_Three_Points_To_Rot_Matrix()
#check if threepoints.config exists. If no, prompt the user to calibrate the screen's pose.
self._require_configuration()
class Liatris(object):
#THE FOLLOWING VALUES MAY NEED TO BE CHANGED ----------------------------------------
#CHANGE THIS TO YOUR SERVER'S DOMAIN
_server = "http://ec2-52-25-236-123.us-west-2.compute.amazonaws.com"
#All of the following units are meters
#Custom values to offset x & y coordinates in meters - this makes it easier to fix slightly inaccurate config files
_screen_x_offset = -0.003 #if your X coordinates are slightly off (RELATIVE TO SCREEN; NOT ROBOT!) tweak this
_screen_y_offset = -0.021 #if your Y coordinates are slightly off (RELATIVE TO SCREEN; NOT ROBOT!) tweak this
_height_of_rfid_scanner = 0.21 #What is the height of your RFID reader (only measure the distance below the gripper)
_z_height_of_gripper = 0.1 #Height of gripper. Change this if you aren't using the stock grippers
_height_of_table = -0.175 #Note: This is the relative height of the table compared to the bottom of Baxter (excluding the stand!). Mine is negative because the table is lower than than the base of baxter (regardless of the stand & wheels which are lower than the table but don't influence the math.).
#The following 3 values control where objects should be dropped off by the left arm
_drop_off_cord_x = 0.5
_drop_off_cord_y = 0.5
_drop_off_cord_z = 0.1
#Review function self.draw_table_in_rviz for more custom values related to the dimensions / pose of your table
#END THE FOLLOWING VALUES MAY NEED TO BE CHANGED ----------------------------------------
#You shouldn't need to modify any of the following defaults
_objects_on_table = []
_three_points_matrix = False
_ik = False
_tkinter = False
_move_left_arm = True
_move_arm_offset = 0.1
_last_move_to_file_x = False
_last_move_to_file_y = False
_currently_moving = False
_grippers = False
_ik = False
_right_arm_rotation = 0
_object_count = 0
_object_already_added_to_moveit = []
_last_object_type = False
_last_object_pose = False
_head = False
_colors = dict()
def __init__(self):
#All of the robot's logic is decoupled and found in ./robot/[nameofrobot]/
self._robot = Robot()
#enable move it
self.robot = moveit_commander.RobotCommander()
self._planningscene = PlanningScene()
self.scene = moveit_commander.PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene',PlanningScene)
#if you don't sleep for a couple seconds you'll get errors that the scene or robot doesn't exist
print "Sleeping for 2 seconds to load MoveIt scene."
rospy.sleep(2)
#Add the table to MoveIt
self.draw_table_in_rviz()
#_liatris_three_points_matrix allows this script to transform any touchscreen's point to the robot's coordinate system.
self._liatris_three_points_matrix = Liatris_Three_Points_To_Rot_Matrix()
#check if threepoints.config exists. If no, prompt the user to calibrate the screen's pose.
self._require_configuration()
#note: if the config file doesn't exist yet the init() script will never get past this point. They'll create the config file and then restart the script to load it.
def get_json_from_server(self,script_url):
#download information from the API
url = self._server + "/" + script_url
response = urllib.urlopen(url)
#convert json to python datatype
return json.loads(response.read())
def _check_for_new_object_on_table(self):
#ask the server if a new object exists
data = self.get_json_from_server("touchread/")
#False = no valid 3 point pose found
if data == False:
return -1
self._last_object_pose = data
#_screen_x_offset & _screen_y_offset are manually set above and allow you to tweak an imperfect calibration
data["y"] = float(data["y"]) + float(self._screen_y_offset / self._liatris_three_points_matrix.ret_height_of_screen())
data["x"] = float(data["x"]) + float(self._screen_x_offset / self._liatris_three_points_matrix.ret_width_of_screen())
pose = [data["x"],data["y"]]
#Was this pose already imported? If yes, ignore it.
if pose in self._object_already_added_to_moveit:
print "check for new object: already imported"
else:
#prevent duplicates by adding pose to already completed list
self._object_already_added_to_moveit.append(pose)
#Step 1: read the RFID
#determine the relative pose of the RFID.
rfid_pose = self.determine_object_rfid_pose(data["x"],data["y"],data["orientation_in_radians"])
#go to pose (but way above table)
self.go_to_relative_position(float(rfid_pose[0]),float(rfid_pose[1]),True)
#turn to the right orientation and drop to RFID reading height
self._point_rfid_reader_down_on_right_arm(self._liatris_three_points_matrix.calc_relative_radians_angle(data["orientation_in_radians"]))
#Detect everything there is to know about the object from the API
self._objectapi()
_ret = -1
if self._last_object_type == "does not exist":
print "RFID returned false"
else:
print "Adding a new " + str(self._last_object_type["nickname"]) + " to RVIZ"
#Draw the object in RVIZ
_ret = self.draw_collision_object_in_rviz()
#Move the right arm out of the way (2 steps)
#First move it up without rotation to avoid collision with object
self.move_right_arm_up_with_same_radians_and_xy(self._liatris_three_points_matrix.calc_relative_radians_angle(data["orientation_in_radians"]))
#Now that we're above the table get the arm out of the way
self._get_right_arm_out_of_the_way()
return _ret
return -1
def _require_configuration(self):
# If a config file (that tells transformation of touch screeen's vs robot's poses) already exists skip the configuration step
if not self._liatris_three_points_matrix.does_config_file_exist():
#TkInter is just an easy way to capture key stroaks in python.
self._point_arms_straight_down()
self._tkinter = tk.Tk()
#Size doesn't matter
self._tkinter.geometry('300x200')
#On each keypress call _onKeypress function
self._tkinter.bind('<KeyPress>', self._onKeypress)
#instructions on how to configure robot
print "INSTRUCTIONS FOR CALIBRATION"
print "Opening TkInter window to capture key strokes. Make sure it stays in focus."
print "Moving left arm first. Set to orientation 0,0 (relative to the robot, bottom, left corner of screen) on screen.";
print "Move arm with W,A,S,D like an old school video game.";
print "Movement Speed can be adjusted with the following keys. T: Fast, G: Medium, B: Slow, Y: Slowest";
print "To save a point. Press M.";
self._tkinter.mainloop()
def draw_table_in_rviz(self):
#draw the table in rviz. You'll likely need to tweak the following values for your table.
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0.28 + (0.72/2) #0.28 is offset from center of robot to the front of the table. 0.72/2 is the distance to the center of the table
p.pose.position.y = 0
p.pose.position.z = self.height_of_table() #is the Z value of the table (not relative to the robot like self.height_of_table returns)
quaternion = tf.transformations.quaternion_from_euler(0,0,float(math.pi / 2)) #turn table 90 degrees
p.pose.orientation.x = quaternion[0]
p.pose.orientation.y = quaternion[1]
p.pose.orientation.z = quaternion[2]
p.pose.orientation.w = quaternion[3]
self.set_object_color("table",1.71,0.79,0.22)
self.scene.add_mesh("table",p,"stl/table.stl")
def set_object_color(self,id,r,g,b):
#defines the color of an object in MoveIt
color = ObjectColor()
color.id = str(id)
color.color.r = float(r)
color.color.g = float(g)
color.color.b = float(b)
color.color.a = 1.0
self._colors[str(id)] = color
self._planningscene.is_diff = True
for color in self._colors.values():
self._planningscene.object_colors.append(color)
self.scene_pub.publish(self._planningscene)
def height_of_table(self):
return self._height_of_table
def _onKeypress(self,event):
#Used for calibration only
_c = event.char
#when calibrating it's convinient to be able to change the distance each arm moves at any given time (t being the greatest and y being the smallest)
if _c == 't':
self._move_arm_offset = 0.1
if _c == 'g':
self._move_arm_offset = 0.03
if _c == 'b':
self._move_arm_offset = 0.01
if _c == 'y':
self._move_arm_offset = 0.003
#m saves a point
if _c == 'm':
self.save_point()
#movement of either are is done with WASD like a video game
if(not (_c == 'w' or _c == 'a' or _c == 's' or _c == 'd')):
return False
if(self._move_left_arm):
pose = self._robot.ik.get_pose('left')
else:
pose = self._robot.ik.get_pose('right')
y = pose.y
x = pose.x
if _c == 'a':
y = y + self._move_arm_offset
if _c == 'd':
y = y - self._move_arm_offset
if _c == 'w':
x = x + self._move_arm_offset
if _c == 's':
x = x - self._move_arm_offset
print "moving... (wait for success prior to pressing another key)"
#prohibit multiple _ik requests simul
if(self._currently_moving):
return True
self._currently_moving = True #prevent invalid key strokes
offset_of_gripper_above_table = 0.02
if(self._move_left_arm):
if not self._robot.ik.set_left(float(x),float(y),float(self.height_of_table()) + float(self._z_height_of_gripper) + float(offset_of_gripper_above_table)):
print "left failed to point down at pose"
else:
print "success left arm"
else:
if not self._robot.ik.set_right(float(x),float(y),float(self.height_of_table()) + float(self._z_height_of_gripper) + float(offset_of_gripper_above_table)):
print "right failed to point down at pose"
else:
print "success right arm"
self._currently_moving = False
def save_point(self):
#save the current pose when calibrating
if self._move_left_arm:
self._move_left_arm = False
_pos = self._robot.ik.get_pose('left')
print "0,0 (bottom, left) saved"
print "Move the right arm to 1,0 (bottom,right) and press m to save that point."
self._liatris_three_points_matrix.add_cord(_pos.x,_pos.y)
else:
_pos = self._robot.ik.get_pose('right')
print "saved"
if self._liatris_three_points_matrix.count_cords() == 1:
print "Finally move the right arm to 1,1 (top,right) and press m to complete the calibration."
self._liatris_three_points_matrix.add_cord(_pos.x,_pos.y)
def go_to_relative_position(self,x_per,y_per, force_right = False, force_left = False):
#This function is required because the screen only knows the coordinates of the object relative to the screen. Not relative to the robot. This converts from the screen percentages to a robot pose.
pos = self._liatris_three_points_matrix.determine_a_relative_point(x_per,y_per)
return self._go_to_position(pos[0],pos[1],x_per,force_right,force_left)
def determine_center_of_object(self):
#This function is required for adding objects to RVIZ. In practice it isn't convinient to add the 3 points directly in the middle of each object. x_offset & y_offset allow you to add it any location and then offset (via the API) the location for the collision box & stl.
#offsets are based on the orientation of the object so calculate the correct center of object coordinates by using rotation matrix
x = float(self._last_object_pose["x"])
y = float(self._last_object_pose["y"])
#The x & y coordinates are percentages of the screen width. The offsets are in meters so we need to covert these to percents as well (of screen).
x_offset = 0
if(float(self._last_object_type["transformation"]["x_offset"]) != 0):
x_offset = float(self._last_object_type["transformation"]["x_offset"]) / float(self._liatris_three_points_matrix.ret_width_of_screen())
y_offset = 0
if(float(self._last_object_type["transformation"]["y_offset"]) != 0):
y_offset = float(self._last_object_type["transformation"]["y_offset"]) / float(self._liatris_three_points_matrix.ret_height_of_screen())
radians = float(self._last_object_pose["orientation_in_radians"])
#only the offset is rotated using x/y as the origin
pre_rotated_x = x_offset
pre_rotated_y = y_offset
rotMatrix = numpy.array([[numpy.cos(radians), -numpy.sin(radians)],
[numpy.sin(radians), numpy.cos(radians)]])
pre_rotated_matrix = [[pre_rotated_x],[pre_rotated_y]]
post_rotated_matrix = numpy.dot(rotMatrix,pre_rotated_matrix)
return [float(post_rotated_matrix[0][0]) + x,float(post_rotated_matrix[1][0]) + y]
def draw_collision_object_in_rviz(self):
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.z = self.height_of_table() + (float(self._last_object_type["size"]["h"]) / 2)
#determine point relative to robot's pose
coordinates_of_last_object_with_offset = self.determine_center_of_object()
tmp_pos = self._liatris_three_points_matrix.determine_a_relative_point(float(coordinates_of_last_object_with_offset[0]),float(coordinates_of_last_object_with_offset[1]))
p.pose.position.x = tmp_pos[0]
p.pose.position.y = tmp_pos[1]
quaternion = tf.transformations.quaternion_from_euler(0,0,float(self._last_object_pose["orientation_in_radians"]))
p.pose.orientation.x = quaternion[0]
p.pose.orientation.y = quaternion[1]
p.pose.orientation.z = quaternion[2]
p.pose.orientation.w = quaternion[3]
#add this to an array of objects
self._objects_on_table.append([self._object_count,self._last_object_type,self._last_object_pose,0]) #final ,0 means that the object hasn't been removed from the table
#download stl file if it doesn't exist yet (from API server); in practice we'd expect millions of different STL files to be on a CDN and constantly introducing new files so can't rely on local storage.
self.download_stl_file_from_api_if_its_not_local(str(self._last_object_type["stl_file"]))
#add the stl file to the scene as a collision object
self.set_object_color("object" + str(self._object_count),self._last_object_type["color"]["r"],self._last_object_type["color"]["g"],self._last_object_type["color"]["b"])
self.scene.add_mesh("object" + str(self._object_count),p,"stl/" + str(self._last_object_type["stl_file"]) + ".stl")
#If you'd prefer to use a box instead of an STL file use the following
#self.scene.add_box("object" + str(self._object_count),p,(float(self._last_object_type["size"]["l"]), float(self._last_object_type["size"]["w"]), float(self._last_object_type["size"]["h"]))) #0.72 ... is the size of the object
self._object_count = self._object_count + 1
return self._object_count - 1
def download_stl_file_from_api_if_its_not_local(self,stl_file):
#sync STL files to local if they don't exist
if os.path.isfile("stl/" + str(stl_file) + ".stl"):
return True
conn = httplib.HTTPConnection(self._server[7:],80) #[7: removes http://
conn.request('GET',"/stl/" + str(stl_file) + ".stl")
resp = conn.getresponse()
data = resp.read()
with open("stl/" + str(stl_file) + ".stl","wb") as f:
f.write(data)
conn.close()
def determine_object_pickup_pose(self,object_id):
#offsets are based on the orientation of the object so calculate the correct center of object coordinates by using rotation matrix
x = float(self._objects_on_table[object_id][2]["x"])
y = float(self._objects_on_table[object_id][2]["y"])
#The x & y coordinates are percentages of the screen width. The offsets are in meters so we need to covert these to percents as well (of screen).
x_offset = 0
if(float(self._objects_on_table[object_id][1]["grasp"]["x_offset"]) != 0):
x_offset = float(self._objects_on_table[object_id][1]["grasp"]["x_offset"]) / float(self._liatris_three_points_matrix.ret_width_of_screen())
y_offset = 0
if(float(self._objects_on_table[object_id][1]["grasp"]["y_offset"]) != 0):
y_offset = float(self._objects_on_table[object_id][1]["grasp"]["y_offset"]) / float(self._liatris_three_points_matrix.ret_height_of_screen())
radians = float(self._objects_on_table[object_id][2]["orientation_in_radians"])
#only the offset is rotated using x/y as the origin
pre_rotated_x = x_offset
pre_rotated_y = y_offset
rotMatrix = numpy.array([[numpy.cos(radians), -numpy.sin(radians)],
[numpy.sin(radians), numpy.cos(radians)]])
pre_rotated_matrix = [[pre_rotated_x],[pre_rotated_y]]
post_rotated_matrix = numpy.dot(rotMatrix,pre_rotated_matrix)
ret = [float(post_rotated_matrix[0][0]) + x,float(post_rotated_matrix[1][0])+y,self.height_of_table() + self._objects_on_table[object_id][1]["grasp"]["z_offset"],self._objects_on_table[object_id][1]["grasp"]["yaw"]]
return ret
def determine_object_rfid_pose(self,x,y,orientation_in_radians):
#offsets are based on the orientation of the object so calculate the correct center of object coordinates by using rotation matrix
x = float(x)
y = float(y)
#When an object is placed on the table we only know it's location and orientation. Not it's identity. Therefore all RFIDs must be on the same relative position of each object (relative to 3 points)
#in meters below but they'll be converted to %s
x_offset = 0.20
y_offset = 0.0
#The x & y coordinates are percentages of the screen width. The offsets are in meters so we need to covert these to percents as well (of screen).
if(x_offset != 0):
x_offset = float(x_offset) / float(self._liatris_three_points_matrix.ret_width_of_screen())
if(y_offset != 0):
y_offset = float(y_offset) / float(self._liatris_three_points_matrix.ret_height_of_screen())
radians = float(orientation_in_radians)
#only the offset is rotated using x/y as the origin
pre_rotated_x = x_offset
pre_rotated_y = y_offset
rotMatrix = numpy.array([[numpy.cos(radians), -numpy.sin(radians)],
[numpy.sin(radians), numpy.cos(radians)]])
pre_rotated_matrix = [[pre_rotated_x],[pre_rotated_y]]
post_rotated_matrix = numpy.dot(rotMatrix,pre_rotated_matrix)
return [float(post_rotated_matrix[0][0]) + x,float(post_rotated_matrix[1][0]) + y]
def pickup_object(self,object_id):
pickup_pose = self.determine_object_pickup_pose(object_id)
self._robot.gripper.open(True) #open left gripper
self.go_to_relative_position(float(pickup_pose[0]),float(pickup_pose[1]),False, True) #move left arm to pickup location
self._drop_left_arm_to_pickup_height(self._liatris_three_points_matrix.calc_relative_radians_angle(self._objects_on_table[object_id][2]["orientation_in_radians"]) + pickup_pose[3] ,pickup_pose[2])
self._robot.gripper.close(True)
self.scene.remove_world_object("object" + str(self._objects_on_table[object_id][0])) #remove object from rviz
self._objects_on_table[object_id][3] = 1 #mark object as deleted
self._move_left_arm_straight_up(self._default_height())
self._robot.ik.set_left(float(self._drop_off_cord_x),float(self._drop_off_cord_y),float(self._drop_off_cord_z)) #just above drop off point
self._robot.gripper.open(True)
def _objectapi(self):
self._last_object_type = self.get_json_from_server("objectapi/?objectapi_id=" + str(self._last_rfid_value()))
def _last_rfid_value(self):
rospy.sleep(2) #2 seconds to read RFID
rfid_json = self.get_json_from_server("rfidread/")
return rfid_json
def _go_to_position(self,_tmp_x,_tmp_y,x_per, force_right = False, force_left = False):
_prioritize_left_hand = True
if not force_left:
if force_right:
_prioritize_left_hand = False
if(x_per > 0.5):
_prioritize_left_hand = False
if _prioritize_left_hand:
if(not self._robot.ik.set_left(_tmp_x,_tmp_y,self._default_height())):
print "left failed trying right..."
if force_left:
print "right hand prohibited"
return False
if(not self._robot.ik.set_right(_tmp_x,_tmp_y,self._default_height())):
print "neither arm code reach position: x:" + str(_tmp_x) + " y: " + str(_tmp_y)
return False
else:
if(not self._robot.ik.set_right(_tmp_x,_tmp_y,self._default_height())):
print "right failed trying left..."
if force_right:
print "left hand prohibited"
return False
if(not self._robot.ik.set_left(_tmp_x,_tmp_y,self._default_height())):
print "neither arm code reach position: x:" + str(_tmp_x) + " y: " + str(_tmp_y)
return False
return True
def _default_height(self):
#height used to get the hands out of the way etc.
return float(self.height_of_table()) + float(0.6) #was 0.375
def move_right_arm_up_with_same_radians_and_xy(self,radians_for_rfid = 0):
#used to get RFID out of the way without rotating or moving (X/Y) the gripper
pose = self._robot.ik.get_pose('right')
if not self._robot.ik.set_right_rfid_down(float(pose.x),float(pose.y),self._default_height(),0,radians_for_rfid):
print "right failed to go up (move_right_arm_up_with_same_radians_and_xy)"
def _point_rfid_reader_down_on_right_arm(self,radians_for_rfid = 0):
#used to drop the RFID reader near the table height to read the RFID's value
pose = self._robot.ik.get_pose('right')
#to avoid colis turn prior to dropping
if not self._robot.ik.set_right_rfid_down(float(pose.x),float(pose.y),float(pose.z),0,radians_for_rfid):
print "failed to turn prior to going down"
if not self._robot.ik.set_right_rfid_down(float(pose.x),float(pose.y),self.height_of_table() + self._z_height_of_gripper + self._height_of_rfid_scanner,0,radians_for_rfid):
print "right failed to point rfid down at this radians... should try opposite here with correct offsets: " + str(radians_for_rfid)
if radians_for_rfid >= math.pi:
if self._robot.ik.set_right_rfid_down(float(pose.x),float(pose.y),self.height_of_table() + self._z_height_of_gripper + self._height_of_rfid_scanner,0,radians_for_rfid - math.pi):
print "opposite orientation success"
return True
print "opposite failed as well"
return False
#print "RFID is down"
return True
def _drop_left_arm_to_pickup_height(self, orient_radians, z_value_for_pickup):
#drops the left arm to the pickup height required to pickup the object (defined in the API)
pose = self._robot.ik.get_pose('left')
if not self._robot.ik.set_left_down_for_pickup(float(pose.x),float(pose.y),float(pose.z),0,orient_radians):
print "failed to turn prior to going down: " + str(orient_radians)
if not self._robot.ik.set_left_down_for_pickup(float(pose.x),float(pose.y),float(pose.z),0,orient_radians - math.pi):
print "failed to turn prior to going down opposite: " + str(orient_radians - math.pi)
if not self._robot.ik.set_left_down_for_pickup(float(pose.x),float(pose.y),z_value_for_pickup,0,orient_radians):
print "Left arm failed to drop"
if not self._robot.ik.set_left_down_for_pickup(float(pose.x),float(pose.y),z_value_for_pickup,0,orient_radians - math.pi):
print "Left arm failed to drop"
def _get_right_arm_out_of_the_way(self):
#primarily so the left arm won't have problems with the left arm
if not self._robot.ik.set_right(float(0.5),float(-0.5),self._default_height()):
print "couldnt go out of the way first"
def _move_left_arm_straight_up(self,z):
pose = self._robot.ik.get_pose('left')
if not self._robot.ik.set_left(float(pose.x),float(pose.y),z):
print "left couldnt go up"
def _point_arms_straight_down(self):
#End effectors must point down for calibration (so laser pointers are accurate)
time.sleep(2)
pose = self._robot.ik.get_pose('left')
if not self._robot.ik.set_left(float(pose.x),float(pose.y),self._default_height()):
print "left failed to point down at pose"
exit()
pose = self._robot.ik.get_pose('right')
if not self._robot.ik.set_right(float(pose.x),float(pose.y),self._default_height()):
print "right failed to point down at pose"
exit()
print "Ready for calibration."
class Liatris(object):
#THE FOLLOWING VALUES MAY NEED TO BE CHANGED ----------------------------------------
#CHANGE THIS TO YOUR SERVER'S DOMAIN
_server = "http://ec2-52-25-236-123.us-west-2.compute.amazonaws.com"
#All of the following units are meters
#Custom values to offset x & y coordinates in meters - this makes it easier to fix slightly inaccurate config files
_screen_x_offset = -0.003 #if your X coordinates are slightly off (RELATIVE TO SCREEN; NOT ROBOT!) tweak this
_screen_y_offset = -0.021 #if your Y coordinates are slightly off (RELATIVE TO SCREEN; NOT ROBOT!) tweak this
_height_of_rfid_scanner = 0.21 #What is the height of your RFID reader (only measure the distance below the gripper)
_z_height_of_gripper = 0.1 #Height of gripper. Change this if you aren't using the stock grippers
_height_of_table = -0.175 #Note: This is the relative height of the table compared to the bottom of Baxter (excluding the stand!). Mine is negative because the table is lower than than the base of baxter (regardless of the stand & wheels which are lower than the table but don't influence the math.).
#The following 3 values control where objects should be dropped off by the left arm
_drop_off_cord_x = 0.5
_drop_off_cord_y = 0.5
_drop_off_cord_z = 0.1
#Review function self.draw_table_in_rviz for more custom values related to the dimensions / pose of your table
#END THE FOLLOWING VALUES MAY NEED TO BE CHANGED ----------------------------------------
#You shouldn't need to modify any of the following defaults
_objects_on_table = []
_three_points_matrix = False
_ik = False
_tkinter = False
_move_left_arm = True
_move_arm_offset = 0.1
_last_move_to_file_x = False
_last_move_to_file_y = False
_currently_moving = False
_grippers = False
_ik = False
_right_arm_rotation = 0
_object_count = 0
_object_already_added_to_moveit = []
_last_object_type = False
_last_object_pose = False
_head = False
_colors = dict()
def __init__(self):
#All of the robot's logic is decoupled and found in ./robot/[nameofrobot]/
self._robot = Robot()
#enable move it
self.robot = moveit_commander.RobotCommander()
self._planningscene = PlanningScene()
self.scene = moveit_commander.PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
#if you don't sleep for a couple seconds you'll get errors that the scene or robot doesn't exist
print "Sleeping for 2 seconds to load MoveIt scene."
rospy.sleep(2)
#Add the table to MoveIt
self.draw_table_in_rviz()
#_liatris_three_points_matrix allows this script to transform any touchscreen's point to the robot's coordinate system.
self._liatris_three_points_matrix = Liatris_Three_Points_To_Rot_Matrix(
)
#check if threepoints.config exists. If no, prompt the user to calibrate the screen's pose.
self._require_configuration()
#note: if the config file doesn't exist yet the init() script will never get past this point. They'll create the config file and then restart the script to load it.
def get_json_from_server(self, script_url):
#download information from the API
url = self._server + "/" + script_url
response = urllib.urlopen(url)
#convert json to python datatype
return json.loads(response.read())
def _check_for_new_object_on_table(self):
#ask the server if a new object exists
data = self.get_json_from_server("touchread/")
#False = no valid 3 point pose found
if data == False:
return -1
self._last_object_pose = data
#_screen_x_offset & _screen_y_offset are manually set above and allow you to tweak an imperfect calibration
data["y"] = float(data["y"]) + float(
self._screen_y_offset /
self._liatris_three_points_matrix.ret_height_of_screen())
data["x"] = float(data["x"]) + float(
self._screen_x_offset /
self._liatris_three_points_matrix.ret_width_of_screen())
pose = [data["x"], data["y"]]
#Was this pose already imported? If yes, ignore it.
if pose in self._object_already_added_to_moveit:
print "check for new object: already imported"
else:
#prevent duplicates by adding pose to already completed list
self._object_already_added_to_moveit.append(pose)
#Step 1: read the RFID
#determine the relative pose of the RFID.
rfid_pose = self.determine_object_rfid_pose(
data["x"], data["y"], data["orientation_in_radians"])
#go to pose (but way above table)
self.go_to_relative_position(float(rfid_pose[0]),
float(rfid_pose[1]), True)
#turn to the right orientation and drop to RFID reading height
self._point_rfid_reader_down_on_right_arm(
self._liatris_three_points_matrix.calc_relative_radians_angle(
data["orientation_in_radians"]))
#Detect everything there is to know about the object from the API
self._objectapi()
_ret = -1
if self._last_object_type == "does not exist":
print "RFID returned false"
else:
print "Adding a new " + str(
self._last_object_type["nickname"]) + " to RVIZ"
#Draw the object in RVIZ
_ret = self.draw_collision_object_in_rviz()
#Move the right arm out of the way (2 steps)
#First move it up without rotation to avoid collision with object
self.move_right_arm_up_with_same_radians_and_xy(
self._liatris_three_points_matrix.calc_relative_radians_angle(
data["orientation_in_radians"]))
#Now that we're above the table get the arm out of the way
self._get_right_arm_out_of_the_way()
return _ret
return -1
def _require_configuration(self):
# If a config file (that tells transformation of touch screeen's vs robot's poses) already exists skip the configuration step
if not self._liatris_three_points_matrix.does_config_file_exist():
#TkInter is just an easy way to capture key stroaks in python.
self._point_arms_straight_down()
self._tkinter = tk.Tk()
#Size doesn't matter
self._tkinter.geometry('300x200')
#On each keypress call _onKeypress function
self._tkinter.bind('<KeyPress>', self._onKeypress)
#instructions on how to configure robot
print "INSTRUCTIONS FOR CALIBRATION"
print "Opening TkInter window to capture key strokes. Make sure it stays in focus."
print "Moving left arm first. Set to orientation 0,0 (relative to the robot, bottom, left corner of screen) on screen."
print "Move arm with W,A,S,D like an old school video game."
print "Movement Speed can be adjusted with the following keys. T: Fast, G: Medium, B: Slow, Y: Slowest"
print "To save a point. Press M."
self._tkinter.mainloop()
def draw_table_in_rviz(self):
#draw the table in rviz. You'll likely need to tweak the following values for your table.
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0.28 + (
0.72 / 2
) #0.28 is offset from center of robot to the front of the table. 0.72/2 is the distance to the center of the table
p.pose.position.y = 0
p.pose.position.z = self.height_of_table(
) #is the Z value of the table (not relative to the robot like self.height_of_table returns)
quaternion = tf.transformations.quaternion_from_euler(
0, 0, float(math.pi / 2)) #turn table 90 degrees
p.pose.orientation.x = quaternion[0]
p.pose.orientation.y = quaternion[1]
p.pose.orientation.z = quaternion[2]
p.pose.orientation.w = quaternion[3]
self.set_object_color("table", 1.71, 0.79, 0.22)
self.scene.add_mesh("table", p, "stl/table.stl")
def set_object_color(self, id, r, g, b):
#defines the color of an object in MoveIt
color = ObjectColor()
color.id = str(id)
color.color.r = float(r)
color.color.g = float(g)
color.color.b = float(b)
color.color.a = 1.0
self._colors[str(id)] = color
self._planningscene.is_diff = True
for color in self._colors.values():
self._planningscene.object_colors.append(color)
self.scene_pub.publish(self._planningscene)
def height_of_table(self):
return self._height_of_table
def _onKeypress(self, event):
#Used for calibration only
_c = event.char
#when calibrating it's convinient to be able to change the distance each arm moves at any given time (t being the greatest and y being the smallest)
if _c == 't':
self._move_arm_offset = 0.1
if _c == 'g':
self._move_arm_offset = 0.03
if _c == 'b':
self._move_arm_offset = 0.01
if _c == 'y':
self._move_arm_offset = 0.003
#m saves a point
if _c == 'm':
self.save_point()
#movement of either are is done with WASD like a video game
if (not (_c == 'w' or _c == 'a' or _c == 's' or _c == 'd')):
return False
if (self._move_left_arm):
pose = self._robot.ik.get_pose('left')
else:
pose = self._robot.ik.get_pose('right')
y = pose.y
x = pose.x
if _c == 'a':
y = y + self._move_arm_offset
if _c == 'd':
y = y - self._move_arm_offset
if _c == 'w':
x = x + self._move_arm_offset
if _c == 's':
x = x - self._move_arm_offset
print "moving... (wait for success prior to pressing another key)"
#prohibit multiple _ik requests simul
if (self._currently_moving):
return True
self._currently_moving = True #prevent invalid key strokes
offset_of_gripper_above_table = 0.02
if (self._move_left_arm):
if not self._robot.ik.set_left(
float(x), float(y),
float(self.height_of_table()) +
float(self._z_height_of_gripper) +
float(offset_of_gripper_above_table)):
print "left failed to point down at pose"
else:
print "success left arm"
else:
if not self._robot.ik.set_right(
float(x), float(y),
float(self.height_of_table()) +
float(self._z_height_of_gripper) +
float(offset_of_gripper_above_table)):
print "right failed to point down at pose"
else:
print "success right arm"
self._currently_moving = False
def save_point(self):
#save the current pose when calibrating
if self._move_left_arm:
self._move_left_arm = False
_pos = self._robot.ik.get_pose('left')
print "0,0 (bottom, left) saved"
print "Move the right arm to 1,0 (bottom,right) and press m to save that point."
self._liatris_three_points_matrix.add_cord(_pos.x, _pos.y)
else:
_pos = self._robot.ik.get_pose('right')
print "saved"
if self._liatris_three_points_matrix.count_cords() == 1:
print "Finally move the right arm to 1,1 (top,right) and press m to complete the calibration."
self._liatris_three_points_matrix.add_cord(_pos.x, _pos.y)
def go_to_relative_position(self,
x_per,
y_per,
force_right=False,
force_left=False):
#This function is required because the screen only knows the coordinates of the object relative to the screen. Not relative to the robot. This converts from the screen percentages to a robot pose.
pos = self._liatris_three_points_matrix.determine_a_relative_point(
x_per, y_per)
return self._go_to_position(pos[0], pos[1], x_per, force_right,
force_left)
def determine_center_of_object(self):
#This function is required for adding objects to RVIZ. In practice it isn't convinient to add the 3 points directly in the middle of each object. x_offset & y_offset allow you to add it any location and then offset (via the API) the location for the collision box & stl.
#offsets are based on the orientation of the object so calculate the correct center of object coordinates by using rotation matrix
x = float(self._last_object_pose["x"])
y = float(self._last_object_pose["y"])
#The x & y coordinates are percentages of the screen width. The offsets are in meters so we need to covert these to percents as well (of screen).
x_offset = 0
if (float(self._last_object_type["transformation"]["x_offset"]) != 0):
x_offset = float(
self._last_object_type["transformation"]["x_offset"]) / float(
self._liatris_three_points_matrix.ret_width_of_screen())
y_offset = 0
if (float(self._last_object_type["transformation"]["y_offset"]) != 0):
y_offset = float(
self._last_object_type["transformation"]["y_offset"]) / float(
self._liatris_three_points_matrix.ret_height_of_screen())
radians = float(self._last_object_pose["orientation_in_radians"])
#only the offset is rotated using x/y as the origin
pre_rotated_x = x_offset
pre_rotated_y = y_offset
rotMatrix = numpy.array([[numpy.cos(radians), -numpy.sin(radians)],
[numpy.sin(radians),
numpy.cos(radians)]])
pre_rotated_matrix = [[pre_rotated_x], [pre_rotated_y]]
post_rotated_matrix = numpy.dot(rotMatrix, pre_rotated_matrix)
return [
float(post_rotated_matrix[0][0]) + x,
float(post_rotated_matrix[1][0]) + y
]
def draw_collision_object_in_rviz(self):
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.z = self.height_of_table() + (
float(self._last_object_type["size"]["h"]) / 2)
#determine point relative to robot's pose
coordinates_of_last_object_with_offset = self.determine_center_of_object(
)
tmp_pos = self._liatris_three_points_matrix.determine_a_relative_point(
float(coordinates_of_last_object_with_offset[0]),
float(coordinates_of_last_object_with_offset[1]))
p.pose.position.x = tmp_pos[0]
p.pose.position.y = tmp_pos[1]
quaternion = tf.transformations.quaternion_from_euler(
0, 0, float(self._last_object_pose["orientation_in_radians"]))
p.pose.orientation.x = quaternion[0]
p.pose.orientation.y = quaternion[1]
p.pose.orientation.z = quaternion[2]
p.pose.orientation.w = quaternion[3]
#add this to an array of objects
self._objects_on_table.append([
self._object_count, self._last_object_type, self._last_object_pose,
0
]) #final ,0 means that the object hasn't been removed from the table
#download stl file if it doesn't exist yet (from API server); in practice we'd expect millions of different STL files to be on a CDN and constantly introducing new files so can't rely on local storage.
self.download_stl_file_from_api_if_its_not_local(
str(self._last_object_type["stl_file"]))
#add the stl file to the scene as a collision object
self.set_object_color("object" + str(self._object_count),
self._last_object_type["color"]["r"],
self._last_object_type["color"]["g"],
self._last_object_type["color"]["b"])
self.scene.add_mesh(
"object" + str(self._object_count), p,
"stl/" + str(self._last_object_type["stl_file"]) + ".stl")
#If you'd prefer to use a box instead of an STL file use the following
#self.scene.add_box("object" + str(self._object_count),p,(float(self._last_object_type["size"]["l"]), float(self._last_object_type["size"]["w"]), float(self._last_object_type["size"]["h"]))) #0.72 ... is the size of the object
self._object_count = self._object_count + 1
return self._object_count - 1
def download_stl_file_from_api_if_its_not_local(self, stl_file):
#sync STL files to local if they don't exist
if os.path.isfile("stl/" + str(stl_file) + ".stl"):
return True
conn = httplib.HTTPConnection(self._server[7:],
80) #[7: removes http://
conn.request('GET', "/stl/" + str(stl_file) + ".stl")
resp = conn.getresponse()
data = resp.read()
with open("stl/" + str(stl_file) + ".stl", "wb") as f:
f.write(data)
conn.close()
def determine_object_pickup_pose(self, object_id):
#offsets are based on the orientation of the object so calculate the correct center of object coordinates by using rotation matrix
x = float(self._objects_on_table[object_id][2]["x"])
y = float(self._objects_on_table[object_id][2]["y"])
#The x & y coordinates are percentages of the screen width. The offsets are in meters so we need to covert these to percents as well (of screen).
x_offset = 0
if (float(self._objects_on_table[object_id][1]["grasp"]["x_offset"]) !=
0):
x_offset = float(
self._objects_on_table[object_id][1]["grasp"]["x_offset"]
) / float(self._liatris_three_points_matrix.ret_width_of_screen())
y_offset = 0
if (float(self._objects_on_table[object_id][1]["grasp"]["y_offset"]) !=
0):
y_offset = float(
self._objects_on_table[object_id][1]["grasp"]["y_offset"]
) / float(self._liatris_three_points_matrix.ret_height_of_screen())
radians = float(
self._objects_on_table[object_id][2]["orientation_in_radians"])
#only the offset is rotated using x/y as the origin
pre_rotated_x = x_offset
pre_rotated_y = y_offset
rotMatrix = numpy.array([[numpy.cos(radians), -numpy.sin(radians)],
[numpy.sin(radians),
numpy.cos(radians)]])
pre_rotated_matrix = [[pre_rotated_x], [pre_rotated_y]]
post_rotated_matrix = numpy.dot(rotMatrix, pre_rotated_matrix)
ret = [
float(post_rotated_matrix[0][0]) + x,
float(post_rotated_matrix[1][0]) + y,
self.height_of_table() +
self._objects_on_table[object_id][1]["grasp"]["z_offset"],
self._objects_on_table[object_id][1]["grasp"]["yaw"]
]
return ret
def determine_object_rfid_pose(self, x, y, orientation_in_radians):
#offsets are based on the orientation of the object so calculate the correct center of object coordinates by using rotation matrix
x = float(x)
y = float(y)
#When an object is placed on the table we only know it's location and orientation. Not it's identity. Therefore all RFIDs must be on the same relative position of each object (relative to 3 points)
#in meters below but they'll be converted to %s
x_offset = 0.20
y_offset = 0.0
#The x & y coordinates are percentages of the screen width. The offsets are in meters so we need to covert these to percents as well (of screen).
if (x_offset != 0):
x_offset = float(x_offset) / float(
self._liatris_three_points_matrix.ret_width_of_screen())
if (y_offset != 0):
y_offset = float(y_offset) / float(
self._liatris_three_points_matrix.ret_height_of_screen())
radians = float(orientation_in_radians)
#only the offset is rotated using x/y as the origin
pre_rotated_x = x_offset
pre_rotated_y = y_offset
rotMatrix = numpy.array([[numpy.cos(radians), -numpy.sin(radians)],
[numpy.sin(radians),
numpy.cos(radians)]])
pre_rotated_matrix = [[pre_rotated_x], [pre_rotated_y]]
post_rotated_matrix = numpy.dot(rotMatrix, pre_rotated_matrix)
return [
float(post_rotated_matrix[0][0]) + x,
float(post_rotated_matrix[1][0]) + y
]
def pickup_object(self, object_id):
pickup_pose = self.determine_object_pickup_pose(object_id)
self._robot.gripper.open(True) #open left gripper
self.go_to_relative_position(float(pickup_pose[0]),
float(pickup_pose[1]), False,
True) #move left arm to pickup location
self._drop_left_arm_to_pickup_height(
self._liatris_three_points_matrix.calc_relative_radians_angle(
self._objects_on_table[object_id][2]["orientation_in_radians"])
+ pickup_pose[3], pickup_pose[2])
self._robot.gripper.close(True)
self.scene.remove_world_object("object" + str(
self._objects_on_table[object_id][0])) #remove object from rviz
self._objects_on_table[object_id][3] = 1 #mark object as deleted
self._move_left_arm_straight_up(self._default_height())
self._robot.ik.set_left(
float(self._drop_off_cord_x), float(self._drop_off_cord_y),
float(self._drop_off_cord_z)) #just above drop off point
self._robot.gripper.open(True)
def _objectapi(self):
self._last_object_type = self.get_json_from_server(
"objectapi/?objectapi_id=" + str(self._last_rfid_value()))
def _last_rfid_value(self):
rospy.sleep(2) #2 seconds to read RFID
rfid_json = self.get_json_from_server("rfidread/")
return rfid_json
def _go_to_position(self,
_tmp_x,
_tmp_y,
x_per,
force_right=False,
force_left=False):
_prioritize_left_hand = True
if not force_left:
if force_right:
_prioritize_left_hand = False
if (x_per > 0.5):
_prioritize_left_hand = False
if _prioritize_left_hand:
if (not self._robot.ik.set_left(_tmp_x, _tmp_y,
self._default_height())):
print "left failed trying right..."
if force_left:
print "right hand prohibited"
return False
if (not self._robot.ik.set_right(_tmp_x, _tmp_y,
self._default_height())):
print "neither arm code reach position: x:" + str(
_tmp_x) + " y: " + str(_tmp_y)
return False
else:
if (not self._robot.ik.set_right(_tmp_x, _tmp_y,
self._default_height())):
print "right failed trying left..."
if force_right:
print "left hand prohibited"
return False
if (not self._robot.ik.set_left(_tmp_x, _tmp_y,
self._default_height())):
print "neither arm code reach position: x:" + str(
_tmp_x) + " y: " + str(_tmp_y)
return False
return True
def _default_height(self):
#height used to get the hands out of the way etc.
return float(self.height_of_table()) + float(0.6) #was 0.375
def move_right_arm_up_with_same_radians_and_xy(self, radians_for_rfid=0):
#used to get RFID out of the way without rotating or moving (X/Y) the gripper
pose = self._robot.ik.get_pose('right')
if not self._robot.ik.set_right_rfid_down(float(pose.x), float(
pose.y), self._default_height(), 0, radians_for_rfid):
print "right failed to go up (move_right_arm_up_with_same_radians_and_xy)"
def _point_rfid_reader_down_on_right_arm(self, radians_for_rfid=0):
#used to drop the RFID reader near the table height to read the RFID's value
pose = self._robot.ik.get_pose('right')
#to avoid colis turn prior to dropping
if not self._robot.ik.set_right_rfid_down(float(pose.x), float(
pose.y), float(pose.z), 0, radians_for_rfid):
print "failed to turn prior to going down"
if not self._robot.ik.set_right_rfid_down(
float(pose.x), float(pose.y),
self.height_of_table() + self._z_height_of_gripper +
self._height_of_rfid_scanner, 0, radians_for_rfid):
print "right failed to point rfid down at this radians... should try opposite here with correct offsets: " + str(
radians_for_rfid)
if radians_for_rfid >= math.pi:
if self._robot.ik.set_right_rfid_down(
float(pose.x), float(pose.y),
self.height_of_table() + self._z_height_of_gripper +
self._height_of_rfid_scanner, 0,
radians_for_rfid - math.pi):
print "opposite orientation success"
return True
print "opposite failed as well"
return False
#print "RFID is down"
return True
def _drop_left_arm_to_pickup_height(self, orient_radians,
z_value_for_pickup):
#drops the left arm to the pickup height required to pickup the object (defined in the API)
pose = self._robot.ik.get_pose('left')
if not self._robot.ik.set_left_down_for_pickup(float(
pose.x), float(pose.y), float(pose.z), 0, orient_radians):
print "failed to turn prior to going down: " + str(orient_radians)
if not self._robot.ik.set_left_down_for_pickup(
float(pose.x), float(pose.y), float(pose.z), 0,
orient_radians - math.pi):
print "failed to turn prior to going down opposite: " + str(
orient_radians - math.pi)
if not self._robot.ik.set_left_down_for_pickup(float(
pose.x), float(pose.y), z_value_for_pickup, 0, orient_radians):
print "Left arm failed to drop"
if not self._robot.ik.set_left_down_for_pickup(
float(pose.x), float(pose.y), z_value_for_pickup, 0,
orient_radians - math.pi):
print "Left arm failed to drop"
def _get_right_arm_out_of_the_way(self):
#primarily so the left arm won't have problems with the left arm
if not self._robot.ik.set_right(float(0.5), float(-0.5),
self._default_height()):
print "couldnt go out of the way first"
def _move_left_arm_straight_up(self, z):
pose = self._robot.ik.get_pose('left')
if not self._robot.ik.set_left(float(pose.x), float(pose.y), z):
print "left couldnt go up"
def _point_arms_straight_down(self):
#End effectors must point down for calibration (so laser pointers are accurate)
time.sleep(2)
pose = self._robot.ik.get_pose('left')
if not self._robot.ik.set_left(float(pose.x), float(pose.y),
self._default_height()):
print "left failed to point down at pose"
exit()
pose = self._robot.ik.get_pose('right')
if not self._robot.ik.set_right(float(pose.x), float(pose.y),
self._default_height()):
print "right failed to point down at pose"
exit()
print "Ready for calibration."
