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 — TypeDiagonalQuatCostQuadratic 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.
TrajectoryOptimization.QuatLQRCost — FunctionQuatLQRCost(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)TrajectoryOptimization.ErrorQuadratic — TypeErrorQuadratic{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.