NIH SBIR · Athtec Systems
Concept on a large 6-DOF robot with a 6-axis wrist force/torque sensor
Kinematics · Dynamics · Control · Force sensing · Path replay
The award
Phase I · 1984
$50,000
Two-jointed robotic arm on a floor-fixed base, attached to the patient's extremity for active and passive movement.
Phase II · 1986
$500,000
Three-dimensional movement at two adjacent joints, a learn mode that records the path, isokinetic and isotonic resistance, and redundant safety features.
$550,000 total from NIH · FDA to Athtec Systems, Sacramento CA.
Source: inknowvation.com SBIR award record
2
My contribution
Kinematics
Forward, inverse, and Jacobian for the arm
Dynamics
Inertia, Coriolis, gravity, and friction model
Control
Servo and force-based control of the arm
Six-axis force sensing
Full wrench at the end effector
Path recording
Motion path captured with the end effector on the patient's limb
Path replay
Isotonic and isokinetic motion modes
Testing and validation of the complete system
Limb Mimic Device · 6-DOF robot concept
3
System
Patient limb
Moves actively or is moved passively
Cuff + F/T sensor
Measures the full wrench at the wrist
6-DOF robot
Positions the cuff along the path
Servo controller
Kinematics, dynamics, control modes
Data and safety
Logging, limits, redundant trips
Motion path
Joint commands move the cuff along the recorded limb path.
Force path
The wrench tells the controller what the patient is doing.
Limb Mimic Device · 6-DOF robot concept
4
Robot
J1
J2
J3
J4
J5
J6
L2 · upper arm
L3 · forearm
L4–L6 · wrist
L1 · base, pedestal
x0
z0
6-axis F/T sensor
Limb cuff and
patient limb
Large 6-DOF arm
Six revolute joints place and orient the cuff.
Spherical wrist
J4–J6 axes meet at one point.
Wrist F/T sensor
Sees every force the limb applies.
Limb Mimic Device · 6-DOF robot concept
5
Kinematics
| Joint | α(i−1) | a(i−1) | d(i) | θ(i) | Motion |
|---|---|---|---|---|---|
| 1 | 0 | 0 | 0 | θ1 | Base yaw |
| 2 | −90° | 0 | 0 | θ2 | Shoulder pitch |
| 3 | 0 | a2 | d3 | θ3 | Elbow pitch |
| 4 | −90° | a3 | d4 | θ4 | Forearm roll |
| 5 | 90° | 0 | 0 | θ5 | Wrist pitch |
| 6 | −90° | 0 | 0 | θ6 | Wrist roll, flange |
Craig convention for a Puma-type arm. Link lengths a2, a3, d3, d4 are robot-specific [__ m]. The sensor and cuff attach as fixed transforms after joint 6.
Limb Mimic Device · 6-DOF robot concept
6
Kinematics
Forward: joints to cuff pose
A_i = Rx(α) Tx(a) Rz(θ) Tz(d)
T_06 = A1 A2 A3 A4 A5 A6
T_cuff = T_06 T_6s T_sc
T_6s is the sensor mount and T_sc the cuff mount, both fixed transforms.
Inverse: cuff pose to joints
p_w = p_cuff − R (d_s + d_c)
θ1, θ2, θ3 from p_w
θ4, θ5, θ6 from R_36
Closed form because J4–J6 meet at the wrist center. Up to 8 solutions; choose by joint limits and continuity.
Limb Mimic Device · 6-DOF robot concept
7
Kinematics
v = J(q) q_dot cuff velocity from joint rates
tau = J(q)^T F joint torques from cuff wrench
w = sqrt(det(J J^T)) manipulability
Shoulder
Wrist center lies on the J1 axis
Elbow
Arm fully stretched or folded
Wrist
J4 and J6 axes align when θ5 = 0
Recorded paths are checked against these poses, and speed drops as w falls.
Limb Mimic Device · 6-DOF robot concept
8
Dynamics
tau = M(q) q_ddot + C(q,q_dot) q_dot + G(q) + F_f(q_dot) + J^T F_ext
M(q)
Inertia
Link and rotor inertia
C(q,q_dot)
Coriolis
Joint velocity coupling
G(q)
Gravity
Weight of arm, cuff, limb
F_f(q_dot)
Friction
Viscous plus Coulomb
J^T F_ext
Limb wrench
Measured by the sensor
Real time: recursive Newton-Euler computed torque. Analysis: Lagrange form.
Limb Mimic Device · 6-DOF robot concept
9
Force sensing
Robot flange
6-axis F/T sensor
Tool plate + cuff
Fx
Fy
Fz
Mx
My
Mz
Fx · Fy · Fz
Linear force on the cuff: push, pull, axial load
Mx · My · Mz
Moment about each axis: bending and twist
Wrench
Six values per sample, read every control cycle
Use
Resistance load, effort direction, torque at the patient's joint
Limb Mimic Device · 6-DOF robot concept
10
Force sensing
1
Bias removal
Zero the sensor at rest
2
Gravity compensation
Subtract cuff and limb weight
3
Frame transform
Move wrench to the cuff or base frame
4
Filter
Low-pass and reject spikes
5
Safety check
Trip on force or torque limit
F_b = R_bs (F_s − F_0 − m g_s)
M_b = R_bs (M_s − M_0 − m r_c × g_s) + p_bs × F_b
tau_ext = J(q)^T F_b
Limb Mimic Device · 6-DOF robot concept
11
Control
Wrench F
Admittance law
Inverse kinematics
Joint servo
Robot + limb
Outer loop
M_d a + D_d v + K_d e = F
Measured force becomes commanded motion
Inner loop
Joint position and velocity servo with gravity and friction feedforward from the dynamic model
Mode selection
Passive, isotonic, isokinetic, and isometric set by M_d, D_d, K_d and path limits
Limb Mimic Device · 6-DOF robot concept
12
Learn mode
Start, s = 0
End, s = 1
Cuff path, one dot per control cycle
1 Attach and float
Cuff on the limb, arm gravity-compensated.
2 Move through range
The patient moves the limb; the arm follows.
3 Sample and store
Joints, pose, and wrench saved each cycle.
Limb Mimic Device · 6-DOF robot concept
13
Learn mode records the motion. Replay modes shape the load along it.
Limb Mimic Device · 6-DOF robot concept
14
Replay
Isotonic: constant load
Force held at F_set along the path. Speed follows the patient.
Isokinetic: constant speed
Speed held at v_set along the path. Force follows patient effort.
Limb Mimic Device · 6-DOF robot concept
15
Testing
| Test | What it checks | Metric |
|---|---|---|
| Kinematic accuracy | Model cuff pose vs. measured pose | Position error [__ mm] |
| Sensor check | Known weights at several arm poses | Force error [__ N] |
| Path repeatability | Repeated replay of one recorded path | RMS deviation [__ mm] |
| Isokinetic tracking | Commanded vs. actual path speed under load | Speed error [__ %] |
| Isotonic tracking | Commanded vs. measured load along the path | Force error [__ N] |
| Safety trips | Force limit, joint limit, e-stop, watchdog | Trip time [__ ms] |
Bracketed values are placeholders for measured results.
Limb Mimic Device · 6-DOF robot concept
16