Update docs for rotations

bjack205 · bjack205 · commit 5d8024e07f8d · 2022-02-28T15:38:52.000-05:00
diff --git a/docs/make.jl b/docs/make.jl
@@ -13,10 +13,19 @@ makedocs(
             "continuous.md",
             "discrete.md",
         ],
+        "Rotations" => [
+            # "liestate.md"
+            "rotationstate.md",
+        ],
         "Important Types" => [
             "knotpoints.md",
+            "trajectories.md",
+        ],
+        "Internal API" => [
+            "functionbase.md",
+            "scalarfunction.md",
+            "autodiff.md",
         ],
-        "Function Base" => "functionbase.md",
 
         # "Getting Started" => [
         #     "models.md",
diff --git a/docs/src/autodiff.md b/docs/src/autodiff.md
@@ -0,0 +1,30 @@
+# Differentiation API
+```@meta
+CurrentModule = RobotDynamics
+```
+RobotDynamics allows users to define different methods for obtaining derivatives of their 
+functions. This is accomplished via the [`DiffMethod`](@ref) trait, which by default 
+has three options:
+* `ForwardAD`: forward-mode automatic differentiation using 
+   [ForwardDiff.jl](https://github.com/JuliaDiff/ForwardDiff.jl)
+* `FiniteDifference`: finite difference approximation using 
+   [FiniteDiff.jl](https://github.com/JuliaDiff/FiniteDiff.jl)
+* `UserDefined`: analytical function defined by the user.
+
+Almost every Jacobian is queried using the same form:
+
+    jacobian!(sig, diff, fun, J, y, z)
+
+where `sig` is a [`FunctionSignature`](@ref), `diff` is a [`DiffMethod`](@ref), `fun` is 
+an [`AbstractFunction`](@ref), `J` is the Jacobian, `y` is a vector the size of the 
+output of the function, and `z` is an [`AbstractKnotPoint`]. Users are free to add more 
+[`DiffMethod`](@ref) types to dispatch on their own differentiation method. 
+
+By default, no differentiation methods are provided for an [`AbstractFunction`](@ref), 
+allowing the user to choose what methods they want defined, and to allow customization 
+of the particular method to their function. However, we do provide the following macro 
+to provide efficient implementations for `ForwardAD` and `FiniteDifference`:
+
+```@docs
+@autodiff
+```
diff --git a/docs/src/functionbase.md b/docs/src/functionbase.md
@@ -1,4 +1,4 @@
-# `AbstractFunction` API
+# [`AbstractFunction` API](@id AbstractFunction)
 ```@meta
 CurrentModule = RobotDynamics
 ```
@@ -21,6 +21,10 @@ input_dim
 ```@docs
 FunctionSignature
 DiffMethod
+StateVectorType
+EuclideanState
+RotationState
+statevectortype
 FunctionInputs
 functioninputs
 ```
diff --git a/docs/src/index.md b/docs/src/index.md
@@ -218,7 +218,7 @@ them as a concatenated vector.
 
 ### Discretizing our dynamics
 We often need to discretize our continuous dynamics, either to simulate it or 
-feed it into optimization frameworks. The [`DiscretizedModel`](@ref) type 
+feed it into optimization frameworks. The [`DiscretizedDynamics`](@ref) type 
 discretizes a [`ContinuousDynamics`](@ref) model, turning it into a 
 [`DiscreteDynamics`](@ref) model by applying a [`QuadratureRule`](@ref).
 To discretize our system using, e.g. Runge-Kutta 4, we create a new 
diff --git a/docs/src/liestate.md b/docs/src/liestate.md
@@ -0,0 +1 @@
+# 
diff --git a/docs/src/rotationstate.md b/docs/src/rotationstate.md
@@ -0,0 +1,66 @@
+# The Rotation State
+```@meta
+CurrentModule = RobotDynamics
+```
+As mentioned in [AbstractFunction](@ref AbstractFunction), RobotDynamics can work
+with non-Euclidean state vectors, or state vectors whose composition rule is not 
+simple addition. The most common example of non-Euclidean state vectors in robotics 
+is that of 3D rotations. Frequently our state vectors include a 3D rotation together 
+with normal Euclidean states such as position, linear or angular velocities, etc. 
+The [`RotationState`](@ref) [`StateVectorType`](@ref) represents this type of state vector.
+In general, this represents a state vector of the following form:
+
+```math
+\begin{bmatrix}
+x_1 \\
+q_1 \\
+x_2 \\
+q_2 \\
+\vdots \\
+x_{N-1} \\
+q_N \\
+x_N
+\end{bmatrix}
+```
+where ``x_k \in \mathbb{R}^{n_k}`` and ``q_k \in SO(3)``. Any of the ``n_k`` can be zero.
+
+This state is described by the [`LieState`](@ref) struct:
+```@docs
+LieState
+QuatState
+```
+
+## The `LieGroupModel` type
+To simplify the definition of models whose state vector is a [`RotationState`](@ref), we 
+provide the abstract [`LieGroupModel`](@ref) type:
+
+```@docs
+LieGroupModel
+```
+
+## Single rigid bodies
+A lot of robotic systems, such as airplanes, quadrotors, underwater vehicles, satellites, 
+etc., can be described as a single rigid body subject to external forces and actuators.
+RobotDynamics provides the [`RigidBody`](@ref) model type for this type of system:
+
+```@docs
+RigidBody
+Base.position(::RBState)
+orientation
+linear_velocity
+angular_velocity
+build_state
+parse_state
+gen_inds
+flipquat
+```
+
+## The `RBState` type
+When working with rigid bodies, the rotation can be represented a variety of methods and 
+dealing with this ambiguity can be tedious. We provide the [`RBState`](@ref) type which 
+represents a generic state for a rigid body, representing the orietation as a quaternion.
+It provides easy methods to convert to and from the state vector for a given `RigidBody{R}`.
+
+```@docs
+RBState
+```
diff --git a/docs/src/scalarfunction.md b/docs/src/scalarfunction.md
@@ -0,0 +1,13 @@
+# Scalar Functions
+```@meta
+CurrentModule = RobotDynamics
+```
+Instead of working with vector-valued functions like dynamics functions, we often need 
+to define scalar functions that accept our state and control vectors, such as cost / 
+objective / reward functions. This page provides the API for working with these 
+types of functions, represented by the abstract [`ScalarFunction`](@ref) type, which 
+is a specialization of an [`AbstractFunction`](@ref) with an output dimension of 1.
+
+```@docs
+ScalarFunction
+```
diff --git a/docs/src/trajectories.md b/docs/src/trajectories.md
@@ -1,4 +1,9 @@
 # Trajectories
+
+```@meta
+CurrentModule = RobotDynamics
+```
+
 When dealing with dynamical sytems, we often need to keep track of and represent 
 trajectories of our system. RobotDynamics provides an [`AbstractTrajectory`](@ref)
 that can represent any trajectory, continuous or discrete. The only requirements on an 
@@ -11,7 +16,7 @@ AbstractTrajectory
 
 One convenient way of representing a trajectory is by sampling the states and controls 
 along it. RobotDynamics provides the `SampledTrajectory` type which is basically a vector 
-of [`AbstractKnotPoint](@ref) types. This is described by the following API:
+of [`AbstractKnotPoint`](@ref) types. This is described by the following API:
 
 ```@docs
 SampledTrajectory
diff --git a/src/discretized_dynamics.jl b/src/discretized_dynamics.jl
@@ -74,7 +74,7 @@ abstract type Implicit <: QuadratureRule end
     DiscretizedDynamics
 
 Represents a [`DiscreteDynamics`](@ref) model formed by integrating a continuous 
-dynamics model. It is essentially a [`ContinuousModel`](@ref) paired with a 
+dynamics model. It is essentially a [`ContinuousDynamics`](@ref) paired with a 
 [`QuadratureRule`](@ref) that defines how to use the [`dynamics!`](@ref) function 
 to get a [`discrete_dynamics!`](@ref) function. 
 
diff --git a/src/dynamics.jl b/src/dynamics.jl
@@ -52,7 +52,7 @@ longer computed using pure subtraction. We can define a custom function for taki
 differences between 2 states vectors based on the type of the state vector. 
 
 We can query the type of the state vector using [`statevectortype(model)`](@ref), 
-which returns a [`StatVectorType`](@ref) trait (by default, [`EuclideanState`](@ref)).
+which returns a [`StateVectorType`](@ref) trait (by default, [`EuclideanState`](@ref)).
 After defining a new `StateVectorType` and the following methods (described in more 
 detail in the documentation for [`StateVectorType`](@ref)):
 
diff --git a/src/functionbase.jl b/src/functionbase.jl
@@ -15,7 +15,7 @@ The dimensions `n`, `m`, and `p` can
 be queried individual by calling `state_dim`, `control_dim`, and `ouput_dim` or 
 collectively via [`dims`](@ref).
 
-## Evaluation 
+# Evaluation 
 An `AbstractFunction` can be evaluated in-place by calling any of 
 
     evaluate!(fun, y, z::AbstractKnotPoint)
@@ -35,7 +35,7 @@ Alternatively, the user can dispatch on [`FunctionSignature`](@ref) by calling
 
     evaluate!(::FunctionSignature, fun, y, z::AbstractKnotPoint)
 
-## Jacobians
+# Jacobians
 The Jacobian of ``f(x,u)`` with respect to both ``x`` and ``u`` can be computed 
 by calling the following function:
 
@@ -55,12 +55,12 @@ the `AbstractFunction`:
     jacobian!(fun, J, y, x, u, p)
     jacobian!(fun, J, y, x, u)
 
-## Functions of just the state or control
+# Functions of just the state or control
 Alternatively, the function can also be limited to an input of just the state or control 
 by defining the [`FunctionInputs`](@ref) trait. See trait documentation for more 
 information.
 
-## Convenience functions
+# Convenience functions
 The methods `fill(fun, v)`, `randn(fun)`, `rand(fun)`, and `zeros(fun)` are defined
 that provide a tuple with a state and control vector initialized using the corresponding
 function. The data type can be specified as the first argument for `randn`, `rand`, and 
diff --git a/src/liestate.jl b/src/liestate.jl
@@ -1,12 +1,23 @@
 """
-	LieGroupModel <: AbstractModel
+	LieGroupModel <: ContinuousDynamics 
 
 Abstraction of a dynamical system whose state contains at least one arbitrary rotation.
+
+# Usage
+To use this model, you only need to define the following functions:
+
+- `control_dim(model::MyLieGroupModel)` 
+- `LieState(::MyLieGroupModel)`
+- Either [`dynamics!`](@ref) or [`dynamics`](@ref)
+
+where `LieState(model)` should return a [`LieState`](@ref) for your model, 
+describing how the state vector is composed of Euclidean and 3D rotations.
+Note that this function should only be a function of the type, not the actual 
+struct itself (i.e. your method definition should look like the one above).
 """
 abstract type LieGroupModel <: ContinuousDynamics end
 const DiscreteLieDynamics = DiscretizedDynamics{L,Q} where {L<:LieGroupModel, Q<:QuadratureRule}
 
-
 # RotationState interface
 statevectortype(::Type{<:LieGroupModel}) = RotationState() 
 
diff --git a/src/plot_recipes.jl b/src/plot_recipes.jl
@@ -23,7 +23,7 @@ end
     xguide --> "time (s)"
     yguide --> "states"
     label --> reshape(default_labels[inds], 1, :)
-    gettimes(Z), (hcat(Vector.(get_data(Z))...)[inds,:])'
+    gettimes(Z), (hcat(Vector.(getdata(Z))...)[inds,:])'
 end
 
 """
diff --git a/src/rigidbody.jl b/src/rigidbody.jl
@@ -9,23 +9,39 @@ where `p` is the 3D position, `q` is the 3 or 4-dimension attitude representatio
 # Interface
 Any single-body system can leverage the `RigidBody` type by inheriting from it and defining the
 following interface:
+
 ```julia
-forces(::MyRigidBody, x, u)  # return the forces in the world frame
-moments(::MyRigidBody, x, u) # return the moments in the body frame
+forces(::MyRigidBody, x, u, [t])  # return the forces in the world frame
+moments(::MyRigidBody, x, u, [t]) # return the moments in the body frame
 inertia(::MyRigidBody, x, u) # return the 3x3 inertia matrix
 mass(::MyRigidBody, x, u)  # return the mass as a real scalar
 ```
 
 Instead of defining `forces` and `moments` you can define the higher-level `wrenches` function
-	wrenches(model::MyRigidbody, z::AbstractKnotPoint)
-	wrenches(model::MyRigidbody, x, u)
+
+	wrenches(model::MyRigidbody, x, u, t)
 
 # Rotation Parameterization
 A `RigidBody` model must specify the rotational representation being used. Any `Rotations.Rotation{3}`
 can be used, but we suggest one of the following:
 * `UnitQuaternion`
 * `MRP`
 * `RodriguesParam`
+
+# Working with state vectors for a `RigidBody`
+Several methods are provided for working with the state vectors for a `RigidBody`.
+Also see the documentation for [`RBState`](@ref) which provides a unified representation
+for working with states for rigid bodies, which can be easily converted to and 
+from the state vector representation for the given model.
+
+- [`Base.position`](@ref)
+- [`orientation`](@ref)
+- [`linear_velocity`](@ref)
+- [`angular_velocity`](@ref)
+- [`build_state`](@ref)
+- [`parse_state`](@ref)
+- [`gen_inds`](@ref)
+- [`flipquat`](@ref)
 """
 abstract type RigidBody{R<:Rotation} <: LieGroupModel end
 
@@ -82,15 +98,20 @@ end
 @inline linear_velocity(model::RigidBody, x) = x[gen_inds(model).v]
 @inline angular_velocity(model::RigidBody, x) = x[gen_inds(model).ω]
 
-for rot in [RodriguesParam, MRP, RotMatrix, RotationVec, AngleAxis]
-    @eval orientation(model::RigidBody{<:$rot}, x::AbstractVector, renorm=false) = ($rot)(x[4],x[5],x[6])
-end
 function orientation(model::RigidBody{<:UnitQuaternion}, x::AbstractVector,
         renorm=false)
     q = UnitQuaternion(x[4],x[5],x[6],x[7], renorm)
     return q
 end
+for rot in [RodriguesParam, MRP, RotMatrix, RotationVec, AngleAxis]
+    @eval orientation(model::RigidBody{<:$rot}, x::AbstractVector, renorm=false) = ($rot)(x[4],x[5],x[6])
+end
 
+"""
+    flipquat(model, x)
+
+Flips the quaternion sign for a `RigidBody{<:UnitQuaternion}`.
+"""
 function flipquat(model::RigidBody{<:UnitQuaternion}, x)
     return @SVector [x[1], x[2], x[3], -x[4], -x[5], -x[6], -x[7],
         x[8], x[9], x[10], x[11], x[12], x[13]]
diff --git a/src/scalar_function.jl b/src/scalar_function.jl
diff --git a/test/scalar_function_test.jl b/test/scalar_function_test.jl
