A robot runs on a handful of maths tools: frames, rotations, Jacobians, filters, feedback and least squares. Here you meet each one by reaching a two-link arm toward a cup.
Learn the five questions a robot asks every moment, and the maths tool that answers each. Then see where to go deeper.
Drag the cup, or use the two sliders. The arm bends to reach it. Switch on Frames to see the coordinate axes the robot keeps, and Speed ellipse to see where the hand can move fastest. Then we build every piece from zero.
You need high-school algebra and a little trigonometry (sine and cosine of an angle). We build the rest from zero.
Built on Lynch and Park, Modern Robotics (Cambridge University Press, 2017), and Thrun, Burgard and Fox, Probabilistic Robotics (MIT Press, 2005). All arm sizes, noise levels and gains here are illustrative toy numbers.
Toy robot: link lengths L1 = 0.40 m and L2 = 0.30 m, a camera 0.06 m ahead of and 0.04 m to the side of the hand, drawn from above. The speed ellipse is drawn at one third of true size. Real arms have other sizes; the maths is the same.
Chapter 0
Follow one small robot as it reaches for a cup, and meet the five questions that need maths
A small robot sits at the edge of a table. It has one arm: two stiff bars joined by a hinge, with the first bar hinged to the base. A camera sits on the end of the arm, near the hand. On the table stands a cup. Your job is to make the arm pick up the cup.
You might think this is a job for code alone. It is not. Before one motor turns, the robot needs numbers. It must know where the cup is, how its own arm is bent, and how to move. Each of those needs a small piece of maths.
This lesson shows you those pieces. We use one robot for all of them, so the pieces fit together like parts of one machine. The robot has two links (the rigid bars) joined at two joints (the hinges). The first link is 0.40 m long. The second is 0.30 m long. These are toy sizes, chosen to keep the arithmetic clean.
Watch the robot for one reach. It asks the same five questions, in this order, on every reach it ever makes.
Where is the cup? The camera sees the cup as a spot in its own picture. The robot needs the spot as a position it can use, measured from the base. That is a question about coordinates (numbers that name a place), and the answer comes from vectors and transforms.
How is the arm posed? The motors report two angles, one per joint. The robot needs the position of its hand. Going from joint angles to hand position is called forward kinematics.
How fast does the hand move when a joint moves? If joint one turns a little, the hand slides sideways. If joint two turns, it slides a different way. The rule that links joint speeds to hand speed is the Jacobian.
How sure am I? Wheels slip and cameras blur. Every number the robot holds is a little wrong. The tool for handling doubt is probability, and its robot form is the Kalman filter.
How do I get there? The robot now knows where the cup is and where the hand is. It must find joint angles that put the hand on the cup, then make the motors hit those angles. The first part is inverse kinematics. The second is feedback control. Both lean on one more tool: finding the best answer by optimization.
The robot is in its start pose. Press each question. The drawing shows what the robot needs to know, and the read-out names the tool and the chapter that answers it.
Toy robot, drawn from above. The start pose has joint angles 10 degrees and 85 degrees. The cup is 0.30 m right of and 0.55 m ahead of the base. The four hand positions in the route view are the exact steps of the Newton loop in Chapter 6.
Each question feeds the next. Without a place for the cup, there is no target. Without the hand position, you cannot compare hand and target. Without the Jacobian, you do not know which way to turn the joints. Without a measure of doubt, you trust a bad number. Without feedback, the arm drifts off course.
Nothing here is exotic. The maths is mostly the maths you already know: angles, sine and cosine, a little algebra, and a slope. What is new is how the pieces connect. A robot textbook uses hundreds of pages to connect them. This lesson uses ten short chapters, and gives you a map.
Each chapter teaches one tool from zero. You see the idea in plain words, then the formula, then a worked example with every number. Then you run it on the toy arm. At the end of each chapter a short line points to a deeper lesson on this site that already goes further.
This lesson does not replace those deeper lessons. It does not derive the full three-dimensional theory, and it does not prove the filters are optimal. It gives you the first real step on each road, so that when you open a deeper lesson, you know why it exists.
A note on honesty. Every length, noise level and gain in this lesson is a toy number picked to teach. A real arm has different sizes, and a real camera has different noise. The formulas stay the same. Only the numbers change.
Chapter 1
Name a place with numbers, and see why the same cup has different numbers for different observers
Ask the camera where the cup is, and it says "ahead, a bit to the left." Ask the base, and it says "to the right and far in front." Both are correct. They disagree because each one measures from its own spot, facing its own way.
A robot hears many such answers at once. Before it can combine them, it needs one clear way to write down a place. That way is a vector and a frame.
A vector is a list of numbers that says how far to go in each direction. Think of walking directions in a city grid: "3 blocks east, then 5 blocks north." The list (3, 5) is a vector. In a drawing of the table from above, our cup sits 0.30 m to the right and 0.55 m forward of the base. We write it as p = (0.30, 0.55). In code this is an array of shape (2,).
The list means nothing alone. "3 blocks east" needs a starting corner and a meaning for "east." Those two choices make a frame: an origin (the starting point) and axes (the directions the numbers count along). Our base frame has its origin at the base of the arm. Its x-axis points right. Its y-axis points forward.
The numbers in a vector are its coordinates. They depend on the frame. Move the origin or turn the axes, and the coordinates change. The cup does not move. Only the way we write it down changes.
How far is the cup from the base? The two coordinates are the legs of a right triangle. The distance is the long side. Pythagoras gives its length: the square root of 0.30 squared plus 0.55 squared. That is the square root of 0.0900 plus 0.3025, which is 0.6265 m. We write this length as |p|.
Now a second tool. The dot product of two vectors multiplies their matching coordinates and adds the results. It also equals the product of the two lengths and the cosine of the angle between them.
Here is the picture to keep. If b has length 1, then a · b is the length of the shadow of a on the line through b. Shine a light straight down onto that line, and the shadow is the dot product. When a and b are at right angles, the shadow is zero. The dot product is zero.
Here is the payoff. Suppose a second frame sits at the same origin but is turned 30 degrees. Its x-axis is a vector of length 1 with coordinates (cos 30°, sin 30°) = (0.866, 0.500). Call it u. Its y-axis is v = (−0.500, 0.866). The coordinates of the cup in the turned frame are its shadows on these two axes.
p · u = 0.30 × 0.866 + 0.55 × 0.500 = 0.2598 + 0.2750 = 0.535
p · v = 0.30 × (−0.500) + 0.55 × 0.866 = −0.1500 + 0.4763 = 0.326
So the same cup is (0.535, 0.326) in the turned frame. Check the length: 0.535 squared plus 0.326 squared is 0.2862 plus 0.1063, and the square root of 0.3925 is 0.6265 m. The distance did not change. Turning the axes cannot change how far away the cup is.
The blue axes are the base frame. The purple axes are a second frame that you can tilt. Move the cup with the two sliders, or drag it with a mouse. Then drag the tilt slider. Watch the dashed shadows: they are the dot products.
The cup starts at (0.30, 0.55) m, the lesson's running example. The tilted frame shares the base origin, so only the axes differ. Distances are computed in both frames to show that they match.
This is the whole idea in four lines of NumPy. The dot product @ does the shadow, one axis at a time.
pythonimport numpy as np p = np.array([0.30, 0.55]) # shape (2,), metres, in the base frame a = np.deg2rad(30) u = np.array([np.cos(a), np.sin(a)]) # tilted x-axis, a unit vector v = np.array([-np.sin(a), np.cos(a)]) # tilted y-axis, also length 1 p_tilt = np.array([p @ u, p @ v]) # shape (2,) -> [0.5348, 0.3263] print(np.linalg.norm(p), np.linalg.norm(p_tilt)) # 0.6265 and 0.6265
Here is why a robot cares. The camera reports the cup in the camera frame. The planner wants it in the base frame. Every hop between frames is a change like this one. Chapter 2 adds the missing piece: how to write the turn as a matrix. Chapter 3 adds the move of the origin.
Go deeper: Frames and Transforms builds frames for whole robots, and Maths for ML covers vectors and dot products at length.
Chapter 2
Turn a frame with a small grid of numbers, and learn why turning in a different order gives a different result
The robot's hand points one way. The camera is bolted to the hand and points the same way. The cup is off to one side. To aim the hand, the robot must say how far to turn. It needs a precise way to write "turn by this much."
A rotation turns something around a fixed point without stretching it. Lengths stay the same. Only the direction changes. This chapter writes a rotation as a small grid of numbers called a matrix.
Start with the unit circle: all points exactly 1 m from the origin. Take the point (1, 0) on the x-axis and swing it counterclockwise by an angle θ. Where does it land? By the definition of sine and cosine, it lands at (cos θ, sin θ). So after the turn, the x-axis arrow is (cos θ, sin θ).
Now take the y-axis arrow (0, 1). It also swings by θ. It was 90 degrees ahead of the x-axis arrow, and it still is. So it lands at (−sin θ, cos θ). Check at θ = 90°: the x arrow lands at (0, 1), and the y arrow lands at (−1, 0). That is correct.
Any vector is a mix of these two arrows: p = px (1, 0) + py (0, 1). A rotation turns both arrows and keeps the mix. So the turned vector is px times the turned x arrow, plus py times the turned y arrow. Write the two turned arrows as the columns of a matrix, and you have the rotation matrix.
To multiply a matrix by a vector, take the dot product of each row with the vector. That is the same dot product as in Chapter 1. Try it on the cup with θ = 30°, where cos is 0.866 and sin is 0.500.
x′ = 0.866 × 0.30 − 0.500 × 0.55 = 0.2598 − 0.2750 = −0.015
y′ = 0.500 × 0.30 + 0.866 × 0.55 = 0.1500 + 0.4763 = 0.626
The cup swings from (0.30, 0.55) to (−0.015, 0.626). Its length is still 0.6265 m. Drag the angle and watch the point ride the circle.
The warm arrow is the x axis and the teal arrow is the y axis. Drag the angle. Their tips are the columns of the matrix below.
The cup at (0.30, 0.55) m is drawn at its rotated place. The circle is the set of points 1 m from the origin, drawn smaller than the table to fit.
First, undoing a turn is easy. To undo a turn of θ, turn by −θ. Because cos(−θ) = cos θ and sin(−θ) = −sin θ, this swaps the signs of the sine terms. That is the transpose of the matrix: flip it over its main diagonal. So the inverse of R is RT. No hard algebra is needed.
Second, this explains Chapter 1. The coordinates of the cup in a frame turned by 30 degrees were (0.535, 0.326). Compute R(30°)T p, and you get the same numbers. Describing the cup in a turned frame is the same as turning the cup the other way.
In two dimensions, the order of turns does not matter. Turn 20 degrees, then 40, and you get 60. Everything lives on one circle. In three dimensions that is no longer true, and it causes real trouble.
Hold your hand flat, palm down, fingers pointing away from you. Turn it 90 degrees about the line of your fingers (a roll). Then turn it 90 degrees about the vertical (a yaw). Note where your thumb points. Now start again and do the same two turns in the opposite order. Your thumb ends up somewhere else.
The matrices agree. Let Rx be a 90 degree turn about the x-axis, and Rz a 90 degree turn about the z-axis. Take the arrow a = (1, 0, 0). Rx sends (x, y, z) to (x, −z, y). Rz sends (x, y, z) to (−y, x, z). Turn about x first, then z:
Rz Rx a: (1, 0, 0) → (1, 0, 0) → (0, 1, 0)
Rx Rz a: (1, 0, 0) → (0, 1, 0) → (0, 0, 1)
Two different answers. Matrix products are not commutative, which means AB is not the same as BA. Try it below.
The three arrows are a tiny frame fixed to a hand. Choose an order, then drag the slider from 0 to 2. The first half of the slider does the first turn. The second half does the second.
Turns are about the fixed world axes, 90 degrees each. The picture is a simple tilted view of a right-handed frame: x to the right, y into the page, z up.
In three dimensions, a rotation matrix has nine numbers. Many people prefer fewer. Euler angles use three: roll, pitch and yaw, one turn about each axis in a set order. They are easy to read and to type. They also fail. At a pitch of 90 degrees, two of the axes line up, and a change in roll looks the same as a change in yaw. This is gimbal lock: one degree of freedom disappears, and the angles jump.
A quaternion uses four numbers of total length 1. It has no gimbal lock, it is cheap to multiply, and it blends smoothly between two rotations. It is hard to read by eye. Most robot software stores rotations as quaternions or matrices, and shows Euler angles to humans. This lesson stays in the plane, where one angle is enough. The deeper lessons below treat the full 3D case.
Go deeper: Rotations and Transforms covers 3D rotations, Euler angles and quaternions in detail. Lie Groups shows how rotations form a smooth family you can differentiate.
Chapter 3
Pack a turn and a move into one matrix, then chain the camera, wrist and base frames into a single answer
The wrist camera sees the cup 0.127 m straight ahead and 0.012 m to the left. That is a good measurement, but it is useless to the planner. The planner thinks in base coordinates. Between the two sit the camera mount, the wrist, the elbow and the shoulder.
The robot needs one tool that crosses a frame boundary, and that chains cleanly when there are many boundaries. That tool is the homogeneous transform.
To move a point from the camera frame to the wrist frame, do two things. First turn it, because the axes point differently. Second shift it, because the origins sit in different places. In symbols, with R for the turn and t for the shift:
This works, but chaining is clumsy. Two hops give R1(R2 p + t2) + t1, and three hops get worse. We want a single multiplication per hop.
Append a 1 to the end of every point. The point (0.127, 0.012) becomes (0.127, 0.012, 1). This extra 1 is the key to homogeneous coordinates. Now build a 3 by 3 matrix: put R in the top-left, t in the top-right, and the row (0, 0, 1) at the bottom.
Multiply T by the extended point. The top two rows give R p + t. The bottom row gives 1. So one matrix multiplication does both the turn and the shift. Modern Robotics (Lynch and Park, 2017) calls these matrices SE(2). The name means the set of rigid motions of the plane.
Now chaining is easy. Name each transform by its two frames: TBW takes wrist coordinates to base coordinates, and TWC takes camera coordinates to wrist coordinates. Chain them by multiplying. The inner letters cancel like fractions: B W times W C leaves B C.
pB = TBW · TWC · pC
Read the product from the right. The point starts in camera coordinates. The nearest matrix moves it to wrist coordinates. The next moves it to base coordinates. Here are the numbers for our start pose, where the joints are at 10 and 85 degrees.
The hand is at (0.368, 0.368) m and points at 10 + 85 = 95 degrees. Its transform is:
The camera sits 0.06 m ahead of the hand and 0.04 m to its side, with the same heading. So TWC is a pure shift by (0.06, 0.04), with R equal to the identity (no turn). Now follow the cup, which the camera sees at pC = (0.1269, 0.0117):
pW = TWC pC = (0.1269 + 0.06, 0.0117 + 0.04) = (0.1869, 0.0517)
xB = −0.0872 × 0.1869 − 0.9962 × 0.0517 + 0.3678 = 0.300
yB = 0.9962 × 0.1869 − 0.0872 × 0.0517 + 0.3683 = 0.550
The cup is at (0.300, 0.550) m in the base frame. This is the number the planner needed, and the cup was never given in base coordinates. The chain built it from three small pieces.
Bend the two joints. The camera's view of the cup changes, but the chain always returns the same base coordinates. The matrices update live.
Toy robot as in the hero. The camera is fixed to the hand at (0.06, 0.04) m with the same heading. The cup stays at (0.30, 0.55) m. The wrong order is TWC TBW pC.
Sometimes you need the opposite: base to camera. Undo the shift first, then undo the turn. The result has a neat form, because the inverse of R is RT.
For the camera: subtract the camera origin (0.3227, 0.4246) from the cup, giving (−0.0227, 0.1254), then apply R(95°)T. You get (0.1269, 0.0117) again. The loop closes.
Now the failure. Suppose a tired engineer writes the product in the wrong order, TWC TBW pC. The code runs. No error appears. The symptom is that the hand lands 0.106 m from the cup, mostly to the right, and knocks it over. The measurement: the computed cup position is (0.405, 0.534) instead of (0.300, 0.550). The fix: read the frame letters. Inner letters must match, as in B W times W C. If they do not touch, the order is wrong. Press the red button above to see this.
pythonimport numpy as np def T(theta, tx, ty): # a 3x3 planar transform c, s = np.cos(theta), np.sin(theta) return np.array([[c, -s, tx], [s, c, ty], [0, 0, 1.0]]) T_BW = T(np.deg2rad(95), 0.3678, 0.3683) # wrist in base T_WC = T(0.0, 0.06, 0.04) # camera in wrist p_C = np.array([0.1269, 0.0117, 1.0]) # shape (3,): x, y, then the 1 p_B = T_BW @ T_WC @ p_C # [0.3000, 0.5500, 1.0] T_CB = np.linalg.inv(T_BW @ T_WC) # base-to-camera print(T_CB @ p_B) # back to [0.1269, 0.0117, 1.0]
Go deeper: Rigid-Body Transforms (SE(2) and SE(3)) builds the 2D and 3D cases step by step. Frames and Transforms shows how a whole robot keeps its frame tree.
Chapter 4
Turn two joint angles into a hand position, and map the ring of places the hand can reach
The two motors report their angles: 10 degrees at the shoulder, 85 degrees at the elbow. Where is the hand? You cannot see it. The robot has to work it out, and it has to do so thousands of times a second.
The rule that turns joint angles into the position of the hand is forward kinematics. The word "kinematics" means the study of motion without asking what pushes it. We care only about geometry here, not forces.
Define the angles. The shoulder angle θ1 is measured from the base x-axis to the first link. The elbow angle θ2 is measured from the first link to the second. That makes it a relative angle: it says how much the second link bends away from the first.
The elbow is the end of the first link. Its position is a length-L1 step in the direction θ1: (L1 cos θ1, L1 sin θ1). That is the same unit-circle fact as Chapter 2.
The hand is the end of the second link, which starts at the elbow. Its heading is not θ2. It is the first link's heading plus the bend: θ1 + θ2. Walking along it for L2 gives the second step. Add the two steps.
Take the start pose, θ1 = 10° and θ2 = 85°, with L1 = 0.40 m and L2 = 0.30 m. The second link points at 10 + 85 = 95 degrees.
Elbow = (0.40 cos 10°, 0.40 sin 10°) = (0.3939, 0.0695)
Second step = (0.30 cos 95°, 0.30 sin 95°) = (−0.0261, 0.2989)
Hand = (0.3939 − 0.0261, 0.0695 + 0.2989) = (0.3678, 0.3683) m
That is the point from Chapter 3. The hand is 0.5205 m from the base. The second step has a negative x part because the forearm points a little past straight up.
There is a second way to see the same answer. Each joint is a turn of θ, then a walk of L along the new x-axis. Chain the pieces as in Chapter 3: T = Rot(θ1) Shift(L1) Rot(θ2) Shift(L2). Multiply them out, and the last column is the formula above. The formula is the transform chain, written out for two joints. A 6-joint arm just has a longer chain.
Not every point is reachable. The farthest the hand can be is when the arm is straight: L1 + L2 = 0.70 m. The nearest is when the arm is folded back on itself: |L1 − L2| = 0.10 m. Between those, the distance depends on the elbow bend alone. By the law of cosines, r2 = L12 + L22 + 2 L1 L2 cos θ2. At 85 degrees, r2 = 0.16 + 0.09 + 0.24 × 0.0872 = 0.2709, so r = 0.5205 m. That matches.
The set of reachable points is a flat ring, called the workspace, between 0.10 m and 0.70 m from the base. The shoulder angle swings the whole ring around. In this toy, both joints turn freely, so the ring is full. Real joints have stops, and the ring has bites taken out.
Drag the two joints. The warm trail records where the hand has been. The shaded ring is everywhere the hand can ever go.
Toy arm: L1 = 0.40 m, L2 = 0.30 m, joints free to turn through 360 degrees. The ring runs from |L1 minus L2| = 0.10 m to L1 + L2 = 0.70 m.
Here is the classic bug. NumPy's cos and sin expect radians, a way of measuring angles where a half turn is π = 3.14159. A programmer types np.cos(10) meaning ten degrees. NumPy reads ten radians, which is more than one and a half turns.
The symptom: the hand drifts to the wrong side of the robot, and a reach pushes the cup off the table. The measurement: the computed hand position is (−0.117, −0.013) m, but the true hand is at (0.368, 0.368) m. The error is 0.616 m. The fix is one line: convert with np.deg2rad at the boundary, and keep every angle in radians inside the code.
pythonimport numpy as np L1, L2 = 0.40, 0.30 # metres (toy sizes) def fk(theta1, theta2): # radians in, metres out x = L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2) y = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2) return np.array([x, y]) # shape (2,) print(fk(np.deg2rad(10), np.deg2rad(85))) # [0.3678, 0.3683] correct print(fk(10, 85)) # [-0.1166, -0.0126] the radians bug
Notice what forward kinematics gives you: one answer for each pair of angles. The reverse is not true. Many angle pairs can reach the same hand position. Chapter 6 deals with that.
Go deeper: Inverse Kinematics starts from forward kinematics for longer arms. Rigid-Body Transforms gives the chain-of-transforms view.
Chapter 5
Learn how a small joint nudge becomes hand motion, and see why a straight arm gets stuck
The camera shows the cup 5 cm to the right of the hand. The arm must slide the hand 5 cm. Which joint should it turn, and by how much? Forward kinematics tells you where the hand is. It does not tell you which way a turn will push it.
For that you need the speed of the hand for a given speed of the joints. The rule is the Jacobian. It is built from derivatives, so we start there.
Drive a car. Your position changes with time, and your speed is how fast. Speed is a derivative: the change in one quantity per small change in another. On a graph of position against time, speed is the slope of the line.
For the arm, a joint angle plays the part of time, and the hand position plays the part of distance. If turning joint 1 by 1 degree moves the hand 6 mm, the slope is 6 mm per degree. A steeper slope means a small turn makes a big move.
The hand has two coordinates and two joints, so we have four slopes. Each one answers: if I nudge only this joint, how much does this coordinate change? A slope measured with the other variables held fixed is a partial derivative.
Take the hand formula from Chapter 4. Differentiate each coordinate with respect to each angle. The rules you need are that the slope of sine is cosine, and the slope of cosine is minus sine. For a nudge to θ1, both terms move, because θ1 appears in both. For a nudge to θ2, only the second link moves.
Look at column 1. It equals (−y, x), the hand's own position turned by 90 degrees. That makes sense. Turning joint 1 spins the whole arm about the base, so the hand moves at a right angle to the line from the base.
At the start pose (10°, 85°), the hand is at (0.3678, 0.3683) and the second link's heading has sin 95° = 0.9962 and cos 95° = −0.0872. So:
Let joint 1 turn at ω1 = 0.50 rad/s and joint 2 at ω2 = −0.20 rad/s. Multiply:
vx = −0.3683 × 0.50 + (−0.2989) × (−0.20) = −0.1842 + 0.0598 = −0.124 m/s
vy = 0.3678 × 0.50 + (−0.0261) × (−0.20) = 0.1839 + 0.0052 = 0.189 m/s
The hand moves up and to the left at 0.226 m/s. As a check, nudge joint 1 by half a degree and recompute the forward kinematics. The hand moves (−0.00323, 0.00320) m. The Jacobian predicts (−0.00321, 0.00321). They agree because the nudge is small. The Jacobian is the best straight-line approximation to the hand's motion, near this pose.
Now imagine all the joint speeds with length 1, the circle of "unit effort." Each one gives a hand velocity. Together they trace an ellipse (a stretched circle). The long axis shows the direction in which the hand can move fastest. The short axis is the weakest direction. The hero shows this as the yellow ellipse.
Warm arrows show each joint's own push on the hand. The teal arrow is their sum. Drag the joint speeds. Then drag joint 2 toward 0 and watch the ellipse flatten.
Joint 1 is fixed at 10 degrees. Speeds are in rad/s. The ellipse is drawn at one third of true size to fit: its two half-widths are the hand speeds, in m/s, along the longest and shortest directions for one unit of joint speed. The determinant is L1 L2 sin(theta2).
The determinant of a 2 by 2 matrix tells you whether it is invertible, which means whether you can undo it. For a grid with rows (a, b) and (c, d), the determinant is a d minus b c. If it is zero, two columns point the same way, and the grid squashes the plane onto a line. You cannot undo a squash.
Work it out for J. Here a = −L1 sin θ1 − L2 sin θ12, b = −L2 sin θ12, c = L1 cos θ1 + L2 cos θ12, and d = L2 cos θ12. The L22 terms cancel. What is left is L1 L2 (sin θ12 cos θ1 − cos θ12 sin θ1). This is the sine of a difference, sin(θ12 − θ1) = sin θ2. One clean result: det J = L1 L2 sin θ2.
At θ2 = 85 degrees, det J = 0.12 × 0.9962 = 0.1195. Healthy. At θ2 = 0, the determinant is zero. The arm is stretched straight. Both columns of J now point the same way, at a right angle to the arm. Nothing you do with the joints can move the hand along the arm. This is a kinematic singularity: a pose where the hand loses a direction of motion. Modern Robotics, section 5.3, treats singularities for general arms.
The trouble starts before you reach it. Take the pose (40°, 5°), nearly straight, and ask the hand to move outward along the arm at 0.1 m/s. The symptom: the joints spin wildly and the arm jerks. The measurement: solving for joint speeds gives 7.27 rad/s, against 0.46 rad/s for the same hand move at an elbow bend of 85 degrees. That is 16 times more. The fix: stay away from straight poses when you plan, or use the damped step of Chapter 6.
pythonimport numpy as np def jacobian(t1, t2, L1=0.40, L2=0.30): s1, c1 = np.sin(t1), np.cos(t1) s12, c12 = np.sin(t1 + t2), np.cos(t1 + t2) return np.array([[-L1*s1 - L2*s12, -L2*s12], [ L1*c1 + L2*c12, L2*c12]]) # shape (2, 2) J = jacobian(np.deg2rad(10), np.deg2rad(85)) v = J @ np.array([0.5, -0.2]) # [-0.1244, 0.1891] m/s print(np.linalg.det(J)) # 0.1195 = 0.4*0.3*sin(85 deg) print(np.linalg.svd(J)[1]) # the two ellipse semi-axes
Go deeper: Jacobians and Linearization builds the Jacobian for any function. Eigenvalues explains the ellipse axes.
Chapter 6
Find the joint angles that put the hand on the cup, first by a formula and then by a loop that always works
The cup stands at (0.30, 0.55) m. The motors do not take positions. They take angles. So the robot must run forward kinematics backward: given a place, find the angles that reach it.
This job is called inverse kinematics, or IK. It is harder than the forward direction, because the answer may be none, one, two, or a whole family.
Draw the triangle with the base, the elbow and the cup at its corners. Its sides are known: L1, L2, and the distance r from base to cup. The elbow angle is fixed by that triangle alone. The law of cosines gives it:
Work it for the cup. The squared distance is 0.30 squared plus 0.55 squared, which is 0.0900 + 0.3025 = 0.3925. Subtract 0.16 and 0.09:
cos θ2 = (0.3925 − 0.16 − 0.09) / 0.24 = 0.1425 / 0.24 = 0.594, so θ2 = ±53.58°
There are two answers: bend the elbow one way, or bend it the other. The two bends give mirror-image arms that reach the same cup. Robot books call these elbow-up and elbow-down when the arm is seen from the side. From above, we call them elbow right and elbow left.
For θ2 = +53.58°: atan2(0.55, 0.30) = 61.39°, and atan2(0.3 sin 53.58°, 0.4 + 0.3 × 0.594) = atan2(0.2414, 0.5781) = 22.66°. So θ1 = 61.39 − 22.66 = 38.73°. For the other sign, the correction flips: θ1 = 61.39 + 22.66 = 84.05° with θ2 = −53.58°. Run forward kinematics on either pair and you land on (0.300, 0.550).
Two more facts. If the right side of the formula for cos θ2 is bigger than 1 or smaller than −1, there is no angle. The cup is out of the workspace of Chapter 4, and the arm cannot reach it. And which of the two answers should the robot take? Usually the one nearest the current pose, so the arm does not swing across the table, or the one that clears an obstacle.
For most arms, no formula exists. The fix is a loop that creeps toward the answer. It rests on one idea: over a small move, the arm is nearly a straight-line machine, and Chapter 5 told us the line.
Here is the loop. Let e be the error, the gap between the cup and the hand. Treat the arm as exactly linear, and ask: which joint change would close the gap in one go? Solve J Δθ = e. That is two equations in two unknowns, the two joint changes, and any pair of equations like it has a short solution. Take that step. The hand will not land exactly, because the arm is only nearly linear. But it will land closer. Repeat. This is the Newton-Raphson method; Modern Robotics, section 6.2, derives it from a first-order Taylor expansion.
Start at the start pose (10°, 85°), with the hand 0.194 m from the cup. The loop runs like this:
Look at the last column. The error falls from 0.19 to 0.044 to 0.0037 to 0.00003 m. Each step makes the error roughly the square of the one before. After three steps, the hand is 0.03 mm from the cup. And the loop found the elbow-right answer, 38.73 and 53.58 degrees, with no formula.
Choose a method and a start. Press Step to run one iteration, or Run to run until it settles. Faded arms are earlier steps. You can also drag the cup.
Toy arm as before, cup at (0.30, 0.55) m unless you drag it. The nearly straight start is 30 and 3 degrees. The damped step is Δθ = JT(J JT + λ2 I)−1 e.
Start the arm nearly straight, at (30°, 3°). The symptom: the first step throws the elbow around the table, and the arm never settles. The measurement: Δθ comes out as (−406°, +1007°), because J is almost singular and J−1 is huge. After seven steps the hand is still 0.75 m from the cup. Try it with the Newton button above.
The fix is damping. Instead of demanding the exact step, ask for a gentle one, and pay a penalty for large joint motion. Near a singularity, J JT has an almost zero entry, and inverting it gives a huge number. Adding λ2 keeps that entry away from zero, so the inverse stays small. The damped least squares step uses Δθ = JT(J JT + λ2I)−1 e. The number λ is the damping. At λ = 0.1, the first step is (18.5°, 15.9°). The error then falls 0.35, 0.084, 0.043, 0.020, 0.007, 0.0025, 0.0008, 0.0003 m. It is slower than clean Newton, but it never blows up.
pythonimport numpy as np # fk(theta) and jacobian(t1, t2) as in Chapters 4 and 5 def ik_step(theta, target, lam=0.1): # theta: shape (2,), radians e = target - fk(*theta) # shape (2,), metres J = jacobian(*theta) # shape (2, 2) dtheta = J.T @ np.linalg.solve(J @ J.T + lam**2 * np.eye(2), e) return theta + dtheta, np.linalg.norm(e) theta = np.deg2rad([10.0, 85.0]) for k in range(12): theta, err = ik_step(theta, np.array([0.30, 0.55])) print(np.rad2deg(theta), err) # about [38.7, 53.6], error under 1 mm
Go deeper: Inverse Kinematics covers longer arms and redundant joints, and Jacobians and Linearization explains why the loop converges.
Chapter 7
Describe doubt with a bell curve, then blend two doubtful readings into one better guess
The robot sits on a small wheeled base. It drives 0.50 m toward the table. The wheel sensor says "0.50 m." The wrist camera then looks at a mark on the floor and says "you are at 0.46 m." Which number is right?
Neither, probably. Wheels slip a little. Cameras blur a little. The robot holds two guesses that disagree, and it must still decide where it is. The maths of doing this well is the maths of uncertainty.
Measure the same distance ten times with a cheap tape measure, and you get ten slightly different numbers. The scatter is noise. The values cluster around a middle value and thin out on both sides. Plot how often each value appears, and you get a bell shape.
That bell is the Gaussian, also called the normal distribution. Two numbers describe it. The mean μ is the centre, the best single guess. The standard deviation σ is the typical distance from the centre. About 68 out of 100 readings fall within one σ of the mean. About 95 fall within two. The variance is σ2, the spread squared.
Why a bell? Many small independent errors add up: a bit of wheel slip, a bit of floor dust, a bit of timing jitter. A famous result says such sums look Gaussian, almost whatever the small errors are. The robot rarely checks. It assumes the bell, and gets a very good tool in return.
A belief is the robot's picture of where it is: a Gaussian with a mean (where it thinks it is) and a σ (how unsure it is). A narrow bell means "I am sure." A wide bell means "I could be anywhere in this range."
Back to the two numbers. The odometry belief is a Gaussian with μ = 0.50 m and σ = 0.04 m. The camera belief has μ = 0.46 m and σ = 0.02 m. The camera is twice as sharp. So the answer should sit nearer to 0.46 than to 0.50.
The fix is to weight each reading by how much we trust it. Trust is the inverse of variance. Here are the steps, which together make the 1-D Kalman filter update (Thrun, Burgard and Fox, Probabilistic Robotics, 2005, chapter 3).
Put in the numbers: σ2 = 0.042 = 0.0016, and r2 = 0.022 = 0.0004.
K = 0.0016 / (0.0016 + 0.0004) = 0.80
μ′ = 0.50 + 0.80 × (0.46 − 0.50) = 0.50 − 0.032 = 0.468 m
σ′2 = (1 − 0.80) × 0.0016 = 0.00032, so σ′ = 0.0179 m
The fused guess is 0.468 m. It is closer to the camera, as it should be. And its σ of 0.018 m is better than either input, 0.040 or 0.020. Two doubtful readings together beat the best one alone.
The robot keeps driving, so the loop repeats. Each cycle has two moves. Predict: shift the mean by the odometry distance, and add the odometry's own noise to the variance. Doubt grows when you move without looking. Update: blend in a camera reading with the formula above. Doubt shrinks when you look.
The pink bell is the robot's belief. The blue bell is the camera reading. The dashed line is the true position, which the robot never sees. Press Drive, then Look.
Toy robot. Each drive commands 0.50 m, but the wheels slip: the true move is 0.47 m. Camera noise values come from a fixed list so every visit matches. The start belief is 0.00 m with a standard deviation of 0.01 m.
Follow two cycles on paper. Start with μ = 0 and σ2 = 0.0001. Drive 0.50 m: μ = 0.50, and σ2 = 0.0001 + 0.0015 = 0.0016. Look at z = 0.46: μ = 0.468, σ2 = 0.00032. Drive again: μ = 0.968, σ2 = 0.00182, so σ = 0.043. Look at z = 0.956: K = 0.82, μ = 0.958, σ2 = 0.00033. The belief tracks the truth (0.94 m after two slipping drives) even though the odometry alone says 1.00 m.
Suppose the engineer says "my odometry is perfect" and sets the added noise to zero. The belief then only ever shrinks. The gain falls each cycle: K is 1 over (n + 4) after n looks in this setup. By the twentieth look, K = 0.042. The camera now gets 4% of the vote.
The symptom: the robot slowly drifts off course and the estimate does not follow. The measurement: with a 6% wheel slip, the odometry error grows by 0.03 m per drive, but the filter pulls back only 4% of each gap. The fix: never set noise to zero. Keep a small floor, so the filter always listens a little. Real systems tune this noise with data.
pythondef predict(mu, var, move, q2): # q2: odometry noise variance per drive return mu + move, var + q2 def update(mu, var, z, r2): # r2: camera noise variance K = var / (var + r2) # trust slider, 0 to 1 return mu + K * (z - mu), (1 - K) * var mu, var = 0.0, 0.0001 mu, var = predict(mu, var, 0.50, 0.0015) # 0.50, 0.0016 mu, var = update(mu, var, 0.46, 0.0004) # 0.468, 0.00032
Go deeper: The Bayes Filter builds beliefs without assuming a bell. The Kalman Filter does the full matrix version. The Extended Kalman Filter handles curved motion models. Barfoot, State Estimation for Robotics (Cambridge University Press, 2017), is the standard textbook.
Chapter 8
Write the motion of one joint as an equation, step it forward in time, and steer it with a controller
The planner says: turn joint 1 to 57 degrees, which is 1.0 rad. The motor does not jump there. It pushes, the joint speeds up, it overshoots, it swings back. If the planner sends the angle and hopes, the arm wobbles, or stops short, or goes wild.
To steer the joint, the robot needs two things. A model of how the joint moves when pushed. And a rule that looks at the error and decides how hard to push. The first is dynamics. The second is feedback control.
You know F = ma: a push makes a mass speed up. A turning joint obeys the same law in spin form: torque = inertia × angular acceleration. Torque is a twisting push. Inertia I measures how hard the joint is to spin up. Angular acceleration is how fast the spin speed changes.
Add two real effects. Friction pushes back in proportion to the speed: b times the speed. And a steady load (a cable that drags, say) pushes back with a fixed torque. We get:
This is an ordinary differential equation, or ODE: it ties a quantity to its own rates of change. Such an equation says how the state changes, not what the state is. To find the motion, you step through time.
The simplest way to follow an ODE is Euler integration. From the current angle and speed, compute the acceleration. Move the speed by acceleration times a small time step dt. Move the angle by speed times dt. Repeat. It is "walk a little, look again" with a stopwatch.
Here are the first steps of a joint that must go from 0 to 1 rad. The stiffness is Kp = 20 N m per rad. The damping gain is Kd = 6 N m s per rad, which we meet in a moment. The time step is dt = 0.01 s. At the start the speed is 0, the error is 1, and the torque is 20 × 1 = 20 N m. So the acceleration is 20 / 0.5 = 40 rad/s2.
Step 1: angle stays 0 + 0.01 × 0 = 0. Speed = 0 + 0.01 × 40 = 0.40 rad/s.
Step 2: angle = 0 + 0.01 × 0.40 = 0.004. Torque = 20 × 0.996 − 6 × 0.40 = 17.52. Acceleration = (17.52 − 0.2 × 0.40) / 0.5 = 34.88. Speed = 0.40 + 0.3488 = 0.749.
You can do this by hand for a few steps, and a computer does it a thousand times a second.
Feedback means the command depends on the error: the target minus what you measure. The P term pushes in proportion to the error: τ = Kp e. A big error means a big push, a small error means a small one. It acts like a spring that pulls the joint to its target.
A spring alone makes the joint swing forever, like a ball on a spring with almost no friction. With Kp = 20, the joint overshoots by 100% and still rings after three seconds. So add D, for derivative: push against the speed, τ = Kp e − Kd θ̇. It acts like a shock absorber.
With Kd = 2 the overshoot falls to 35%. With Kd = 6 the joint glides to the target with no overshoot, settling to within 2% in 0.86 s. Modern Robotics, section 11.2, names two numbers to measure on such a response. One is the overshoot (how far past the target it swings, as a percentage). The other is the 2% settling time (the time after which it stays within 2% of the target).
The warm line is the joint angle after you command a step from 0 to 1 rad. The faint band is within 2% of the target. Switch the controller, then try the load and the time step.
Toy joint: inertia 0.5 kg m2, friction 0.2 N m s/rad, explicit Euler steps, 3 s shown. In P mode Kd is off. In PID mode the integral gain is fixed at Ki = 20 N m per rad second. These numbers are illustrative.
Turn on the load. The symptom: the joint stops short of the target and sits there. The measurement: with Kp = 20 and Kd = 6, it ends at 0.90 rad, not 1.00. The error is 0.1 rad, about 5.7 degrees. This is because at rest the spring torque Kp e must balance the load: 20 e = 2, so e = 0.1.
The fix is I, for integral. Keep a running sum of the error, and add Ki times that sum to the torque. While any error remains, the sum keeps growing, and the push keeps growing, until the error is exactly zero. With Ki = 20, the joint reaches 1.00 rad, though it overshoots 15% on the way and takes 2.55 s to settle. That is the price of memory.
Euler's method is exact only for tiny steps. Set Kp = 20, Kd = 2, and raise dt. At dt = 0.15 s the same controller swings to −6 rad and keeps growing. The control law is fine. The simulation, or the real control loop, is running too slowly to track the motion. The fix: shrink dt. A real robot runs its joint loop at 500 to 1000 times a second. The rule of thumb: make the step much shorter than the fastest swing.
pythonI, b, load = 0.5, 0.2, 2.0 # inertia, friction, steady drag Kp, Kd, Ki, dt = 20.0, 6.0, 20.0, 0.01 theta, omega, integral, target = 0.0, 0.0, 0.0, 1.0 for step in range(300): # 3 seconds e = target - theta integral += e * dt tau = Kp * e - Kd * omega + Ki * integral # the PID law alpha = (tau - b * omega - load) / I # Newton, in spin form theta += dt * omega # Euler: angle uses the old speed omega += dt * alpha print(theta) # about 1.01 rad: the integral removed the load error
Go deeper: Trajectory Tracking, PID and LQR builds tracking controllers from this base. Modern Robotics, chapters 8 and 11, covers full-arm dynamics and motion control.
Chapter 9
Find the best answer when the data disagree, using the slope of the error to walk downhill
The robot's drawing says the links are 0.40 m and 0.30 m long. But the factory cut them a little off, and every IK answer inherits that error. The robot can measure the hand position at several poses, say with a ruler or an overhead camera. The readings are noisy. Which link lengths fit them best?
No single pose gives the answer, and the poses disagree a little. The task is to find the numbers that fit all the data as well as possible. That is least squares, the workhorse of optimization, which means choosing numbers to make a score as good as possible.
Guess two link lengths. For each reading, compute where the hand should be, using the Chapter 4 formula. The gap between that and the measured position is the residual. A good guess has small residuals.
To score the whole guess, square each residual and add them up: the sum of squared errors. Squaring does three useful things. It makes every error positive, so errors cannot cancel. It punishes big misses more than small ones. And it gives a smooth bowl-shaped score that you can slide down, which a sum of absolute values does not.
Look at the forward kinematics once more: x = L1 cos θ1 + L2 cos θ12. The angles are known from the motor encoders. The unknowns, L1 and L2, appear only multiplied by constants. So each measurement gives one equation that is linear in the unknowns. Stack all the equations into one matrix form: A l = m.
Here A has one row per measurement and one column per unknown. An x reading adds the row (cos θ1, cos θ12). A y reading adds (sin θ1, sin θ12). The vector l = (L1, L2), and m holds the measured coordinates.
Why do the normal equations on the right give the best fit? The bottom of a bowl is where the slope is zero. The slope of C is 2AT(A l − m). Set it to zero and rearrange. You get ATA l = AT m. It is two equations in two unknowns, solved like any pair.
Take three poses: (10°, 85°), (40°, 60°), (70°, 30°). Suppose the true lengths are 0.410 and 0.290 m (toy values), and each reading is off by a few millimetres. Six numbers result:
Add up the products to form ATA and ATm. The diagonal entries are each exactly 3, because cos2 + sin2 = 1 for each of three poses.
Solve: L1 = 0.4101 m, L2 = 0.2893 m
The fit recovers 0.410 and 0.290 to within 0.7 mm in total. The drawing's values, 0.40 and 0.30, leave an RMS residual of 8.0 mm. The fitted values cut it to 2.9 mm, which is the noise level of the readings. That is calibration: the data corrected the model.
For this problem you can solve the normal equations in one step. For curvy problems, like the IK of Chapter 6 or training a neural network, you cannot. The general method is to walk downhill. At the current guess, find the slope, take a step in the opposite direction, and repeat: l ← l − η × slope. That is gradient descent, and η is the step size.
Colour shows the cost for each guess of (L1, L2): bright is low. The pink dot is the best fit. The teal path is gradient descent. Drag the start guess anywhere on the map, then press Step.
Toy data: three poses (six readings), true lengths 0.410 m and 0.290 m, noise values from a fixed list scaled by the slider. The step size is a multiple of 1 over the largest curvature of the cost, so 1 is a safe size and anything above 2 diverges. The map spans 0.30 to 0.50 m for L1 and 0.20 to 0.40 m for L2.
Push the step size above 2. The symptom: each step jumps over the bottom to the far wall of the bowl, higher up, and the path spirals out. The measurement: the cost grows at every step instead of shrinking. The fix: lower the step size, or let the step shrink as you go.
Now switch to "Arm nearly straight." With the same 3 mm of noise, poses near (40°, 5°) give a fit that is 27 mm off, against 0.7 mm for the spread poses. The two columns of A point almost the same way. So ATA has eigenvalues of 5.99 and 0.013. An eigenvalue says how steeply the bowl rises along one of its two main directions: 5.99 is a steep wall, and 0.013 is an almost flat floor. The bowl has become a long thin valley. The data cannot say how to split the reach between link 1 and link 2. This is the singularity of Chapter 5 again, in disguise. The fix is to collect data at varied poses.
pythonimport numpy as np # A: shape (2N, 2), one row per x or y reading; m: shape (2N,) measured coordinates l_hat = np.linalg.solve(A.T @ A, A.T @ m) # normal equations: [0.4101, 0.2893] l_alt, *_ = np.linalg.lstsq(A, m, rcond=None) # the same, but more stable: prefer this # gradient descent on C(l) = |A l - m|^2, step size eta below 2 / lambda_max lam_max = 2 * np.linalg.eigvalsh(A.T @ A).max() eta, l = 0.9 / lam_max, np.array([0.40, 0.30]) for k in range(100): l = l - eta * 2 * A.T @ (A @ l - m) # walk downhill
Go deeper: Nonlinear Least Squares handles models that are not linear in the unknowns. Maths for ML covers gradients and optimization. The Robotics Math Workbook has exercises on every tool in this lesson.
Chapter 10
Keep one table of the tools, match a robot's symptom to the right one, and pick the lesson to take next
You have followed one reach from start to finish. The cup sat at (0.30, 0.55) m. Frames and transforms put it in the base frame. Forward kinematics told the arm where its hand was. The Jacobian showed which way each joint pushes. Inverse kinematics found the angles. A filter fused two doubtful readings. A controller drove the joints to those angles. And least squares tuned the model that all of it rests on.
Nine tools, one robot. This last chapter packs them into a map you can keep, and shows you where to go next.
| Tool | The question it answers | Where it shows up in a robot | Lesson to take next |
|---|---|---|---|
| Vectors and frames | Where is it, measured from what? | Every sensor reading and every map | Frames and Transforms |
| Rotations | How is it turned? | Camera and IMU orientation, wrist control | Rotations and Transforms, Lie Groups |
| Homogeneous transforms | How do I move a point from one frame to another? | Camera to base, tool to wrist, sensor calibration | Rigid-Body Transforms |
| Forward kinematics | Where is the hand for these joint angles? | Collision checks, visual servoing, simulation | Inverse Kinematics |
| The Jacobian | How do joint speeds turn into hand speed? | Velocity control, force control, singularity checks | Jacobians and Linearization |
| Inverse kinematics | Which angles put the hand there? | Pick and place, teleoperation, animation | Inverse Kinematics |
| Probability and Kalman filters | How sure am I, and how do I combine readings? | Localization, tracking, SLAM | Bayes Filter, Kalman Filter, EKF |
| ODEs and feedback | How do I make the joint follow a target? | Every motor loop, trajectory tracking | Tracking, PID and LQR |
| Least squares | What fits the data best? | Calibration, mapping, learning | Nonlinear Least Squares, Maths for ML |
One more tool hides behind the Jacobian ellipse and the fit valley: eigenvalues, which measure how stretched a matrix is. They return in the filter and the control lessons.
Real robots rarely say "my Jacobian is singular." They say something odd. Match each symptom to the tool that explains it.
Press a symptom. The card names the likely cause, the chapter, and the lesson to open.
The symptoms are typical, not a full list. Several tools can cause the same symptom, so the card gives the most likely one.
| Quantity | Value | Chapter |
|---|---|---|
| Cup in the base frame | (0.300, 0.550) m | 1, 3 |
| Cup in the camera frame, start pose | (0.127, 0.012) m | 3 |
| Hand at joints 10 and 85 degrees | (0.368, 0.368) m | 4 |
| Determinant of J at the start pose | 0.1195 (zero when straight) | 5 |
| IK joint angles for the cup | 38.73 and 53.58 degrees, or 84.05 and −53.58 | 6 |
| Newton error after 0, 1, 2, 3 steps | 0.194, 0.044, 0.0037, 0.00003 m | 6 |
| Fused estimate and its sigma | 0.468 m, 0.018 m | 7 |
| PD step response, Kp 20 and Kd 6 | No overshoot, 0.86 s to settle | 8 |
| Fitted link lengths | 0.4101 m and 0.2893 m | 9 |
pythonimport numpy as np L1, L2 = 0.40, 0.30 def T(th, tx, ty): # Ch 3: a planar transform c, s = np.cos(th), np.sin(th) return np.array([[c, -s, tx], [s, c, ty], [0, 0, 1.0]]) def fk(q): # Ch 4: joint angles to hand return np.array([L1*np.cos(q[0]) + L2*np.cos(q[0]+q[1]), L1*np.sin(q[0]) + L2*np.sin(q[0]+q[1])]) def jac(q): # Ch 5: joint speeds to hand speed s1, c1, s12, c12 = np.sin(q[0]), np.cos(q[0]), np.sin(q[0]+q[1]), np.cos(q[0]+q[1]) return np.array([[-L1*s1-L2*s12, -L2*s12], [L1*c1+L2*c12, L2*c12]]) def ik(target, q, lam=0.1, n=20): # Ch 6: a damped loop for _ in range(n): e, J = target - fk(q), jac(q) q = q + J.T @ np.linalg.solve(J @ J.T + lam**2 * np.eye(2), e) return q def fuse(mu, var, z, r2): # Ch 7: a Kalman update K = var / (var + r2); return mu + K*(z - mu), (1 - K)*var def pd(e, w, Kp=20.0, Kd=6.0): # Ch 8: a torque from error and speed return Kp*e - Kd*w # Ch 9: l_hat = np.linalg.lstsq(A, m, rcond=None)[0] q = ik(np.array([0.30, 0.55]), np.deg2rad([10.0, 85.0])) print(np.rad2deg(q), fk(q)) # about [38.7, 53.6] and [0.30, 0.55]
Study in the order the robot needs the tools. Read the lessons below one at a time. Do not try to finish a whole textbook before you touch a robot.
Now press Teach and explain one reach from memory. Say how the camera reading reaches the base frame, how the joints are found, and what could go wrong at each step. Then go back to the arm at the top. Drag the cup to the edge of the reachable ring, and say which of the nine tools you see at work.