This page covers the mathematical theory behind the 3-DOF anthropomorphic arm simulation on the /robot-arm page. The arm uses a position-only 3×3 Jacobian for velocity-level control, with damped least-squares near singularity. Reference: MEC781 Appendix A4.

1. Kinematic Model

3R anthropomorphic configuration with base rotation, shoulder, and elbow joints.

Forward Kinematics

End-effector position in base frame. Defining s₁ = sin θ₁, c₁ = cos θ₁, s₂₃ = sin(θ₂ + θ₃), c₂₃ = cos(θ₂ + θ₃):

p_x = c₁ ( L₁ c₂ + L₂ c₂₃ )

(1)

p_y = s₁ ( L₁ c₂ + L₂ c₂₃ )

(2)

p_z = L₀ + L₁ s₂ + L₂ s₂₃

(3)

Reach envelope. The workspace is a solid of revolution about the z-axis. Max reach r_max = L₁ + L₂ = 0.40 m. Min reach r_min = |L₁ − L₂|. Orientation is uncontrolled (position-only task, 3 DOF).

2. Jacobian Construction

Partial differentiation of Eqs. (1)–(3) with respect to [θ₁, θ₂, θ₃] yields the 3×3 position Jacobian.

J(q) =

−s₁(L₁c₂ + L₂c₂₃)	−c₁(L₁s₂ + L₂s₂₃)	−c₁ L₂ s₂₃
c₁(L₁c₂ + L₂c₂₃)	−s₁(L₁s₂ + L₂s₂₃)	−s₁ L₂ s₂₃
0	L₁c₂ + L₂c₂₃	L₂ c₂₃

(4)

Column 1: ∂p/∂θ₁ (base rotation) Column 2: ∂p/∂θ₂ (shoulder) Column 3: ∂p/∂θ₃ (elbow)

Geometric Verification

Each column J_i satisfies the revolute-joint formula:

J_i = z_i−1 × ( o_n − o_i−1 )

(5)

with joint axes z₀ = [0, 0, 1]ᵀ, z₁ = [−s₁, c₁, 0]ᵀ, z₂ = z₁. The geometric result matches Eq. (4) column-for-column.

Determinant & Manipulability

det J = L₁ L₂ sin θ₃ ( L₁ cos θ₂ + L₂ cos(θ₂ + θ₃) )

(6)

w(q) = √ det( J Jᵀ ) = | det J |

(7)

Yoshikawa manipulability index — for square J, reduces to absolute determinant. Displayed in real-time on the simulation HUD.

3. Singularities

From Eq. (6), det J = 0 in three cases. The simulation highlights singular configurations with a red EE marker and warning overlay.

4. Velocity-Level Control

Resolved-Rate Control

Given a desired Cartesian EE velocity, compute joint velocities by direct Jacobian inversion:

q̇ = J⁻¹(q) ṗ_d

(8)

Valid only when w(q) > w_min. Diverges near singularity — use DLS instead.

5. Damped Least-Squares (DLS)

Near singularity, the controller switches to DLS — a single formula that handles both regular and singular configurations with no mode switching.

q̇ = Jᵀ ( J Jᵀ + λ² I )⁻¹ ṗ_d

(9)

Adaptive damping — λ increases smoothly as manipulability drops below threshold w₀:

λ² =

0if w ≥ w₀

λ₀² ( 1 − w / w₀ )²if w < w₀

(10)

DLS Defaults

w₀ (manipulability threshold)0.01

Below this value of w, damping activates. Lower = closer to singularity before engaging.

λ₀ (max damping factor)0.05

Maximum damping at exact singularity. Higher = more stable but less accurate tracking.

Behaviour Comparison

Resolved-Rate (w > w₀)exact

J⁻¹ gives exact velocity mapping. Fastest response, zero tracking error.

DLS (w < w₀)bounded

Trades accuracy for stability. Joint velocities stay bounded. λ shown in real-time on HUD.

Resolved-Rate at singularity∞ diverge

Without DLS, joint velocities explode. Simulation throws error.

Closed-Loop Position Tracking

For EE position tracking with error feedback, the commanded Cartesian velocity is synthesised as:

ṗ_d = ṗ_ref + K_p ( p_ref − p )

(11)

K_p diagonal, default [2.0, 2.0, 2.0]. Error dynamics are first-order stable for K_p > 0 under non-singular conditions.

6. Implementation Architecture

Connection Modes

Mode	Backend	Physics	Use Case
Simulation	None (client-side)	arm3dof.ts Euler integration	Algorithm development, classroom
ROS2 Virtual	hardware_arm.py --mock	Server-side simulation	ROS2 integration testing
ROS2 Hardware	hardware_arm.py	Real Dynamixel servos	Physical arm deployment

7. ROS2 Topic Interface

Services

Service	Type	Description
/arm/enable_torque	std_srvs/Trigger	Enable servo torque
/arm/disable_torque	std_srvs/Trigger	Disable servo torque
/arm/home	std_srvs/Trigger	Move to home pose [0, π/4, −π/2]

Heartbeat safety. If no command is received for 500 ms, the hardware node automatically disables torque. This prevents damage if the frontend disconnects or crashes.

8. Hardware Reference

Bill of materials for physical deployment with 0.5 kg EE payload, L₁ = L₂ = 0.20 m.

Component	Specification	Rationale
Shoulder servo	Dynamixel MX-64AT (6 Nm stall)	Worst-case ~5.2 Nm at full extension
Elbow servo	Dynamixel MX-28T (2.5 Nm)	Worst-case ~2.3 Nm
Base servo	Dynamixel XL430-W250 (1.4 Nm)	Low torque — vertical axis only
Controller	OpenRB-150 or U2D2	TTL bus, position feedback native
Power	12 V 5 A SMPS (isolated)	MX-64 peak current 4.1 A

Deployment Commands

Mock mode (no hardware)

ros2 run arbiterros hardware_arm --mock

Real Dynamixel bus

ros2 run arbiterros hardware_arm --port /dev/ttyUSB0

Equation Reference

#	Equation	Description
(1)–(3)	p = FK(q₁, q₂, q₃)	Forward kinematics — EE position from joint angles
(4)	J(q) = ∂p/∂q	3×3 analytical position Jacobian
(5)	Jᵢ = zᵢ₋₁ × (oₙ − oᵢ₋₁)	Geometric verification (revolute joint)
(6)	det J = L₁L₂ sin θ₃ (L₁c₂ + L₂c₂₃)	Determinant — zeros identify singularities
(7)	w = \|det J\|	Yoshikawa manipulability index
(8)	q̇ = J⁻¹ ṗ_d	Resolved-rate control
(9)	q̇ = Jᵀ(JJᵀ + λ²I)⁻¹ ṗ_d	Damped least-squares
(10)	λ² = λ₀²(1 − w/w₀)²	Adaptive damping schedule
(11)	ṗ_d = ṗ_ref + Kp(p_ref − p)	Closed-loop position tracking