A simulation-based assistive feeding robot with impedance-based safety monitoring for individuals with upper-limb impairments

Developed by Team 1 - MS Robotics, Northeastern University

Note: Demo video showcasing the feeding robot in action. Upload media/videos/feeding_demo.webm by editing this README on GitHub and dragging the file here

This project presents a comprehensive simulation framework for assistive feeding robotics, addressing a critical need for approximately 6.8 million Americans living with upper-limb impairments. The system employs a Universal Robots UR5e manipulator to execute safe, smooth spoon-to-mouth feeding trajectories while maintaining continuous safety monitoring.

1. Hybrid Motion Planning Strategy

   • Combines joint-space interpolation for singularity avoidance during large reconfigurations
   • Uses Cartesian linear interpolation for precise, orientation-preserving feeding motions
   • Intelligently switches between methods based on task phase requirements

2. Sensorless Impedance-Based Safety Monitoring

   • Estimates contact forces from position tracking errors without expensive force/torque sensors
   • Implements phase-dependent adaptive stiffness scaling for context-aware safety
   • Provides emergency stop capability when estimated forces exceed 25 N threshold

3. MoveIt2 Integration with Semantic IK Seeding

   • Phase-aware inverse kinematics seed selection for consistent, predictable trajectories
   • Collision-aware motion planning with full environmental modeling
   • Real-time trajectory execution via ros2_control

Commercial feeding devices like Obi and Meal Buddy rely on pre-programmed, open-loop trajectories without force feedback. This creates two significant limitations:

• No adaptability: Robot continues on its path even if the user moves unexpectedly
• No force detection: System cannot detect contact or modulate behavior for gentle interaction

Our solution addresses these limitations through intelligent motion planning and real-time safety monitoring.

Comparison showing improved trajectory smoothness and force regulation with impedance-based monitoring

The hybrid approach successfully resolves the fundamental trade-off between singularity avoidance and end-effector orientation preservation:

• Transport Phase (Joint-Space): Robust, singularity-free motion for large reconfigurations
• Feeding Phase (Cartesian-Space): Straight-line paths with < 2° orientation deviation
• Pure Cartesian planning: Failed due to singularities near folded configurations
• Pure joint-space planning: Succeeded but produced 15° orientation deviations

The impedance-based safety monitoring demonstrated:

• Nominal operation: Peak force 19.12 N (24% below 25 N threshold)
• Emergency stop test: Correctly triggered at 45.91 N during challenging conditions
• Average tracking force: 5.72 N indicates accurate trajectory following
• Zero false negatives: All unsafe conditions detected
• Signal filtering: Rolling mean filter (window=8) essential for eliminating physics engine noise

Trajectory execution achieved research-grade smoothness:

• Joint velocities: Smooth sinusoidal profiles, peak 0.148 rad/s (well below UR5e limits)
• Jerk magnitude: ±0.015 rad/s³ indicates smooth, comfortable motion
• No discontinuities: Continuous trajectories throughout 8-waypoint sequence
• Time parameterization: 8-10 seconds per segment with real-time factor 1.0

Cartesian Waypoints → Inverse Kinematics (MoveIt2) → Hybrid Motion Planner
                                                              ↓
                                                    Joint Trajectories
                                                              ↓
                                            ros2_control Trajectory Controller
                                                              ↓
                                                    Gazebo Simulation
                                                              ↓
                                            Joint State Feedback (100 Hz)
                                                              ↓
                                          Impedance Safety Monitor (10 Hz)
                                                              ↓
                                              Emergency Stop (if force > 25 N)

• Middleware: ROS2 Jazzy Jalisco
• Simulation: Gazebo Harmonic
• Motion Planning: MoveIt2 with KDL kinematics
• Control: ros2_control with scaled joint trajectory controller
• Robot Model: Universal Robots UR5e (6-DOF manipulator)
• Languages: Python 3.12, YAML configuration

1. Inverse Kinematics with Semantic Seeding

   • Phase-aware seed configuration selection
   • Biases IK solver toward kinematically appropriate solutions
   • Prevents unnecessary joint rotations between waypoints

2. Phase-Dependent Stiffness Scaling

   • Base stiffness: 100 N/m in each Cartesian direction
   • Adaptive gains: 0.15-1.0 based on task phase
   • Lower gains near user's face increase safety sensitivity

3. Emergency Stop Mechanism

   • "Hold position" trajectory with zero velocities
   • Response time: 0.5 seconds
   • Prevents continued force application during collisions

4. Collision Environment Setup

   • Human model: Torso (box), head (sphere), arms (cylinders)
   • Furniture: Table, chair primitives
   • Enables MoveIt collision checking during planning

• Operating System: Ubuntu 24.04 LTS (required)
• ROS2: Jazzy Jalisco
• Simulation: Gazebo Harmonic
• Hardware: 8GB RAM minimum (16GB recommended), multi-core processor

# Install ROS2 Jazzy
sudo apt update
sudo apt install ros-jazzy-desktop

# Install Gazebo Harmonic
sudo add-apt-repository ppa:openrobotics/gazebo-harmonic
sudo apt update
sudo apt install gz-harmonic

# Install MoveIt2 and UR packages
sudo apt install -y \
    ros-jazzy-moveit \
    ros-jazzy-moveit-visual-tools \
    ros-jazzy-ur-simulation-gz \
    ros-jazzy-ur-robot-driver \
    ros-jazzy-ur-moveit-config \
    ros-jazzy-ros-gz-sim \
    ros-jazzy-ros-gz-bridge \
    ros-jazzy-plotjuggler-ros

# Install Python dependencies
pip3 install numpy matplotlib pandas scipy

# Clone and build
cd ~/feeding_robot_ws
colcon build --packages-select feeding_trajectory

# Source the workspace
source install/setup.bash

Terminal 1 - Launch Gazebo:

source /opt/ros/jazzy/setup.bash
ros2 launch ur_simulation_gz ur_sim_control.launch.py ur_type:=ur5e

Terminal 2 - Launch MoveIt + RViz:

source /opt/ros/jazzy/setup.bash
ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur5e use_sim_time:=true

Terminal 3 - Run Feeding Motion:

source ~/feeding_robot_ws/install/setup.bash
ros2 run feeding_trajectory monitored_feeding

source ~/feeding_robot_ws/install/setup.bash
ros2 launch feeding_trajectory visualized_feeding.launch.py

This automatically launches Gazebo, MoveIt, RViz, PlotJuggler, and the feeding node with coordinated timing.

The robot will:

1. Move from home position to safe height
2. Approach the plate with downward orientation
3. Reach the plate position (food scooping)
4. Lift and approach the user's mouth region
5. Deliver food at mouth position
6. Retract safely from face
7. Return to home position

Duration: Approximately 80-85 seconds for complete sequence

After each run, comprehensive logs are saved to:

~/feeding_robot_ws/logs/run_YYYYMMDD_HHMMSS/
├── joint_states.csv          # Joint positions, velocities, efforts
├── ee_poses.csv              # End-effector poses (position + orientation)
├── forces.csv                # Virtual force estimates
├── tracking_errors.csv       # Position tracking errors
├── metadata.json             # Run configuration and statistics
└── analysis_dashboard.png    # Auto-generated visualization

# Navigate to visualization directory
cd ~/feeding_robot_ws/src/feeding_trajectory/feeding_trajectory/visualization

# Generate plots from latest run
python3 generate_plots.py

# Or specify a specific run
python3 generate_plots.py --log-dir ~/feeding_robot_ws/logs/run_20241210_171356

The analysis dashboard includes:

• Joint position trajectories over time
• Joint velocity profiles
• Estimated interaction forces with safety threshold
• Motion smoothness (jerk) analysis

Configuration parameters are defined in:

src/feeding_trajectory/feeding_trajectory/config/robot_config.yaml

Safety Thresholds:

safety:
  max_force: 25.0              # Emergency stop threshold (N)
  workspace_limits:            # Cartesian boundaries (m)
    x_min: 0.15, x_max: 0.80
    y_min: -0.20, y_max: 0.65
    z_min: 0.35, z_max: 0.95
  human_zone:
    center: [0.50, 0.0, 0.87]
    radius: 0.25
    max_velocity: 0.02
    max_force: 3.5

Impedance Control:

impedance_control:
  stiffness:
    translation: [100.0, 100.0, 100.0]  # N/m
    rotation: [10.0, 10.0, 10.0]         # Nm/rad
  adaptive_gains:
    home: 1.0
    at_plate: 0.3
    at_mouth: 0.15                       # Most compliant near face

Feeding Waypoints: Waypoints specified as [x, y, z] position (meters) and [roll, pitch, yaw] orientation (radians):

feeding_waypoints:
  home:
    position: [0.22, 0.0, 0.75]
    orientation: [3.14, 0.0, 0.0]
  at_plate:
    position: [0.35, 0.58, 0.52]
    orientation: [3.14, 0.3, 0.9]
  at_mouth:
    position: [0.43, 0.0, 0.78]
    orientation: [3.14, 0.0, 0.0]

For detailed methodology, experimental validation, and theoretical foundations, see: AR Team 1 Final Report.pdf

The report includes:

• Comprehensive literature review
• Detailed system architecture
• Mathematical foundations of impedance control
• Safety system validation experiments
• Comparative analysis of planning strategies
• Future research directions

• Detailed Setup Guide: src/feeding_trajectory/README.md
• Code Documentation: See CLAUDE.md for architecture overview
• Configuration Reference: robot_config.yaml with inline comments

Based on experimental findings, three key development directions are planned:

1. Camera-Based Head Tracking

   • Integrate depth camera (RealSense/Kinect)
   • Implement real-time head pose estimation
   • Enable dynamic waypoint adjustment as user moves

2. Physical Hardware Deployment

   • Port to real UR5e manipulator
   • Integrate force/torque sensor for ground truth validation
   • Compare sensorless estimation with actual force measurements
   • Address sim-to-real transfer challenges

3. True Impedance Control

   • Implement real-time trajectory modulation based on forces
   • Replace binary safe/unsafe logic with continuous compliance
   • Enable gentle, adaptive interaction during contact

Team 1 - Assistive Robotics, Fall 2024

Program: MS Robotics Institution: Northeastern University, Boston, USA Date: December 2024

• ROS2 Jazzy and MoveIt2 communities
• Universal Robots for UR5e simulation models
• Gazebo simulation framework
• Open Robotics for ros2_control infrastructure
• Course instructors and teaching assistants

1. D. Park et al., "Active robot-assisted feeding with a general-purpose mobile manipulator in real homes," Robotics and Autonomous Systems, vol. 124, 2020.

2. N. Hogan, "Impedance control: An approach to manipulation," Journal of Dynamic Systems, Measurement, and Control, vol. 107, no. 1, pp. 1–24, 1985.

3. ISO/TS 15066:2016, "Robots and robotic devices—Collaborative robots," International Organization for Standardization, 2016.

4. T. Bhattacharjee et al., "Towards robotic feeding: Role of haptics in fork-based food manipulation," IEEE Robotics and Automation Letters, vol. 4, no. 2, pp. 1485–1492, 2019.

5. S. Haddadin et al., "Robot collisions: A survey on detection, isolation, and identification," IEEE Transactions on Robotics, vol. 33, no. 6, pp. 1292–1312, 2017.

This project was developed as part of academic coursework at Northeastern University.

• ROS2 Jazzy Documentation
• MoveIt2 Tutorials
• Gazebo Harmonic Documentation
• Universal Robots ROS2 Driver