Skip to content
Tutorials Center
My learning

Topics

Lesson 36 of 38 ยท Projects and Practice

Project: A Two-Link Arm That Avoids an Obstacle

Build a motion planner for a two-link robot arm: inverse kinematics, a C-space collision map, BFS in joint space, and a smooth timed trajectory, all in Python.

This project builds the full pipeline for a small robot arm: where to go (inverse kinematics), how to get there without hitting a post (planning in configuration space), and when to be where (a smooth trajectory). It uses ideas from the Kinematics chapter and from path planning. Everything runs in plain Python.

The arm is planar, with two links L1 = 1.0 and L2 = 0.8, its base at the origin, and two revolute joints with angles q1 and q2. A round post stands at (0.9, 0.9) with radius 0.25. The task: move the hand from a start pose to a target point on the other side of the post.

Pipeline

Target A point in the workspace.
Inverse kinematics Target point to joint angles.
Plan in C-space Search joint angles around the obstacle.
Trajectory Smooth, timed joint motion.
1 / 2

Step 1: forward and inverse kinematics

Forward kinematics (FK) gives the elbow and hand positions from the angles:

elbow = (L1 cos q1, L1 sin q1), hand = elbow + (L2 cos(q1+q2), L2 sin(q1+q2))

Inverse kinematics (IK) goes the other way. For a target (x, y) at distance d = sqrt(x^2 + y^2) from the base, the law of cosines gives the elbow angle:

cos q2 = (d^2 - L1^2 - L2^2) / (2 L1 L2)

If the value lies outside [-1, 1] the point is unreachable. Otherwise q2 = +acos(...) ("elbow down") or -acos(...) ("elbow up"), and then q1 = atan2(y, x) - atan2(L2 sin q2, L1 + L2 cos q2). Two solutions mean the planner can choose whichever is collision free.

Python Step 1: forward and inverse kinematics

Press Run (or Ctrl + Enter) to see the result here.

Program input ยท what you'd type while the program runs

Type the answers before you press Run, one per line. Each time the program reads a line (with input()), it gets the next line from this box.

The forward-kinematics check closes the loop: IK followed by FK must return the target. Always test your IK that way.

Step 2: plan in joint space

The arm cannot simply move the hand in a straight line to the target: it would sweep through the post. We plan in configuration space (q1, q2), a 2D grid in which each cell is either free or in collision.

  1. Discretize both angles into 5-degree steps (72 by 72 cells). Angles wrap around, so the grid is a torus: going past 180 degrees returns to -180.
  2. A cell is in collision if any sample point along either link is inside the post (inflated slightly for safety).
  3. Run BFS from the start cell to the goal cell (one of the two IK solutions, whichever is reachable).
  4. Convert the cell path to joint angles.
Python Step 2: BFS in the arm's C-space

Press Run (or Ctrl + Enter) to see the result here.

Program input ยท what you'd type while the program runs

Type the answers before you press Run, one per line. Each time the program reads a line (with input()), it gets the next line from this box.

The planner returns a short chain of joint-space cells. Because cell moves are small (5 degrees), checking each cell is nearly enough; the final test samples between waypoints too, which is what a real continuous collision checker does.

Step 3: a smooth timed trajectory

The path is a staircase in angle space. We turn it into motion by (1) shortcutting waypoints that can be joined by a collision-free straight joint move, and (2) timing the remaining segments with a trapezoidal or quintic profile (see Trajectory Generation). The code below shortcuts and then times the move so no joint exceeds a speed limit, finally printing the hand position through time.

Python Step 3: shortcut and time the motion

Press Run (or Ctrl + Enter) to see the result here.

Program input ยท what you'd type while the program runs

Type the answers before you press Run, one per line. Each time the program reads a line (with input()), it gets the next line from this box.

The three moves swing the whole arm the long way around the base (angle -20 to -200 degrees, which is the same direction as 160 degrees, hence the repeated time stamp), because the post blocks the short way, and then raise the hand to the target. Read the collision= column: every sampled instant must be False. In a real system you would also check torque limits and use a better time scaling, but the structure (IK, plan in C-space, shortcut, time) is what libraries such as MoveIt do.

Ideas to extend it

  1. Replace BFS with A* using the joint-space distance as the heuristic, and count expanded cells.
  2. Use RRT in joint space instead of a grid. Compare run time and path length.
  3. Add a second post and a joint limit (for example q1 between -90 and 180 degrees, so the grid is no longer a torus).
  4. Plan a Cartesian straight line for the hand with IK at every point, and see where it fails (post collisions, unreachable points, the switch between the two elbow solutions).
  5. Add a third link and observe that IK now has infinitely many solutions (redundancy).

Check your understanding

Quick check ยท 4 questions

of 4 answered You scored / 4 ยท perfect!

  1. 1. What does it mean if the inverse kinematics value cos(q2) is outside [-1, 1]?

    The law of cosines has no solution when the target is farther than L1 + L2 or closer than |L1 - L2|.
  2. 2. Why does the project plan in joint space rather than moving the hand in a straight line?

    The arm body, not just the hand, must avoid the post. C-space captures that with a point in a 2D grid.
  3. 3. Why does the angle grid wrap around (a torus)?

    Revolute joints without limits are periodic, so the planner must treat the edges of the grid as connected.
  4. 4. After finding a path in a grid, why shortcut and time it before execution?

    Grid paths have many tiny steps. Shortcutting simplifies them and a time law makes the motion smooth and feasible.

Summary

  • Forward kinematics maps joint angles to the hand; inverse kinematics solves the reverse, often with two solutions (elbow up and elbow down).
  • Collision checking on the whole arm turns into a free/blocked map in joint space.
  • BFS (or A*, RRT) finds a path in C-space; angles wrap around.
  • Shortcutting and a smooth time law convert the path into a trajectory that respects joint limits.
  • Always verify with FK and dense collision checks.

Next: Project: Tracking a Moving Target with a Kalman Filter