Robot Control

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

9 minute read

For robot control specifically, an additional Python package has been created on top of motorcortex-python: motorcortex-robot-control-python. This package simplifies robot control tasks such as moving the robot to a specific position, controlling the gripper, and converting between joint and Cartesian coordinates.

You can install it using pip:

pip install motorcortex-robot-control-python

Read the tabs in order: the Initialise tab creates the robot and pose_transformer objects and the joint positions that the other three continue from.

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 math
import time
import motorcortex
import robot_control

# Connect to the Motorcortex application, see the Fundamentals chapter.
# This gives you the `req`, `sub` and `motorcortex_types` objects.

# 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")

# Joint-space positions used by the examples in the other tabs, in radians
home_position = [math.radians(0.0), math.radians(0.0), math.radians(90.0),
                 math.radians(0.0), math.radians(90.0), math.radians(0.0)]
position_1 = [math.radians(-30.0), math.radians(0.0), math.radians(90.0),
              math.radians(0.0), math.radians(90.0), math.radians(0.0)]
position_2 = [math.radians(30.0), math.radians(0.0), math.radians(90.0),
              math.radians(0.0), math.radians(90.0), math.radians(0.0)]

# Wrap them as waypoints for the motion program examples
jpos_1 = robot_control.Waypoint(home_position)
jpos_2 = robot_control.Waypoint(position_1)
jpos_3 = robot_control.Waypoint(position_2)

Replace the connection comment with your connection code from the Fundamentals chapter, which 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.

Note

The joint positions above suit the Motorcortex Anthropomorphic Robot application used throughout this chapter. Adjust them to your own robot’s joint count and reachable range before running a motion program.

The examples in the Modes, Motion and Other tabs all continue from this snippet.

Engaging and disengaging the robot

These examples continue from the Initialise tab, which creates robot and the joint positions.

Warning

Engaging powers up the drives, and the examples below command motion. Make sure the robot’s workspace is clear and an emergency stop is within reach.

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 clears 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)

These examples continue from the Initialise tab, which creates robot, motion_prog_* inputs and the jpos_* waypoints.

Warning

Playing a motion program moves the robot. Clear the workspace and keep an emergency stop within reach before running these examples.

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)

# 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 and inverse kinematics)

These examples continue from the Initialise tab, which creates pose_transformer, robot and the jpos_* waypoints.

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")

Setting parameters from a motion program

During motion control, you might want to set parameters at a specific point in a program. The following example demonstrates addSet by opening and closing the robot’s gripper between moves:

Warning

This program rotates joint 0 and actuates the gripper. Clear the workspace before running it.

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")