Hi ROS Community,
I’m excited to share details about implementing continuous trajectory recording and replay for the AgileX PIPER robotic arm. This solution leverages time-series data to accurately replicate complex motion trajectories, with full code, usage guides, and step-by-step demos included. It’s designed to support teaching demonstrations and automated operations, and I hope it brings value to your projects.
Abstract
This paper achieves continuous trajectory recording and replay based on the AgileX PIPER robotic arm. Through the recording and reproduction of time-series data, it achieves the perfect replication of the complex motion trajectories of the robotic arm. In this paper, we will analyze the code implementation and provide complete code, usage guidelines, and step-by-step demonstrations.
Keywords
Trajectory control; Continuous motion; Time series; Motion reproduction; AgileX PIPER
Code Repository
github link: https://github.com/agilexrobotics/Agilex-College.git
Function Demonstration
1. Preparation Before Use
1.1. Preparation Work
- There are no obstacles in the workspace to provide sufficient movement space for the robotic arm.
- Confirm that the power supply of the robotic arm is normal and all indicator lights are in a normal state.
- The lighting conditions are good for the observation of the position and state of the robotic arm.
- If equipped with a gripper, check whether the gripper actions are normal. The ground is stable to avoid vibration affecting the recording accuracy.
- Verify that the teach button functions normally.
1.2. Environment Configuration
- Operating system: Ubuntu (Ubuntu 18.04 or higher is recommended).
- Python environment: Python 3.6 or higher.
- Git code management tool: Used to clone remote code repositories.
sudo apt install git - Pip package manager: Used to install Python dependency packages.
sudo apt install python3-pip - Install CAN tools.
sudo apt install can-utils ethtool - Install the official Python SDK package, among which 1_0_0_beta is the version with API.
git clone -b 1_0_0_beta https://github.com/agilexrobotics/piper_sdk.git cd piper_sdk pip3 install . - Reference document: https://github.com/agilexrobotics/piper_sdk/blob/1_0_0_beta/README(ZH).MD
2. Operation Steps for Continuous Trajectory Recording and Replay Function
2.1. Operation Steps
-
Power on the robotic arm and connect the USB-to-CAN module to the computer (ensure that only one CAN module is connected).
-
Open the terminal and activate the CAN module.
sudo ip link set can0 up type can bitrate 1000000 -
Clone the remote code repository.
git clone https://github.com/agilexrobotics/Agilex-College.git -
Switch to the recordAndPlayTraj directory.
cd Agilex-College/piper/recordAndPlayTraj/ -
Run the recording program.
python3 recordTrajectory_en.py -
Short-press the teach button to enter the teaching mode.
-
Set the initial position of the robotic arm. After pressing Enter in the terminal, drag the robotic arm to record the trajectory.
-
After recording, short-press the teach button again to exit the teaching mode.
-
Precautions before replay:
When exiting the teaching mode for the first time, a specific initialization process is required to switch from the teaching mode to the CAN mode. Therefore, the replay program will automatically perform a reset operation to return joints 2, 3, and 5 to safe positions (zero points) to prevent the robotic arm from suddenly falling due to gravity and causing damage. In special cases, manual assistance may be needed to return joints 2, 3, and 5 to zero points. -
Run the replay program.
python3 playTrajectory_en.py -
After successful enabling, press Enter in the terminal to play the trajectory.
2.2. Recording Techniques and Strategies
Motion Planning Strategies:
Before starting the recording, the trajectory to be recorded should be planned:
-
Starting Position Selection:
- Select a safe position of the robotic arm as the starting point.
- Ensure that the starting position is convenient for initialization during subsequent replay.
- Avoid choosing a position close to the joint limit.
-
Trajectory Path Design:
- Plan a smooth motion path to avoid sharp direction changes.
- Consider the kinematic constraints of the robotic arm to avoid singular positions.
- Reserve sufficient safety margins to prevent collisions.
-
Speed Control:
- Maintain a moderate movement speed to ensure both recording quality and avoid being too slow.
- Appropriately slow down at key positions to improve accuracy.
- Avoid sudden acceleration or deceleration.
3. Problems and Solutions
Problem 1: No Piper Class
Reason: The currently installed SDK is not the version with API.
Solution: Execute pip3 uninstall piper_sdk to uninstall the current SDK, and then install the 1_0_0_beta version of the SDK according to the method in 1.2. Environment Configuration.
Problem 2: The Robotic Arm Does Not Move, and the Terminal Outputs as Follows
Reason: The teach button was short-pressed during the program operation.
Solution: Check whether the indicator light of the teach button is off. If it is, re-run the program. If not, short-press the teach button to exit the teaching mode first and then run the program.
4. Implementation of Trajectory Recording Program
The trajectory recording program is the data collection module of the system, responsible for capturing the position information of the continuous joint movements of the robotic arm in the teaching mode.
4.1. Program Initialization and Configuration
4.1.1. Parameter Configuration Design
# Whether there is a gripper
have_gripper = True
# Maximum recording time in seconds (0 = unlimited, stop by terminating program)
record_time = 10.0
# Teach mode detection timeout in seconds
timeout = 10.0
# CSV file path for saving trajectory
CSV_path = os.path.join(os.path.dirname(__file__), "trajectory.csv")
Analysis of Configuration Parameters:
- The
have_gripperparameter is of boolean type, and True indicates the presence of a gripper. - The
record_timeparameter sets the maximum recording time. The default value is 10s, that is, it records a 10-second trajectory. When this parameter is set to 0, the system enters the infinite recording mode. - The
timeoutparameter sets the timeout for teaching mode detection. After starting the program, if the teaching mode is not entered within 10s, the program will exit. - The
CSV_pathparameter sets the save path of the trajectory file. By default, it is in the same path as the program, and the file name istrajectory.csv.
4.1.2. Robotic Arm Connection and Initialization
# Initialize and connect to robotic arm
piper = Piper("can0")
piper.connect()
interface = piper.init()
time.sleep(0.1)
Analysis of Connection Mechanism:
Piper()is the core class of the API, which simplifies some common methods based on the interface.init()creates and returns an interface instance, which can be used to call some special methods of Piper.connect()starts a thread to connect to the CAN port and process CAN data.- The addition of
time.sleep(0.1)ensures that the connection is fully established. In embedded systems, hardware initialization usually takes a certain amount of time, and this short delay ensures the reliability of subsequent operations.
4.1.3. Position Acquisition and Data Storage
4.1.3.1. Position Acquisition Function
def get_pos():
joint_state = piper.get_joint_states()[0]
if have_gripper:
'''Get current joint angles and gripper opening distance'''
return joint_state + (piper.get_gripper_states()[0][0], )
return joint_state
4.1.3.2. Position Change Detection
if current_pos != last_pos: # Record only when position changes
current_pos = get_pos()
wait_time = round(time.time() - last_time, 4)
print(f"INFO: Wait time: {wait_time:0.4f}s, current position: {current_pos}")
last_pos = current_pos
last_time = time.time()
csv.write(f"{wait_time}," + ",".join(map(str, current_pos)) + "\n")
Position Processing:
current_pos != last_pos: When the robotic arm is stationary or the positions obtained in two consecutive times are the same, the position change processing can greatly reduce the recorded data.
Time Processing:
round(time.time() - last_time, 4):- A precision of 0.1 milliseconds is sufficient for robotic arm control.
- Shortening the length of time can reduce the occupied storage space.
4.1.4. Mode Detection and Switching
print("step 1: Press teach button to enter teach mode")
while interface.GetArmStatus().arm_status.ctrl_mode != 2:
over_time = time.time() + timeout
if over_time < time.time():
print("ERROR: Teach mode detection timeout. Please check if teach mode is enabled")
exit()
time.sleep(0.01)
Status Polling Strategy:
The program uses the polling method to detect the control mode. This method has the following characteristics:
- Simple implementation and clear logic.
- Low requirements for system resources.
Timeout Protection Mechanism:
The 10-second timeout setting takes into account the needs of actual operations:
- Time for users to understand the prompt information.
- Time to find and press the teach button.
- Time for system response and state switching.
- Fault-tolerance handling in abnormal situations.
Safety Features of Teaching Mode:
- Joint torque is released, allowing manual operation.
- Maintain position feedback and monitor position changes in real-time.
4.1.5. Data Storage
# ... Recording loop ...
csv.write(f"{wait_time}," + ",".join(map(str, current_pos)) + "\n")
# ... End of recording ...
csv = open(CSV_path, "w")
csv.close()
Data Integrity Guarantee:
After each recording, the data is immediately written to the file, and the buffer is refreshed to ensure that the data will not be lost due to abnormal program exit.
Data Format Selection:
Reasons for Choosing CSV Format for Data Storage:
- High versatility. Almost all data processing tools support the CSV format.
- Strong readability, facilitating debugging and verification.
- Simple structure and high parsing efficiency.
- Widely supported, facilitating integration with other tools.
Data Column Attributes:
- Column 1: Time interval, unit: second.
- Columns 2 - 7: Joint motor radians.
- Column 8: Gripper opening distance, unit: m.
4.1.6. Recording Control Logic
over_time = last_time + record_time
while record_time == 0 or time.time() < over_time:
csv = open(CSV_path, "w")
input("step 2: Press Enter to start recording trajectory")
last_pos = get_pos()
last_time = time.time()
# Recording logic
time.sleep(0.01)
csv.close()
Time Control Strategy:
The system supports two recording modes:
- Timed recording: when
record_time > 0, the recording stops automatically after the specified duration. - Infinite recording: When
record_time == 0, the program needs to be manually closed.
The flexibility of this design:
- Adapting to the recording requirements of different-length trajectories.
- Providing a safe automatic stop mechanism.
- Supporting long-time continuous recording tasks.
4.1.7. Complete Code Implementation of Trajectory Recording Program
#!/usr/bin/env python3
# -*-coding:utf8-*-
# Record continuous trajectory
import os, time
from piper_sdk import *
if __name__ == "__main__":
# Whether there is a gripper
have_gripper = True
# Maximum recording time in seconds (0 = unlimited, stop by terminating program)
record_time = 10.0
# Teach mode detection timeout in seconds
timeout = 10.0
# CSV file path for saving trajectory
CSV_path = os.path.join(os.path.dirname(__file__), "trajectory.csv")
# Initialize and connect to robotic arm
piper = Piper("can0")
interface = piper.init()
piper.connect()
time.sleep(0.1)
def get_pos():
joint_state = piper.get_joint_states()[0]
if have_gripper:
'''Get current joint angles and gripper opening distance'''
return joint_state + (piper.get_gripper_states()[0][0], )
return joint_state
print("step 1: Press teach button to enter teach mode")
over_time = time.time() + timeout
while interface.GetArmStatus().arm_status.ctrl_mode != 2:
if over_time < time.time():
print("ERROR: Teach mode detection timeout. Please check if teach mode is enabled")
exit()
time.sleep(0.01)
input("step 2: Press Enter to start recording trajectory")
csv = open(CSV_path, "w")
last_pos = get_pos()
last_time = time.time()
over_time = last_time + record_time
while record_time == 0 or time.time() < over_time:
current_pos = get_pos()
if current_pos != last_pos: # Record only when position changes
wait_time = round(time.time() - last_time, 4)
print(f"INFO: Wait time: {wait_time:0.4f}s, current position: {current_pos}")
csv.write(f"{wait_time}," + ",".join(map(str, current_pos)) + "\n")
last_pos = current_pos
last_time = time.time()
time.sleep(0.01)
csv.close()
print("INFO: Recording complete. Press teach button again to exit teach mode")
5. Implementation of Trajectory Replay Program
The trajectory replay program is the execution module of the system, responsible for reading recorded position data and controlling the robotic arm to reproduce these positions.
5.1. Parameter Configuration and Data Loading
Replay Parameter Configuration:
# replay times (0 means infinite loop)
play_times = 1
# replay interval in seconds
play_interval = 1.0
# Motion speed percentage (recommended range: 10-100)
move_spd_rate_ctrl = 100
# replay speed multiplier (recommended range: 0.1-2)
play_speed = 1.0
Play times control:
The play_times parameter supports three modes:
- Single replay (play_times = 1): Suitable for demonstrations and testing.
- Multiple replays (play_times > 1): Suitable for repetitive tasks.
- Infinite loop (play_times = 0): Suitable for continuous production operations.
Dual Mechanism for Speed Control:
The system provides two speed control methods:
move_spd_rate_ctrl: Controls the overall movement speed of the robotic arm.play_speed: Controls the time scaling of trajectory replay.
Advantages of this dual control:
- Hardware-level speed control ensures safety.
- Software-level time scaling provides flexibility.
- The combination of both enables precise speed control.
Role of Play Interval:
The play_interval parameter plays an important role in continuous replay:
- Provides recovery time for the system.
- Facilitates observation and analysis of replay effects.
- Can be used for rhythm control in production environments.
5.2. Data Loading: Reading Data Files
try:
with open(CSV_path, 'r', encoding='utf-8') as f:
track = list(csv.reader(f))
if not track:
print("ERROR: Trajectory file is empty")
exit()
track = [[float(j) for j in i] for i in track # Convert to float lists
except FileNotFoundError:
print("ERROR: Trajectory file not found")
exit()
Exception Handling Strategy:
The program adopts a comprehensive exception handling mechanism to cover common file operation errors:
FileNotFoundError: Handles cases where the file does not exist.- Empty file check: Prevents reading empty data files.
- Data format verification: Ensures data can be correctly converted to numerical types.
Data type conversion: The program uses list comprehensions to convert string data to floating-point numbers.
5.3. Safety Stop Function
def stop():
'''Stop robotic arm; must call this function when first exiting teach mode before using CAN mode'''
interface.EmergencyStop(0x01)
time.sleep(1.0)
limit_angle = [0.1745, 0.7854, 0.2094] # Arm only restored when joints 2,3,5 are within safe range
pos = get_pos()
while not (abs(pos[1]) < limit_angle[0] and abs(pos[2]) < limit_angle[0] and pos[4] < limit_angle[1] and pos[4] > limit_angle[2]):
# Restore arm
piper.disable_arm()
time.sleep(0.01)
pos = get_pos()
time.sleep(1.0)
Staged Stop Strategy:
The stop function adopts a phased safety stop strategy:
- Emergency stop phase:
EmergencyStop(0x01)sends an emergency stop command to immediately halt all joint movements (joints with impedance). - Safe position waiting: Waits for key joints (joints 2, 3, and 5) to move within the safe range.
- System recovery phase: Sends a recovery command to reactivate the control system.
Safety Range Design:
The program focuses on the positions of joints 2, 3, and 5 based on the mechanical structure characteristics of the PIPER robotic arm:
- Joint 2 (shoulder joint): Controls the pitching movement of the upper arm, affecting overall stability.
- Joint 3 (elbow joint): Controls the angle of the forearm, directly influencing the end position.
- Joint 5 (wrist joint): Controls the end posture, affecting the gripper direction.
The safety angle ranges (10°, 45°, 12°) are set based on the following considerations:
- Prevent the robotic arm from falling rapidly under gravity.
- Ensure joints do not collide with mechanical limits.
- Provide sufficient operating space for subsequent movements.
Real-time monitoring mechanism: The program uses real-time polling to monitor joint positions, ensuring the next operation is performed only when safety conditions are met.
5.4. System Enable Function
def enable():
while not piper.enable_arm():
time.sleep(0.01)
if have_gripper:
interface.ModeCtrl(0x01, 0x01, move_spd_rate_ctrl, 0x00)
piper.enable_gripper()
time.sleep(0.01)
print("INFO: Enable successful")
Robotic Arm Enable: enable_arm()
Gripper Enable: enable_gripper()
Control Mode Settings:
Parameters for ModeCtrl(0x01, 0x01, move_spd_rate_ctrl, 0x00):
- First parameter (0x01): Set to CAN command control mode.
- Second parameter (0x01): Set to joint movement mode.
- Third parameter (0 to 100): Set the movement speed percentage.
- Fourth parameter (0x00): Set to position-speed mode.
5.5. Replay Control Logic
count = 0
while play_times == 0 or abs(play_times) != count:
input("step 2: Press Enter to start trajectory replay")
for n, pos in enumerate(track):
piper.move_j(pos[1:-1], move_spd_rate_ctrl)
if have_gripper and len(pos) == 8:
piper.move_gripper(pos[-1], 1)
print(f"INFO: replay #{count + 1}, wait time: {pos[0] / play_speed:0.4f}s, target position: {pos[1:]}")
if n == len(track) - 1:
time.sleep(play_interval)
else:
time.sleep(pos[0] / play_speed)
count += 1
Joint Control: move_j()
- First parameter: A tuple containing the radians of the six joint motors.
- Second parameter: Movement speed percentage, ranging from 0 to 100.
Gripper Control: move_gripper()
- First parameter: Gripper opening distance in meters.
- Second parameter: Gripper torque in N/m.
Replay Speed Adjustment Mechanism:
pos[0] / play_speed enables dynamic adjustment of trajectory replay speed:
play_speed > 1.0: Accelerated replay with shortened time intervals.play_speed < 1.0: Decelerated replay with extended time intervals.play_speed = 1.0: Replay at original speed.
Advantages of this implementation:
- Maintains the relative time relationship of the trajectory.
- Provides simple and intuitive speed control.
- Eliminates the need to recalculate the entire trajectory.
5.6. Complete Code of the Trajectory Replay Program
#!/usr/bin/env python3
# -*-coding:utf8-*-
# Play continuous trajectory
import os, time, csv
from piper_sdk import *
if __name__ == "__main__":
# Whether there is a gripper
have_gripper = True
# replay times (0 means infinite loop)
play_times = 1
# replay interval in seconds
play_interval = 1.0
# Motion speed percentage (recommended range: 10-100)
move_spd_rate_ctrl = 100
# replay speed multiplier (recommended range: 0.1-2)
play_speed = 1.0
# CAN mode switch timeout in seconds
timeout = 5.0
# CSV file path for saved trajectory
CSV_path = os.path.join(os.path.dirname(__file__), "trajectory.csv")
# Read trajectory file
try:
with open(CSV_path, 'r', encoding='utf-8') as f:
track = list(csv.reader(f))
if not track:
print("ERROR: Trajectory file is empty")
exit()
track = [[float(j) for j in i] for i in track # Convert to float lists
except FileNotFoundError:
print("ERROR: Trajectory file not found")
exit()
# Initialize and connect to robotic arm
piper = Piper("can0")
interface = piper.init()
piper.connect()
time.sleep(0.1)
def get_pos():
joint_state = piper.get_joint_states()[0]
if have_gripper:
'''Get current joint angles and gripper opening distance'''
return joint_state + (piper.get_gripper_states()[0][0], )
return joint_state
def stop():
'''Stop robotic arm; must call this function when first exiting teach mode before using CAN mode'''
interface.EmergencyStop(0x01)
time.sleep(1.0)
limit_angle = [0.1745, 0.7854, 0.2094] # Arm only restored when joints 2,3,5 are within safe range
pos = get_pos()
while not (abs(pos[1]) < limit_angle[0] and abs(pos[2]) < limit_angle[0] and pos[4] < limit_angle[1] and pos[4] > limit_angle[2]):
time.sleep(0.01)
pos = get_pos()
# Restore arm
piper.disable_arm()
time.sleep(1.0)
def enable():
while not piper.enable_arm():
time.sleep(0.01)
if have_gripper:
interface.ModeCtrl(0x01, 0x01, move_spd_rate_ctrl, 0x00)
piper.enable_gripper()
time.sleep(0.01)
print("INFO: Enable successful")
print("step 1: Ensure robotic arm has exited teach mode before replay")
if interface.GetArmStatus().arm_status.ctrl_mode != 1:
over_time = time.time() + timeout
stop() # Required when first exiting teach mode
while interface.GetArmStatus().arm_status.ctrl_mode != 1:
if over_time < time.time():
print("ERROR: CAN mode switch failed. Please confirm teach mode is exited")
exit()
interface.ModeCtrl(0x01, 0x01, move_spd_rate_ctrl, 0x00)
time.sleep(0.01)
enable()
count = 0
input("step 2: Press Enter to start trajectory replay")
while play_times == 0 or abs(play_times) != count:
for n, pos in enumerate(track):
piper.move_j(pos[1:-1], move_spd_rate_ctrl)
if have_gripper and len(pos) == 8:
piper.move_gripper(pos[-1], 1)
print(f"INFO: replay #{count + 1}, wait time: {pos[0] / play_speed:0.4f}s, target position: {pos[1:]}")
if n == len(track) - 1:
time.sleep(play_interval) # Final point delay
else:
time.sleep(pos[0] / play_speed) # Point-to-point delay
count += 1
6. Summary
Based on the AgileX PIPER robotic arm, the above has realized the continuous trajectory recording and replay functions. Through the application of Python SDK, the recording and repeated execution of the robotic arm’s trajectory can be achieved, providing strong technical support for teaching demonstrations and automated operations.
If you have any questions or feedback about this implementation, feel free to share in the comments. Let’s discuss and improve it together! You can also contact us directly at support@agilex.ai for further assistance.
Thanks,
AgileX Robotics