Robot Control

This chapter will show the add-on package motorcortex-robot-control-python.

7 minute read

Robot Control

For robot control specifically, an additional python package has been created on top of motorcortex-python called motorcortex-robot-control-python. This package simplifies robot control tasks such as moving the robot to a specific position, controlling the gripper, etc.

You can install it using pip:

pip install motorcortex-robot-control-python

Initialise

In order to control the robot using the motorcortex-robot-control-python package, you first need to create instances of the RobotCommand and PoseTransformer classes. These classes provide high-level functions for controlling the robot and converting between different coordinate systems.

The following code snippet demonstrates how to initialise these classes after establishing a connection to the Motorcortex application:

# Import the required libraries
import motorcortex
import robot_control

# Connect to Motorcortex application

# Create RobotCommand for controlling the robot arm
robot = robot_control.RobotCommand(req, motorcortex_types)
print("✓ RobotCommand instance created")

# Create PoseTransformer for coordinate conversions
pose_transformer = robot_control.PoseTransformer(req, motorcortex_types)
print("✓ PoseTransformer instance created")

Make sure to replace the connection part with your existing connection code that creates the req and motorcortex_types objects. Once you have these instances, you can use them to send commands to the robot and perform coordinate transformations.

Engaging & Disengaging the Robot

To engage the robot, reset it, and prepare it for motion commands, you can use the following code snippet:

# Reset the robot state
robot.reset()
print("✓ Robot reset")

# Engage the robot
print("Engaging robot...")
if robot.engage():
    print("✓ Robot is engaged (state: ENGAGED_S)")
else:
    print("✗ Failed to engage robot")
    exit(1)

Resetting stops the program and clear the interpreter buffer, while engaging powers up the robot and prepares it for motion commands. Make sure to check if the robot is successfully engaged before sending any motion commands.

When the robot does not need to be controlled anymore, it is good practice to disengage it to ensure safety:

# Disengage the robot
print("Disengaging robot...")
if robot.disengage():
    print("✓ Robot is disengaged (state: DISENGAGED_S)")
else:
    print("✗ Failed to disengage robot")

To further turn off the robot’s power, you can use the off() method:

# Turn off the robot power
print("Turning off robot power...")
if robot.off():
    print("✓ Robot power is off (state: OFF_S)")
else:
    print("✗ Failed to turn off robot power")

Switching Control Modes

To switch between different control modes (Manual Joint Mode, Manual Cartesian Mode, and Semi-Auto Mode), you can use the following example:

# Switch to Manual Joint Mode
if robot.manualJointMode():
    print("✓ Robot switched to Manual Joint Mode")
else:
    print("✗ Failed to switch to Manual Joint Mode")

time.sleep(1)

# Switch to Manual Cartesian Mode
if robot.manualCartMode():
    print("✓ Robot switched to Manual Cartesian Mode")
else:
    print("✗ Failed to switch to Manual Cartesian Mode")

time.sleep(1)

# Switch to Semi-Auto Mode
if robot.semiAutoMode():
    print("✓ Robot switched to Semi-Auto Mode")
else:
    print("✗ Failed to switch to Semi-Auto Mode")

time.sleep(1)

Pause, Stop, Reset

During a motion plan execution, you can pause, stop, and reset the robot. This can be useful for safety and control purposes. Here is an example demonstrating these functionalities:

robot.reset()

# Create a simple program for testing pause/stop
test_prog = robot_control.MotionProgram(req, motorcortex_types)
test_prog.addMoveJ([jpos_1, jpos_2, jpos_3, jpos_1], 
                   rotational_velocity=0.5, rotational_acceleration=0.8)
test_prog.send("Control_Test").get()

print("Starting program for pause/stop demonstration...")
robot.play()
time.sleep(2)

# Pause the program
print("Pausing program...")
pause_state = robot.pause()
print(f"✓ Program paused (state: {pause_state})")
time.sleep(2)

# Resume by playing again
print("Resuming program...")
robot.play()
print("✓ Program resumed")
time.sleep(2)

# Stop the program
print("Stopping program...")
stop_state = robot.stop()
print(f"✓ Program stopped (state: {stop_state})")

# Reset the interpreter
print("Resetting interpreter...")
reset_state = robot.reset()
print(f"✓ Interpreter reset (state: {reset_state})")

MoveJ (Joint Space Motion)

This example demonstrates how to create and execute a motion program using MoveJ commands to move the robot in joint space.

robot.reset()

# Create a motion program
motion_prog_j = robot_control.MotionProgram(req, motorcortex_types)

# Define waypoints in joint space
jpos_1 = robot_control.Waypoint(home_position)
jpos_2 = robot_control.Waypoint(position_1)
jpos_3 = robot_control.Waypoint(position_2)

# Add MoveJ commands (joint space motion)
motion_prog_j.addMoveJ([jpos_1], rotational_velocity=1.0, rotational_acceleration=1.5)
motion_prog_j.addWait(1.0)  # Wait 1 second
motion_prog_j.addMoveJ([jpos_2], rotational_velocity=1.0, rotational_acceleration=1.5)
motion_prog_j.addWait(1.0)
motion_prog_j.addMoveJ([jpos_3], rotational_velocity=1.0, rotational_acceleration=1.5)
motion_prog_j.addWait(1.0)
motion_prog_j.addMoveJ([jpos_1], rotational_velocity=1.0, rotational_acceleration=1.5)

print("✓ Created MoveJ motion program with waypoints and wait commands")

# Send the program to the robot
motion_prog_j.send("MoveJ_Program").get()
print("✓ Sent MoveJ program to robot")

# Play the program
print("Executing MoveJ program...")
play_result = robot.play()
if play_result == robot_control.InterpreterStates.MOTION_NOT_ALLOWED_S.value:
    print("Robot not at start position, moving to start...")
    if robot.moveToStart(10):
        print("✓ Moved to start position")
        robot.play()
    else:
        print("✗ Failed to move to start position")

# Monitor execution
while robot.getState() == robot_control.InterpreterStates.PROGRAM_IS_DONE.value:
    time.sleep(0.1)
    
print(f"Program started, state: {robot.getState()}")

while robot.getState() == robot_control.InterpreterStates.PROGRAM_RUN_S.value:
    time.sleep(0.5)
    print(f"  Executing... (state: {robot.getState()})")

print("✓ MoveJ program execution complete")

MoveL (Linear Cartesian Motion)

This example demonstrates how to create and execute a motion program using MoveL commands to move the robot in Cartesian space.

robot.reset()

# Define Cartesian waypoints for linear motion
cart_pos_1 = robot_control.Waypoint([0.4, 0.0, 0.35, 0, math.pi, 0])
cart_pos_2 = robot_control.Waypoint([0.4, -0.1, 0.35, 0, math.pi, 0])
cart_pos_3 = robot_control.Waypoint([0.4, -0.1, 0.25, 0, math.pi, 0])
cart_pos_4 = robot_control.Waypoint([0.4, 0.0, 0.25, 0, math.pi, 0])

# Create motion program with MoveL
motion_prog_l = robot_control.MotionProgram(req, motorcortex_types)
motion_prog_l.addMoveL([cart_pos_1], velocity=0.1, acceleration=0.2, 
                       rotational_velocity=2.0, rotational_acceleration=3.0)
motion_prog_l.addMoveL([cart_pos_2], velocity=0.1, acceleration=0.2)
motion_prog_l.addMoveL([cart_pos_3], velocity=0.1, acceleration=0.2)
motion_prog_l.addMoveL([cart_pos_4], velocity=0.1, acceleration=0.2)
motion_prog_l.addMoveL([cart_pos_1], velocity=0.1, acceleration=0.2)

print("✓ Created MoveL motion program with Cartesian waypoints")

# Send and execute
motion_prog_l.send("MoveL_Program").get()
print("✓ Sent MoveL program to robot")

print("Executing MoveL program...")
play_result = robot.play()
if play_result == robot_control.InterpreterStates.MOTION_NOT_ALLOWED_S.value:
    print("Robot not at start position, moving to start...")
    if robot.moveToStart(10):
        print("✓ Moved to start position")
        robot.play()

# Monitor execution
while robot.getState() == robot_control.InterpreterStates.PROGRAM_IS_DONE.value:
    time.sleep(0.1)

print(f"Program started, state: {robot.getState()}")

while robot.getState() == robot_control.InterpreterStates.PROGRAM_RUN_S.value:
    time.sleep(0.5)
    print(f"  Executing... (state: {robot.getState()})")

print("✓ MoveL program execution complete")

MoveC (Circular Cartesian Motion)

Demonstrates how to create and execute a motion program using MoveC commands to move the robot in a circular path in Cartesian space.

robot.reset()

# Define waypoints for circular motion
circ_pos_1 = robot_control.Waypoint([0.4, 0.0, 0.3, 0, math.pi, 0])
circ_pos_2 = robot_control.Waypoint([0.35, 0.0, 0.3, 0, math.pi, 0])
circ_pos_3 = robot_control.Waypoint([0.35, 0.05, 0.3, 0, math.pi, 0])

# Create motion program with MoveC
motion_prog_c = robot_control.MotionProgram(req, motorcortex_types)
motion_prog_c.addMoveC([circ_pos_1, circ_pos_2, circ_pos_3], 
                       angle=math.pi,  # 180 degrees rotation
                       velocity=0.05, 
                       acceleration=0.1,
                       rotational_velocity=1.5,
                       rotational_acceleration=2.0)

print("✓ Created MoveC motion program for circular motion")

# Send and execute
motion_prog_c.send("MoveC_Program").get()
print("✓ Sent MoveC program to robot")

print("Executing MoveC program...")
play_result = robot.play()
if play_result == robot_control.InterpreterStates.MOTION_NOT_ALLOWED_S.value:
    print("Robot not at start position, moving to start...")
    if robot.moveToStart(10):
        print("✓ Moved to start position")
        robot.play()

# Monitor execution
while robot.getState() == robot_control.InterpreterStates.PROGRAM_IS_DONE.value:
    time.sleep(0.1)

print(f"Program started, state: {robot.getState()}")

while robot.getState() == robot_control.InterpreterStates.PROGRAM_RUN_S.value:
    time.sleep(0.5)
    print(f"  Executing... (state: {robot.getState()})")

print("✓ MoveC program execution complete")

Pose transformations (Forward/Inverse Kinematics)

Pose transformation can be used when you need to:

Convert joint positions to Cartesian coordinates (forward kinematics)
Convert Cartesian coordinates back to joint positions (inverse kinematics)
Plan or verify robot trajectories
Calibrate robot positions

import math

# Convert current joint position to Cartesian coordinates (forward kinematics)
ref_joint_coord = [0.0, 0.0, math.radians(90.0), 0.0, math.radians(90.0), 0.0]
cart_result = pose_transformer.calcJointToCartPose(joint_coord_rad=ref_joint_coord)
ref_cart_coord = cart_result.jointtocartlist[0].cartpose.coordinates
print(f"✓ Joint to Cart conversion:")
print(f"  Joint: {[round(math.degrees(j), 1) for j in ref_joint_coord]} degrees")
print(f"  Cart:  {[round(c, 3) for c in ref_cart_coord]}")

# Convert Cartesian back to joint coordinates (inverse kinematics)
joint_result = pose_transformer.calcCartToJointPose(cart_coord=ref_cart_coord, 
                                                     ref_joint_coord_rad=ref_joint_coord)
new_joint_coord = joint_result.carttojointlist[0].jointpose.coordinates
print(f"✓ Cart to Joint conversion:")
print(f"  Cart:  {[round(c, 3) for c in ref_cart_coord]}")
print(f"  Joint: {[round(math.degrees(j), 1) for j in new_joint_coord]} degrees")

Set Parameters

During motion control, you might want to adjust certain robot parameters such as speed limits or acceleration profiles. The following example demonstrates how to set these parameters by opening and closing the robot’s gripper:

robot.stop()
robot.reset()

# Create a program that opens and closes the gripper 3 times while rotating joint 0
motion_prog_set = robot_control.MotionProgram(req, motorcortex_types)

# Move to starting position
motion_prog_set.addMoveJ([jpos_1], rotational_velocity=1.0, rotational_acceleration=1.5)

# Define joint positions for rotation (only joint 0 changes)
joint_rot_left = [math.radians(-30.0), 0.0, math.radians(90.0), 0.0, math.radians(90.0), 0.0]   # -30 degrees
joint_rot_right = [math.radians(30.0), 0.0, math.radians(90.0), 0.0, math.radians(90.0), 0.0]  # +30 degrees

# Open and close gripper 3 times while rotating joint 0
for i in range(3):
    print(f"Cycle {i+1}: Rotating left and opening gripper...")
    motion_prog_set.addMoveJ([robot_control.Waypoint(joint_rot_left)], rotational_velocity=0.5, rotational_acceleration=1.0)
    motion_prog_set.addSet("root/UserParameters/IO/gripper", True)   # Open gripper
    motion_prog_set.addWait(1.0)  # Wait 1 second
    
    print(f"Cycle {i+1}: Rotating right and closing gripper...")
    motion_prog_set.addMoveJ([robot_control.Waypoint(joint_rot_right)], rotational_velocity=0.5, rotational_acceleration=1.0)
    motion_prog_set.addSet("root/UserParameters/IO/gripper", False)  # Close gripper
    motion_prog_set.addWait(1.0)  # Wait 1 second

# Return to center position
motion_prog_set.addMoveJ([jpos_1], rotational_velocity=0.5, rotational_acceleration=1.0)

print("✓ Created motion program with gripper cycles and joint 0 rotation")

# Send and execute
motion_prog_set.send("Gripper_Rotation_Program").get()
print("✓ Sent gripper + rotation program to robot")

print("Executing gripper + rotation program...")
play_result = robot.play()
if play_result == robot_control.InterpreterStates.MOTION_NOT_ALLOWED_S.value:
    print("Robot not at start position, moving to start...")
    if robot.moveToStart(10):
        print("✓ Moved to start position")
        robot.play()

# Monitor execution
while robot.getState() == robot_control.InterpreterStates.PROGRAM_IS_DONE.value:
    time.sleep(0.1)

print(f"Program started, state: {robot.getState()}")

while robot.getState() == robot_control.InterpreterStates.PROGRAM_RUN_S.value:
    time.sleep(0.5)
    print(f"  Executing... (state: {robot.getState()})")

print("✓ Gripper + rotation program execution complete")