"Point toward/away from point" constraint

[Lewis-Seiden]

useful for intaking and scoring where you want one end of the robot to face a game piece/goal

[shueja]

This would be extra nice when used with #297

[calcmogul]

This constraint could be implemented like so:

heading = atan2(point.y - y, point.x - x)
tan(heading) = (point.y - y)/(point.x - x)
sin(heading)/cos(heading) = (point.y - y)/(point.x - x)
sin(heading) * (point.x - x) = cos(heading) * (point.y - y)

The rearrangement is to avoid potential ill-conditioning.

[shueja]

Is there a formulation that could allow a particular error boundary around the desired heading? I expect that would make convergence easier and most uses probably don’t require exact pointing.

[calcmogul]

I'm not sure how to correctly manipulate nonlinear inequalities, so you'd probably be stuck with the following:

atan2(point.y - y, point.x - x) < heading + ε
atan2(point.y - y, point.x - x) > heading - ε