Robot kinematics, planning & control

Inverse kinematics inverts the FK map. Applying the law of cosines to the triangle formed by the two links and the line to the target (x,y) gives the elbow angle directly:

The ± sign selects elbow-up vs elbow-down. If |cosθ2| > 1 the point is unreachable. At the workspace boundary the two solutions merge — a singularity where the arm loses a degree of freedom.

For a 3R planar arm the tip position accumulates three link contributions, each rotated by the running sum of joint angles:

where c₁₂₃ = cos(θ1+θ2+θ3) and so on. The arm is kinematically redundant: it has 3 joints for a 2-DOF task, so infinitely many configurations reach most points. The workspace is the set of all reachable tips — here a disc of radius L1+L2+L3, with a possible unreachable inner hole when one link dominates. Sampling the joint space and plotting every resulting tip reveals this envelope empirically.

A differential-drive robot is modelled as a unicycle with pose (x, y, θ), linear speed v and turn rate ω:

The controller computes a heading error to the goal, wrapped to [-π,π], and commands turn rate proportionally; forward speed scales with remaining distance and de-rates when the heading error is large:

The pose is integrated each frame with Euler steps. This is a nonholonomic system — it cannot move sideways — so it must turn toward a goal before advancing.

The robot descends the gradient of a total potential U = U_att + U_rep. The attractive part pulls toward the goal; each obstacle contributes a repulsion that grows as the robot nears it, active only within an influence radius d₀:

The net force vector sets the desired heading; the same unicycle controller then tracks it. Potential fields are fast and reactive but can trap the robot in a local minimum where attraction and repulsion cancel before reaching the goal.

A* expands grid cells in increasing order of an estimated total cost, popping the open cell with the smallest:

where g(n) is the exact cost from the start and h(n) is a heuristic estimate to the goal. If h never overestimates (it is admissible) A* returns an optimal path; if it also satisfies the triangle inequality (consistent) no cell is re-expanded. With h=0 A* degenerates to Dijkstra. For 8-connected grids the octile distance is the exact metric and so is admissible. Diagonal corner-cutting through walls is forbidden, and diagonal moves cost √2.

The plant is a unit mass with drag c under control force u: m·a = u - c·v. The PID law acts on the tracking error e = setpoint - x:

Kp gives a force proportional to error — bigger Kp means faster rise but more overshoot. Kd reacts to the rate of change, adding damping that suppresses overshoot and oscillation. Ki accumulates past error to eliminate steady-state offset, at the cost of possible wind-up (mitigated here by clamping the integral). Key metrics: overshoot (peak above setpoint), settling time (entering and staying within a 2% band).

The cart (mass M) carries a hinged pole (mass m, half-length l) at angle θ from vertical, driven by force F. The coupled nonlinear dynamics are:

The upright equilibrium θ=0 is unstable — any deviation grows without control. A linear state-feedback law F = kθ·θ + kω·θ̇ + kx·x + kv·ẋ stabilises it: the angle terms right the pole, the cart terms keep it centred. Integrated with sub-stepped Euler, the controller catches and balances the pole.

For an n-link planar arm the end-effector position p = f(q) depends nonlinearly on the joint vector q = (θ1..θn). Linearising gives the velocity relation through the Jacobian J, where ṗ = J(q) q̇. For the planar tip, column j is the perpendicular of the vector from joint j to the tip:

where Θᵤ = θ1+..+θᵤ. A naive Newton step Δq = J⁻¹ e (with error e = p_target - f(q)) diverges near singularities where J loses rank and J⁻¹ explodes. The damped least squares update adds a regulariser λ²I:

This is the Levenberg-Marquardt / Tikhonov solution: it behaves like the pseudo-inverse far from singularities and like gradient descent near them, trading a little tracking accuracy for bounded joint rates. The manipulability ellipse, with axes the singular values of J, shows the directions the tip can move easily; it collapses to a line at a singularity, where the manipulability w = √det(J Jᵀ) drops toward zero.

RRT* builds a tree of collision-free configurations rooted at the start. Each iteration: sample a random point x_rand (occasionally the goal); find the nearest tree node x_near; steer a fixed step η toward it to get x_new; reject if the segment hits an obstacle.

What makes it RRT* (asymptotically optimal, unlike plain RRT) is two extra steps over the near set within radius r:

The connection radius shrinks as the tree grows, r = min(γ·(log N / N)^(1/d), η), which guarantees the path cost converges to the optimum as samples N→∞. cost(n) is the path length from the root along the tree.

The state is the pose x = (px, py, θ) with covariance P. Predict propagates the velocity-motion model with control (v, ω) over dt, linearising about the current mean via the Jacobian F = ∂g/∂x and adding process noise Q:

Update folds in each landmark observation z = (range, bearing). The expected measurement h(x) and its Jacobian H for a landmark at (mx, my), with dx=mx-px, dy=my-py, q=dx²+dy², are:

The innovation z - h has its bearing wrapped to [-π,π]. Each correction shrinks P — the uncertainty ellipse contracts — while prediction between observations re-inflates it. This is EKF localization with a known map; extending the state to also hold landmark positions makes it EKF-SLAM.