Joints, targets & PD control

Meet your robot

The arm in the viewer has four joints:

#	Joint	Moves	Axis
0	base	spins the whole arm	vertical (z)
1	shoulder	tilts the upper arm	horizontal (y)
2	elbow	bends the forearm	horizontal (y)
3	wrist	tilts the gripper	horizontal (y)

A list of angles like [base, shoulder, elbow, wrist] is called a configuration, written $q$. Angles are in radians, and the pitch joints are measured from straight up.

You command targets, not motion

Real robot arms almost never take motor torques straight from your program. Each joint runs its own PD position controller hundreds of times per second:

$$\tau = k_p\,(q_{\text{target}} - q) - k_d\,\dot q$$

Your job is to send target angles. The controller pushes the joint towards the target (the $k_p$ term) and damps the motion so it doesn't overshoot (the $k_d$ term).

The second big idea is that nothing moves until simulated time passes. arm.set_joint_targets(q) only changes the target. sim.wait(seconds) then runs the physics. Real robot code has the same shape: a command is sent and then the program waits for the motion to finish.

arm.set_joint_targets(q)   # jump: moves as fast as it can
arm.move_joints(q, 2.0)    # glide there over 2 s
sim.wait(0.5)              # run physics for 0.5 s

Your task

1. Move the arm to the ready pose [0.0, 0.5, 1.5, 1.14] and hold it for at least half a second. Notice that $0.5 + 1.5 + 1.14 \approx \pi$, which points the gripper straight down.
2. Turn the base to face the cup. world.cup.position gives you $(x, y, z)$. The base angle is measured from the $+x$ axis, so you need $\operatorname{atan2}(y, x)$.
3. Close the gripper, then open it again.

Press Run and watch the result replay in the viewer. Try swapping move_joints for set_joint_targets to see the difference.