def main(): base_frame = (0, 0, 0, 0, 0, 0) work_frame = (400, 0, 300, 180, 0, 180 ) # base frame: x->front, y->right, z->up with AsyncRobot(SyncRobot(FrankxController(ip='172.16.0.2'))) as robot_1, \ AsyncRobot(SyncRobot(FrankxController(ip='172.16.1.2'))) as robot_2: robots = [robot_1, robot_2] # Set robot axes and TCP for i, robot in enumerate(robots): robot.axes = 'sxyz' # static/extrinsic frame xyz convention robot.tcp = (0, 0, 75, 0, 0, -135) # Set Franka-specific robot parameters robot.sync_robot.controller.rel_velocity = 0.3 robot.sync_robot.controller.rel_accel = 0.2 robot.sync_robot.controller.rel_jerk = 0.1 # Display robot info print("Robot {} info: {}".format(i + 1, robot.info)) # Initialize joint angles print("Initializing joint angles ...") for robot in robots: robot.move_joints((0, -35, 0, -150, 0, 115, -45)) # Move to origin of work frame print("Moving to origin of work frame ...") for robot in robots: robot.coord_frame = work_frame robot.move_linear((0, 0, 0, 0, 0, 0)) # Increase and decrease all joint angles print("Increasing and decreasing all joint angles ...") for robot in robots: robot.move_joints(robot.joint_angles + (10, ) * 7) robot.move_joints(robot.joint_angles - (10, ) * 7) # Move backward and forward print("Moving backward and forward ...") for robot in robots: robot.move_linear((50, 0, 0, 0, 0, 0)) robot.move_linear((0, 0, 0, 0, 0, 0)) # Move right and left print("Moving right and left ...") for robot in robots: robot.move_linear((0, 50, 0, 0, 0, 0)) robot.move_linear((0, 0, 0, 0, 0, 0)) # Move down and up print("Moving down and up ...") for robot in robots: robot.move_linear((0, 0, 50, 0, 0, 0)) robot.move_linear((0, 0, 0, 0, 0, 0)) # Roll right and left print("Rolling right and left ...") for robot in robots: robot.move_linear((0, 0, 0, 30, 0, 0)) robot.move_linear((0, 0, 0, 0, 0, 0)) # Roll forward and backward print("Rolling forward and backward ...") for robot in robots: robot.move_linear((0, 0, 0, 0, 30, 0)) robot.move_linear((0, 0, 0, 0, 0, 0)) # Turn clockwise and anticlockwise around work frame z-axis print( "Turning clockwise and anticlockwise around work frame z-axis ...") for robot in robots: robot.move_linear((0, 0, 0, 0, 0, 30)) robot.move_linear((0, 0, 0, 0, 0, 0)) print("Moving to offset pose and making tap move up and down ...") for robot in robots: robot.move_linear((50, 50, 50, 30, 30, 30)) robot.coord_frame = base_frame robot.coord_frame = robot.target_pose robot.move_linear((0, 0, 50, 0, 0, 0)) robot.move_linear((0, 0, 0, 0, 0, 0)) print("Moving to origin of work frame ...") for robot in robots: robot.coord_frame = work_frame robot.move_linear((0, 0, 0, 0, 0, 0)) # Pause before commencing asynchronous tests print("Waiting for 5 secs ...") time.sleep(5) print("Repeating test sequence for asynchronous moves ...") # Increase and decrease all joint angles (async) print("Increasing and decreasing all joint angles ...") for robot in robots: robot.async_move_joints(robot.joint_angles + (10, ) * 7) for robot in robots: robot.async_result() for robot in robots: robot.async_move_joints(robot.joint_angles - (10, ) * 7) for robot in robots: robot.async_result() # Move backward and forward (async) print("Moving backward and forward (async) ...") for robot in robots: robot.async_move_linear((50, 0, 0, 0, 0, 0)) for robot in robots: robot.async_result() for robot in robots: robot.async_move_linear((0, 0, 0, 0, 0, 0)) for robot in robots: robot.async_result() # Move right and left print("Moving right and left (async) ...") for robot in robots: robot.async_move_linear((0, 50, 0, 0, 0, 0)) for robot in robots: robot.async_result() for robot in robots: robot.async_move_linear((0, 0, 0, 0, 0, 0)) for robot in robots: robot.async_result() # Move down and up (async) print("Moving down and up (async) ...") for robot in robots: robot.async_move_linear((0, 0, 50, 0, 0, 0)) for robot in robots: robot.async_result() for robot in robots: robot.async_move_linear((0, 0, 0, 0, 0, 0)) for robot in robots: robot.async_result() # Roll right and left (async) print("Rolling right and left (async) ...") for robot in robots: robot.async_move_linear((0, 0, 0, 30, 0, 0)) for robot in robots: robot.async_result() for robot in robots: robot.async_move_linear((0, 0, 0, 0, 0, 0)) for robot in robots: robot.async_result() # Roll forward and backward (async) print("Rolling forward and backward (async) ...") for robot in robots: robot.async_move_linear((0, 0, 0, 0, 30, 0)) for robot in robots: robot.async_result() for robot in robots: robot.async_move_linear((0, 0, 0, 0, 0, 0)) for robot in robots: robot.async_result() # Turn clockwise and anticlockwise around work frame z-axis (async) print( "Turning clockwise and anticlockwise around work frame z-axis (async) ..." ) for robot in robots: robot.async_move_linear((0, 0, 0, 0, 0, 30)) for robot in robots: robot.async_result() for robot in robots: robot.async_move_linear((0, 0, 0, 0, 0, 0)) for robot in robots: robot.async_result() # Move to offset pose then tap down and up in sensor frame (async) print( "Moving to offset pose and making tap move up and down (async) ..." ) for robot in robots: robot.async_move_linear((50, 50, 50, 30, 30, 30)) for robot in robots: robot.async_result() for robot in robots: robot.coord_frame = base_frame robot.coord_frame = robot.target_pose robot.async_move_linear((0, 0, 50, 0, 0, 0)) for robot in robots: robot.async_result() for robot in robots: robot.async_move_linear((0, 0, 0, 0, 0, 0)) for robot in robots: robot.async_result() print("Moving to origin of work frame ...") for robot in robots: robot.coord_frame = work_frame robot.async_move_linear((0, 0, 0, 0, 0, 0)) for robot in robots: robot.async_result()
def main(): base_frame = (0, 0, 0, 0, 0, 0) work_frame = (400, 0, 300, 180, 0, 180 ) # base frame: x->front, y->right, z->up with AsyncRobot(SyncRobot(FrankxController(ip='172.16.0.2'))) as robot: # Set robot axes and TCP robot.axes = 'sxyz' # static/extrinsic frame xyz convention robot.tcp = (0, 0, 75, 0, 0, -135) # Set Franka-specific robot parameters robot.sync_robot.controller.rel_velocity = 0.3 robot.sync_robot.controller.rel_accel = 0.2 robot.sync_robot.controller.rel_jerk = 0.1 # Display robot info print("Robot info: {}".format(robot.info)) # Initialize all 7 joints and display joint angles and pose robot.move_joints((0, -35, 0, -150, 0, 115, -45)) print("Initial joint angles: {}".format(robot.joint_angles)) print("Initial pose in base frame: {}".format(robot.pose)) robot.coord_frame = work_frame print("Initial pose in work frame: {}".format(robot.pose)) # Move to origin of work frame print("Moving to origin of work frame ...") robot.move_linear((0, 0, 0, 0, 0, 0)) print("Pose at origin of work frame: {}".format(robot.pose)) # Increase and decrease all joint angles print("Increasing and decreasing all joint angles ...") robot.move_joints(robot.joint_angles + (10, ) * 7) print("Target joint angles after increase: {}".format( robot.target_joint_angles)) print("Joint angles after increase: {}".format(robot.joint_angles)) robot.move_joints(robot.joint_angles - (10, ) * 7) print("Target joint angles after decrease: {}".format( robot.target_joint_angles)) print("Joint angles after decrease: {}".format(robot.joint_angles)) # Move backward and forward print("Moving backward and forward ...") robot.move_linear((50, 0, 0, 0, 0, 0)) print("Pose in work frame: {}".format(robot.pose)) robot.move_linear((0, 0, 0, 0, 0, 0)) # Move right and left print("Moving right and left ...") robot.move_linear((0, 50, 0, 0, 0, 0)) print("Pose in work frame: {}".format(robot.pose)) robot.move_linear((0, 0, 0, 0, 0, 0)) # Move down and up print("Moving down and up ...") robot.move_linear((0, 0, 50, 0, 0, 0)) print("Pose in work frame: {}".format(robot.pose)) robot.move_linear((0, 0, 0, 0, 0, 0)) # Roll right and left print("Rolling right and left ...") robot.move_linear((0, 0, 0, 30, 0, 0)) print("Pose in work frame: {}".format(robot.pose)) robot.move_linear((0, 0, 0, 0, 0, 0)) # Roll forward and backward print("Rolling forward and backward ...") robot.move_linear((0, 0, 0, 0, 30, 0)) print("Pose in work frame: {}".format(robot.pose)) robot.move_linear((0, 0, 0, 0, 0, 0)) # Turn clockwise and anticlockwise around work frame z-axis print( "Turning clockwise and anticlockwise around work frame z-axis ...") robot.move_linear((0, 0, 0, 0, 0, 30)) print("Pose in work frame: {}".format(robot.pose)) robot.move_linear((0, 0, 0, 0, 0, 0)) # Move to offset pose then tap down and up in sensor frame print("Moving to 50 mm / 30 deg offset in pose ...") robot.move_linear((50, 50, 50, 30, 30, 30)) print("Target pose after offset move: {}".format(robot.target_pose)) print("Pose after offset move: {}".format(robot.pose)) print("Tapping down and up ...") robot.coord_frame = base_frame robot.coord_frame = robot.target_pose robot.move_linear((0, 0, 50, 0, 0, 0)) robot.move_linear((0, 0, 0, 0, 0, 0)) robot.coord_frame = work_frame print("Moving to origin of work frame ...") robot.move_linear((0, 0, 0, 0, 0, 0)) print("Pose at origin of work frame: {}".format(robot.pose)) # Pause before commencing asynchronous tests print("Waiting for 5 secs ...") time.sleep(5) print("Repeating test sequence for asynchronous moves ...") # Increase and decrease all joint angles (async) print("Increasing and decreasing all joint angles ...") robot.async_move_joints(robot.joint_angles + (10, ) * 7) print("Getting on with something else while command completes ...") robot.async_result() print("Target joint angles after increase: {}".format( robot.target_joint_angles)) print("Joint angles after increase: {}".format(robot.joint_angles)) robot.async_move_joints(robot.joint_angles - (10, ) * 7) print("Getting on with something else while command completes ...") robot.async_result() print("Target joint angles after decrease: {}".format( robot.target_joint_angles)) print("Joint angles after decrease: {}".format(robot.joint_angles)) # Move backward and forward (async) print("Moving backward and forward (async) ...") robot.async_move_linear((50, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() robot.async_move_linear((0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() # Move right and left print("Moving right and left (async) ...") robot.async_move_linear((0, 50, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() robot.async_move_linear((0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() # Move down and up (async) print("Moving down and up (async) ...") robot.async_move_linear((0, 0, 50, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() robot.async_move_linear((0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() # Roll right and left (async) print("Rolling right and left (async) ...") robot.async_move_linear((0, 0, 0, 30, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() robot.async_move_linear((0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() # Roll forward and backward (async) print("Rolling forward and backward (async) ...") robot.async_move_linear((0, 0, 0, 0, 30, 0)) print("Getting on with something else while command completes ...") robot.async_result() robot.async_move_linear((0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() # Turn clockwise and anticlockwise around work frame z-axis (async) print( "Turning clockwise and anticlockwise around work frame z-axis (async) ..." ) robot.async_move_linear((0, 0, 0, 0, 0, 30)) print("Getting on with something else while command completes ...") robot.async_result() robot.async_move_linear((0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() # Move to offset pose then tap down and up in sensor frame (async) print( "Moving to 50 mm / 30 deg offset in all pose dimensions (async) ..." ) robot.async_move_linear((50, 50, 50, 30, 30, 30)) print("Getting on with something else while command completes ...") robot.async_result() print("Target pose after offset move: {}".format(robot.target_pose)) print("Pose after offset move: {}".format(robot.pose)) print("Tapping down and up (async) ...") robot.coord_frame = base_frame robot.coord_frame = robot.target_pose robot.async_move_linear((0, 0, 50, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() robot.async_move_linear((0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() robot.coord_frame = work_frame print("Moving to origin of work frame ...") robot.async_move_linear((0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() print("Final target pose in work frame: {}".format(robot.target_pose)) print("Final pose in work frame: {}".format(robot.pose))
def make_robot(): return AsyncRobot(SyncRobot( RTDEController(ip='127.0.0.1'))) # local host sim
def main(): base_frame = (0, 0, 0, 0, 0, 0) work_frame = (109.1, -487.0, 341.3, 180, 0, -90 ) # base frame: x->right, y->back, z->up # work_frame = (487.0, -109.1, 341.3, 180, 0, 180) # base frame: x->front, y->right, z->up with AsyncRobot(SyncRobot(RTDEController(ip='192.11.72.10'))) as robot: # For testing in URSim simulator # with AsyncRobot(SyncRobot(RTDEController(ip='127.0.0.1'))) as robot: # Set TCP, linear speed, angular speed and coordinate frame robot.tcp = (0, 0, 89.1, 0, 0, 0) robot.linear_speed = 100 robot.angular_speed = 10 robot.coord_frame = work_frame # Display robot info print("Robot info: {}".format(robot.info)) # Display initial joint angles print("Initial joint angles: {}".format(robot.joint_angles)) # Display initial pose in work frame print("Initial pose in work frame: {}".format(robot.pose)) # Move to origin of work frame print("Moving to origin of work frame ...") robot.move_linear((0, 0, 0, 0, 0, 0)) # Increase and decrease all joint angles print("Increasing and decreasing all joint angles ...") robot.move_joints(robot.joint_angles + (10, ) * 6) print("Target joint angles after increase: {}".format( robot.target_joint_angles)) print("Joint angles after increase: {}".format(robot.joint_angles)) robot.move_joints(robot.joint_angles - (10, ) * 6) print("Target joint angles after decrease: {}".format( robot.target_joint_angles)) print("Joint angles after decrease: {}".format(robot.joint_angles)) # Move backward and forward print("Moving backward and forward ...") robot.move_linear((50, 0, 0, 0, 0, 0)) robot.move_linear((0, 0, 0, 0, 0, 0)) # Move right and left print("Moving right and left ...") robot.move_linear((0, 50, 0, 0, 0, 0)) robot.move_linear((0, 0, 0, 0, 0, 0)) # Move down and up print("Moving down and up ...") robot.move_linear((0, 0, 50, 0, 0, 0)) robot.move_linear((0, 0, 0, 0, 0, 0)) # Roll right and left print("Rolling right and left ...") robot.move_linear((0, 0, 0, 30, 0, 0)) robot.move_linear((0, 0, 0, 0, 0, 0)) # Roll forward and backward print("Rolling forward and backward ...") robot.move_linear((0, 0, 0, 0, 30, 0)) robot.move_linear((0, 0, 0, 0, 0, 0)) # Turn clockwise and anticlockwise around work frame z-axis print( "Turning clockwise and anticlockwise around work frame z-axis ...") robot.move_linear((0, 0, 0, 0, 0, 30)) robot.move_linear((0, 0, 0, 0, 0, 0)) # Make a circular move down/up, via a point on the right/left print( "Making a circular move down and up, via a point on the right/left ..." ) robot.blend_radius = 10 robot.move_circular((0, 20, 20, 0, 0, 0), (0, 0, 40, 0, 0, 0)) robot.blend_radius = 0 robot.move_circular((0, -20, 20, 0, 0, 0), (0, 0, 0, 0, 0, 0)) # Move to offset pose then tap down and up in sensor frame print("Moving to 50 mm / 30 deg offset in pose ...") robot.move_linear((50, 50, 50, 30, 30, 30)) print("Target pose after offset move: {}".format(robot.target_pose)) print("Pose after offset move: {}".format(robot.pose)) print("Tapping down and up ...") robot.coord_frame = base_frame robot.coord_frame = robot.target_pose robot.move_linear((0, 0, 50, 0, 0, 0)) robot.move_linear((0, 0, 0, 0, 0, 0)) robot.coord_frame = work_frame print("Moving to origin of work frame ...") robot.move_linear((0, 0, 0, 0, 0, 0)) # Increase blend radius and move through a sequence of waypoints print( "Increasing blend radius and moving through a sequence of waypoints ..." ) robot.blend_radius = 20 print("Moving to first waypoint ...") robot.move_linear((100, 0, 0, 0, 0, 0)) print("Moving to second waypoint ...") robot.move_linear((100, 100, 0, 0, 0, 0)) robot.blend_radius = 0 print("Moving to final destination ...") robot.move_linear((100, 100, 100, 0, 0, 0)) print("Moving to origin of work frame ...") robot.move_linear((0, 0, 0, 0, 0, 0)) print("Final target pose in work frame: {}".format(robot.target_pose)) print("Final pose in work frame: {}".format(robot.pose)) # Pause before commencing asynchronous tests print("Waiting for 5 secs ...") time.sleep(5) print("Repeating test sequence for asynchronous moves ...") # Increase and decrease all joint angles (async) print("Increasing and decreasing all joint angles ...") robot.async_move_joints(robot.joint_angles + (10, ) * 6) print("Getting on with something else while command completes ...") robot.async_result() print("Target joint angles after increase: {}".format( robot.target_joint_angles)) print("Joint angles after increase: {}".format(robot.joint_angles)) robot.async_move_joints(robot.joint_angles - (10, ) * 6) print("Getting on with something else while command completes ...") robot.async_result() print("Target joint angles after decrease: {}".format( robot.target_joint_angles)) print("Joint angles after decrease: {}".format(robot.joint_angles)) # Move backward and forward (async) print("Moving backward and forward (async) ...") robot.async_move_linear((50, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() robot.async_move_linear((0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() # Move right and left print("Moving right and left (async) ...") robot.async_move_linear((0, 50, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() robot.async_move_linear((0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() # Move down and up (async) print("Moving down and up (async) ...") robot.async_move_linear((0, 0, 50, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() robot.async_move_linear((0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() # Roll right and left (async) print("Rolling right and left (async) ...") robot.async_move_linear((0, 0, 0, 30, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() robot.async_move_linear((0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() # Roll forward and backward (async) print("Rolling forward and backward (async) ...") robot.async_move_linear((0, 0, 0, 0, 30, 0)) print("Getting on with something else while command completes ...") robot.async_result() robot.async_move_linear((0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() # Turn clockwise and anticlockwise around work frame z-axis (async) print( "Turning clockwise and anticlockwise around work frame z-axis (async) ..." ) robot.async_move_linear((0, 0, 0, 0, 0, 30)) print("Getting on with something else while command completes ...") robot.async_result() robot.async_move_linear((0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() # Make a circular move down/up, via a point on the right/left print( "Making a circular move down and up, via a point on the right/left (async) ..." ) robot.blend_radius = 10 robot.async_move_circular((0, 20, 20, 0, 0, 0), (0, 0, 40, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() robot.blend_radius = 0 robot.async_move_circular((0, -20, 20, 0, 0, 0), (0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() # Move to offset pose then tap down and up in sensor frame (async) print("Moving to 50 mm / 30 deg offset in pose (async) ...") robot.async_move_linear((50, 50, 50, 30, 30, 30)) print("Getting on with something else while command completes ...") robot.async_result() print("Target pose after offset move: {}".format(robot.target_pose)) print("Pose after offset move: {}".format(robot.pose)) print("Tapping down and up (async) ...") robot.coord_frame = base_frame robot.coord_frame = robot.target_pose robot.async_move_linear((0, 0, 50, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() robot.async_move_linear((0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() robot.coord_frame = work_frame print("Moving to origin of work frame ...") robot.async_move_linear((0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() # Increase blend radius and move through a sequence of waypoints (async) print( "Increasing blend radius and moving through a sequence of waypoints ..." ) robot.blend_radius = 20 print("Moving to first waypoint ...") robot.async_move_linear((100, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() print("Moving to second waypoint ...") robot.async_move_linear((100, 100, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() robot.blend_radius = 0 print("Moving to final destination ...") robot.async_move_linear((100, 100, 100, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() print("Moving to origin of work frame ...") robot.async_move_linear((0, 0, 0, 0, 0, 0)) print("Getting on with something else while command completes ...") robot.async_result() print("Final target pose in work frame: {}".format(robot.target_pose)) print("Final pose in work frame: {}".format(robot.pose))
def main(): base_frame = (0, 0, 0, 0, 0, 0) work_frame = (109.1, -487.0, 341.3, 180, 0, -90 ) # base frame: x->right, y->back, z->up # work_frame = (487.0, -109.1, 341.3, 180, 0, 180) # base frame: x->front, y->right, z->up with AsyncRobot(SyncRobot(RTDEController(ip='192.11.72.10'))) as robot: # For testing in URSim simulator # with AsyncRobot(SyncRobot(RTDEController(ip='127.0.0.1'))) as robot: # Set TCP, linear speed, angular speed and coordinate frame robot.tcp = (0, 0, 89.1, 0, 0, 0) robot.linear_speed = 50 robot.angular_speed = 5 robot.coord_frame = work_frame rtde_client = robot.sync_robot.controller._client # Display robot info print("Robot info: {}".format(robot.info)) # Display initial joint angles print("Initial joint angles: {}".format(robot.joint_angles)) # Display initial pose in work frame print("Initial pose in work frame: {}".format(robot.pose)) # Move to origin of work frame print("Moving to origin of work frame ...") robot.move_linear((0, 0, 0, 0, 0, 0)) # Single velocity move print("Making single velocity move, then stopping ...") rtde_client.move_linear_speed((-20, -20, -20, 0, 0, 0), 10, 9) rtde_client.stop_linear(10) # Sequence of velocity moves print("Making multiple velocity moves, then stopping ...") rtde_client.move_linear_speed((40, 0, 0, 0, 0, 0), 20, 3) print("Calculating next move ...") time.sleep(2) rtde_client.move_linear_speed((0, 40, 0, 0, 0, 0), 20, 3) print("Calculating next move ...") time.sleep(2) rtde_client.move_linear_speed((0, 0, 40, 0, 0, 0), 20, 3) print("Calculating next move ...") time.sleep(2) rtde_client.stop_linear(10) print("Waiting for 5 secs ...") time.sleep(5) print( "Making multiple velocity moves, then stopping (multi-threaded) ..." ) # Sequence of velocity moves on background thread running = False lock = threading.Lock() velocity = (0, 0, 0, 0, 0, 0) accel = 10 ret_time = 1 / 30 def worker(): while running: with lock: worker_velocity = velocity worker_accel = accel worker_ret_time = ret_time rtde_client.move_linear_speed(worker_velocity, worker_accel, worker_ret_time) thread = threading.Thread(target=worker, args=[], kwargs={}) running = True thread.start() try: with lock: velocity = (-40, 0, 0, 0, 0, 0) print("Calculating next move ...") time.sleep(2) with lock: velocity = (0, -40, 0, 0, 0, 0) print("Calculating next move ...") time.sleep(2) with lock: velocity = (0, 0, -40, 0, 0, 0) print("Calculating next move ...") time.sleep(2) with lock: velocity = (0, 0, 0, 0, 0, 0) print("Calculating next move ...") time.sleep(2) finally: running = False thread.join() print("Waiting for 5 secs ...") time.sleep(5) # Move to origin of work frame print("Moving to origin of work frame ...") robot.move_linear((0, 0, 0, 0, 0, 0)) print("Final target pose in work frame: {}".format(robot.target_pose)) print("Final pose in work frame: {}".format(robot.pose))
class JoggerDialog(QDialog): """Robot jogger dialog allows user to connect/disconnect to/from robot, modify its settings (axes, linear speed, angular speed, blend radius, tcp and coordinate frame), and control its pose and joint angles. """ class TabIndex: SETTINGS = 0 POSE = 1 JOINTS = 2 ROBOTS = ("abb", "ur", "franka") def __init__(self, parent=None): super().__init__(parent) self.robot = None QApplication.setStyle(QStyleFactory.create("Fusion")) QToolTip.setFont(QFont("SansSerif", 10)) self.setFixedSize(800, 450) self.setWindowTitle("Robot Jogger") self.setWindowIcon( QIcon(os.path.join(os.path.dirname(__file__), "robot.png"))) self.setToolTip("Robot jogger based on Common Robot Interface") self.robotLabel = QLabel("robot:") self.robotLabel.setFixedWidth(50) self.robotLabel.setFixedHeight(20) self.robotLabel.setAlignment(Qt.AlignRight | Qt.AlignVCenter) self.robotComboBox = QComboBox() self.robotComboBox.addItems(self.ROBOTS) self.robotComboBox.setCurrentText("abb") self.robotComboBox.setFixedWidth(70) self.robotComboBox.setFixedHeight(20) self.robotComboBox.setEnabled(True) self.ipLabel = QLabel("ip:") self.ipLabel.setFixedWidth(20) self.ipLabel.setFixedHeight(20) self.ipLabel.setAlignment(Qt.AlignRight | Qt.AlignVCenter) self.ipEditBox = LineEdit() self.ipEditBox.setFixedHeight(20) self.ipEditBox.setEnabled(True) self.portLabel = QLabel("port:") self.portLabel.setFixedWidth(40) self.portLabel.setFixedHeight(20) self.portLabel.setAlignment(Qt.AlignRight | Qt.AlignVCenter) self.portEditBox = LineEdit() self.portEditBox.setFixedWidth(60) self.portEditBox.setFixedHeight(20) self.portEditBox.setEnabled(True) self.connectPushButton = QPushButton("&Connect") self.connectPushButton.setFixedHeight(20) self.connectPushButton.setDefault(False) self.connectPushButton.setAutoDefault(True) self.connectPushButton.setEnabled(True) self.disconnectPushButton = QPushButton("&Disconnect") self.disconnectPushButton.setFixedHeight(20) self.disconnectPushButton.setDefault(False) self.disconnectPushButton.setAutoDefault(False) self.disconnectPushButton.setEnabled(False) topLayout = QGridLayout() topLayout.setHorizontalSpacing(20) topLayout.setVerticalSpacing(20) topLayout.setContentsMargins(10, 10, 10, 10) topLayout.addWidget(self.robotLabel, 0, 0) topLayout.addWidget(self.robotComboBox, 0, 1) topLayout.addWidget(self.ipLabel, 0, 2) topLayout.addWidget(self.ipEditBox, 0, 3) topLayout.addWidget(self.portLabel, 0, 4) topLayout.addWidget(self.portEditBox, 0, 5) topLayout.addWidget(self.connectPushButton, 0, 6) topLayout.addWidget(self.disconnectPushButton, 1, 6) robot = self.robotComboBox.currentText() self.createTabWidget(robot) self.tabWidget.setEnabled(False) mainLayout = QVBoxLayout() mainLayout.addLayout(topLayout) mainLayout.addWidget(self.tabWidget) self.setLayout(mainLayout) self.robotComboBox.currentIndexChanged[str].connect( self.onRobotChanged) self.ipEditBox.textEditingFinished.connect(self.onIPAddressChanged) self.portEditBox.textEditingFinished.connect(self.onPortChanged) self.connectPushButton.clicked.connect(self.onConnect) self.disconnectPushButton.clicked.connect(self.onDisconnect) self.axesComboBox.currentIndexChanged.connect(self.onAxesChanged) self.linearSpeedEditBox.textEditingFinished.connect( self.onLinearSpeedChanged) self.angularSpeedEditBox.textEditingFinished.connect( self.onAngularSpeedChanged) self.blendRadiusEditBox.textEditingFinished.connect( self.onBlendRadiusChanged) self.tcpEditWidget.editingFinished.connect(self.onTCPChanged) self.coordFrameEditWidget.editingFinished.connect( self.onCoordFrameChanged) self.poseControlWidget.valueChanged.connect(self.onPoseChanged) self.tabWidget.currentChanged.connect(self.onTabChanged) def createTabWidget(self, robot): self.tabWidget = QTabWidget() self.tabWidget.setSizePolicy(QSizePolicy.Preferred, QSizePolicy.Ignored) settingsTab = QWidget() self.createControlSettingsWidget() settingsTabLayout = QHBoxLayout() settingsTabLayout.setContentsMargins(5, 5, 5, 5) settingsTabLayout.addWidget(self.controlSettings) settingsTab.setLayout(settingsTabLayout) poseTab = QWidget() self.poseControlWidget = MultiSliderControlWidget( names=("x", "y", "z", "alpha", "beta", "gamma"), units=("mm", ) * 3 + ("°", ) * 3, ranges=((-1000, 1000), ) * 3 + ((-360, 360), ) * 3, values=(0, ) * 6, ) poseTabLayout = QHBoxLayout() poseTabLayout.setContentsMargins(5, 5, 5, 5) poseTabLayout.addWidget(self.poseControlWidget) poseTab.setLayout(poseTabLayout) jointsTab = QWidget() # widget content depends on robot type self.tabWidget.addTab(settingsTab, "&Settings") self.tabWidget.addTab(poseTab, "&Pose") self.tabWidget.addTab(jointsTab, "&Joints") def createControlSettingsWidget(self): self.controlSettings = QWidget() self.minLinearSpeed = 0.001 self.maxLinearSpeed = 1000 self.minAngularSpeed = 0.001 self.maxAngularSpeed = 1000 self.minBlendRadius = 0 self.maxBlendRadius = 1000 miscSettingsGroupBox = QGroupBox(title="Misc:") self.axesLabel = QLabel("axes:") self.axesLabel.setFixedWidth(50) self.axesLabel.setFixedHeight(20) self.axesComboBox = QComboBox() self.axesComboBox.addItems(SyncRobot.EULER_AXES) self.axesComboBox.setCurrentText("rxyz") self.axesComboBox.setFixedHeight(20) self.axisUnitsLabel = QLabel() self.axisUnitsLabel.setFixedWidth(40) self.axisUnitsLabel.setFixedHeight(20) self.linearSpeedLabel = QLabel("linear\nspeed:") self.linearSpeedLabel.setFixedWidth(50) self.linearSpeedLabel.setFixedHeight(40) self.linearSpeedEditBox = LineEdit() self.linearSpeedEditBox.setText("{:.1f}".format(0)) self.linearSpeedEditBox.setFixedHeight(20) self.linearSpeedEditBox.setAlignment(Qt.AlignRight | Qt.AlignVCenter) self.linearSpeedUnitsLabel = QLabel("mm/s") self.linearSpeedUnitsLabel.setFixedWidth(40) self.linearSpeedUnitsLabel.setFixedHeight(20) self.angularSpeedLabel = QLabel("angular\nspeed:") self.angularSpeedLabel.setFixedWidth(50) self.angularSpeedLabel.setFixedHeight(40) self.angularSpeedEditBox = LineEdit() self.angularSpeedEditBox.setText("{:.1f}".format(0)) self.angularSpeedEditBox.setFixedHeight(20) self.angularSpeedEditBox.setAlignment(Qt.AlignRight | Qt.AlignVCenter) self.angularSpeedUnitsLabel = QLabel("°/s") self.angularSpeedUnitsLabel.setFixedWidth(40) self.angularSpeedUnitsLabel.setFixedHeight(20) self.blendRadiusLabel = QLabel("blend\nradius:") self.blendRadiusLabel.setFixedWidth(50) self.blendRadiusLabel.setFixedHeight(40) self.blendRadiusEditBox = LineEdit() self.blendRadiusEditBox.setText("{:.1f}".format(0)) self.blendRadiusEditBox.setFixedHeight(20) self.blendRadiusEditBox.setAlignment(Qt.AlignRight | Qt.AlignVCenter) self.blendRadiusEditBox.setEnabled(False) self.blendRadiusUnitsLabel = QLabel("mm") self.blendRadiusUnitsLabel.setFixedWidth(40) self.blendRadiusUnitsLabel.setFixedHeight(20) miscSettingsLayout = QGridLayout() miscSettingsLayout.setHorizontalSpacing(15) miscSettingsLayout.setVerticalSpacing(5) miscSettingsLayout.addWidget(self.axesLabel, 0, 0) miscSettingsLayout.addWidget(self.axesComboBox, 0, 1) miscSettingsLayout.addWidget(self.linearSpeedLabel, 1, 0) miscSettingsLayout.addWidget(self.linearSpeedEditBox, 1, 1) miscSettingsLayout.addWidget(self.linearSpeedUnitsLabel, 1, 2) miscSettingsLayout.addWidget(self.angularSpeedLabel, 2, 0) miscSettingsLayout.addWidget(self.angularSpeedEditBox, 2, 1) miscSettingsLayout.addWidget(self.angularSpeedUnitsLabel, 2, 2) miscSettingsLayout.addWidget(self.blendRadiusLabel, 3, 0) miscSettingsLayout.addWidget(self.blendRadiusEditBox, 3, 1) miscSettingsLayout.addWidget(self.blendRadiusUnitsLabel, 3, 2) miscSettingsGroupBox.setLayout(miscSettingsLayout) self.tcpEditWidget = MultiNumberEditWidget( names=("x", "y", "z", "alpha", "beta", "gamma"), units=("mm", ) * 3 + ("°", ) * 3, ranges=((-1000, 1000), ) * 3 + ((-360, 360), ) * 3, values=(0, ) * 6, ) tcpGroupBox = QGroupBox(title="TCP:") tcpLayout = QVBoxLayout() tcpLayout.addWidget(self.tcpEditWidget) tcpGroupBox.setLayout(tcpLayout) self.coordFrameEditWidget = MultiNumberEditWidget( names=("x", "y", "z", "alpha", "beta", "gamma"), units=("mm", ) * 3 + ("°", ) * 3, ranges=((-1000, 1000), ) * 3 + ((-360, 360), ) * 3, values=(0, ) * 6, ) coordFrameGroupBox = QGroupBox(title="Coordinate Frame:") coordFrameLayout = QVBoxLayout() coordFrameLayout.addWidget(self.coordFrameEditWidget) coordFrameGroupBox.setLayout(coordFrameLayout) controlSettingsLayout = QGridLayout() controlSettingsLayout.setHorizontalSpacing(15) controlSettingsLayout.setVerticalSpacing(5) controlSettingsLayout.addWidget(miscSettingsGroupBox, 0, 0) controlSettingsLayout.addWidget(tcpGroupBox, 0, 1) controlSettingsLayout.addWidget(coordFrameGroupBox, 0, 2) self.controlSettings.setLayout(controlSettingsLayout) def closeEvent(self, event): reply = QMessageBox.question(self, "Message", "Are you sure you want to quit?", QMessageBox.Yes | QMessageBox.No, QMessageBox.No) if reply == QMessageBox.Yes: if self.robot is not None: self.robot.close() event.accept() else: event.ignore() def onRobotChanged(self, value): logger.debug("JoggerDialog.onRobotChanged(value={})".format(value)) if value == "franka": # No need to specify port for Franka arm self.portEditBox.setText("") self.portEditBox.setEnabled(False) elif value == "ur": # UR controller always listens on port 30004 self.portEditBox.setText(str(30004)) self.portEditBox.setEnabled(False) else: self.portEditBox.setEnabled(True) def onIPAddressChanged(self): ip = self.sender().text() logger.debug("JoggerDialog.onIPAddressChanged(text={})".format(ip)) if not isValidIPAddress(ip): msg = QErrorMessage(self) msg.showMessage("Invalid IP address") msg.exec_() self.sender().setFocus() def onPortChanged(self): port = self.sender().text() logger.debug("JoggerDialog.onPortChanged(text={})".format(port)) if not isValidPortNumber(port): msg = QErrorMessage(self) msg.showMessage("Invalid port number") msg.exec_() self.sender().setFocus() def onConnect(self): robot = self.robotComboBox.currentText() ip = self.ipEditBox.text() port = self.portEditBox.text() logger.debug("JoggerDialog.onConnect(robot={}, ip={}, port={})".format( robot, ip, port)) if robot in ("abb", 'ur') and (not isValidIPAddress(ip) or not isValidPortNumber(port)): msg = QErrorMessage(self) msg.showMessage("Please specify valid IP address and port number") msg.exec_() self.sender().setFocus() return if robot == "franka" and not isValidIPAddress( ip): # no need to specify port for Franka arm msg = QErrorMessage(self) msg.showMessage("Please specify valid IP address") msg.exec_() self.sender().setFocus() return try: if robot == "abb": port = int(port) self.robot = SyncRobot(ABBController(ip, port)) elif robot == "ur": self.robot = SyncRobot(RTDEController(ip)) elif robot == "franka": self.robot = SyncRobot(PyfrankaController(ip)) except: msg = QErrorMessage(self) msg.showMessage("Failed to connect to server") msg.exec_() else: self.robotComboBox.setEnabled(False) self.ipEditBox.setEnabled(False) self.portEditBox.setEnabled(False) self.connectPushButton.setEnabled(False) self.disconnectPushButton.setEnabled(True) self.axesComboBox.setCurrentText(self.robot.axes) if robot == "franka": # Disable settings that don't apply to Franka arm self.linearSpeedEditBox.setText("n/a") self.angularSpeedEditBox.setText("n/a") self.blendRadiusEditBox.setText("n/a") self.linearSpeedEditBox.setEnabled(False) self.blendRadiusEditBox.setEnabled(False) self.angularSpeedEditBox.setEnabled(False) else: self.linearSpeedEditBox.setText("{:.1f}".format( self.robot.linear_speed)) self.angularSpeedEditBox.setText("{:.1f}".format( self.robot.angular_speed)) self.blendRadiusEditBox.setText("{:.1f}".format( self.robot.blend_radius)) self.linearSpeedEditBox.setEnabled(True) self.blendRadiusEditBox.setEnabled(True) self.angularSpeedEditBox.setEnabled(True) self.tcpEditWidget.value = self.robot.tcp self.coordFrameEditWidget.value = self.robot.coord_frame # Set up joint angles tab - need to do this dynamically now because layout depends on # whether ABB/UR5 robot is selected (6 joints), or Franka robot is selected (7 joints) jointsTab = QWidget() n_joints = 7 if robot == "franka" else 6 self.jointControlWidget = MultiSliderControlWidget( names=["joint {}".format(i + 1) for i in range(n_joints)], units=("°", ) * n_joints, ranges=((-360, 360), ) * n_joints, values=(0, ) * n_joints, ) self.jointControlWidget.valueChanged.connect( self.onJointAnglesChanged) jointsTabLayout = QHBoxLayout() jointsTabLayout.setContentsMargins(5, 5, 5, 5) jointsTabLayout.addWidget(self.jointControlWidget) jointsTab.setLayout(jointsTabLayout) self.tabWidget.removeTab(2) self.tabWidget.addTab(jointsTab, "&Joints") self.tabWidget.setCurrentIndex(self.TabIndex.SETTINGS) self.tabWidget.setEnabled(True) def onDisconnect(self): logger.debug("JoggerDialog.onDisconnect") self.robotComboBox.setEnabled(True) self.ipEditBox.setEnabled(True) robot = self.robotComboBox.currentText() if robot == "abb": self.portEditBox.setEnabled(True) elif robot in ("ur", "franka"): self.portEditBox.setEnabled(False) self.connectPushButton.setEnabled(True) self.disconnectPushButton.setEnabled(False) self.tabWidget.setCurrentIndex(self.TabIndex.SETTINGS) self.tabWidget.setEnabled(False) self.robot.close() self.robot = None def onAxesChanged(self): axes = self.sender().currentText() logger.debug("JoggerDialog.onAxesChanged(axes={})".format(axes)) self.robot.axes = axes self.tcpEditWidget.value = self.robot.tcp self.coordFrameEditWidget.value = self.robot.coord_frame def onLinearSpeedChanged(self): linearSpeed = self.sender().text() logger.debug( "JoggerDialog.onLinearSpeedChanged(speed={})".format(linearSpeed)) if isValidNumber(linearSpeed, self.minLinearSpeed, self.maxLinearSpeed): linearSpeed = float(linearSpeed) self.sender().setText("{:.1f}".format(linearSpeed)) self.robot.linear_speed = linearSpeed else: msg = QErrorMessage(self) msg.showMessage("Invalid or out of range [{}, {}] number".format( self.minLinearSpeed, self.maxLinearSpeed)) msg.exec_() self.sender().setText("{:.1f}".format(0)) self.sender().setFocus() def onAngularSpeedChanged(self): angularSpeed = self.sender().text() logger.debug("JoggerDialog.onAngularSpeedChanged(speed={})".format( angularSpeed)) if isValidNumber(angularSpeed, self.minAngularSpeed, self.maxAngularSpeed): angularSpeed = float(angularSpeed) self.sender().setText("{:.1f}".format(angularSpeed)) self.robot.angular_speed = angularSpeed else: msg = QErrorMessage(self) msg.showMessage("Invalid or out of range [{}, {}] number".format( self.minAngularSpeed, self.maxAngularSpeed)) msg.exec_() self.sender().setText("{:.1f}".format(0)) self.sender().setFocus() def onBlendRadiusChanged(self): blendRadius = self.blendRadiusEditBox.text() logger.debug( "JoggerDialog.onBlendRadiusChanged(radius={})".format(blendRadius)) if isValidNumber(blendRadius, self.minBlendRadius, self.maxBlendRadius): blendRadius = float(blendRadius) self.sender().setText("{:.1f}".format(blendRadius)) self.robot.blend_radius = blendRadius else: msg = QErrorMessage(self) msg.showMessage("Invalid or out of range [{}, {}] number".format( self.minBlendRadius, self.maxBlendRadius)) msg.exec_() self.sender().setText("{:.1f}".format(0)) self.sender().setFocus() def onTCPChanged(self): logger.debug("JoggerDialog.onTCPChanged(tcp={})".format( self.tcpEditWidget.value)) self.robot.tcp = self.tcpEditWidget.value def onCoordFrameChanged(self): logger.debug("JoggerDialog.onCoordFrameChanged(frame={})".format( self.coordFrameEditWidget.value)) self.robot.coord_frame = self.coordFrameEditWidget.value def onPoseChanged(self): logger.debug("JoggerDialog.onPoseChanged(pose={})".format( self.poseControlWidget.value)) self.robot.move_linear(self.poseControlWidget.value) def onJointAnglesChanged(self): logger.debug("JoggerDialog.onJointAnglesChanged(angles={})".format( self.jointControlWidget.value)) self.robot.move_joints(self.jointControlWidget.value) def onTabChanged(self, index): logger.debug("JoggerDialog.onTabChanged(index={}, text={})".format( index, self.tabWidget.tabText(index))) if index == self.TabIndex.POSE: self.poseControlWidget.value = self.robot.pose elif index == self.TabIndex.JOINTS: self.jointControlWidget.value = self.robot.joint_angles
def onConnect(self): robot = self.robotComboBox.currentText() ip = self.ipEditBox.text() port = self.portEditBox.text() logger.debug("JoggerDialog.onConnect(robot={}, ip={}, port={})".format( robot, ip, port)) if robot in ("abb", 'ur') and (not isValidIPAddress(ip) or not isValidPortNumber(port)): msg = QErrorMessage(self) msg.showMessage("Please specify valid IP address and port number") msg.exec_() self.sender().setFocus() return if robot == "franka" and not isValidIPAddress( ip): # no need to specify port for Franka arm msg = QErrorMessage(self) msg.showMessage("Please specify valid IP address") msg.exec_() self.sender().setFocus() return try: if robot == "abb": port = int(port) self.robot = SyncRobot(ABBController(ip, port)) elif robot == "ur": self.robot = SyncRobot(RTDEController(ip)) elif robot == "franka": self.robot = SyncRobot(PyfrankaController(ip)) except: msg = QErrorMessage(self) msg.showMessage("Failed to connect to server") msg.exec_() else: self.robotComboBox.setEnabled(False) self.ipEditBox.setEnabled(False) self.portEditBox.setEnabled(False) self.connectPushButton.setEnabled(False) self.disconnectPushButton.setEnabled(True) self.axesComboBox.setCurrentText(self.robot.axes) if robot == "franka": # Disable settings that don't apply to Franka arm self.linearSpeedEditBox.setText("n/a") self.angularSpeedEditBox.setText("n/a") self.blendRadiusEditBox.setText("n/a") self.linearSpeedEditBox.setEnabled(False) self.blendRadiusEditBox.setEnabled(False) self.angularSpeedEditBox.setEnabled(False) else: self.linearSpeedEditBox.setText("{:.1f}".format( self.robot.linear_speed)) self.angularSpeedEditBox.setText("{:.1f}".format( self.robot.angular_speed)) self.blendRadiusEditBox.setText("{:.1f}".format( self.robot.blend_radius)) self.linearSpeedEditBox.setEnabled(True) self.blendRadiusEditBox.setEnabled(True) self.angularSpeedEditBox.setEnabled(True) self.tcpEditWidget.value = self.robot.tcp self.coordFrameEditWidget.value = self.robot.coord_frame # Set up joint angles tab - need to do this dynamically now because layout depends on # whether ABB/UR5 robot is selected (6 joints), or Franka robot is selected (7 joints) jointsTab = QWidget() n_joints = 7 if robot == "franka" else 6 self.jointControlWidget = MultiSliderControlWidget( names=["joint {}".format(i + 1) for i in range(n_joints)], units=("°", ) * n_joints, ranges=((-360, 360), ) * n_joints, values=(0, ) * n_joints, ) self.jointControlWidget.valueChanged.connect( self.onJointAnglesChanged) jointsTabLayout = QHBoxLayout() jointsTabLayout.setContentsMargins(5, 5, 5, 5) jointsTabLayout.addWidget(self.jointControlWidget) jointsTab.setLayout(jointsTabLayout) self.tabWidget.removeTab(2) self.tabWidget.addTab(jointsTab, "&Joints") self.tabWidget.setCurrentIndex(self.TabIndex.SETTINGS) self.tabWidget.setEnabled(True)