The shape of one joint

TLDR

Using low level comms with explicit trajectories, we make the elbow joint on a Unitree R1 go up and down. Along the way we take detours through configuration spaces and topology, differential geometry, rigid body motion, PD control, and the DDS plumbing with some Python at the end of each part. There are also some cool interactive diagrams sprinkled throughout.

But before any of that, obligatory video of the robot spinning elbowing the wall.

The wall did not stand a chance.

Thanks to my wife Sabrina I got my hands on the new R1 EDU from Unitree. It came with dextrous hands that I haven’t installed yet, and a compute backpack that I screwed up installing because the ethernet port was covered with tape lol.

This post is mostly about how I use low level control to make the robot do one simple motion. The most simplified version of drumming would be just moving the elbow joint, but in reality this is mostly for me to get acquainted with the robot and how it works, and make some content out of it.

Of course if you read the thing I just wrote and said “um no”, then you’re right. But I had to make sure the elbow motion kind of matches something real. Drumming really would involve both the wrist and the elbow, controlling the rebound, and at higher speeds, or on double and triple strokes, it’s going to be the fingers. I have about a year of pretty basic drumming lessons behind me, which is just enough to know that. In any case all of that is going to take forever, so I wanted to share at least some basic shit.

What did you think I use the motion for…

Along the way we’ll do a very basic robotics intro, except with a real robot. We only look at one joint, the elbow joint I was actually controlling, how it connects to the links, and some basic lingo, so that you don’t have to go look at a reference. You can obviously look all of this up in a robotics book, but you shouldn’t have to for this.

One joint

The elbow moves up and down. That’s called a revolute joint, meaning it rotates about a single fixed axis.

I did not define a joint rigorously, but you can think of it as a connection between two rigid bodies with some positive number of degrees of freedom. I did not define those either. At this point I know more diffgeom than robotics unfortunately lol, so we probably should define things.

If it was unconstrained, moving my elbow up and down would just move it in a circle. So topologically the space of positions that joint can be in is $S^1$. Note that the R1 has its joint limits published in the developer docs.

The joint is obviously connected to the ground, but in this case we treat the rest of the robot as the ground and just look at the one joint hanging off it.

Once we constrain it, we see that it no longer wraps around. As you can see from the link above, the joints on this robot are constrained, and the elbow has a certain min and max in radians. So look at how the topology changed just by adding limits, since an interval is contractible and a circle isn’t.

Two joints

Now take two unconstrained revolute joints. Each one is its own circle and they don’t interact, so the configuration space is the product

which is a torus. I don’t define a torus, but I assume you have a mental picture of a sheet of paper glued along opposing edges (also see the interactive below).

Now compare that with the sphere $S^2$. Both are 2-dimensional manifolds, so locally they look the same, since every point of either one has a neighborhood homeomorphic to an open subset of $\mathbb{R}^2$. Globally they’re not the same at all, and that difference is topological.

The way I like to see it is with the direction a link points, which lives on $S^2$, so think of a rod pinned at one end and free to point anywhere. If you put it towards the north pole and use the same angle of rotation, it degenerates into not moving at all. Precisely: if I’m at the north pole and I specify latitude and longitude, then changing the longitude doesn’t actually move me at all. That’s a failure of the coordinates, not of the sphere, and it’s the same failure that gets called gimbal lock.

The way people actually dodge this is to stop using three angles in the first place. A unit quaternion carries a rotation as four numbers with one constraint holding them on a sphere, and since it was never a three-coordinate chart, there is no chart left to degenerate. This doesn’t change the topology of $SO(3)$ one bit though, it just stops pretending $SO(3)$ is a box of three dials.

The thing that actually tells them apart is the fundamental group. The sphere is simply connected and the torus isn’t:

So $\pi_1$ of the sphere is the trivial group, meaning every loop contracts to a point, and $\pi_1$ of the torus is the free abelian group on two generators, one loop around each circle. Strictly speaking $\pi_1$ needs a basepoint, but all of these spaces are path connected, so the group you get doesn’t depend on which one you pick.

This is the part I actually care about here. Motion is a topological question, and topology is about shape, not about dimension. The torus and the sphere have exactly the same dimension, and that fact bought us nothing: counting degrees of freedom tells you how many numbers you need to write a pose down, and tells you nothing at all about what happens when you start turning them.

While we’re at it we can also do the wedge of two circles, meaning the two circles glued at a single point,

the free group on two generators, which is nonabelian, so now the order of the loops matters. On the torus $ab$ and $ba$ are the same class, and on the wedge they aren’t.

What it actually means to move the arm

Now let’s talk about what it actually means to move the robot hand, and particularly what it means on the R1. Asking a joint to be somewhere means computing a torque.

Quickly, on torque, since everything below is denominated in it. Getting a mass moving in a straight line takes a force, and rotating something is no different, it just happens angularly: swinging a link takes an effort too, and torque is the rotational version of that force. It’s what you get by pushing at some distance from the axis, so a small push far out and a hard push close in come to the same thing. Newton comes along for the ride as well, and $F = ma$ turns into

where the moment of inertia $J$ plays the part of the mass and $\ddot q$ is angular acceleration. It’s the equation every interactive on this page is integrating, at $J = 0.05\ \mathrm{kg\,m^2}$ for something elbow sized.

So the formula for the torque we actually send is your standard PD controller with an added feed forward torque:

Here $q$ and $\dot q$ are the measured position and velocity of the joint, $q^{\text{des}}$ and $\dot q^{\text{des}}$ are the ones we’re asking for, and $\tau$ is a torque in $\mathrm{N\,m}$. That makes $K_p$ a stiffness in $\mathrm{N\,m/rad}$ and $K_d$ a damping in $\mathrm{N\,m\,s/rad}$, which is worth keeping in mind, because the two gains are not on the same scale and you can’t compare their numbers directly.

So it’s the displacement in position times $K_p$, the displacement in velocity times $K_d$, and then whatever torque you want to add directly. What it’s really telling us is how to periodically adjust the force based on the error. You look at where you are versus where you were asked to be, you look at how fast you’re closing that gap, and you push accordingly. The $K_p$ term is the spring that pulls you toward the target, and the $K_d$ term is the damper that keeps you from flying past it.

Funny enough you already know a bunch of these. Cruise control is a controller, where the error is the difference between your set speed and your actual speed, and the output is throttle. Autoscaling is a controller too, where the error is the difference between target utilization and actual utilization, and the output is how many instances you add. Both of those are usually PID rather than PD, so they also integrate the error over time to kill the steady state offset.

Here are the gains I’m actually running. These are the defaults straight out of the R1 tutorials, and I have not sat down and properly tuned them, so treat them as a starting point rather than anything considered.

# stiffness, the proportional part of the PD controller, in N·m/rad
# think of the spring equation, but angular: the further the joint is from
# where we asked it to be, the harder this pulls it back
Kp = [
    200, 200, 200, 200, 200, 200,     # legs
    200, 200, 200, 200, 200, 200,     # legs
    300, 300,                         # waist
    100, 100, 100, 100, 50,           # arms
    100, 100, 100, 100, 50,           # arms
    50, 10                            # head
]

# damping, the derivative part, in N·m·s/rad
# this one reads velocity rather than position, so it pushes against however
# fast the joint is already moving, which is what keeps the spring above from
# overshooting and ringing
Kd = [
    3, 3, 3, 3, 3, 3,                 # legs
    3, 3, 3, 3, 3, 3,                 # legs
    5, 5,                             # waist
    2, 2, 2, 2, 2,                    # arms
    2, 2, 2, 2, 2,                    # arms
    2, 0.1                            # head
]

One entry per joint, 26 of them. Legs and waist are the stiffest, which makes sense given they’re the ones holding the robot up against gravity, arms are softer, and the head is softer still, down to $K_p = 10$ on the last joint, where you’d rather it gave way than fought you.

Both lists are indexed by joint, and that index is worth having in front of you, along with the map into the wire format and the limits every one of them is clamped to.

# continuous indices
class R1JointIndex:
    LeftHipPitch = 0
    LeftHipRoll = 1
    LeftHipYaw = 2
    LeftKnee = 3
    LeftAnklePitch = 4
    LeftAnkleB = 4
    LeftAnkleRoll = 5
    LeftAnkleA = 5
    RightHipPitch = 6
    RightHipRoll = 7
    RightHipYaw = 8
    RightKnee = 9
    RightAnklePitch = 10
    RightAnkleB = 10
    RightAnkleRoll = 11
    RightAnkleA = 11
    WaistRoll = 12
    WaistYaw = 13
    LeftShoulderPitch = 14
    LeftShoulderRoll = 15
    LeftShoulderYaw = 16
    LeftElbow = 17
    LeftWristRoll = 18
    RightShoulderPitch = 19
    RightShoulderRoll = 20
    RightShoulderYaw = 21
    RightElbow = 22
    RightWristRoll = 23
    HeadPitch = 24
    HeadYaw = 25
    
# map from the contigous index to the actual IDL index
# of the data structure that will be sent across
# joint index
joint_idx_in_idl = [
    0, 1, 2, 3, 4, 5,
    6, 7, 8, 9, 10, 11,
    12, 13,
    15, 16, 17, 18, 19,
    22, 23, 24, 25, 26,
    29, 30
]
    
# (min, max) joint limits in rad
R1_JOINT_LIMITS = [
    (-2.9322, 2.5482),
    (-1.0472, 1.7453),
    (-2.7402, 2.7402),
    (-0.1745, 2.4260),
    (-0.8727, 0.5760),
    (-0.2618, 0.2618),
    (-2.9322, 2.5482),
    (-1.7453, 1.0472),
    (-2.7402, 2.7402),
    (-0.1745, 2.4260),
    (-0.8727, 0.5760),
    (-0.2618, 0.2618),
    (-0.5236, 0.5236),
    (-2.6180, 2.6180),
    (-3.1416, 2.0944),
    (-0.2269, 2.4784),
    (-1.9199, 1.9199),
    (-0.9757, 2.1850),
    (-1.9199, 1.9199),
    (-3.1416, 2.0944),
    (-2.4784, 0.2269),
    (-1.9199, 1.9199),
    (-0.9757, 2.1850),
    (-1.9199, 1.9199),
    (-0.6283, 0.6283),
    (-2.0071, 2.0071),
]

Two things in there are worth pulling out. Each arm block runs shoulder pitch, shoulder roll, shoulder yaw, elbow, wrist roll, so the joint this whole post is about is LeftElbow = 17, and it runs at $K_p = 100\ \mathrm{N\,m/rad}$ and $K_d = 2\ \mathrm{N\,m\,s/rad}$. Its limits are $[-0.9757,\ 2.1850]$ rad, so about $-56°$ to $125°$: a range of $181°$, a little over half a turn and nowhere near the full circle the unconstrained joint would have had. That interval is the contractible thing from the first section, written down.

The joint_idx_in_idl list is the other one to notice. The tidy contiguous numbering above is not what goes on the wire, so every command has to be written through that map, which is why joint_idx_in_idl[i] shows up on every line of the code below.

Communicating with the robot

Now this is going to be purely engineering, as in how do we actually communicate with the robot itself. It’s DDS, and specifically Cyclone DDS. I haven’t really dug into the spec too much for these purposes, but if you want to learn more you can check it out there.

Simplifying it a lot, DDS is kind of like pub sub. It’s obviously more than just pub sub, but what I like to think about it is that you read the states from the robot and you send a command to the robot. The commands we’re sending are basically the states of all the motors, and the state of every motor is the state that we’re receiving.

Here’s what that actually looks like in Python. There’s a fair amount of cruft in here, but behind all of it there are only two things going on: we subscribe to the robot’s state updates, and we periodically send commands back. Everything else is setup. (These are methods on my controller class, and there’s a C++ SDK too that works the same way.)

def Init(self):
    # first we typically shut down the motion switcher client
    # coming from the remote controller
    self.msc = MotionSwitcherClient()
    self.msc.SetTimeout(5.0)
    self.msc.Init()

    # keep releasing until no mode is holding the robot
    status, result = self.msc.CheckMode()
    while result is None or result["name"]:
        self.msc.ReleaseMode()
        status, result = self.msc.CheckMode()
        time.sleep(1)
    self.msc.ReleaseMode()

    # sending commands to the robot
    # create publisher #
    self.lowcmd_publisher_ = ChannelPublisher("rt/lowcmd", LowCmd_)
    self.lowcmd_publisher_.Init()

    # receiving robot state
    # create subscriber #
    self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_)
    self.lowstate_subscriber.Init(self.LowStateHandler, 10)

def Start(self):
    # LowCmdWrite gets called every control_dt_ seconds, forever
    self.lowCmdWriteThreadPtr = RecurrentThread(
        interval=self.control_dt_, target=self.LowCmdWrite, name="control"
    )

    # don't start writing until the first state message has told us
    # which mode machine we're talking to
    while not self.update_mode_machine_:
        time.sleep(1)

    self.lowCmdWriteThreadPtr.Start()

# the read side: fires on every state message from the robot
def LowStateHandler(self, msg: LowState_):
    self.low_state = msg

    # seed the shared state before releasing Start(), otherwise the control
    # thread can begin a tick while desired is still empty
    self.shared_state.set_low_state(msg)

    if not self.update_mode_machine_:
        self.mode_machine_ = self.low_state.mode_machine
        self.update_mode_machine_ = True

So Init releases whatever mode the remote controller had the robot in, then opens exactly two channels, rt/lowcmd to write and rt/lowstate to read. LowStateHandler is the read side and it fires whenever the robot has something to say. The write side is LowCmdWrite, which fires on a timer instead, every control_dt_ seconds, and that’s where the trajectory actually turns into a message. We’ll get to it once we have a trajectory worth writing.

The bottom line is that we are continuously getting the state at a certain frequency, like 500 times a second, and sending commands back at the same rate. That read and write loop is the whole interface. This part is actually devoid of any machine learning, it’s just the movement and the trajectories.

It is, though, the basis for reinforcement learning. When I say that, what I mean is: imagine that instead of us trying to hardcode a trajectory, the robot learns a policy that at each point in time reads the state of the robot and tells the actuators the desired position, velocity, acceleration and so on. Obviously the state could be a much bigger thing, like the room or camera input, but in the simplest sense it’s the same read and write loop we already have.

Trajectories

Ideally we want to create a trajectory for our elbow, and I obviously chose an oscillating one. It’s got to be smooth, and most importantly it’s got to be continuous.

What is a trajectory, though? It’s basically a curve on a manifold for position, and optionally velocity and the rest.

Here’s what happens at a discontinuity. Say we reset the robot into the zero state, so $q = 0$, and then we instantly tell the robot that we want it at $-10$ radians. Because we’re asking this every $\Delta t = 1/500$ seconds, the average velocity you’d need over that one tick is

and the PD law happily asks for a torque proportional to a 10 radian error. Never mind that 10 radians is well past the elbow’s limit anyway, the point is what the arithmetic does. Something is definitely going to break here. If I asked you to move that far in two milliseconds it would be quite challenging for you too.

On a real robot this manifests as a wound up jerk. The controller sees a huge error, commands whatever torque it’s allowed to, the joint snaps toward the target, and then the damping term has to catch it on the other side.

Which is exactly the video at the top of this post. That’s not a robot that lost its balance, it’s a robot asked to be somewhere it couldn’t get to in the time it was given, doing its best, and taking the wall and then the floor with it. Same arithmetic, just with more of the arm involved.

So this means we need to give the robot a position that’s continuous and also smooth. And what do we mean by smooth here? Each derivative we ask to exist and be continuous buys us one more thing that can’t jump:

Worth being careful here, because continuity on its own buys you nothing about the velocity. A continuous function doesn’t have to be differentiable anywhere at all. What it does buy is that the commanded position never jumps, which is the thing that blew up the torque above. If you want a velocity that doesn’t jump that’s $C^1$, and an acceleration that doesn’t jump is $C^2$. Jerk is the third derivative $\dddot q$, so actually bounding it wants $C^3$, or at least a Lipschitz $\ddot q$. In practice $C^2$ is already plenty.

Also the word smooth is doing two jobs here. For a trajectory I just mean enough derivatives for whatever we’re doing. For the manifold stuff below it means $C^\infty$.

So now you see what I’m alluding to. We have talked about how the configuration space and the positions are manifolds, because of the constraints imposed on the coordinate system, and how they can move locally, how many dimensions they can move in locally, as well as some global constraints like the gimbal lock we saw. Now we also want to introduce smoothness, and the reason for that is that we want to be able to take velocities, accelerations and so on, in other words infinitely many derivatives. In the real world it’s probably going to be finitely many, because at some point your higher derivatives matter less and less, but that’s kind of the gist.

Manifolds, smooth structure, Lie groups

While the topological manifold definition seems out of the blue at first, the reason for having second countability is certain local to global constraints and partitions of unity, to be able to glue things appropriately across the manifold. For the actual treatment see Lee in the references, any differential geometry book covers it.

Technically charts are just one way of defining smoothness, and there are others. For example, we can do away with what happens at a particular point and work with germs of functions and their direct limits, so presheaves and zooming into the point infinitely. The other way I know about is differential spaces, leading into stratifolds. And then there are diffeological spaces. Same thing, none of it covered here, see the references.

Now look at how we start with a manifold and progressively add more and more abstraction, based on some useful things. When we talk about rotating rigid bodies and translating them, we start thinking about composition and invertibility, and obviously those things remind us of a group structure, one that is jointly continuous and has to respect the operations on the manifold and the smooth structure. What that means in practice is that if I do a rotation and then another rotation, the composition is also an element of that space, and it’s also a rotation.

Notice how we naturally started with a manifold, then added smoothness, then added a group structure. Each step keeps the objects and asks more of the maps between them: continuous, then $C^\infty$, then respecting multiplication too. I want to draw the category diagram for that at some point, it isn’t here yet.

We don’t need any of this for one elbow, but since we’ve built the ladder we may as well look at the thing sitting on top of it.

Back to programming the robot

So I want a very simple trajectory that oscillates up and down, and because I hear the word oscillates, that typically wants some kind of periodic function. In this case I just choose a sine:

Why do I choose a sine? Because I position the robot at zero to begin with, say over the first three seconds I reset all the positions to zero, and I don’t want to have a discontinuity at zero. Since $\sin 0 = 0$, the trajectory starts exactly where the robot already is. So I just make it a regular good old sine.

Funny enough this goes back to parametric curves, because at the end of the day we are describing a curve by simply varying $t$. And not just a curve in 3D space, but a curve on that configuration space manifold:

We put big brackets around the sine to remind that it is in fact an equivalence class. $A\sin(\omega t)$ is a number, and $\mathcal{C}$ is a circle, so to land in $\mathcal{C}$ we read that number as an angle, identifying $S^1 = \mathbb{R} / 2\pi\mathbb{Z}$. For our elbow it hardly matters, since the joint limits keep the whole trajectory inside one chart and we can just work with the angle, but the sine by itself isn’t a point on the manifold until you say how to read it.

So a regular sine over $t$ does actually describe some kind of curve over its configuration space.

Position only

You can see that the basic version going up and down does not actually specify any velocity. Leaving $\dot q^{\text{des}} = 0$ doesn’t mean the velocity term does nothing though, it means the term becomes $-K_d\,\dot q$, so it’s pure damping that fights whatever motion you have. You’re telling the joint to be at a moving target and to be stationary, at the same time, and what you get is the compromise between the two.

The lag that comes out of it is worst where the joint is moving fastest, which for a sine is the middle of the stroke, and it’s smallest at the peaks, where $\dot q$ is heading to zero anyway and the two requests stop disagreeing. So the motion doesn’t fall apart, it just runs behind and comes out shallower than you asked for. In the velocity interactive further down that’s about a quarter radian of error mid stroke against a few hundredths at the turnaround, and you can toggle it on and off there to watch the gap open and close.

While we describe the desired trajectory as a sine, the controller never receives a continuous function. At 500Hz we just sample the sine and send a new position target every 2ms. The robot receives a discrete sequence of setpoints, and the resulting samples closely approximate the continuous trajectory we had in mind.

Here’s the whole thing to play with. Drag Δt up until the corners are obvious, then hit the button to go back to what the robot actually runs at.

And here’s the write side of the Python, which is where every idea above finally turns into something the motors receive.

# send the message to the robot
def LowCmdWrite(self):
    # tick the time
    self.time_ += self.control_dt_

    if self.time_ < self.duration_:
        # stage 1
        # set robot to zero posture
        # for unitree R1 it's actually holding the elbows at 90 deg
        # like so
        #
        #   shoulder ─── o
        #                │
        #                │        upper arm, straight down
        #                │
        #      elbow ─── o ──────  forearm, out at 90 deg
        #
        # forgive my terrible ascii art skills
        for i in range(R1_NUM_MOTOR):
            ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0)
            self.low_cmd.mode_pr = Mode.PR
            self.low_cmd.mode_machine = self.mode_machine_
            self.low_cmd.motor_cmd[
                joint_idx_in_idl[i]
            ].mode = 1  # 1:Enable, 0:Disable
            self.low_cmd.motor_cmd[joint_idx_in_idl[i]].tau = 0.0
            self.low_cmd.motor_cmd[joint_idx_in_idl[i]].q = (
                1.0 - ratio
            ) * self.low_state.motor_state[joint_idx_in_idl[i]].q
            self.low_cmd.motor_cmd[joint_idx_in_idl[i]].dq = 0.0
            self.low_cmd.motor_cmd[joint_idx_in_idl[i]].kp = Kp[i]
            self.low_cmd.motor_cmd[joint_idx_in_idl[i]].kd = Kd[i]

    else:
        # stage 2
        # move the left elbow up and down
        i = R1JointIndex.LeftElbow
        idl_i = joint_idx_in_idl[i]
        max_angle = 20 * np.pi / 180
        t = self.time_ - self.duration_

        self.low_cmd.mode_pr = Mode.PR
        self.low_cmd.mode_machine = self.mode_machine_

        self.low_cmd.motor_cmd[idl_i].mode = 1
        self.low_cmd.motor_cmd[idl_i].tau = 0.0
        self.low_cmd.motor_cmd[idl_i].q = max_angle * np.sin(t)
        self.low_cmd.motor_cmd[idl_i].dq = 0.0
        self.low_cmd.motor_cmd[idl_i].kp = Kp[i]
        self.low_cmd.motor_cmd[idl_i].kd = Kd[i]

    self.low_cmd.crc = self.crc.Crc(self.low_cmd)
    self.lowcmd_publisher_.Write(self.low_cmd)

A few things to notice, because they’re all the abstract stuff from earlier showing up as actual lines.

self.time_ += self.control_dt_ is the only clock. There’s no wall time anywhere, we just count ticks, which is why $t_k = k\,\Delta t$ exactly.

Stage 1 is the reset to 0.0 position. On each tick we take the latest measured joint position from LowState and scale it to 0 over duration_, so by the time stage 2 starts we have $q = 0$ and $\sin 0 = 0$ agreeing with each other.

Stage 2 is the whole post in six lines. One joint, R1JointIndex.LeftElbow, gets $q(t) = A\sin(t)$ with $A = 20°$, which is $0.349$ rad, comfortably inside that $[-0.9757,\ 2.1850]$ limit. There’s no $\omega$ in the expression, so $\omega = 1$ rad/s and the period is $2\pi \approx 6.3$ seconds, which is why the arm in the video is moving as unhurriedly as it is.

And then the honest part: dq = 0.0 and tau = 0.0, on every motor, in both stages. So this is exactly the position-only case, and the $-K_d\,\dot q$ pure damping described above isn’t a hypothetical, it’s what this code does. Kp[i] and Kd[i] are the per-joint tables from earlier, which is why a shoulder carrying the whole arm gets 100 and the wrist hanging off the end of it gets 50.

Here it is running on the arm.

Adding velocity

So now we also want to add velocity, basically so that at the very top we smoothly bring the velocity to zero. Obviously the easiest way to do that is to just do a cosine, the derivative of the sine:

At those peaks, where $\sin(\omega t) = \pm 1$, we get $\cos(\omega t) = 0$, which is exactly where we want the derivative to be zero. And now the damping term has something to subtract against, so instead of $-K_d\,\dot q$ fighting the motion all the way through, it only pushes back on the difference between the speed we asked for and the speed we got.

In the code that is a one line change, since we were already leaving the velocity at zero:

self.low_cmd.motor_cmd[idl_i].q  = max_angle * np.sin(t)
self.low_cmd.motor_cmd[idl_i].dq = max_angle * np.cos(t)   # was 0.0

Sine becomes cosine and that’s it. Worth knowing that this only works out so cleanly because $\omega = 1$ here: in general the derivative of $A\sin(\omega t)$ is $A\omega\cos(\omega t)$, so you’d carry the $\omega$ through, and forgetting it is a good way to ask for a velocity that disagrees with your own position command.

So that’s basically it. All I have to do is program this and not make any algebraic mistakes. What I like about specifying it this way is that I can plot these functions beforehand and make sure there are no discontinuities or anything. Obviously once you run it on a robot, if you mess it up it’ll jerk and stuff.

Visual poser

After doing that, my next quick thing was a visual poser. The Unitree app has something like it buried in there, but it’s nice to have my own thing I can launch from a terminal instead of installing the app and dragging it around, and I can extend it however I want.

The UI itself was pretty quick and dirty, just DearPyGui with a slider per joint, and then I had Claude Code redo the theme into something presentable.

The way I implemented it is pretty rudimentary. Whenever a person changes a slider value we obviously debounce it and request a trajectory change. Once we specify that trajectory change, you kind of lock in that joint to a trajectory and you don’t mess with it, which is the self.shared_state.trajectories dict in the code below: a slider writes an entry, the control thread samples it every tick, and it drops out when it reports done. After that the joint just sits there with whatever torque the $K_p$ term needs to hold the position.

The write side grows a little to support it, since now any joint might have a trajectory attached and most of them don’t:

# the write side: fires every control_dt_ seconds, forever
def LowCmdWrite(self):
    # tick the time
    self.time_ += self.control_dt_

    # nothing to say until the first state message has arrived
    if not self.shared_state.seeded:
        return

    self.low_cmd.mode_pr = Mode.PR
    self.low_cmd.mode_machine = self.mode_machine_

    if self.time_ >= self.init_duration_:
        self.update_trajectories()

    if self.time_ < self.init_duration_:
        # the reset: scale every joint's measured position down to zero over
        # init_duration_ seconds, so we start from a known pose instead of
        # from wherever the arm happened to be left
        for idx in range(R1_NUM_MOTOR):
            idl_idx = joint_idx_in_idl[idx]
            t = np.clip(self.time_ / self.init_duration_, 0.0, 1.0)
            cmd = self.low_cmd.motor_cmd[idl_idx]
            cmd.mode = 1
            cmd.kp = Kp[idx]
            cmd.kd = Kd[idx]
            cmd.tau = 0.0
            cmd.dq = 0.0
            cmd.q = (1.0 - t) * self.low_state.motor_state[idl_idx].q
    else:
        # the actual run: every joint that has a trajectory gets sampled at
        # this instant, and joints without one are simply not commanded
        for idx in range(R1_NUM_MOTOR):
            if idx not in self.shared_state.trajectories:
                continue
            idl_idx = joint_idx_in_idl[idx]
            traj_fn = self.shared_state.trajectories[idx]
            cmd = self.low_cmd.motor_cmd[idl_idx]
            cmd.mode = 1
            cmd.kp = Kp[idx]
            cmd.kd = Kd[idx]
            cmd.tau = 0.0
            cmd.dq = 0.0
            cmd.q, done = traj_fn()
            # a finished trajectory drops out and the joint just holds
            if done:
                del self.shared_state.trajectories[idx]

    self.record_diagnostics()

    self.low_cmd.crc = self.crc.Crc(self.low_cmd)
    self.lowcmd_publisher_.Write(self.low_cmd)

Which is kind of nice, so you get to play around with different hand positions and see where the limitations are. Obviously there are specs as well, so try not to move it around too much. But it was kind of cool.

Next steps

Definitely buying a mocap. I’m just kidding. Am I?

I think the next step is going to be doing some interesting reinforcement learning stuff, so you can actually train it and make it do things.

References

Leaving some references here just in case you want to learn more.

• John M. Lee, Introduction to Smooth Manifolds, and Introduction to Riemannian Manifolds.
• Bredon, Topology and Geometry
• Lynch and Park, Modern Robotics
• Cyclone DDS and the DDS spec.
• Unitree R1 developer documentation.