Project files

Mockway Robot - Inverse Dynamics Testing

Inverse dynamics testing project for the Mockway robot based on the Pinocchio library.

Overview

This test project uses Pinocchio (an efficient rigid-body dynamics library) to compute the inverse dynamics of the Mockway robot.

Inverse Dynamics: Given the joint position, velocity, and acceleration, compute the required joint torques.

Mathematical expression:

τ = M(q)·a + C(q,v)·v + g(q)

Where:

• τ: joint torque
• M(q): mass matrix (inertia matrix)
• C(q,v): Coriolis and centrifugal force matrix
• g(q): gravity term
• q: joint position
• v: joint velocity
• a: joint acceleration

Features

This test program implements the following features:

Offline Simulation Testing

1. Static Configuration Test: Calculate gravity compensation torque for multiple static poses
2. Trajectory Tracking Test: Calculate required torque along a sine trajectory and visualize it
3. Dynamics Decomposition Test: Verify the correctness of M, C, and g terms
4. Visualization Output: Generate time-series plots for position, velocity, acceleration, and torque

Real-time Torque Compensation Control (Newly Added)

1. Real-time Gravity Compensation: Gravity torque compensation with a control frequency of 200Hz
2. Full Dynamics Compensation: Full dynamics compensation (inertia + Coriolis + centrifugal + gravity) based on acceleration estimation
3. Acceleration Estimation: Velocity differentiation + low-pass filtering (adjustable cutoff frequency)
4. Multi-mode Switching: Supports runtime switching of compensation modes
5. Configuration System: Supports YAML configuration files and command-line parameter overrides

Installing Dependencies

Method 1: Install using pip

cd tools/dynamics_test
pip install -r requirements.txt

Method 2: Install using conda

# Create a new environment (recommended)
conda create -n mockway_dynamics python=3.8
conda activate mockway_dynamics

# Install Pinocchio
conda install pinocchio -c conda-forge

# Install other dependencies
pip install numpy matplotlib

Dependency Information

• pin (Pinocchio >=3.0.0): Rigid body dynamics library
• numpy (>=1.20.0): Numerical computation
• matplotlib (>=3.3.0): Data visualization

Usage

Running Dynamics Simulation Tests

cd tools/dynamics_test
python inverse_dynamics_test.py

This will run all three tests:

1. Static configuration test
2. Dynamics decomposition test
3. Trajectory tracking test (and generate visualization charts)

Running Real-Time Torque Compensation Control (New)

cd tools/dynamics_test
python realtime_torque_compensation.py

Prerequisites:

• Hardware connection: Ensure the USB-CAN adapter is connected (default COM9)
• Motor configuration:
  • Joint 1: DM-J4310-2EC motor, CAN ID = 1
  • Joint 2: DM4340 motor, CAN ID = 2
• Safety note: Before the first run, ensure the robotic arm is in a safe position

Function Modes:

1. Gravity Compensation: Real-time compensation for gravitational torque, allowing the robotic arm to be manually moved and feel "weightless"
2. Full Dynamics Compensation: Compensates for gravity, Coriolis force, and centrifugal force
3. Interactive Control: Allows real-time switching of compensation modes and parameter adjustments

Output Example

============================================================
Mockway Robot - Inverse Dynamics Testing with Pinocchio
============================================================

Model loaded: mockway_description
Number of joints: 2
Joint names: ['joint1', 'joint2']

============================================================
TEST 1: Static Configurations (Zero Velocity & Acceleration)
============================================================

Zero position:
  Joint positions: [0. 0.] deg
  Required torques: [0. 0.] Nm
  Gravity torques:  [0. 0.] Nm

Joint1 = 45°:
  Joint positions: [45. 0.] deg
  Required torques: [0.123 0.456] Nm
  Gravity torques:  [0.123 0.456] Nm

...

Code Structure

dynamics_test/
├── inverse_dynamics_test.py         # Inverse dynamics simulation test program
├── realtime_torque_compensation.py  # Real-time torque compensation control program (newly added)
├── requirements.txt                 # Python dependencies
├── README.md                        # This document
└── trajectory_results.png           # Generated visualization results (created after execution)

Core APIs

MockwayDynamics Class

class MockwayDynamics:
    def __init__(self, urdf_path)
    def compute_inverse_dynamics(self, q, v, a) -> tau
    def compute_mass_matrix(self, q) -> M
    def compute_coriolis(self, q, v) -> c
    def compute_gravity(self, q) -> g
    def get_end_effector_pose(self, q) -> (position, orientation)

Usage Example

from inverse_dynamics_test import MockwayDynamics
import numpy as np

# Initialize
dynamics = MockwayDynamics("path/to/mockway_description.urdf")

# Define joint states
q = np.array([0.5, 0.3])      # Position [rad]
v = np.array([0.1, -0.2])     # Velocity [rad/s]
a = np.array([1.0, 0.5])      # Acceleration [rad/s²]

# Compute required torques
tau = dynamics.compute_inverse_dynamics(q, v, a)
print(f"Required torques: {tau} Nm")

# Compute individual terms separately
M = dynamics.compute_mass_matrix(q)
c = dynamics.compute_coriolis(q, v)
g = dynamics.compute_gravity(q)

Test Instructions

Test 1: Static Configuration Test

Test the static balance (zero velocity, zero acceleration) at different joint positions. In this case, the required torque should be equal to the gravity compensation torque.

Test 2: Dynamics Decomposition Test

Verify the correctness of inverse dynamics by comparing:

• Results from the RNEA algorithm (Recursive Newton-Euler Algorithm)
• Results from manual calculation of M·a + C·v + g

The two should be consistent.

Test 3: Trajectory Tracking Test

Move along a sine trajectory, calculate the required joint torque at each moment, and generate visualization charts:

• Joint position curve
• Joint velocity curve
• Joint acceleration curve
• Required joint torque curve

References

Pinocchio Documentation

• Official website: https://stack-of-tasks.github.io/pinocchio/ (opens in a new tab)
• GitHub: https://github.com/stack-of-tasks/pinocchio (opens in a new tab)
• Tutorials: https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/ (opens in a new tab)

Inverse Dynamics Related

• Rigid Body Dynamics Algorithms (opens in a new tab) by Roy Featherstone
• Modern Robotics (opens in a new tab) by Kevin M. Lynch and Frank C. Park

Frequently Asked Questions

Q: Why is the torque not zero under static configuration?

A: Even when the robot is stationary, gravity still produces torque. Inverse dynamics calculates the torque required to maintain the given state, including gravity compensation.

Q: How do I modify the test trajectory?

A: Modify the amplitude and frequency parameters in the test_trajectory_tracking() function.

Q: Can it be used for other robots?

A: Yes, simply change the URDF file path to your robot model. Ensure the URDF includes correct inertial parameters.

Q: What should I do if the torque output jitters in full dynamics mode?

A: This is usually caused by inaccurate acceleration estimates due to velocity measurement noise. Try the following:

1. Lower the filter cutoff frequency (e.g., from 5 Hz to 2-3 Hz)
2. Check if the motor velocity feedback is stable
3. Ensure the control frequency is sufficiently high (recommended ≥200 Hz)

Q: How do I choose an appropriate filter cutoff frequency?

A: Select based on motion speed:

• Slow teaching (< 30°/s): 2-3 Hz, smoother torque output
• Normal motion (30-90°/s): 5 Hz (default), balances response and noise
• Fast motion (> 90°/s): 8-10 Hz, reduces phase delay

Principle: The faster the speed, the higher the cutoff frequency; the smoother the requirement, the lower the cutoff frequency.

Q: Why is the difference between gravity compensation and full dynamics compensation not significant?

A: In slow motion, inertial forces, Coriolis forces, and centrifugal forces are relatively small, with gravity dominating. The advantages of full dynamics compensation only become apparent during fast motion or frequent acceleration/deceleration.

Detailed Explanation of Real-Time Torque Compensation Control

RealtimeTorqueController Class

New Feature: Real-time dynamics torque compensation controller

class RealtimeTorqueController:
    def __init__(self, config: DynamicsTestConfig = None)
    def setup()                           # Initialize CAN and motors
    def enable_motors()                   # Enable motors
    def disable_motors()                  # Disable motors
    def start_control(mode="gravity")     # Start control loop
    def stop_control()                    # Stop control loop
    def get_state_snapshot()              # Get current state
    def shutdown()                        # Shut down controller

    # Internal methods
    def _compute_filter_alpha(cutoff_freq)     # Compute filter coefficient
    def _apply_lowpass_filter(new, old, alpha) # Apply low-pass filter
    def compute_compensation_torque(q, v, mode) # Compute compensation torque (including acceleration estimation)

Compensation Mode Description

1. "gravity" - Gravity compensation mode

   • Compensates only gravity torque: tau = g(q)
   • Applicable scenarios: Manual teaching, drag teaching
   • Effect: The robotic arm feels "weightless," making manual movement easier

2. "full_dynamics" - Full dynamics compensation mode

   • Compensates all dynamics terms: tau = M(q)·a + C(q,v)·v + g(q)
   • Acceleration estimation: Calculated via velocity differentiation a = (v - v_prev) / dt
   • Low-pass filtering: Applied to acceleration to reduce noise (cutoff frequency 5Hz)
   • Applicable scenarios: Control requiring precise dynamics modeling, compensation during fast motion
   • Effect: Compensates gravity, Coriolis force, centrifugal force, and inertial force

3. "none" - No compensation mode

   • Provides no compensation
   • Used for comparative testing

Control Parameters

• control_rate: Control frequency (default 200 Hz)
• kp: Position gain in MIT control mode (default 0.0, pure torque control)
• kd: Damping gain in MIT control mode (default 1.0)
• accel_filter_cutoff: Cutoff frequency of the low-pass filter for acceleration (default 5.0 Hz)
  • Lower frequency (2-3 Hz): Smoother filtering, suitable for slow movements
  • Higher frequency (8-10 Hz): Faster response, suitable for fast movements

Usage Examples

from realtime_torque_compensation import RealtimeTorqueController
from config_loader import load_config
import time

# Method 1: Use default configuration
controller = RealtimeTorqueController()

# Method 2: Load configuration file
config = load_config('config/dynamics_test.yaml')
controller = RealtimeTorqueController(config)

# Method 3: Adjust filter parameters at runtime
controller = RealtimeTorqueController()
controller._accel_filter_cutoff = 8.0  # Increase cutoff frequency to 8Hz
controller._accel_filter_alpha = controller._compute_filter_alpha(8.0)

try:
    # Setup and enable
    controller.setup()
    controller.enable_motors()

    # Start full dynamics compensation (using acceleration estimation)
    controller.start_control(mode="full_dynamics")

    # Run for a period of time
    time.sleep(10.0)

    # Get current state
    state = controller.get_state_snapshot()
    print(f"Position: {state['q']} rad")
    print(f"Velocity: {state['v']} rad/s")
    print(f"Torque: {state['tau']} Nm")

finally:
    # Shutdown
    controller.shutdown()

Safety Notes

⚠️ Important Safety Instructions:

1. First Run: Ensure the robotic arm is in a safe position with no obstacles around
2. Emergency Stop: Press Ctrl+C at any time to stop the program
3. Torque Limit: Motors have torque limits (Joint1: 9Nm, Joint2: 28Nm)
4. Temperature Monitoring: Monitor motor temperature during prolonged operation
5. CAN Connection: Ensure stable CAN communication to avoid communication loss

Technical Features

• Real-time performance: 200Hz control frequency, meeting real-time control requirements
• Thread safety: Uses lock mechanisms to protect shared state
• Modular design: Dynamics computation decoupled from motor control
• Multi-mode support: Compensation modes can be switched at runtime
• Acceleration estimation: Real-time acceleration estimation based on velocity differentiation
• Noise suppression: First-order low-pass filter reduces the impact of measurement noise

Acceleration Estimation and Filtering Techniques

In the full dynamics compensation mode, the system uses the following methods to estimate joint acceleration:

1. Velocity Differencing

   a_raw[k] = (v[k] - v[k-1]) / dt
   

   where dt is the control period (default 5ms @ 200Hz)

2. First-Order Low-Pass Filter

   a_filtered[k] = α · a_raw[k] + (1-α) · a_filtered[k-1]
   

   Filter coefficient: α = dt / (RC + dt), where RC = 1 / (2π · fc)

   Default cutoff frequency fc = 5 Hz corresponds to:

   • 3dB attenuation point at 5Hz
   • Phase delay of approximately 18ms
   • Effective suppression of high-frequency noise (>20Hz)

3. Parameter Tuning Recommendations

   • Slow motion (< 30°/s): Use 2-3 Hz cutoff frequency for smoother torque output
   • Medium-speed motion (30-90°/s): Use 5 Hz cutoff frequency (default), balancing response speed and noise
   • Fast motion (> 90°/s): Use 8-10 Hz cutoff frequency to reduce phase delay

Comparison of Dynamic Compensation Effects

License

This test project follows the overall project license agreement.