Simulating a 2D Hovering SpaceX Grasshopper with a Thrust Vector Control engine.

Back when SpaceX was testing their grasshopper (a test vehicle for relanding the first stage), I thought it'd be fun to do a simple 2D simulation to get an understanding of how Thrust Vector Controlled (TVC) systems work. I chose python and pygame for ease of programming and visualization .

Real-time simulation of TVC controlled engine running with pygame

This is a very nuts and bolts simplistic view of the conversion of GNC (Guidance, Navigation and Controls) abstract concepts of turning the goal of putting a rocket into a certain state into a sequence of controls that cause the rocket to go there.

At the heart of it, the system is controlled by three PD controllers (Proportional and Derivative, a subset of PID controllers). The first controls throttle based on altitude. The second controls TVC angle based on orientation. The third modifies TVC angle based on lateral drift.

By slightly altering the vector of thrust we can control the vehicle's rotation in addition to it's thrust along the axis.

The rocket is modelled as a simple 5.5x33m box massing 10 tons, with a maximum thrust of 500 kN. A blue line shows the force in the normal direction. Thrust is shown as a red line, which is in the direction dictated by the TVC controllers. In our Python code, we can build a simple PID controller class that contains three values for Proportional, Integral, and Derivative gain.

	class PID:
	def __init__(self, p, i, d):
	self.p = p
	self.i = i
	self.d = d
	def __str__(self):
	return "PID(%g,%g,%g)" % (self.p, self.i, self.d)

Then, we can set up our three PID controllers we described earlier.

	# PID values [P, I, D] for TVC
	pid_values = {'altitude': PID(0.3, 0.02, 0.67),
	'lateral drift': PID(1.5, 0, 3.55),
	'attitude': PID(0.4, 0, 0.2)}

Pygame provides a nice framework for changing configurations in real-time, sliders etc., I used this to tweak the gains till we found a comfortable response.

In the main loop, we run our PIDs and use it to update our thrust magnitude and vector based on our current state.

	# PD controller for altitude
	pos_error = goalPos.y - pos.y
	vel_error = 0 - self.ship.linearVelocity.y
	force_mag = pid_values['altitude'].p*pos_error + \
	pid_values['altitude'].d*vel_error + \
	self.ship.mass*10 / self.max_tvc_thrust_mult # gravity bias
	########################################
	# PD for lateral position to goal orientation
	lat_error = goalPos.x - pos.x
	lat_vel_error = 0 - self.ship.linearVelocity.x
	tvc_offset = (lat_error*pid_values['lateral drift'].p + \
	lat_vel_error*pid_values['lateral drift'].d) * -pi/180.0
	########################################
	# PD controller for TVC
	ang_error = goal_orientation - (self.ship.angle)
	ang_vel_error = 0 - self.ship.angularVelocity
	d_angle = pid_values['attitude'].p * ang_error + \
	pid_values['attitude'].d * ang_vel_error

Finally, to set goals we can click points within the area which the ship will attempt to hover over to. For fun, it also automatically resets to a random goal position when it reaches, so that it keeps moving around continuously.

Source Code on GitHub