The arm learns to touch a point

A red ball appears somewhere around the quadruped's OpenMANIPULATOR-X. The arm must touch it with its fingertips, within 1.5 cm. Nobody tells it how: no inverse kinematics, only the four joint motors and a reward that says touched or not touched. With SAC and Hindsight Experience Replay it learns this anyway, because it learns from its misses too.

1The goal

2A sparse reward

The reward is as simple as it gets:

reward = 0   # fingertips within 1.5 cm of the ball
reward = -1  # otherwise, every step (0.1 s)

That is honest (it says exactly what we want) and easy to make on a real robot. But it is sparse: a network that starts by moving the joints at random almost never ends up within 1.5 cm of a random point. Every attempt ends with −50 and nothing to learn from: no hint whether it was close or far.

You could reward the distance instead (closer = better). That works for touching a point, but for harder jobs (push a cube to a spot, press a door handle) a good “closer” number is hard to write and easy to get wrong, as tutorial 4 showed. HER keeps the honest reward and makes it learnable.

3Hindsight Experience Replay

The trick (Andrychowicz et al., 2017) is one sentence: a miss is a success for another goal. After an attempt that missed the ball, HER stores the same movements once more, as if the goal had been a point the hand did reach:

So from the very first attempt the network learns “this is how my hand gets somewhere”, and step by step “to any point I am asked for”. That only works when the goal is an input of the network: it gets the ball's position next to its own state, so the same network can be asked for any point. This is called a goal-conditioned policy.

We use the future strategy: for every step, HER also stores it with 4 goals the hand reached later in that same attempt.

HER needs an algorithm that learns from a memory of old steps (off-policy), because it rewrites that memory. PPO learns only from its latest attempts and throws them away; SAC (Soft Actor-Critic) keeps a replay buffer of all of them, is good at smooth continuous movements, and explores by itself (it is rewarded for staying a bit random).

4What you need

As in tutorial 1 (the robotSimulatorAPI repository, Node.js 22+, Chrome, Python 3.10+) with stable-baselines3:

pip install stable-baselines3 gymnasium

The script is python/arm_reach_her.py:

node server.mjs                           # terminal 1: the simulators
python python/arm_reach_her.py check    # our kinematics = the simulator's?
python python/arm_reach_her.py train    # SAC + HER, 60 000 steps (~20 min)
python python/arm_reach_her.py eval     # success rate on 100 new points

To watch it, start the server with a visible browser:

node server.mjs --show
python python/arm_reach_her.py watch --episodes 10

A trained model is included: add --model python/models/reach_sac_her.zip to eval or watch.

5The environment

HER needs the observation split in three parts, as a dictionary:

The action is a change of the four joint angles, −1 … 1 = −9 … +9° per step of 0.1 s (the servos turn at most 90°/s). The Python side keeps the four target angles and sends them as servo commands; the simulator reports where the fingertips ended up:

self.q = np.clip(self.q + action * 9.0, LIMITS[:, 0], LIMITS[:, 1])
obs = api.step(env_id, {"motors": {"arm_joint1": q1, "arm_joint2": q2, …}}, dt=0.1)
tip = obs["extra"]["reach"]["tip"]    # fingertips in the arm frame

HER calls compute_reward itself, with many rewritten goals at once, so it must work on a whole batch:

def compute_reward(self, achieved_goal, desired_goal, info):
    d = np.linalg.norm(achieved_goal - desired_goal, axis=-1)
    return -(d > 0.015).astype(np.float32)

An attempt lasts 50 steps (5 s). The next attempt starts where the arm is, with a new ball: that gives varied starting poses for free. The red ball is drawn in the simulator with {"reach": {"x", "y", "z"}} in the action, so you can see what it is aiming for.

6Where can the arm reach?

A goal the arm can't reach teaches nothing. So the script picks goals the safe way: a random pose of the four joints, and the point where the fingertips would be. That needs the arm's forward kinematics, a few lines with the lengths from ROBOTIS' model:

def fingertips(q):                              # q: 4 joint angles in degrees
    q1, q2, q3, q4 = np.radians(q)
    rot = lambda a, x, z: (x*cos(a) + z*sin(a), -x*sin(a) + z*cos(a))
    r1, z1 = rot(q2, 0.024, 0.128)               # upper arm
    r2, z2 = rot(q2 + q3, 0.124, 0.0)            # forearm
    r3, z3 = rot(q2 + q3 + q4, 0.126, 0.0)       # wrist → fingertips
    r, z = r1 + r2 + r3, z1 + z2 + z3
    return (0.012 + r*cos(q1), r*sin(q1), 0.0765 + z)

check moves the arm to a few poses and compares: our numbers and the simulator's agree to 0.1 mm. Goals below the base plate or right next to the arm's own column are skipped. Note that the network does not get this function: it has to find out how its joints move its hand.

7SAC + HER in code

In stable-baselines3 HER is a replay buffer you give to SAC:

from stable_baselines3 import SAC, HerReplayBuffer

model = SAC("MultiInputPolicy", env,
            replay_buffer_class=HerReplayBuffer,
            replay_buffer_kwargs=dict(n_sampled_goal=4, goal_selection_strategy="future"),
            learning_starts=1000, batch_size=256, gamma=0.95, learning_rate=1e-3, tau=0.05,
            policy_kwargs=dict(net_arch=[256, 256, 256]))
model.learn(total_timesteps=60_000)

MultiInputPolicy reads the dictionary observation; gamma=0.95 is enough for attempts of 50 steps.

8Train it

python python/arm_reach_her.py train
step   2000  episodes   40  touched (last 100)   0.0 %  27 s
step  10000  episodes  200  touched (last 100)   1.0 %  180 s
step  20000  episodes  400  touched (last 100)  24.0 %  380 s
step  30000  episodes  600  touched (last 100)  65.0 %  577 s
step  40000  episodes  800  touched (last 100)  82.0 %  776 s
step  50000  episodes 1000  touched (last 100)  97.0 %  975 s
step  60000  episodes 1200  touched (last 100)  96.0 %  1174 s
Saved reach_sac_her.zip after 1174 s of training

“Touched (last 100)” counts attempts during training, while it still explores, so it is lower than the final result. Test the saved network on 100 points it never saw:

python python/arm_reach_her.py eval
reach_sac_her.zip: touched 98 % of 100 new points, mean final distance 1.0 cm, 8.8 steps of 0.1 s to touch

9With and without HER

Each 30 000 steps (SAC + HER also 60 000), the same environment, the same sparse reward:

python python/arm_reach_her.py train --algo sac-her --steps 30000 --model her30k.zip
python python/arm_reach_her.py train --algo sac --steps 30000
python python/arm_reach_her.py train --algo ppo --steps 30000
python python/arm_reach_her.py eval --algo sac
python python/arm_reach_her.py eval --algo ppo

Without HER neither algorithm touched a single point, not even during training: 600 attempts, 600 times −50, nothing to learn from. SAC without HER did learn to move its hand roughly towards the ball (18.9 cm instead of PPO's 31 cm, it gets the ball's position as input), but never close enough to find out what “touched” is. With HER the same SAC touches 82 % after 30 000 steps and 98 % after 60 000: the only difference is that it learns from its misses.

10From reaching to a skill

In tutorial 4 the skills were hand-written; pick uses inverse kinematics to put the gripper at the cube. The network trained here can do that part: give it the cube's position as the goal. The same recipe (a goal in the observation, a sparse “done or not” reward, HER) works for harder jobs, as in the original HER paper with a Fetch robot:

• Push a cube to a spot on the table: the goal is the cube's position, not the hand's.
• Press the door handle: the goal is the handle's angle.
• Pick and place: add the gripper as a fifth action.

11Experiments

12Troubleshooting