Continuous Trajectory Recording and Replay for AgileX PIPER

Introduction

Master the art of robotic arm motion! This project demonstrates how to implement continuous trajectory recording and replay on the AgileX PIPER robotic arm. Whether you're building teaching demonstrations or automating complex operations, this comprehensive guide will walk you through every step—from setup to deployment.

Function Demonstration

What you'll learn:

• Record complex motion trajectories with time-series data
• Replay recorded movements with precision control
• Integrate Python SDK with CAN communication
• Implement safety mechanisms for robotic arms

Project Overview

This solution captures and reproduces the intricate movements of the PIPER robotic arm through intelligent time-series data management. The system supports both one-time demonstrations and infinite loop operations, making it perfect for teaching, testing, and production automation.

Key Features:

• Real-time continuous trajectory recording in teach mode
• Multiple replay modes (single, multiple, infinite loop)
• Dual-level speed control system
• Safety mechanisms with automatic joint positioning
• CSV-based data persistence for easy portability
• Full gripper integration support

Getting Started

What You'll Need

Hardware:

• AgileX PIPER robotic arm
• USB-to-CAN module
• Computer with Linux capability

Software Requirements:

• Ubuntu 18.04 or higher
• Python 3.6 or higher
• Git version control

Pre-Operation Checklist

Before you start, ensure your workspace is ready:

• Clear all obstacles around the robotic arm
• Verify power supply is stable and all indicator lights are normal
• Ensure good lighting for position observation
• Test gripper actions (if equipped)
• Confirm stable ground to avoid vibration
• Test teach button functionality

Environment Setup

Step 1: Install System Tools

bash

sudo apt install git python3-pip can-utils ethtool

Step 2: Install CAN Tools and Configure

bash

# Install CAN configuration
sudo ip link set can0 up type can bitrate 1000000

Step 3: Clone and Install PIPER SDK

bash

# Clone the 1_0_0_beta version (API-enabled)
git clone -b 1_0_0_beta https://github.com/agilexrobotics/piper_sdk.git
cd piper_sdk
pip3 install .

Step 4: Clone Project Repository

bash

git clone https://github.com/agilexrobotics/Agilex-College.git
cd Agilex-College/piper/recordAndPlayTraj/

Recording Your First Trajectory

Recording Workflow

Step 1: Power Up

• Power on the PIPER arm
• Connect USB-to-CAN module to your computer

Step 2: Start Recording Program

bash

python3 recordTrajectory_en.py

Step 3: Enter Teach Mode

• Short-press the teach button
• The arm enters teach mode (joints become free to move)

Step 4: Record Motion

• Press Enter in terminal when ready
• Manually guide the arm through your desired trajectory
• The program records position changes in real-time

Step 5: Exit Teaching

• Short-press the teach button again to exit teach mode
• Recording automatically stops

Recording Best Practices

Motion Planning:

• Start from a safe, accessible initial position
• Avoid positions close to joint limits
• Plan smooth paths without sharp direction changes
• Consider kinematic constraints and singular positions

Speed Control:

• Maintain moderate movement speed for consistent data quality
• Slow down at critical positions for better accuracy
• Avoid sudden acceleration or deceleration
• Test your trajectory speed before final recording

Data Quality Tips:

• Record in stable conditions without vibrations
• Use smooth, deliberate movements
• Include natural pauses for process steps
• Aim for 10-30 second recordings (adjust record_time parameter as needed)

Technical Deep Dive: Recording Implementation

Configuration Parameters

python

# Whether gripper is attached
have_gripper = True

# Maximum recording duration (seconds)
# 0 = unlimited, stop by terminating program
record_time = 10.0

# Teach mode detection timeout
timeout = 10.0

# CSV file save location
CSV_path = os.path.join(os.path.dirname(__file__), "trajectory.csv")

How It Works

Position Acquisition:The system continuously reads joint states and gripper position (if attached) using the get_pos() function. Data is only recorded when the arm position changes, significantly reducing file size while maintaining accuracy.

Timestamp Processing:Time intervals between recorded positions are calculated with 0.1ms precision using round(time.time() - last_time, 4). This balance provides robotic control accuracy without excessive storage overhead.

Data Format (CSV):Each row contains: [time_interval, joint1, joint2, joint3, joint4, joint5, joint6, gripper_opening]

Example:

0.1234,0.523,1.047,-0.349,0.785,-0.262,1.571,0.05
0.0856,0.524,1.049,-0.347,0.786,-0.261,1.572,0.05

Complete Recording Code

python

#!/usr/bin/env python3
# -*-coding:utf8-*-
# Record continuous trajectory
import os, time
from piper_sdk import *

if __name__ == "__main__":
    have_gripper = True
    record_time = 10.0
    timeout = 10.0
    CSV_path = os.path.join(os.path.dirname(__file__), "trajectory.csv")

    # Initialize connection
    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:
            return joint_state + (piper.get_gripper_states()[0][0], )
        return joint_state

    # Wait for teach mode
    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")
            exit()
        time.sleep(0.01)

    # Start recording
    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:
            wait_time = round(time.time() - last_time, 4)
            print(f"INFO: Wait time: {wait_time:0.4f}s, 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 to exit teach mode")

Playing Back Your Trajectory

Replay Workflow

Step 1: Exit Teach Mode

• Short-press the teach button to exit teach mode
• Important: The system will automatically perform a reset operation to bring joints 2, 3, and 5 to safe positions (zero points)

Step 2: Start Replay Program

bash

python3 playTrajectory_en.py

Step 3: Execute Replay

• Press Enter when prompted
• The arm replays the recorded trajectory
• Monitor the console output for real-time feedback

Replay Configuration

python

# Number of replays (0 = infinite loop)
play_times = 1

# Interval between replays (seconds)
play_interval = 1.0

# Motion speed percentage (10-100 recommended)
move_spd_rate_ctrl = 100

# Replay speed multiplier (0.1-2 recommended)
# 0.5 = half speed, 2.0 = double speed
play_speed = 1.0

Dual-Level Speed Control

The system provides two complementary speed controls:

• Hardware-Level (move_spd_rate_ctrl): Controls the robotic arm's physical movement speed (10-100%). Ensures safety and mechanical reliability.
• Software-Level (play_speed): Scales the time intervals between trajectory points. Doesn't require re-recording the trajectory.

Examples:

• play_speed = 0.5: Replay at half speed (perfect for demonstration)
• play_speed = 2.0: Replay at double speed (for production cycles)
• play_speed = 1.0: Replay at original recorded speed

Technical Deep Dive: Replay Implementation

Safety Stop Mechanism

When first exiting teach mode, a critical safety sequence is executed:

python

def stop():
    # Emergency stop all joint movements
    interface.EmergencyStop(0x01)
    time.sleep(1.0)
    
    # Safe joint angle ranges (in radians)
    limit_angle = [0.1745, 0.7854, 0.2094]  # 10°, 45°, 12°
    
    # Wait for joints 2, 3, 5 to reach safe positions
    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 system control
    piper.disable_arm()
    time.sleep(1.0)

Why These Three Joints?

• Joint 2 (Shoulder): Controls upper arm pitch and overall stability
• Joint 3 (Elbow): Controls forearm angle and end-effector position
• Joint 5 (Wrist): Controls end orientation and gripper direction

Safe Angle Ranges: Prevent gravity-induced falling, mechanical collisions, and ensure adequate operating space.

Arm Enable Function

python

def enable():
    # Enable joint control
    while not piper.enable_arm():
        time.sleep(0.01)
    
    # Enable gripper if attached
    if have_gripper:
        interface.ModeCtrl(0x01, 0x01, move_spd_rate_ctrl, 0x00)
        piper.enable_gripper()
        time.sleep(0.01)
    
    print("INFO: Enable successful")

Complete Replay Code

python

#!/usr/bin/env python3
# -*-coding:utf8-*-
# Play continuous trajectory
import os, time, csv
from piper_sdk import *

if __name__ == "__main__":
    have_gripper = True
    play_times = 1
    play_interval = 1.0
    move_spd_rate_ctrl = 100
    play_speed = 1.0
    timeout = 5.0
    CSV_path = os.path.join(os.path.dirname(__file__), "trajectory.csv")

    # Load 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]
    except FileNotFoundError:
        print("ERROR: Trajectory file not found")
        exit()

    # Initialize connection
    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:
            return joint_state + (piper.get_gripper_states()[0][0], )
        return joint_state

    def stop():
        interface.EmergencyStop(0x01)
        time.sleep(1.0)
        limit_angle = [0.1745, 0.7854, 0.2094]
        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()
        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")

    # Ensure CAN mode is active
    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()
        while interface.GetArmStatus().arm_status.ctrl_mode != 1:
            if over_time < time.time():
                print("ERROR: CAN mode switch failed")
                exit()
            interface.ModeCtrl(0x01, 0x01, move_spd_rate_ctrl, 0x00)
            time.sleep(0.01)
    enable()

    # Execute replay
    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: {pos[0] / play_speed:0.4f}s")
            if n == len(track) - 1:
                time.sleep(play_interval)
            else:
                time.sleep(pos[0] / play_speed)
        count += 1

Troubleshooting Guide

Issue 1: "No Piper Class" Error

Error Message:ModuleNotFoundError: No module named 'Piper'

Cause: Incorrect SDK version installed

Solution:

bash

pip3 uninstall piper_sdk
git clone -b 1_0_0_beta https://github.com/agilexrobotics/piper_sdk.git
cd piper_sdk
pip3 install .

Issue 2: Arm Won't Move During Replay

Symptoms: Terminal shows "Mode switch failed" or "No response from arm"

Cause: Teach button was pressed during program execution

Solution:

• Check if the teach button indicator light is off
• If on, short-press the teach button to exit teach mode
• Re-run the replay program

Issue 3: Connection Fails

Symptoms: "CAN connection error" or "Device not found"

Cause: USB-to-CAN module not properly connected or multiple CAN modules detected

Solution:

• Verify only one USB-to-CAN module is connected
• Restart the CAN interface:

bash

sudo ip link set can0 down
   sudo ip link set can0 up type can bitrate 1000000

• Check USB permissions if needed

Advanced Usage

Recording Multiple Trajectories

Create separate recording sessions with different parameter sets:

python

# Record_fast.py
record_time = 5.0  # 5-second trajectory

# Record_slow.py
record_time = 15.0  # 15-second trajectory

# Record_infinite.py
record_time = 0  # Infinite (manual stop)

Production Automation

For continuous production cycles:

python

play_times = 0      # Infinite loop
play_interval = 2.0 # 2-second rest between cycles
play_speed = 1.2    # 20% speed increase for efficiency

Performance Optimization

Reduce file size and processing time:

python

# In recording program - increase sleep interval
time.sleep(0.02)  # Record every 20ms instead of 10ms

# Or limit recording time
record_time = 5.0  # Shorter recordings use less storage

Code Repository

Full source code available:https://github.com/agilexrobotics/Agilex-College.git

Navigate to: piper/recordAndPlayTraj/ for all scripts

Summary

You now have a complete system for recording and replaying PIPER arm trajectories with precision control. Whether for teaching demonstrations or production automation, this solution provides the foundation you need. For questions or feedback, contact support@agilex.ai or visit the GitHub repository. Happy building!