Optimizing Rotations

Optimization over the space of rotations is non-trivial due to the group structure of 3D rotations. TrajectoryOptimization.jl provides methods for accounting for this group structure, both in the constraints and in the objective. We make the assumption that 3D rotation only show up in the state vector, and never in the control vector. TrajectoryOptimization.jl relies on the dynamics model to determine if the state vector contains rotations. See the RobotDynamics.jl documentation for more details on defining models with rotations. From here, we assume that we are dealing with a model that inherits from RobotDynamics.LieGroupModel.

Cost Functions (experimental)

While normal quadratic cost functions can work with rotations (e.g. $q_k^T Q q_k$, where $q_k$ is a quaternion, MRP, or RP), this distance metric isn't well-defined. Since we often want to penalize the distance from a reference rotation (such as a nominal or goal state), TrajectoryOptimization.jl provides a couple different methods for penalizing distance from a reference rotation. However, we've discovered that the quaternion geodesic distance:

\[\min 1 \pm q_d^T q_k\]

where $q_d$ is the desired, or reference, quaternion, works the best. We've also found that, while technically incorrect, a naive quadratic penalty can work quite well, especially when the difference between the rotations isn't significant.

The following cost functions are provided. Note that these methods should still be considered experimental, and the interface made change in the future. If you encounter any issues using these functions, please submit an issue.

TrajectoryOptimization.DiagonalQuatCost — Type

DiagonalQuatCost

Quadratic cost function for states that includes a 3D rotation, that penalizes deviations from a provided 3D rotation, represented as a Unit Quaternion.

The cost function penalizes geodesic distance between unit quaternions:

$\frac{1}{2} \big( x^T Q x + u^T R u \big) + q^T x + r^T u + c + w \min 1 \pm p_f^T p$

where $p$ is the quaternion extracted from $x$ (i.e. p = x[q_ind]), and $p_f$ is the reference quaternion. $Q$ and $R$ are assumed to be diagonal.

We've found this perform better than penalizing a quadratic on the quaternion error state (ErrorQuadratic). This cost should still be considered experimental.

Constructors

DiagonalQuatCost(Q::Diagonal, R::Diagonal, q, r, c, w, q_ref, q_ind; terminal)
DiagonalQuatCost(Q::Diagonal, R::Diagonal; q, r, c, w, q_ref, q_ind, terminal)

where q_ref is the reference quaternion (provided as a SVector{4}), and q_ind::SVector{4,Int} provides the indices of the quaternion in the state vector (default = SA[4,5,6,7]). Note that Q and q are the size of the full state, so Q.diag[q_ind] and q[qind] should typically be zero.

source

TrajectoryOptimization.QuatLQRCost — Function

QuatLQRCost(Q, R, xf, [uf; w, quat_ind])

Defines a cost function of the form:

$\frac{1}{2} \big( (x - x_f)^T Q (x - x_f) + (u - u_f)^T R (u - u_f) \big) + w \min 1 \pm q_f^T q$

where $Q$ and $R$ are diagonal, $x_f$ is the goal state, $u_f$ is the reference control, and $q_f$, $q$ are the quaternions, extracted from $x$ using quat_ind, i.e. q = x[quat_ind].

The last term is the geodesic distance between quaternions. It's typically recommended that Q.diag[quad_ind] == zeros(4).

This is just a convenience constructor for DiagonalQuatCost.

Example

For a standard rigid body state vector x = [p; q; v; ω], where q is a unit quaternion, we could define a cost function that penalizes the distance to the goal state xf. We can create this cost function as follows:

Q = Diagonal(SVector(RBState(fill(0.1,3), zeros(4), fill(0.1,3), fill(0.1,3))))
R = Diagonal(@SVector fill(0.01, 6))
xf = RBState([1,2,3], rand(UnitQuaternion), zeros(3), zeros(3))
QuatLQRCost(Q,R,xf)

We can add a reference control and change the weight on the rotation error with the optional arguments:

QuatLQRCost(Q,R,xf,uf, w=10.0)

which is equivalent to

QuatLQRCost(Q,R,xf,uf, w=10.0, quat_inds=4:7)

source

TrajectoryOptimization.ErrorQuadratic — Type

ErrorQuadratic{Rot,N,M}

Cost function of the form:

$\frac{1}{2} (x_k \ominus x_d)^T Q_k (x_k \ominus x_d)$

where $x_k \ominus x_d$ is the error state, computed using RobotDynamics.state_diff. This cost function isn't recommended: we've found that DiagonalQuatCost usually peforms better and is much more computationally efficient.

source

Cost and Constraint Expansions

The key to performing optimization on Lie Groups such as rotations is to perform the optimization on the tangent bundle, or simply the hyperplane tangent to the group at the current iterate. Since the tangent space is Euclidean, we use standard optimization methods such as Newton's method to find a step direction. After we find a candidate step direction, we project back onto the group using the exponential map (or any other "retraction map"). For rotations and unit quaternions, this means we can either use the exponential map, or use the conversions between unit quaternions and MRPs or RPs (Rodrigues Parameters). Since most optimization methods require gradient or Hessian information, we need to correctly account for the mapping to the the tangent plane when computing derivatives.

TrajectoryOptimization.jl handles this by first computing the derivatives as normal, treating the state (naively) as a vector in Euclidean space. This means methods such as ForwardDiff can be used without problem. These derivatives are then "converted" to work on the error state. This conversion ends up being one or two extra matrix multiplications with the so-called "error-state Jacobian," which is a function of the rotation at the current iterate.

Therefore, computing the full derivative information is split into 2 steps:

Compute the derivatives as normal.
Compute the "error expansion," using the result of step 1.

For objectives this looks like:

TrajectoryOptimization.state_diff_jacobian!(G, model, Z)
TrajectoryOptimization.cost_expansion!(E0, obj, Z, [init, rezero])
TrajectoryOptimization.error_expansion!(E, E0, model, Z, G)

The corrected expansion is stored in E::QuadraticObjective. The first line computes the error-state Jacobians, storing them in G. The intermediate expansion is stored in E0::QuadraticObjective. For models that do not have rotations, E === E0 and the last line is a no-op.

For dynamics this looks very similar:

TrajectoryOptimization.state_diff_jacobian!(G, model, Z)
TrajectoryOptimization.dynamics_expansion!(::Type{Q}, D::DynamicsExpansion, model, Z)
TrajectoryOptimization.error_expansion!(D::DynamicsExpansion, model, G)

The first line can be omitted if it has already be computed for the current trajectory Z::AbstractTrajectory.

This functionality is still under development for constraints. Since augmented Lagrangian methods incorporate the constraints into the objective, the error expansion is computed on the entire cost, rather than computing an intermediate error expansion for the constraints, saving computation. If you need this functionality, please submit an issue.

The experimental interface is currently:

TrajectoryOptimization.jacobian!(conSet::AbstractConstraintSet, Z::AbstractTrajectory, [init])
TrajectoryOptimization.error_expansion!(conSet::AbstractConstraintSet, model, G)