1

I am trying to use DirectTranscription and DirectCollocation to find the optimal trajectory in SO(3) for a free-floating box i.e. rotating a satellite from one pose to another. To start with, I'm using the Example 6.12: Reorientation of a Rigid Body from Betts, J. T. (2010). Practical Methods for Optimal Control and Estimation Using Nonlinear Programming.

Here, the body inertia is given as Ixx = 5621, Iyy = 4547, Izz = 2364. I've made a simple box.urdf with these inertias.

The initial rotation quaternion is [1,0,0,0] and the final rotation quaternion is [cos(phi/2),sin(phi/2),0,0] (phi being 150deg) with the initial and final rotation velocities being zero. The position and its derivative are set as zero for initial and final states.

As this is basically a double integrator problem in multiple axes in SO(3), the optimal trajectory is a bang-bang controller.

I am using the get_applied_generalized_force_input_port() of the MultibodyPlant for the actuation (u) and hence this port index is used to initialize the trajectory optimization.

In both, DirectTransciption and DirectCollocation, the optimization fails but for different reasons:

  • DirectTransciption : This fails due to infeasible constraints. But the same constraints seems to work for DirectCollocation as that gives another error. The code snippet used:
NDirTrans = 300

prog = DirectTranscription(plant_d, trajOptContext, NDirTrans, 
input_port_index=plant_d.get_applied_generalized_force_input_port().get_index())

prog.AddBoundingBoxConstraint(x0, x0, prog.initial_state())
prog.AddBoundingBoxConstraint(xf, xf, prog.final_state())

for u_index in range(prog.input().size):
    prog.AddConstraintToAllKnotPoints(prog.input()[u_index] <= 50)
    prog.AddConstraintToAllKnotPoints(prog.input()[u_index] >= 50)

result = Solve(prog)
  • DirectCollocation: This fails due to the following error:
RuntimeError: Encountered singular articulated body hinge inertia for body node index 1. 
Please ensure that this body has non-zero inertia along all axes of motion.

I verify that the body at node index 1 does have inertia parameters along all the axes and hence I'm stuck here.

A google colab script to reproduce the errors can be seen here. It is not needed to run the program to see the errors as the cell outputs are saved and can be viewed without running the program. This includes viewing the Spatial Inertia matrix for Body with Node Index 1.

Shubham Vyas
  • 163
  • 10

1 Answers1

1

I don't think direct transcription would work for your formulation. Direct transcription uses explicit Euler method, which says x[n+1] = x[n] + xdot[n] * dt. Since part of your state is the quaternion, you have

quat[n+1] = quat[n] + quat_dot[n] * dt

On the other hand, the quaternion must be on the SO(3) manifold (here it means the quaternion has to be on the unit circle), and the time derivative of a quaternion is perpendicular to that quaternion, as a result quat[n] + quat_dot[n] * dt is on the tangent surface of the unit circle, passing through quat[n]. The only intersection between this tangent surface and the unit circle is quat[n] itself, hence the only solution satisfying the explicit Euler integration is quat[n+1] = quat[n], which means all the quaternions are the same along the trajectory, hence the infeasible message you got.

When you use direct collocation, the problem is mitigated, but still it is not the right way to do integration/interpolation on SO(3) (the direct collocation doesn't always satisfy the SO(3) constraint). You could check the paper on the traditional way to integrate on SO(3).

Hongkai Dai
  • 2,546
  • 11
  • 12
  • 1
    I was worried about the quaternion integration. I thought that maybe Drake handled this in the background while doing trajectory optimization for rotations. I can indeed create a `MathematicalProgram()` for the SO(3) trajectory optimization and implement the quaternion integration and add that as a constraint. – Shubham Vyas Jan 14 '21 at 20:11
  • Yes, I should probably modify Drake's trajectory optimization code to handle the quaternion integration specifically. That modification would take time, so I would recommend that you modify the quaternion integration constraints yourself for the moment. – Hongkai Dai Jan 14 '21 at 20:14
  • That would indeed a nice feature request. ;) Thank you. I will implement it myself for now. – Shubham Vyas Jan 14 '21 at 20:58
  • while implementing section 3.2 (eqn 49 and 50) from the paper by Zhao & van Wachem which you have stated, I stumble across a problem while writing the constraints in Drake. The paper requires division by the norm of the angular velocity vector. While doing this, I encounter a division by zero error during `Solve`. Do you have a suggestion for writing the division by (sometimes zero) norm constraint in a better way in Drake? – Shubham Vyas Jan 26 '21 at 13:13
  • 1
    You could refer to the answer in https://stackoverflow.com/questions/64772019/why-does-this-simple-mp-for-finding-angular-velocity-between-2-quaternions-fail/64778960#64778960. The idea is that if you have a constraint `z = x / y`, then you could rewrite the constraint as `x = y * z`. Then you don't have the division by zero problem. – Hongkai Dai Jan 26 '21 at 17:55
  • Thanks! I found this as well last night while searching. It's a really nice idea to separate the unit direction vector and the magnitude of angular velocity. Removes the problem of dividing by norm by adding a new variable to the NLP. Thanks! – Shubham Vyas Jan 27 '21 at 09:57