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)