from fairino import Robot
import time
# Term index at bottom
#############################################
# Point table; {name: joint_pos} dictionary #
#############################################
points = {
'home': [0, -90, 90, -90, -90, 0],
'pick_pt_hover': [-50.469, -61.425, 97.799, -126.37, -90, -50.469],
'place_pt_hover': [38.045, -48.84, 76.11, -117.27, -90, 38.045]
}
def pick_obj(robot: Robot.RPC):
robot.MoveJ(points['home'], tool=0, user=0, vel=20, acc=20) # Move the robot home first to avoid unplanned trajectories
robot.MoveJ(points['pick_pt_hover'], tool=0, user=0,vel=20, acc=20) # Move robot to hover point of target pick position
e, curr = robot.GetActualTCPPose() # Gets the current CARTESIAN POSITION of the robot in format (error, [x,y,z,rx,ry,rz])
curr[2] -= 50 # subtract 50 from the z of the current position, this will be our target
robot.MoveGripper(1, 1000, 50, 50, 5000, 0) # Open gripper_1 to 100.0% at 50% speed, 50% force, with a timeout of 5 seconds
time.sleep(2)
robot.MoveL(curr, tool=0, user=0,vel=20, acc=20) # Move 50mm DOWN to target obj to grab
robot.MoveGripper(1, 0, 50, 50, 5000, 0) # Close gripper_1 to 0.0% at 50% speed, 50% force, with a timeout of 5 seconds
time.sleep(2)
e, hover_as_cart = robot.GetForwardKin(points['pick_pt_hover']) # Gets the CARTESIAN coordinates of the hover position we came from
robot.MoveL(hover_as_cart, tool=0, user=0, vel=20, acc=20) # Move LINEARLY back to hover point
return
def place_obj(robot: Robot.RPC):
robot.MoveJ(points['home'], tool=0, user=0,vel=20, acc=20) # Move the robot home first to avoid unplanned trajectories
robot.MoveJ(points['place_pt_hover'], tool=0, user=0,vel=20, acc=20) # Move robot to hover point of target drop position
e, curr = robot.GetActualTCPPose() # Gets the current CARTESIAN POSITION of the robot in format (error, [x,y,z,rx,ry,rz])
curr[2] -= 50 # subtract 50 from the z of the current position. This will be manipulated to become our target
robot.MoveL(curr, tool=0, user=0,vel=20, acc=20) # Move 50mm DOWN to target obj to grab
robot.MoveGripper(1, 1000, 50, 50, 5000, 0) # Open gripper_1 to 100.0% at 50% speed, 50% force, with a timeout of 5 seconds
time.sleep(2)
e, hover_as_cart = robot.GetForwardKin(points['place_pt_hover']) # Gets the CARTESIAN coordinates from the joint values of the hover position we came from
robot.MoveL(hover_as_cart, tool=0, user=0,vel=20, acc=20) # Move LINEARLY back to hover point
return
def go_home(robot: Robot.RPC):
error = robot.MoveJ(points['home'], tool=0, user=0,vel=20, acc=20) # Move back home
return error
def main():
robot = Robot.RPC('192.168.58.2') # Replace IP with your robot IP
robot.ActGripper(1, 1) # Activate the gripper connected at the end of arm
pick_obj(robot) # Runs routine to grab the object
go_home(robot) # Return home to avoid any collisions before next routine
place_obj(robot) # Runs the drop routine
go_home(robot) # return home
if __name__ == "__main__":
main()
"""
Term Index:
- Hover point: a location that sets the robot slightly above target position. This is meant to
avoid collisions with the target when moving and to ensure accurate and controlled approach.
"""