Quadruped Robot with Custom Leg Controllers

Project Logs

Collapse

Pre-Calculations, Optimizations, and Integer Math
Jake Wachlin • 10/31/2020 at 22:01 • 0 comments
In the last log I showed a few computational issues:
- The motion primitives calculations are too slow (around 1ms after some initial optimization)
- The control function is too slow (around 250us)
- The control function is not called at a consistent rate (uses a software timer)
This log explains some updates I did to make things faster and more consistent. Some are still works in progress, but it is looking promising.
Pre-calculations
All of the motion primitives are set up as cyclical motions defined in Cartesian space. Both linear and quadratic Bezier curves are supported. I realized with typical control periods, and primitive calculation rates it would not be difficult to pre-calculate the positions, and then simply use a lookup-table when it is running to get the position. I implemented this right away. In addition, for simple position control through these profiles, inverse kinematics is required to calculate the motor positions. I also added a layer to pre-calculate the motor positions (in ticks, as integers). Now that everything was set up as simple integer lookup tables, motion primitives with position control are very fast. My timing showed that this calculation required only about 34us, around 30X faster than the original Bezier VMC setup.
Integer PID Controller
To make this control method even faster, I also had to set up the PID controller as integer only. Care must be taken when doing this, as it is easy to get odd rounding, overflow, and underflow issues with integer math. Thankfully, for position control my inputs and outputs were already scaled to be in the thousands typically, and the controller gains were reasonably high. Furthermore, the inputs were actually integers cast to floats, and the output was a float cast back to an integer. I therefore thought that I should be able to achieve decent control with a simple implementation. The code below shows the implementation. There are only two parts that are non-standard. First, the speed is filtered according to this great Hackaday project. In addition, my typical Ki for position control was 0.004. Therefore, I added an integral shift option so that effectively small Ki values are possible.
```
 int32_t calculate_integer_pid(pid_integer_control_t * pid, int32_t setpoint, int32_t current_position)
 {
	static int32_t speed_old_z = 0;

	int32_t error = setpoint - current_position;

	int32_t speed_z = speed_old_z - speed_old_z >> pid->filter_shift + (current_position - pid->last_position);
	pid->speed = speed_z >> pid->filter_shift;

	speed_old_z = speed_z;

	pid->integral += error;

	if(pid->integral > pid->integral_max)
	{
		pid->integral = pid->integral_max;
	}
	else if(pid->integral < pid->integral_min)
	{
		pid->integral = pid->integral_min;
	}

	int32_t cmd = pid->kp * error + pid->kd * pid->speed + (pid->ki * pid->integral >> pid->integral_shift);

	if(cmd > pid->cmd_max)
	{
		cmd	= pid->cmd_max;
	}
	else if(cmd < pid->cmd_min)
	{
		cmd = pid->cmd_min;
	}

	pid->last_error = error;
	pid->last_position = current_position;
	return cmd;
 }
```
With this implementation used for position control, the position control loop is now about 79us, about 3X faster than before. More importantly, it seems to work just as well.
Hardware Timing
The control loop had been set up as a 500Hz FreeRTOS software timer. Although it worked OK for position or speed control, it likely isn't OK for current control. It is not fast enough, and the jitter is quite significant. I therefore set up a hardware timer. As a first test, I set it up to toggle a pin (with some busy delay) on every interrupt. This was quite consistent and accurate, as seen in the next two images.
While I could simply run the controller within the ISR, I'd prefer to use that timer to consistently trigger a very high priority task to run the control. I implemented this using a task notification. While it did have a consistent period, I did see the occasional issue as shown at the center of the next image. Sometimes the computation time looked...
Read more »
VMC Tuning and Computational Limitations
Jake Wachlin • 10/31/2020 at 02:23 • 0 comments
VMC Tuning
The last log walked through some issues seen with implementing virtual model control on the legs. I spent some more time debugging the issues. The first thing I did was to continue tuning the system. There are a lot of parameters in this system. These are the PID current controller parameters, the VMC effective stiffness and damping, and gain parameters which describe the motor and the mapping from torque to current. I spent time tuning all of these, and was able to get somewhat better results. One major improvement was shifting the current controller from mostly proportional to mostly integral. This is due in part to the single-sided current sensing we have. The current control therefore is somewhat odd and discontinuous.
```
} else if(control_type[i] == CURRENT || control_type[i] == PROPRIOCEPTIVE_PRIMITIVE)
		{
			if(motors[i].current_ma_setpoint >= 0)
			{
				float cmd = calculate_pid(&cur_params[i], motors[i].current_ma_setpoint, motors[i].current_mA);
				// Only allow forward drive
				if(cmd > 0)
				{
					tcc_set_compare_value(&modules[i], (motors[i].reverse_direction) ? 1 : 0, 0);
					tcc_set_compare_value(&modules[i], (motors[i].reverse_direction) ? 0 : 1, (int32_t) cmd);
				}
				else
				{
					tcc_set_compare_value(&modules[i], (motors[i].reverse_direction) ? 0 : 1, 0);
					tcc_set_compare_value(&modules[i], (motors[i].reverse_direction) ? 1 : 0, 0);
				}
			}
			else
			{
				// Flip direction of control and direction of setpoint
				float cmd = calculate_pid(&cur_params[i], -motors[i].current_ma_setpoint, motors[i].current_mA);
				// Only allow reverse drive
				if(cmd < 0)
				{
					tcc_set_compare_value(&modules[i], (motors[i].reverse_direction) ? 1 : 0, 0);
					tcc_set_compare_value(&modules[i], (motors[i].reverse_direction) ? 0 : 1, 0);
				}
				else
				{
					tcc_set_compare_value(&modules[i], (motors[i].reverse_direction) ? 0 : 1, 0);
					tcc_set_compare_value(&modules[i], (motors[i].reverse_direction) ? 1 : 0, (int32_t) cmd);
				}
			}
		}
```
If a proportional controller commands driving in the opposite direction of what I know the current should be, I don't command negative. Instead I drop to 0 command. In the next control loop, we then expect the current to shift towards zero, and the controller kicks back on. This inherently means even in a perfect world a proportional controller would bounce around the desired current. However, an integral controller has some inertia to it and would be expected to bounce around less. In testing, this does seem to work. The image below shows the current controlling on one example. The command is oscillatory due to the VMC tuning, but the current tracking looks pretty good.
The image below shows the (unloaded) proprioceptive control that I achieved. However, when I tried to have all four legs use this controller to walk, it doesn't work well. The proprioceptive control is just not ready. I therefore dug deeper into the potential issues.
Computational Issues
The motor controllers use a ATSAMD21 Cortex M0+ running at 48MHz. This is a decent microcontroller, but critically it does not have an FPU. Some benchmarks with the same CPU show that floating point multiplication requires ~16.5X the time of integer multiplication, and floating point division is ~14X slower than integer division. Note that the exact timing does depend on the exact data and optimizations, but nonetheless it is clear that floating point math is much slower than integer math. I knew this of course, but many of the calculations including PID, inverse and forward kinematics, and VMC are far easier to do in floating point than fixed point. It is much harder to make a mistake during development, and I hadn't run into any issues.
In addition, I am running FreeRTOS on the SAMD21. The control loop, motion primitive (and VMC) calculations, and telemetry all are software timers. The control loop is 500Hz, and the motion primitive and telemetry are each 100Hz. Again, running control loops in...
Read more »
Bezier Curves and Virtual Model Control Revisited
Jake Wachlin • 10/24/2020 at 23:29 • 0 comments
In the last log for the motor controllers used on this robot, I discussed how the motion primitives were updated to roughly match the gait of the MIT Cheetah according to the following video:

I also noted that my implementation was pretty far from how the MIT Cheetah does its leg motion control. It was different in at least two critical ways. First, while I had 3-4 points in my profiles, and the leg would track between these points according to linear interpolation, the Cheetah uses Bezier curves for smoother motion (or it did at one time per this paper). Second, while my robot tracked those profiles simply using inverse kinematics and position control, the Cheetah is more advanced. Series-elastic actuators have a long history in robotics in helping to stabilize walking motion. The Cheetah uses virtual model control to simulate elasticity in its motion. In the simplest representation of this, consider the image below. Instead of driving the leg to a given position, we instead create a virtual spring (and damper) between a desired point and the foot. The forces that would occur due to this virtual model are calculated, and we drive the hip and knee motors in current, or torque, control mode to achieve this force. Therefore we can think of the foot as being dragged around in space by a virtual spring and damper. Instead of punching the ground with position control, a virtual model controlled leg impacts the ground as if there was an inline spring, and the motion is therefore much smoother. With its high-performance, low gear-ratio brushless motors, the Cheetah is great at doing this. I am using 150:1 gear ratio <$10 brushed motors with single-sided current sensing. Therefore I do not expect nearly as good performance, but I still want to do as best as possible.

Bezier Motion Profiles

Bezier curves are parametric curves defined by a few control points. Various order Bezier curves exist, and in fact linear Bezier curves are simply linear interpolation. Therefore technically I was already using Bezier curves. But clearly motion like that shown below is not smooth.

And I do mean smooth in the mathematical sense. The derivative of the curve is discontinuous, which can make for jumpy motion. To keep things a bit simpler, I focused on using quadratic Bezier curves. These also do not ensure mathematically smooth profiles, but by tuning the shape, I can get much smoother motion. In the implementation, we must always have an even number of control points. All even indices (starting at 0) are points that the motion moves through. All of the odd index points "pull" the profile to shape it. Wikipedia has a good explanation on how to generate a quadratic Bezier curve. With this implemented, I regenerated the profile above, but with quadratic Bezier curves and some shaping points. For sanity, I also rotated things so that the leg "down" is down also.

Clearly, this motion will be much smoother and hopefully will result in less jerky walking.

I tested this with position control (virtual model control will be discussed later), and it was successful. This is real test data, with a 2s period. The actual foot positions lag somewhat, but the tracking is overall quite decent.

If we speed up the cycle, it is clear that the motors cannot keep up. At this point it isn't clear if this is a PID tuning issue, or a hardware limitation. This example has a 750ms period, so more than twice as fast.

Virtual Model Control
In one of the logs for the motor controller, I had talked about current control and virtual model control, but did not get great results. I revisited this again, now that I have motion primitives working. I would like to use the Bezier curves along with virtual model control. There are a number of challenges here:
- Depending on the leg orientation, it may not be mathematically possible to generate the desired virtual force front joint torques (think about your leg held out straight with locked knee,...
Read more »