AFRo - Assisted Feeding Robot

Brief

Primary Objective

Develop a low-cost robot, to enable a user with limited motor control and/or tremors/palsy to feed themselves independently.  

Secondary Objectives

Considerations

Assumptions

Photographs

Illustration of install location for primary client:

Requirements

Requirement

Commentary

Priority

Spoon-feeding from single plate, of known size, in static location

Enable semi-automated spoon feeding, utilising common techniques from prior art (e.g. revolving plates)

M

Multiple plates/bowls

Support multiple plates/bowls, enabling the user to choose between items (e.g. main meal and snacks)

S

Present cup to suitable location

Move a cup to a suitable location to allow drinking through a straw

S

Robust design

Arm must be resilient to accidental impacts (e.g. through tremors) and recover gracefully

M

Safe design

Arm must minimise potential for harm to the user (e.g. through high speed, high inertia motions)

M

Reliability

Design life of 3 yrs

M

Maintainability

Parts should be readily serviceable by a competent engineer (with potential for remote support)

S

Simple programming interface

Usable by competent engineer in conjunction with OT

S

Remote programming/diagnostics interface

Secure, web accessible interface for remote support

C

Autonomous plate/bowl/cup configuration

Automatically adapt to varying number of plates/bowls, and associated locations/dimensions

C

Autonomous user presentation

Automatically adapt presentation to user (e.g. mouth position)

C

Parts cost target

£500 ceiling, <£300 preferable

M

Parts and labour cost target

<£500 total parts and labour cost

C

Support a range of mounting configurations

e.g. table clamp, bolt down, magnetic, large base

S

Hygienic

User-contactable parts must be removable for cleaning, all other parts must be readily cleanable

M

Quiet

Motor noise should be as quiet as possible

S

Low power

Minimal energy requirements

S

Easy to learn/use!

M

Comforting motions

Motions should be smooth, predictable and within “comfortable” working volumes in order to not disconcert the user

S

Vertical range of >40cm

M

Reach of >40cm

Ideally >50cm

M

Lift >100g at max reach

Sufficient for spoon, plus headroom

M

Lift >300g at max reach

Sufficient for lightweight mug, plus headroom

S

Use standard dessert spoon in a quick-release holder (for easy cleaning)

Established by prior art

S

Lift approx 1kg at max reach

Sufficient for full ceramic mug, with headroom

C

Lift lightweight cup (inc straw) for presentation to user

Could utilise a removable adapter for the cup and/or use a specialist cup e.g. http://www.activemobility.co.uk/cups-mugs-straws-c223_53_160/cupkennedy-p-1602.html

S

Support a variety of cup handle styles

C


Intellectual Property

Final design files, source code, etc are to be open source and made available in the public domain.  

Git repo: https://github.com/Axford/AFRo

Published copy of this document: https://docs.google.com/document/d/1HDJsVby1FxGu6IQ2XBOvq7dboFC9hibPxshcKR86C0M/pub

Licensing

TBC - Options include LGPL, TAPR OHL, MIT

… or Public Domain

Prior Art

1) Neater Eater, approx £2500

Cited reference from client - issues charactised as too bulky, too expensive, limited functionality.  Various accessory options, and choice of manual or semi-automated.

http://www.neater.co.uk/eating-aid

2) Bestic, approx $4000

Portable, single-plate device with multiple accessory options.

www.bestic.se

3) Brazilian Concept (similar to Neater Eater)

https://www.behance.net/gallery/4428639/Eating-Robot-1st-Prototype-(work-in-progress)

4) Northeastern University Prototype, approx $900

University project, using a servo-based robot arm kit and bespoke control software (with face tracking).  Supports multiple bowls.  Reach borderline, torque borderline, cost borderline.

http://www.engadget.com/2012/05/24/northeastern-university-students-develop-eye-controlled-robotic-arm-to-feed-patients/

5) Korean Research Prototype

Highly functional research prototype, non-commercial.  Target cost range was $1800 - $2700.

Extremely useful research paper: http://cdn.intechopen.com/pdfs-wm/27402.pdf

6) Handy1 - University Research Prototype

http://www.techknowlogia.org/TKL_Articles/PDF/430.pdf

7) MySpoon (SECOM, Japan)

50g weight limit.

http://www.secom.co.jp/english/myspoon/index.html

8) Winsford Feeder, $3295

https://www.ncmedical.com/item_223.html

9) Meal Buddy, $3535

Supports up to 3 bowls.

http://www.pattersonmedical.com/app.aspx?cmd=getProduct&key=IF_46883

10) Mealtime Partner Dining System, $7795 (basic model)

Supports 3 “bowls” and a variety of mounting configurations.

http://mealtimepartners.com/description/description.htm

https://www.youtube.com/watch?v=R27e_qzaOok

11) Hello Spoon

Open source, child orientated, portable assisted feeding arm.   Design is based on Dynamixel servos (5DOF), and is controlled via an Android phone.  Reach/torque are limited, but design has potential to scale and could incorporate multiple plates/bowls.  Indiegogo campaign places retail at $499, parts costs <$399.

http://hellospoon.weebly.com/who-is-hello-spoon.html

https://www.youtube.com/channel/UCtfMeLETlUGEQrYLjg5y_9A

Alternate option for cup presentation:

Summary of Prior Art

Although a wide range of existing solutions are available, all of the commercial options fall far outside the target cost and many have limited functionality vs the requirements.  The majority of research prototypes also fall outside the target cost range.

However, there are a number of lessons to be gleaned from associated literature:

Commercial Robot-Arm Options

Several commercial options (kits and fully assembled units) have been evaluated for suitability for adaptation, none fall within the design requirements - most failing on insufficient reach, torque and non-compliant joints.

1)  Maplin Robot Arm Kit (and similar variants)

Insufficient reach, torque and non-compliant joints

http://www.maplin.co.uk/p/robotic-arm-kit-with-usb-pc-interface-a37jn?gclid=CKrTgNrSjcACFVHKtAod5UMAhQ

2)  AREXX RA1-PRO

Insufficient reach, torque and non-compliant joints

http://cpc.farnell.com/jsp/displayProduct.jsp?sku=HK01206&CMP=CPC-PLA&gross_price=true&gclid=CKze9-vSjcACFXDKtAodDTQAgg

3) Lynxmotion AL series (e.g. AL5D)

Insufficient reach (26cm), non-compliant joints

http://www.lynxmotion.com/c-130-al5d.aspx

4) Trossen WidowX

Borderline reach (35-50cm), too expensive ($1500)

http://www.trossenrobotics.com/widowxrobotarm

5) Trossen PhantomX

Borderline reach (36 - 50cm), insufficient torque (150g max), too expensive for base device ($500)

6) Palletisizing Robot 1

Reach ok (50cm radius, 40cm height), borderline torque (125g max), and a bit high on price ($294 CAD) - also lacks compliant joints (geared steppers).  Open source design, parts on thingiverse.

https://www.marginallyclever.com/shop/robot-arms/palletizing-robot-1


Design

Early CAD visual:

Design Notes

Mechanical design

Mechanical Design Thoughts

Stuff to control/interface with

Dynamixel AX-12A Servo(s) x4

Stepper

Homing switch - at bottom of z, analog hall effect (to allow homing above min. position)

Limit switch? (on z) - not necessary

UI (3 button interface)

Serial interface (programming, diagnostics, command line)

Status LED - could use multi-colour for more sophisticated feedback (e.g. mode)

Piezo sounder

Modules

Forward Kinematics (where would these joint positions get me)

Inverse Kinematics (what joint positions are needed to get here)

Target generation (i.e. where does the spoon need to be)

Path planning (how to get between positions, collision avoidance)

Motion control (control the joints to follow planned path)

Safety (torque limits, collision detection)

State machine (for cyclic operations, mode changes)

User Feedback (lighting?  sound?  screen?  serial?)

Calibration (kinematic calibration, e.g. for new spoon)

Teach (teaching mode, for programming target positions)

EEPROM - load/save configuration

Sleep - puts device to sleep if no activity for xx minutes

Modes

Exclusive operating modes:

  1. Teach (entered via serial command, or special key combo?  is that necessary?)
  2. Use (default mode)

Functional modes

  1. Eat - spoon/plate cycle, variants of spoon approach angle (e.g. plate vs bowl), diameter, location
  2. Drink - pickup cup, present to set location

Locations

1 - n programmable locations, user can cycle through them or they can be directly accessed via serial.  Each location has a function associated (i.e. eat or drink), and configuration paramters.  

Eat/Drink Cycles

Changing Location


Hidden States

Although the user sees minimal states to the eat cycle, there are several internal states used to break the path planning into discrete stages:

  1. Eat
  1. Position (ends in IDLE state)
  1. Scoop/Present (ends in ACTIVE state)
  1. Rotate (if in IDLE, ends in IDLE)
  1. Drink
  1. Position (ends in READY)
  1. Pickup/present (ends in ACTIVE)
  1. Put-down (ends in IDLE)
  1. Rotate (if in ACTIVE, ends in ACTIVE)
  1. Change location (ends in SELECT)
  1. Move to max height - quick
  2. Point spoon down - quick
  3. Move to above centre of next location, height unchanged - medium

There is also a hidden “homing” sequence that is initiated at power-on, or before going to SLEEP

UI

Collisions

Forward Kinematics

Simple trig to derive wrist location/angle…  then just need to know spoon length/vertical offset to generate spoon position.

Calibration

Spoon length/vertical offset is calibrated in teach mode, by moving spoon to touch a known point on the robot base…  the forward kinematics can then resolve the wrist location/angle, and allow the spoon length/offset to be calculated.

Inverse Kinematics

Alway solved in terms of wrist position/orientation.  Can be solved for spoon position, by first setting the required spoon angle (world space), then solving for joint angles, then finally solving for wrist rotation.  IK solution will refer to current state to return the “nearest” solution in joint space.

Target Generation

For target state/cycle; determines:

Spoon orientation is planned/updated in world space!  That doesn’t prevent spoon angles between interpolated during arm movements.

Path Planning (in world space)

Path planning only concerns wrist position.  Wrist angles are dealt with by motion control.

Given current position, target position, calculate trajectory of wrist - assumes spoon positions are linearly interpolated along world space path.  Trajectory is formed as a 3D cubic bezier.  Trajectory plus target wrist angles are placed into motion control queue.  Trajectories are formed to avoid collisions (self or world) and to avoid joint limits.

Planning strategies (in order of consideration), first option that avoids collisions is used:

Z-axis motion is always linear, with respect to bezier control value (0 → 1).

Motion Control

Motion control takes motion “tasks” from it’s queue and processes them, applying acceleration/velocity control, torque limits, etc.  Trajectories are converted into straight line segments (in joint space), with a world space velocity limit.  Then a look-ahead acceleration filter is applied to generate the executable motion path.  Straight line segments are processed real-time to update stepper/servo positions.  Use framework from Marlin!

Safety

Joint torques for all servos should be checked real-time during moves.  Servos and stepper should be deactivated when stationary - i.e. passive.  

Sleep

If no activity for xx minutes, then:

  1. Cycle to IDLE for given state
  2. HOME
  3. De-activate servos/stepper
  4. Go into “low power” mode - what does that mean?
  5. Wait for button press (interrupt?)
  6. Return to “full power” mode -what does that mean?
  7. HOME, then do triggered action

Configuration Parameters - stored in EEPROM or ROM

State Parameters - NOT stored in EEPROM

Power On sequence

  1. Load configuration
  2. Initialise state
  3. HOME

Serial Interface

Provides a command line interface to control the arm, including a teaching interface.  Command set:

Max Step Rate Calculation

Servo Update Frequency

Software Architecture

Control loops and interrupts:

Main

  • Serial communications (e.g. host computer, periperhals)
  • State machine (user interface)
  • Trajectory planning with collision constraints
  • Path generation

Timer1 (variable Hz)

Stepper control

Processes current linear move, pulses step(s)

Timer2 (30Hz)

Servo control - Dynamixel and SAM-3

Processes current linear move, sends updated positions/velocities, monitors safety limits (torques)

External Interrupt

User Interface (i.e. buttons)

 - Uses pin change interrupt (to avoid using precious dedicated external interrupt pins)

- Updates internal flags, polled in Main loop

- Also used to wake the processor from sleep mode

Key data structures:

Command Queue

FIFO buffer of commands (see Serial Interface for list). Some carry parameters (e.g. change to plate no. x).  Structure of buffer entries:

Queue length - 10?  doesn’t need to be huge, it’s mostly there to handle multiple/rapid button presses (e.g. rotate plate) in an intuitive manner…  i.e. rapidly tapping the rotate button 5 times will cause the arm to move round 5 segments, even if all button presses happen before the first move has completed.

Setup function

Main Loop Structure

Execution of these various commands (changing mode, state, etc) will comprise a combination of the following:

Handling User Input

Stepper Motion Block

Although these are loosely coupled to servo movements, they can operate using a much simpler planning/execution model (i.e. no look ahead smoothing) - since each movement can complete (z_axis velocity ending at zero), before the next move starts.  A single stepper motion block can execute in parallel with multiple servo motion blocks, and vice versa.  However, completion time of coordinated movements can’t be guaranteed by this approach.  This is unlikely to be an issue for the feeding task, but may be for future applications.  To solve: how to ensure tasks start together when they’re meant to (e.g. when far down the queue).

grbl/Marlin work by varying the interrupt timing to achieve acceleration/deceleration, combined with the potential for multiple step loops within a single ISR call (max 4).  This allows the ISR to run at a lower frequency (max/4) and minimises processing overhead.  The variable timing is achieved using the Timer1 Compare Match A interrupt, with the OCR1A registering controlling the counter target.  It ticks over at 1kHz when idle, waiting for motion blocks.  Max interrupt frequency is 10kHz.  Max step frequency (with 4 step loops) is 40kHz.  Pre-scaler is fixed at 8, yielding a 2MHz timer.  Slowest interrupt rate (OCR1A = 65535) = 30.518 Hz.

Timer values are read from a pre-computed lookup table - the table maps a required step rate to associated timer values.  There are two tables (fast and slow).  Don’t yet fully understand how the timer calculations are working, or what the values in the lookup tables are doing. 

NB: Requires global jerk setting

Servo Motion Block

This incorporates look ahead acceleration planning (trapezoidal), inspired by grbl/Marlin.  Servo updates occur within the Servo ISR, at 30 Hz.  Position and velocity of all servos is bulk updated.  Interrupts are re-enabled within the ISR to allow the Stepper ISR to continue.

Thoughts:

Machine Code

There are two prevalent industry approaches to low-level programming for industrial robotics:

There are a great number of similarities between these various languages, however, G-Code is the most common and standardised.  Fortunately, the core RADID/KRL motion commands have close equivalents in G-Code:

KRL/RAPID

G-Code

Point to Point (PTP)

Fastest point to point motion, not linear

Rapid (G00)

Fastest point to point motion, not linear

Linear (LIN)

Linear interpolation at defined velocity

Linear (G01)

Linear interpolation at defined feed rate

Circular (CIRC)

Circular motion to end point, via intermediate point (optional arc length parameter)

Circular (G02, G03)

Circular motion about a defined center point, or with defined radius

Spline (SPL)

Spline via any number of points

NURB (G06.1)

Spline via any number of points

Most implementations support optional acceleration, look-ahead planning and some support variable path approximation (e.g. KRL CONT parameter).

Broad conceptual similarities:

KRL/RAPID

G-Code

Target positions can be defined in either joint space (J1-6) or cartesian space (XYZ ABC).  Additional joint parameters can be specified to remove joint configuration ambiguity.

Target positions are defined in a cartesian coordinate frame (XYZ ABC).  Many machines are 3DOF, so do not use rotation axes (ABC).

Cartesian coordinate frames are user programmable and typically include World, Base, Flange and Tool. Many Base and Tool frames can be stored and selected within a program.

Generally operate in either a Machine or Work coordinate frame - only XYZ are captured (i.e. Workspace frame is always axes aligned to the Machine frame).

Can operate in either absolute or relative mode

Can operate in either absolute or relative mode

Support extended/external axes

Support extended/external axes

Special consideration required to avoid joint singularities

Singularities generally not an issue due to mutually exclusive axes (e.g. cartesian motion)

Infrequently calibrated using manual/offline procedure

Routinely re-calibrated using homing routine(s) for one or more axes (limit switches)

 

To minimise the learning curve and maximise reusability of the code, AFRo’s machine language will be derived from G-Code, with the introduction of additional commands only where necessary.

Terminology

Term

Description

TCP

Tool Centre Point - for AFRo, this is dependent on operating mode, and is either:

  • The bottom/centre of the bowl of the spoon
  • The bridge of the cup hook

World

World coordinate frame, centred at the base of the Z post (level with the base of the robot chassis)

Tool

Tool coordinate frame - see TCP

Base

Workpiece coordinate frame, for AFRo this is associated with either a plate, bowl or mouth location.  Can be directly referenced in G00/G01 using P<plate index>.  e.g. G00 P3

Command Structure / Syntax

        

Letter Addresses

Variable

Description

A

Absolute or incremental rotation about X

B

Not used - normally represents absolute or incremental rotation about Y

C

Absolute or incremental rotation about Z

F

“Feed” rate, for AFRo this is the target velocity of the TCP (mm/sec)

G

Address for motion commands

I

Absolute or incremental rotation of Shoulder joint

J

Absolute or incremental rotation of Elbow joint

K

Absolute or incremental rotation of Wrist yaw joint

M

Address for machine commands

N

Line number

P

Parameter address (see G and M codes)

T

Tool selection - for AFRo this represents either Eat (0) or Drink mode (1), with associated virtual tools of Spoon and Cup Hook

U

Absolute or incremental rotation of Wrist pitch joint

X

Absolute or incremental motion in X

Y

Absolute or incremental motion in Y

Z

Absolute or incremental motion in Z

G and M Codes

Code

Description

G00

Point to point move in joint space.  Target can be defined in either cartesian (XYZABC), joint (ZIJKU) or frame index (P) format.  Feed rate is honoured with respect to the TCP.  

Example: G00 X0 Y0 Z100 A0 C30 T0

G01

Linear interpolation in cartesian space.   Target can be defined in either cartesian (XYZABC), joint (ZIJKU) or frame index (P) format.  Feed rate is honoured with respect to the TCP.

Example: G01 X0 Y0 Z100 A0 C30 T1

G04

Dwell/wait - address P represents time in seconds.

Example:  G04 P0.5

G28

Home Z axis.  Optional Z address can be specified, which AFRo will attempt to home to - possible because the hall effect sensor end-stop is linear, and can sense pre-calibrated positions above z=0

G53

Select World coordinate frame.  Future motions are in World frame.  Default.

G54

Select a Base coordinate frame (i.e. plate), referenced by parameter address P.  Future motions are in Base frame.

Example:  G54 P3

G90

Absolute motion in current frame

G91

Incremental motion in current frame

M0

Stop - finish moves in buffer, then enter sleep - same as M1

M1

Sleep - finish moves in buffer, then enter sleep

M17

Enable motors (stepper and servos)

M18

Disable motors (stepper and servos) - can be used for “teaching”

M111

Set debug level, defined by parameter address P.

M112

Emergency stop - immediately stop, empty buffer, enter sleep

M114

Get current position

M115

Get firmware version

M117

Get current frame.

M122

Set diagnosis level, defined by parameter address P.

M204

Set acceleration limits for each axis, either cartesian or joint space

M420

Set RGB colour of status LED.  RGB mapped to XYZ axes.

M906

Set joint torques - specified as Joint parameter addresses

Future Exploration

References

Arduino

Dynamixel

Robotics: