Merge pull request #521 from JuliaRobotics/tk/inertia-updates

Improve API and documentation for SpatialInertia construction.

notebooks/Four-bar linkage/Four-bar linkage.ipynb

@@ -183,23 +183,23 @@
    "source": [
     "# link1 and joint1\n",
     "joint1 = Joint(\"joint1\", Revolute(axis))\n",
-    "inertia1 = SpatialInertia(CartesianFrame3D(\"inertia1_centroidal\"), I_1*axis*axis', zero(SVector{3, T}), m_1)\n",
+    "inertia1 = SpatialInertia(CartesianFrame3D(\"inertia1_centroidal\"), moment=I_1*axis*axis', com=zero(SVector{3, T}), mass=m_1)\n",
     "link1 = RigidBody(inertia1)\n",
     "before_joint1_to_world = one(Transform3D, frame_before(joint1), default_frame(world))\n",
     "c1_to_joint = Transform3D(inertia1.frame, frame_after(joint1), SVector(c_1, 0, 0))\n",
     "attach!(fourbar, world, link1, joint1, joint_pose = before_joint1_to_world, successor_pose = c1_to_joint)\n",
     "\n",
     "# link2 and joint2\n",
     "joint2 = Joint(\"joint2\", Revolute(axis))\n",
-    "inertia2 = SpatialInertia(CartesianFrame3D(\"inertia2_centroidal\"), I_2*axis*axis', zero(SVector{3, T}), m_2)\n",
+    "inertia2 = SpatialInertia(CartesianFrame3D(\"inertia2_centroidal\"), moment=I_2*axis*axis', com=zero(SVector{3, T}), mass=m_2)\n",
     "link2 = RigidBody(inertia2)\n",
     "before_joint2_to_after_joint1 = Transform3D(frame_before(joint2), frame_after(joint1), SVector(l_1, 0., 0.))\n",
     "c2_to_joint = Transform3D(inertia2.frame, frame_after(joint2), SVector(c_2, 0, 0))\n",
     "attach!(fourbar, link1, link2, joint2, joint_pose = before_joint2_to_after_joint1, successor_pose = c2_to_joint)\n",
     "\n",
     "# link3 and joint3\n",
     "joint3 = Joint(\"joint3\", Revolute(axis))\n",
-    "inertia3 = SpatialInertia(CartesianFrame3D(\"inertia3_centroidal\"), I_3*axis*axis', zero(SVector{3, T}), m_3)\n",
+    "inertia3 = SpatialInertia(CartesianFrame3D(\"inertia3_centroidal\"), moment=I_3*axis*axis', com=zero(SVector{3, T}), mass=m_3)\n",
     "link3 = RigidBody(inertia3)\n",
     "before_joint3_to_world = Transform3D(frame_before(joint3), default_frame(world), SVector(l_0, 0., 0.))\n",
     "c3_to_joint = Transform3D(inertia3.frame, frame_after(joint3), SVector(c_3, 0, 0))\n",

notebooks/Quickstart - double pendulum/Quickstart - double pendulum.ipynb

@@ -117,7 +117,7 @@
     "c_1 = -0.5 # center of mass location with respect to joint axis\n",
     "m_1 = 1. # mass\n",
     "frame1 = CartesianFrame3D(\"upper_link\") # the reference frame in which the spatial inertia will be expressed\n",
-    "inertia1 = SpatialInertia(frame1, I_1 * axis * axis', m_1 * SVector(0, 0, c_1), m_1)"
+    "inertia1 = SpatialInertia(frame1, moment=I_1 * axis * axis', com=SVector(0, 0, c_1), mass=m_1)"
    ]
   },
   {
@@ -278,7 +278,7 @@
     "I_2 = 0.333 # moment of inertia about joint axis\n",
     "c_2 = -0.5 # center of mass location with respect to joint axis\n",
     "m_2 = 1. # mass\n",
-    "inertia2 = SpatialInertia(CartesianFrame3D(\"lower_link\"), I_2 * axis * axis', m_2 * SVector(0, 0, c_2), m_2)\n",
+    "inertia2 = SpatialInertia(CartesianFrame3D(\"lower_link\"), moment=I_2 * axis * axis', com=SVector(0, 0, c_2), mass=m_2)\n",
     "lowerlink = RigidBody(inertia2)\n",
     "elbow = Joint(\"elbow\", Revolute(axis))\n",
     "before_elbow_to_after_shoulder = Transform3D(frame_before(elbow), frame_after(shoulder), SVector(0, 0, l_1))\n",

notebooks/Symbolic double pendulum/Symbolic double pendulum.ipynb

@@ -116,14 +116,14 @@
     "world = root_body(double_pendulum) # the fixed 'world' rigid body\n",
     "\n",
     "# Attach the first (upper) link to the world via a revolute joint named 'shoulder'\n",
-    "inertia1 = SpatialInertia(CartesianFrame3D(\"upper_link\"), I_1 * axis * axis', m_1 * SVector(zero(T), zero(T), c_1), m_1)\n",
+    "inertia1 = SpatialInertia(CartesianFrame3D(\"upper_link\"), moment=I_1 * axis * axis', com=SVector(zero(T), zero(T), c_1), mass=m_1)\n",
     "body1 = RigidBody(inertia1)\n",
     "joint1 = Joint(\"shoulder\", Revolute(axis))\n",
     "joint1_to_world = one(Transform3D{T}, frame_before(joint1), default_frame(world));\n",
     "attach!(double_pendulum, world, body1, joint1, joint_pose = joint1_to_world);\n",
     "\n",
     "# Attach the second (lower) link to the world via a revolute joint named 'elbow'\n",
-    "inertia2 = SpatialInertia(CartesianFrame3D(\"lower_link\"), I_2 * axis * axis', m_2 * SVector(zero(T), zero(T), c_2), m_2)\n",
+    "inertia2 = SpatialInertia(CartesianFrame3D(\"lower_link\"), moment=I_2 * axis * axis', com=SVector(zero(T), zero(T), c_2), mass=m_2)\n",
     "body2 = RigidBody(inertia2)\n",
     "joint2 = Joint(\"elbow\", Revolute(axis))\n",
     "joint2_to_body1 = Transform3D(frame_before(joint2), default_frame(body1), SVector(zero(T), zero(T), l_1))\n",

src/spatial/motion_force_interaction.jl

@@ -19,6 +19,11 @@ where ``\\rho(x)`` is the density of point ``x``, and ``p(x)`` are the coordinat
 of point ``x`` expressed in frame ``i``.
 ``J`` is the mass moment of inertia, ``m`` is the total mass, and ``c`` is the
 'cross part', center of mass position scaled by ``m``.
+
+!!! warning
+
+    The `moment` field of a `SpatialInertia` is the moment of inertia **about the origin of its `frame`**,
+    not about the center of mass.
 """
 struct SpatialInertia{T}
     frame::CartesianFrame3D
@@ -31,10 +36,54 @@ struct SpatialInertia{T}
     end
 end
 
+"""
+$(SIGNATURES)
+
+Construct a `SpatialInertia` by specifying:
+
+* `frame`: the frame in which the spatial inertia is expressed.
+* `moment`: the moment of inertia expressed in `frame` (i.e., in `frame`'s axes and about the origin of `frame`).
+* `cross_part`: the center of mass expressed in `frame`, multiplied by the mass.
+* `mass`: the total mass.
+
+For more convenient construction of `SpatialInertia`s, consider using the keyword argument constructor instead.
+"""
 @inline function SpatialInertia(frame::CartesianFrame3D, moment::AbstractMatrix{T1}, cross_part::AbstractVector{T2}, mass::T3) where {T1, T2, T3}
     SpatialInertia{promote_type(T1, T2, T3)}(frame, moment, cross_part, mass)
 end
 
+"""
+$(SIGNATURES)
+
+Construct a `SpatialInertia` by specifying:
+
+* `frame`: the frame in which the spatial inertia is expressed.
+* one of:
+  * `moment`: the moment of inertia expressed in `frame` (i.e., about the origin of `frame` and in `frame`'s axes).
+  * `moment_about_com`: the moment of inertia about the center of mass, in `frame`'s axes.
+* `com`: the center of mass expressed in `frame`.
+* `mass`: the total mass.
+
+The `com` and `mass` keyword arguments are required, as well as exactly one of `moment` and `moment_about_com`
+"""
+@inline function SpatialInertia(frame::CartesianFrame3D;
+        moment::Union{AbstractMatrix, Nothing}=nothing,
+        moment_about_com::Union{AbstractMatrix, Nothing}=nothing,
+        com::AbstractVector,
+        mass)
+    if !((moment isa AbstractMatrix) ⊻ (moment_about_com isa AbstractMatrix))
+        throw(ArgumentError("Exactly one of `moment` or `moment_about_com` is required."))
+    end
+    _com = SVector{3}(com)
+    if moment !== nothing
+        _moment = moment
+    else
+        _moment = SMatrix{3, 3}(moment_about_com)
+        _moment -= mass * hat_squared(_com) # parallel axis theorem
+    end
+    SpatialInertia(frame, _moment, mass * _com, mass)
+end
+
 # SpatialInertia-specific functions
 Base.eltype(::Type{SpatialInertia{T}}) where {T} = T
 
@@ -81,10 +130,10 @@ Return the center of mass of the `SpatialInertia` as a [`Point3D`](@ref).
 center_of_mass(inertia::SpatialInertia) = Point3D(inertia.frame, inertia.cross_part / inertia.mass)
 
 function Base.show(io::IO, inertia::SpatialInertia)
-    println(io, "SpatialInertia expressed in \"$(string(inertia.frame))\":")
+    println(io, "SpatialInertia expressed in $(name_and_id(inertia.frame)):")
     println(io, "mass: $(inertia.mass)")
     println(io, "center of mass: $(center_of_mass(inertia))")
-    print(io, "moment of inertia:\n$(inertia.moment)")
+    print(io, "moment of inertia (about origin of $(name_and_id(inertia.frame)):\n$(inertia.moment)")
 end
 
 Base.zero(::Type{SpatialInertia{T}}, frame::CartesianFrame3D) where {T} = SpatialInertia(frame, zero(SMatrix{3, 3, T}), zero(SVector{3, T}), zero(T))
@@ -125,32 +174,24 @@ Transform the `SpatialInertia` to a different frame.
 end
 
 function Random.rand(::Type{<:SpatialInertia{T}}, frame::CartesianFrame3D) where {T}
-    # Try to generate a random but physical moment of inertia
-    # by constructing it from its eigendecomposition
-    Q = rand(RotMatrix3{T}).mat
-    principal_moments = Vector{T}(undef, 3)
+    # Try to generate a random but physical moment of inertia by constructing it from its eigendecomposition.
 
-    # Scale the inertias to make the length scale of the
-    # equivalent inertial ellipsoid roughly ~1 unit
-    principal_moments[1:2] = rand(T, 2) / 10.
+    # Create random principal moments of inertia (about the center of mass).
+    # Scale the inertias to make the length scale of the equivalent inertial ellipsoid roughly ~1 unit
+    ixx = rand(T) / 10.
+    iyy = rand(T) / 10.
 
-    # Ensure that the principal moments of inertia obey the triangle
-    # inequalities:
+    # Ensure that the principal moments of inertia obey the triangle inequalities:
     # http://www.mathworks.com/help/physmod/sm/mech/vis/about-body-color-and-geometry.html
-    lb = abs(principal_moments[1] - principal_moments[2])
-    ub = principal_moments[1] + principal_moments[2]
-    principal_moments[3] = rand(T) * (ub - lb) + lb
-
-    # Construct the moment of inertia tensor
-    J = SMatrix{3, 3, T}(Q * Diagonal(principal_moments) * Q')
+    lb = abs(ixx - iyy)
+    ub = ixx + iyy
+    izz = rand(T) * (ub - lb) + lb
 
-    # Construct the inertia in CoM frame
-    com_frame = CartesianFrame3D()
-    spatial_inertia = SpatialInertia(com_frame, J, zero(SVector{3, T}), rand(T))
+    # Randomly rotate the principal axes.
+    R = rand(RotMatrix3{T})
+    moment_about_com = R * Diagonal(SVector(ixx, iyy, izz)) * R'
 
-    # Put the center of mass at a random offset
-    com_frame_to_desired_frame = Transform3D(com_frame, frame, rand(SVector{3, T}) - T(0.5))
-    transform(spatial_inertia, com_frame_to_desired_frame)
+    SpatialInertia(frame, moment_about_com=moment_about_com, com=rand(SVector{3, T}) .- T(0.5), mass=rand(T))
 end
 
 """

test/test_double_pendulum.jl

@@ -15,28 +15,14 @@
     double_pendulum = Mechanism(RigidBody{Float64}("world"); gravity = SVector(0, 0, g))
     world = root_body(double_pendulum)
 
-    # IMPORTANT for creating the SpatialInertias below:
-    # 1) the second argument, `crosspart` is mass *times* center of mass
-    # 2) these SpatialInertias are expressed in frames directly after each of the joints,
-    # since the moments of inertia are specified in those frames.
-    # If the moment of inertia data were given in the frame after a joint,
-    # it would be easier to do the following:
-    #
-    #   joint1 = Joint("joint1", Revolute(axis))
-    #   inertia1 = SpatialInertia(CartesianFrame3D("inertia1_centroidal"), I1_about_com * axis * axis', zero(SVector{3, Float64}), m1)
-    #   link1 = RigidBody(inertia1)
-    #   before_joint_1_to_world = one(Transform3D, frame_before(joint1), default_frame(world))
-    #   c1_to_joint = Transform3D(inertia1.frame, frame_after(joint1), SVector(lc1, 0, 0))
-    #   attach!(double_pendulum, world, joint1, link1, before_joint_1_to_world, joint_pose = c1_to_joint)
-
     # create first body and attach it to the world via a revolute joint
-    inertia1 = SpatialInertia(CartesianFrame3D("upper_link"), I1 * axis * axis', m1 * SVector(0, 0, lc1), m1)
+    inertia1 = SpatialInertia(CartesianFrame3D("upper_link"), moment=I1 * axis * axis', com=SVector(0, 0, lc1), mass=m1)
     body1 = RigidBody(inertia1)
     joint1 = Joint("shoulder", Revolute(axis))
     joint1_to_world = one(Transform3D, joint1.frame_before, default_frame(world))
     attach!(double_pendulum, world, body1, joint1, joint_pose = joint1_to_world)
 
-    inertia2 = SpatialInertia(CartesianFrame3D("lower_link"), I2 * axis * axis', m2 * SVector(0, 0, lc2), m2)
+    inertia2 = SpatialInertia(CartesianFrame3D("lower_link"), moment=I2 * axis * axis', com=SVector(0, 0, lc2), mass=m2)
     body2 = RigidBody(inertia2)
     joint2 = Joint("elbow", Revolute(axis))
     joint2_to_body1 = Transform3D(joint2.frame_before, default_frame(body1), SVector(0, 0, l1))

test/test_simulate.jl

@@ -161,23 +161,23 @@
 
         # link1 and joint1
         joint1 = Joint("joint1", Revolute(axis))
-        inertia1 = SpatialInertia(CartesianFrame3D("inertia1_centroidal"), I_1*axis*axis', zero(SVector{3, T}), m_1)
+        inertia1 = SpatialInertia(CartesianFrame3D("inertia1_centroidal"), moment=I_1*axis*axis', com=zero(SVector{3, T}), mass=m_1)
         link1 = RigidBody(inertia1)
         before_joint1_to_world = one(Transform3D, frame_before(joint1), default_frame(world))
         c1_to_joint = Transform3D(inertia1.frame, frame_after(joint1), SVector(c_1, 0, 0))
         attach!(mechanism, world, link1, joint1, joint_pose = before_joint1_to_world, successor_pose = c1_to_joint)
 
         # link2 and joint2
         joint2 = Joint("joint2", Revolute(axis))
-        inertia2 = SpatialInertia(CartesianFrame3D("inertia2_centroidal"), I_2*axis*axis', zero(SVector{3, T}), m_2)
+        inertia2 = SpatialInertia(CartesianFrame3D("inertia2_centroidal"), moment=I_2*axis*axis', com=zero(SVector{3, T}), mass=m_2)
         link2 = RigidBody(inertia2)
         before_joint2_to_after_joint1 = Transform3D(frame_before(joint2), frame_after(joint1), SVector(l_1, 0., 0.))
         c2_to_joint = Transform3D(inertia2.frame, frame_after(joint2), SVector(c_2, 0, 0))
         attach!(mechanism, link1, link2, joint2, joint_pose = before_joint2_to_after_joint1, successor_pose = c2_to_joint)
 
         # link3 and joint3
         joint3 = Joint("joint3", Revolute(axis))
-        inertia3 = SpatialInertia(CartesianFrame3D("inertia3_centroidal"), I_3*axis*axis', zero(SVector{3, T}), m_3)
+        inertia3 = SpatialInertia(CartesianFrame3D("inertia3_centroidal"), moment=I_3*axis*axis', com=zero(SVector{3, T}), mass=m_3)
         link3 = RigidBody(inertia3)
         before_joint3_to_world = Transform3D(frame_before(joint3), default_frame(world), SVector(l_0, 0., 0.))
         c3_to_joint = Transform3D(inertia3.frame, frame_after(joint3), SVector(c_3, 0, 0))

test/test_spatial.jl

@@ -76,6 +76,16 @@ end
         # Test that the constructor works with dynamic arrays (which are
         # converted to static arrays internally)
         I4 = @inferred(SpatialInertia(f2, Matrix(1.0I, 3, 3), zeros(3), 1.0))
+
+        # Ensure that the kwarg constructor matches the positional argument constructor.
+        inertia = rand(SpatialInertia{Float64}, f1)
+        centroidal = CartesianFrame3D("centroidal")
+        to_centroidal = Transform3D(f1, centroidal, -center_of_mass(inertia).v)
+        inertia_centroidal = transform(inertia, to_centroidal)
+        @test inertia_centroidal.frame == centroidal
+        @test center_of_mass(inertia_centroidal) ≈ Point3D(Float64, centroidal) atol=1e-12
+        inertia_back = SpatialInertia(inertia.frame, moment_about_com=inertia_centroidal.moment, com=center_of_mass(inertia).v, mass=inertia.mass)
+        @test inertia ≈ inertia_back atol=1e-12
     end
 
     @testset "twist" begin