diff --git a/notebooks/Four-bar linkage/Four-bar linkage.ipynb b/notebooks/Four-bar linkage/Four-bar linkage.ipynb index 665768ce..d484d30e 100644 --- a/notebooks/Four-bar linkage/Four-bar linkage.ipynb +++ b/notebooks/Four-bar linkage/Four-bar linkage.ipynb @@ -183,7 +183,7 @@ "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", @@ -191,7 +191,7 @@ "\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", @@ -199,7 +199,7 @@ "\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", diff --git a/notebooks/Quickstart - double pendulum/Quickstart - double pendulum.ipynb b/notebooks/Quickstart - double pendulum/Quickstart - double pendulum.ipynb index de8030f6..d47354da 100644 --- a/notebooks/Quickstart - double pendulum/Quickstart - double pendulum.ipynb +++ b/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", diff --git a/notebooks/Symbolic double pendulum/Symbolic double pendulum.ipynb b/notebooks/Symbolic double pendulum/Symbolic double pendulum.ipynb index 2fd9e8eb..f243d7b9 100644 --- a/notebooks/Symbolic double pendulum/Symbolic double pendulum.ipynb +++ b/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", diff --git a/src/spatial/motion_force_interaction.jl b/src/spatial/motion_force_interaction.jl index 5cddc52c..de639b48 100644 --- a/src/spatial/motion_force_interaction.jl +++ b/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 """ diff --git a/test/test_double_pendulum.jl b/test/test_double_pendulum.jl index b2303ac5..e51c8335 100644 --- a/test/test_double_pendulum.jl +++ b/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)) diff --git a/test/test_simulate.jl b/test/test_simulate.jl index 7a061147..292a6320 100644 --- a/test/test_simulate.jl +++ b/test/test_simulate.jl @@ -161,7 +161,7 @@ # 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)) @@ -169,7 +169,7 @@ # 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)) @@ -177,7 +177,7 @@ # 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)) diff --git a/test/test_spatial.jl b/test/test_spatial.jl index 0b50a9ab..f34ca971 100644 --- a/test/test_spatial.jl +++ b/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