Implement PID/Position/Orientation controllers

[nycrat]

Description

Creates an abstraction around the following code from #3716:

const Point target_position    = trajectory_path_->getDestination();
const Angle target_orientation = angular_trajectory_->getDestination();
Vector error_linear            = target_position - position_;
Angle error_angular            = (target_orientation - orientation_).clamp();

Vector target_linear_velocity;
AngularVelocity target_angular_velocity;

// if close enough, use special PID to destination
if (error_linear.lengthSquared() < LINEAR_PURE_PID_THRESHOLD_METERS)
{
	target_linear_velocity =
		globalToLocalVelocity({x_pid_close.step(error_linear.x()),
				y_pid_close.step(error_linear.y())},
				orientation_);
}
else
{
	// FEEDFORWARD velocities from the trajectory
	const Vector trajectory_linear_velocity = getTargetLinearVelocity();
	error_linear                            = trajectory_path_->getPosition(
			time_since_linear_trajectory_creation_.toSeconds()) -
		position_;
	const Vector pid_effort_linear(x_pid.step(error_linear.x()),
			y_pid.step(error_linear.y()));
	target_linear_velocity = globalToLocalVelocity(
			trajectory_linear_velocity + pid_effort_linear, orientation_);
}

if (error_angular.abs().toDegrees() < ANGULAR_PURE_PID_THRESHOLD_DEGREES)
{
	target_angular_velocity = AngularVelocity::fromRadians(
			w_pid_close.step(error_angular.toRadians()));
}
else
{
	// FEEDFORWARD velocities from the trajectory
	error_angular =
		(angular_trajectory_->getPosition(time_since_angular_trajectory_creation_.toSeconds()) -
		 orientation_)
		.clamp();
	const AngularVelocity trajectory_angular_velocity =
		getTargetAngularVelocity();
	const AngularVelocity pid_effort_angular =
		AngularVelocity::fromRadians(w_pid.step(error_angular.toRadians()));
	target_angular_velocity =
		(trajectory_angular_velocity + pid_effort_angular);
}

// Make sure target linear velocity is clamped
target_linear_velocity = target_linear_velocity.normalize(
		std::min(target_linear_velocity.length(),
			static_cast<double>(robot_constants_.robot_max_speed_m_per_s)));

In the new software/embedded/motion_control package, creates three classes:

• PidController
• PositionController
• OrientationController

The step() methods in PositionController and OrientationController encapsulate the logic that was originally in PrimitiveExecutor. This makes PrimitiveExecutor take on less responsibility as now the calculations for getting the corrected target linear and angular velocities are in these new classes.

Testing Done

Unit tests were written for PidController. @williamckha told me not to write tests for PositionController and OrientationController since they might change in implementation and constants will need to be tuned.

Resolved Issues

resolves #3726

Length Justification and Key Files to Review

N/A

Review Checklist

It is the reviewers responsibility to also make sure every item here has been covered

• Function & Class comments: All function definitions (usually in the .h file) should have a javadoc style comment at the start of them. For examples, see the functions defined in thunderbots/software/geom. Similarly, all classes should have an associated Javadoc comment explaining the purpose of the class.
• Remove all commented out code
• Remove extra print statements: for example, those just used for testing
• Resolve all TODO's: All TODO (or similar) statements should either be completed or associated with a github issue

[StarrryNight]

lgtm left some comments

[Andrewyx]

Looks good, left some comments. Make sure to link TODO involving writing future test.

[Andrewyx]

why are these public?

[nycrat]

These variables were public in the original implementation. But now that you bring it up, I do think that they shouldn't be publicly accessible

[nycrat]

I guess if we want to change the pid constants at runtime, for example to tune them programmatically? What do you think ?

[StarrryNight]

If we do that, it will probably be through a public function in this class. So these variables should be kept private anyway.

[nycrat]

Actually that does make sense, especially with the max_integral variable we would want a setter function that checks for the invariant. I'll make them all private for now

[Andrewyx]

Not sure if we should continue inserting exceptions into our code (despite it being used in many locations) . See https://google.github.io/styleguide/cppguide.html#Exceptions and make the design decision call. Personally, I would lean towards alternatives since it is unlikely that this exception will be caught at a higher level.

[nycrat]

Hmm I actually didn't know that google's c++ style guide disallows exceptions. I personally think exceptions are good, especially in this context of enforcing correct arguments in a constructor, where we would want the program to explicitly crash. If we consider that the max_integral constant would be provided by the developer, this could be changed to an assert statement?

[StarrryNight]

I agree with the alternative choice of an assertion

[StarrryNight]

Nice work!

[Andrewyx]

This is a really clever design and a fantastic execution. Really nice work here 💯