From cb1977f1ac637ff75e16cbaf9ba1732239232842 Mon Sep 17 00:00:00 2001
From: Twan Koolen <koolen.twan@gmail.com>
Date: Thu, 19 Sep 2019 15:44:02 -0400
Subject: [PATCH] Use promote_eltype instead of promote_type.

Ref: #570.
---
 src/RigidBodyDynamics.jl                |  2 +-
 src/contact.jl                          |  2 +-
 src/joint_types/fixed.jl                | 34 ++++++++---------
 src/joint_types/planar.jl               | 36 +++++++++---------
 src/joint_types/prismatic.jl            | 34 +++++++++--------
 src/joint_types/quaternion_floating.jl  | 37 ++++++++++---------
 src/joint_types/quaternion_spherical.jl | 37 ++++++++++---------
 src/joint_types/revolute.jl             | 28 +++++++-------
 src/joint_types/sin_cos_revolute.jl     | 22 +++++------
 src/joint_types/spquat_floating.jl      | 49 ++++++++++++++-----------
 src/spatial/Spatial.jl                  |  2 +
 src/spatial/motion_force_interaction.jl |  8 ++--
 src/spatial/spatialforce.jl             |  5 ++-
 src/spatial/spatialmotion.jl            |  5 ++-
 src/spatial/transform3d.jl              |  2 +-
 src/spatial/util.jl                     |  4 +-
 16 files changed, 167 insertions(+), 140 deletions(-)

diff --git a/src/RigidBodyDynamics.jl b/src/RigidBodyDynamics.jl
index a73b113b..dc38a241 100644
--- a/src/RigidBodyDynamics.jl
+++ b/src/RigidBodyDynamics.jl
@@ -9,7 +9,7 @@ using DocStringExtensions
 using Reexport
 
 using Base.Iterators: filter, flatten
-using Base: @propagate_inbounds
+using Base: @propagate_inbounds, promote_eltype
 
 # mechanism-related types
 export
diff --git a/src/contact.jl b/src/contact.jl
index d9ddbb46..50ac4a05 100644
--- a/src/contact.jl
+++ b/src/contact.jl
@@ -227,7 +227,7 @@ end
 frame(halfspace::HalfSpace3D) = halfspace.point.frame
 
 function HalfSpace3D(point::Point3D, outward_normal::FreeVector3D)
-    T = promote_type(eltype(point), eltype(outward_normal))
+    T = promote_eltype(point, outward_normal)
     HalfSpace3D(convert(Point3D{SVector{3, T}}, point), convert(FreeVector3D{SVector{3, T}}, outward_normal))
 end
 
diff --git a/src/joint_types/fixed.jl b/src/joint_types/fixed.jl
index 1118014e..d7849cb4 100644
--- a/src/joint_types/fixed.jl
+++ b/src/joint_types/fixed.jl
@@ -15,32 +15,32 @@ num_velocities(::Type{<:Fixed}) = 0
 has_fixed_subspaces(jt::Fixed) = true
 isfloating(::Type{<:Fixed}) = false
 
-@inline function joint_transform(jt::Fixed{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function joint_transform(jt::Fixed, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector)
+    S = promote_eltype(jt, q)
     one(Transform3D{S}, frame_after, frame_before)
 end
 
-@inline function joint_twist(jt::Fixed{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function joint_twist(jt::Fixed, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector)
+    S = promote_eltype(jt, q, v)
     zero(Twist{S}, frame_after, frame_before, frame_after)
 end
 
-@inline function joint_spatial_acceleration(jt::Fixed{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}, vd::AbstractVector{XD}) where {T, X, XD}
-    S = promote_type(T, X, XD)
+@inline function joint_spatial_acceleration(jt::Fixed, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector, vd::AbstractVector)
+    S = promote_eltype(jt, q, v, vd)
     zero(SpatialAcceleration{S}, frame_after, frame_before, frame_after)
 end
 
-@inline function motion_subspace(jt::Fixed{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function motion_subspace(jt::Fixed, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector)
+    S = promote_eltype(jt, q)
     GeometricJacobian(frame_after, frame_before, frame_after, zero(SMatrix{3, 0, S}), zero(SMatrix{3, 0, S}))
 end
 
-@inline function constraint_wrench_subspace(jt::Fixed{T}, joint_transform::Transform3D{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function constraint_wrench_subspace(jt::Fixed, joint_transform::Transform3D)
+    S = promote_eltype(jt, joint_transform)
     angular = hcat(one(SMatrix{3, 3, S}), zero(SMatrix{3, 3, S}))
     linear = hcat(zero(SMatrix{3, 3, S}), one(SMatrix{3, 3, S}))
     WrenchMatrix(joint_transform.from, angular, linear)
@@ -49,9 +49,9 @@ end
 @inline zero_configuration!(q::AbstractVector, ::Fixed) = nothing
 @inline rand_configuration!(q::AbstractVector, ::Fixed) = nothing
 
-@inline function bias_acceleration(jt::Fixed{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function bias_acceleration(jt::Fixed, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector)
+    S = promote_eltype(jt, q, v)
     zero(SpatialAcceleration{S}, frame_after, frame_before, frame_after)
 end
 
diff --git a/src/joint_types/planar.jl b/src/joint_types/planar.jl
index c6c9b6d3..9b41e1b5 100644
--- a/src/joint_types/planar.jl
+++ b/src/joint_types/planar.jl
@@ -38,7 +38,7 @@ struct Planar{T} <: JointType{T}
     end
 end
 
-Planar(x_axis::AbstractVector{X}, y_axis::AbstractVector{Y}) where {X, Y} = Planar{promote_type(X, Y)}(x_axis, y_axis)
+Planar(x_axis::AbstractVector, y_axis::AbstractVector) = Planar{promote_eltype(x_axis, y_axis)}(x_axis, y_axis)
 
 Base.show(io::IO, jt::Planar) = print(io, "Planar joint with x-axis $(jt.x_axis) and y-axis $(jt.y_axis)")
 
@@ -54,53 +54,55 @@ num_velocities(::Type{<:Planar}) = 3
 has_fixed_subspaces(jt::Planar) = true
 isfloating(::Type{<:Planar}) = false
 
-@propagate_inbounds function rand_configuration!(q::AbstractVector{T}, ::Planar) where {T}
+@propagate_inbounds function rand_configuration!(q::AbstractVector, ::Planar)
+    T = eltype(q)
     q[1] = rand() - T(0.5)
     q[2] = rand() - T(0.5)
     q[3] = randn()
     nothing
 end
 
-@propagate_inbounds function joint_transform(jt::Planar{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}) where {T, X}
+@propagate_inbounds function joint_transform(jt::Planar, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector)
     rot = RotMatrix(AngleAxis(q[3], jt.rot_axis[1], jt.rot_axis[2], jt.rot_axis[3], false))
     trans = jt.x_axis * q[1] + jt.y_axis * q[2]
     Transform3D(frame_after, frame_before, rot, trans)
 end
 
-@propagate_inbounds function joint_twist(jt::Planar{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}) where {T, X}
+@propagate_inbounds function joint_twist(jt::Planar, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector)
     angular = jt.rot_axis * v[3]
     linear = jt.x_axis * v[1] + jt.y_axis * v[2]
     Twist(frame_after, frame_before, frame_after, angular, linear)
 end
 
-@propagate_inbounds function joint_spatial_acceleration(jt::Planar{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}, vd::AbstractVector{XD}) where {T, X, XD}
-    S = promote_type(T, X, XD)
+@propagate_inbounds function joint_spatial_acceleration(jt::Planar, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector, vd::AbstractVector)
+    S = promote_eltype(jt, q, v, vd)
     angular = jt.rot_axis * vd[3]
     linear = jt.x_axis * vd[1] + jt.y_axis * vd[2]
     SpatialAcceleration{S}(frame_after, frame_before, frame_after, angular, linear)
 end
 
-@inline function motion_subspace(jt::Planar{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function motion_subspace(jt::Planar, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector)
+    S = promote_eltype(jt, q)
     angular = hcat(zero(SMatrix{3, 2, S}), jt.rot_axis)
     linear = hcat(jt.x_axis, jt.y_axis, zero(SVector{3, S}))
     GeometricJacobian(frame_after, frame_before, frame_after, angular, linear)
 end
 
-@inline function constraint_wrench_subspace(jt::Planar{T}, joint_transform::Transform3D{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function constraint_wrench_subspace(jt::Planar, joint_transform::Transform3D)
+    S = promote_eltype(jt, joint_transform)
     angular = hcat(zero(SVector{3, S}), jt.x_axis, jt.y_axis)
     linear = hcat(jt.rot_axis, zero(SMatrix{3, 2, S}))
     WrenchMatrix(joint_transform.from, angular, linear)
 end
 
-@inline function bias_acceleration(jt::Planar{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}) where {T, X}
-    zero(SpatialAcceleration{promote_type(T, X)}, frame_after, frame_before, frame_after)
+@inline function bias_acceleration(jt::Planar, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector)
+    S = promote_eltype(jt, q, v)
+    zero(SpatialAcceleration{S}, frame_after, frame_before, frame_after)
 end
 
 @propagate_inbounds function joint_torque!(τ::AbstractVector, jt::Planar, q::AbstractVector, joint_wrench::Wrench)
diff --git a/src/joint_types/prismatic.jl b/src/joint_types/prismatic.jl
index b8e79882..1e34dd6d 100644
--- a/src/joint_types/prismatic.jl
+++ b/src/joint_types/prismatic.jl
@@ -50,47 +50,51 @@ end
     nothing
  end
 
-@inline function bias_acceleration(jt::Prismatic{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function bias_acceleration(jt::Prismatic, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector)
+    S = promote_eltype(jt, q, v)
     zero(SpatialAcceleration{S}, frame_after, frame_before, frame_after)
 end
 
-@inline function velocity_to_configuration_derivative_jacobian(::Prismatic{T}, ::AbstractVector) where T
+@inline function velocity_to_configuration_derivative_jacobian(jt::Prismatic, q::AbstractVector)
+    T = promote_eltype(jt, q)
     @SMatrix([one(T)])
 end
 
-@inline function configuration_derivative_to_velocity_jacobian(::Prismatic{T}, ::AbstractVector) where T
+@inline function configuration_derivative_to_velocity_jacobian(jt::Prismatic, q::AbstractVector)
+    T = promote_eltype(jt, q)
     @SMatrix([one(T)])
 end
 
-@propagate_inbounds function joint_transform(jt::Prismatic, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D, q::AbstractVector)
+@propagate_inbounds function joint_transform(jt::Prismatic, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector)
     translation = q[1] * jt.axis
     Transform3D(frame_after, frame_before, translation)
 end
 
-@propagate_inbounds function joint_twist(jt::Prismatic, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D, q::AbstractVector, v::AbstractVector)
+@propagate_inbounds function joint_twist(jt::Prismatic, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector)
     linear = jt.axis * v[1]
     Twist(frame_after, frame_before, frame_after, zero(linear), linear)
 end
 
-@propagate_inbounds function joint_spatial_acceleration(jt::Prismatic{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}, vd::AbstractVector{XD}) where {T, X, XD}
-    S = promote_type(T, X, XD)
+@propagate_inbounds function joint_spatial_acceleration(jt::Prismatic, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector, vd::AbstractVector)
+    S = promote_eltype(jt, q, v, vd)
     linear = convert(SVector{3, S}, jt.axis * vd[1])
     SpatialAcceleration(frame_after, frame_before, frame_after, zero(linear), linear)
 end
 
-@inline function motion_subspace(jt::Prismatic{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function motion_subspace(jt::Prismatic, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector)
+    S = promote_eltype(jt, q)
     angular = zero(SMatrix{3, 1, S})
     linear = SMatrix{3, 1, S}(jt.axis)
     GeometricJacobian(frame_after, frame_before, frame_after, angular, linear)
 end
 
-@inline function constraint_wrench_subspace(jt::Prismatic{T}, joint_transform::Transform3D{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function constraint_wrench_subspace(jt::Prismatic, joint_transform::Transform3D)
+    S = promote_eltype(jt, joint_transform)
     R = convert(RotMatrix3{S}, jt.rotation_from_z_aligned)
     Rcols12 = R[:, SVector(1, 2)]
     angular = hcat(R, zero(SMatrix{3, 2, S}))
diff --git a/src/joint_types/quaternion_floating.jl b/src/joint_types/quaternion_floating.jl
index 1939e5c5..24f4593f 100644
--- a/src/joint_types/quaternion_floating.jl
+++ b/src/joint_types/quaternion_floating.jl
@@ -31,7 +31,8 @@ isfloating(::Type{<:QuaternionFloating}) = true
     quat
 end
 
-@propagate_inbounds function set_rotation!(q::AbstractVector, jt::QuaternionFloating, rot::Rotation{3, T}) where T
+@propagate_inbounds function set_rotation!(q::AbstractVector, jt::QuaternionFloating, rot::Rotation{3})
+    T = eltype(rot)
     quat = convert(Quat{T}, rot)
     q[1] = quat.w
     q[2] = quat.x
@@ -80,26 +81,27 @@ end
     Transform3D(frame_after, frame_before, rotation(jt, q, false), translation(jt, q))
 end
 
-@inline function motion_subspace(jt::QuaternionFloating{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function motion_subspace(jt::QuaternionFloating, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector)
+    S = promote_eltype(jt, q)
     angular = hcat(one(SMatrix{3, 3, S}), zero(SMatrix{3, 3, S}))
     linear = hcat(zero(SMatrix{3, 3, S}), one(SMatrix{3, 3, S}))
     GeometricJacobian(frame_after, frame_before, frame_after, angular, linear)
 end
 
-@inline function constraint_wrench_subspace(jt::QuaternionFloating{T}, joint_transform::Transform3D{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function constraint_wrench_subspace(jt::QuaternionFloating, joint_transform::Transform3D)
+    S = promote_eltype(jt, joint_transform)
     WrenchMatrix(joint_transform.from, zero(SMatrix{3, 0, S}), zero(SMatrix{3, 0, S}))
 end
 
-@inline function bias_acceleration(jt::QuaternionFloating{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function bias_acceleration(jt::QuaternionFloating, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector)
+    S = promote_eltype(q, v)
     zero(SpatialAcceleration{S}, frame_after, frame_before, frame_after)
 end
 
-@propagate_inbounds function configuration_derivative_to_velocity!(v::AbstractVector, jt::QuaternionFloating, q::AbstractVector, q̇::AbstractVector)
+@propagate_inbounds function configuration_derivative_to_velocity!(v::AbstractVector, jt::QuaternionFloating,
+        q::AbstractVector, q̇::AbstractVector)
     quat = rotation(jt, q, false)
     quatdot = SVector(q̇[1], q̇[2], q̇[3], q̇[4])
     ω = angular_velocity_in_body(quat, quatdot)
@@ -120,7 +122,8 @@ end
     nothing
 end
 
-@propagate_inbounds function velocity_to_configuration_derivative!(q̇::AbstractVector, jt::QuaternionFloating, q::AbstractVector, v::AbstractVector)
+@propagate_inbounds function velocity_to_configuration_derivative!(q̇::AbstractVector, jt::QuaternionFloating,
+        q::AbstractVector, v::AbstractVector)
     quat = rotation(jt, q, false)
     ω = angular_velocity(jt, v)
     linear = linear_velocity(jt, v)
@@ -175,17 +178,17 @@ end
     nothing
 end
 
-@propagate_inbounds function joint_twist(jt::QuaternionFloating{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@propagate_inbounds function joint_twist(jt::QuaternionFloating, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector)
+    S = promote_eltype(jt, q, v)
     angular = convert(SVector{3, S}, angular_velocity(jt, v))
     linear = convert(SVector{3, S}, linear_velocity(jt, v))
     Twist(frame_after, frame_before, frame_after, angular, linear)
 end
 
-@propagate_inbounds function joint_spatial_acceleration(jt::QuaternionFloating{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}, vd::AbstractVector{XD}) where {T, X, XD}
-    S = promote_type(T, X, XD)
+@propagate_inbounds function joint_spatial_acceleration(jt::QuaternionFloating, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector, vd::AbstractVector)
+    S = promote_eltype(jt, q, v, vd)
     angular = convert(SVector{3, S}, angular_velocity(jt, vd))
     linear = convert(SVector{3, S}, linear_velocity(jt, vd))
     SpatialAcceleration(frame_after, frame_before, frame_after, angular, linear)
diff --git a/src/joint_types/quaternion_spherical.jl b/src/joint_types/quaternion_spherical.jl
index dd5b307e..38921671 100644
--- a/src/joint_types/quaternion_spherical.jl
+++ b/src/joint_types/quaternion_spherical.jl
@@ -27,7 +27,8 @@ isfloating(::Type{<:QuaternionSpherical}) = false
     Quat(q[1], q[2], q[3], q[4], normalize)
 end
 
-@propagate_inbounds function set_rotation!(q::AbstractVector, jt::QuaternionSpherical, rot::Rotation{3, T}) where {T}
+@propagate_inbounds function set_rotation!(q::AbstractVector, jt::QuaternionSpherical, rot::Rotation{3})
+    T = eltype(rot)
     quat = convert(Quat{T}, rot)
     q[1] = quat.w
     q[2] = quat.x
@@ -47,28 +48,29 @@ end
     Transform3D(frame_after, frame_before, quat)
 end
 
-@inline function motion_subspace(jt::QuaternionSpherical{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function motion_subspace(jt::QuaternionSpherical, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector)
+    S = promote_eltype(jt, q)
     angular = one(SMatrix{3, 3, S})
     linear = zero(SMatrix{3, 3, S})
     GeometricJacobian(frame_after, frame_before, frame_after, angular, linear)
 end
 
-@inline function constraint_wrench_subspace(jt::QuaternionSpherical{T}, joint_transform::Transform3D{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function constraint_wrench_subspace(jt::QuaternionSpherical, joint_transform::Transform3D)
+    S = promote_eltype(jt, joint_transform)
     angular = zero(SMatrix{3, 3, S})
     linear = one(SMatrix{3, 3, S})
     WrenchMatrix(joint_transform.from, angular, linear)
 end
 
-@inline function bias_acceleration(jt::QuaternionSpherical{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function bias_acceleration(jt::QuaternionSpherical, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector)
+    S = promote_eltype(jt, q, v)
     zero(SpatialAcceleration{S}, frame_after, frame_before, frame_after)
 end
 
-@propagate_inbounds function configuration_derivative_to_velocity!(v::AbstractVector, jt::QuaternionSpherical, q::AbstractVector, q̇::AbstractVector)
+@propagate_inbounds function configuration_derivative_to_velocity!(v::AbstractVector, jt::QuaternionSpherical,
+        q::AbstractVector, q̇::AbstractVector)
     quat = rotation(jt, q, false)
     quatdot = SVector(q̇[1], q̇[2], q̇[3], q̇[4])
     v .= angular_velocity_in_body(quat, quatdot)
@@ -82,7 +84,8 @@ end
     nothing
 end
 
-@propagate_inbounds function velocity_to_configuration_derivative!(q̇::AbstractVector, jt::QuaternionSpherical, q::AbstractVector, v::AbstractVector)
+@propagate_inbounds function velocity_to_configuration_derivative!(q̇::AbstractVector, jt::QuaternionSpherical,
+        q::AbstractVector, v::AbstractVector)
     quat = rotation(jt, q, false)
     q̇ .= quaternion_derivative(quat, v)
     nothing
@@ -110,17 +113,17 @@ end
     nothing
 end
 
-@propagate_inbounds function joint_twist(jt::QuaternionSpherical{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@propagate_inbounds function joint_twist(jt::QuaternionSpherical, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector)
+    S = promote_eltype(jt, q, v)
     angular = SVector{3, S}(v)
     linear = zero(SVector{3, S})
     Twist(frame_after, frame_before, frame_after, angular, linear)
 end
 
-@propagate_inbounds function joint_spatial_acceleration(jt::QuaternionSpherical{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}, vd::AbstractVector{XD}) where {T, X, XD}
-    S = promote_type(T, X, XD)
+@propagate_inbounds function joint_spatial_acceleration(jt::QuaternionSpherical, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector, vd::AbstractVector)
+    S = promote_eltype(jt, q, v, vd)
     angular = SVector{3, S}(vd)
     linear = zero(SVector{3, S})
     SpatialAcceleration(frame_after, frame_before, frame_after, angular, linear)
diff --git a/src/joint_types/revolute.jl b/src/joint_types/revolute.jl
index 3ee924b5..e5a7aaa5 100644
--- a/src/joint_types/revolute.jl
+++ b/src/joint_types/revolute.jl
@@ -67,29 +67,29 @@ end
     Twist(frame_after, frame_before, frame_after, angular, zero(angular))
 end
 
-@inline function bias_acceleration(jt::Revolute{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function bias_acceleration(jt::Revolute, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector)
+    S = promote_eltype(jt, q, v)
     zero(SpatialAcceleration{S}, frame_after, frame_before, frame_after)
 end
 
-@propagate_inbounds function joint_spatial_acceleration(jt::Revolute{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}, vd::AbstractVector{XD}) where {T, X, XD}
-    S = promote_type(T, X, XD)
+@propagate_inbounds function joint_spatial_acceleration(jt::Revolute, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector, vd::AbstractVector)
+    S = promote_eltype(jt, q, v, vd)
     angular = convert(SVector{3, S}, jt.axis * vd[1])
     SpatialAcceleration(frame_after, frame_before, frame_after, angular, zero(angular))
 end
 
-@inline function motion_subspace(jt::Revolute{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function motion_subspace(jt::Revolute, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector)
+    S = promote_eltype(jt, q)
     angular = SMatrix{3, 1, S}(jt.axis)
     linear = zero(SMatrix{3, 1, S})
     GeometricJacobian(frame_after, frame_before, frame_after, angular, linear)
 end
 
-@inline function constraint_wrench_subspace(jt::Revolute{T}, joint_transform::Transform3D{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function constraint_wrench_subspace(jt::Revolute, joint_transform::Transform3D)
+    S = promote_eltype(jt, joint_transform)
     R = convert(RotMatrix3{S}, jt.rotation_from_z_aligned)
     Rcols12 = R[:, SVector(1, 2)]
     angular = hcat(Rcols12, zero(SMatrix{3, 3, S}))
@@ -102,11 +102,13 @@ end
     nothing
 end
 
-@inline function velocity_to_configuration_derivative_jacobian(::Revolute{T}, ::AbstractVector) where T
+@inline function velocity_to_configuration_derivative_jacobian(jt::Revolute, q::AbstractVector)
+    T = promote_eltype(jt, q)
     @SMatrix([one(T)])
 end
 
-@inline function configuration_derivative_to_velocity_jacobian(::Revolute{T}, ::AbstractVector) where T
+@inline function configuration_derivative_to_velocity_jacobian(jt::Revolute, q::AbstractVector)
+    T = promote_eltype(jt, q)
     @SMatrix([one(T)])
 end
 
diff --git a/src/joint_types/sin_cos_revolute.jl b/src/joint_types/sin_cos_revolute.jl
index 2b10aa4e..79c3ef8a 100644
--- a/src/joint_types/sin_cos_revolute.jl
+++ b/src/joint_types/sin_cos_revolute.jl
@@ -101,29 +101,29 @@ end
     Twist(frame_after, frame_before, frame_after, angular, zero(angular))
 end
 
-@inline function bias_acceleration(jt::SinCosRevolute{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function bias_acceleration(jt::SinCosRevolute, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector)
+    S = promote_eltype(jt, q, v)
     zero(SpatialAcceleration{S}, frame_after, frame_before, frame_after)
 end
 
-@propagate_inbounds function joint_spatial_acceleration(jt::SinCosRevolute{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}, vd::AbstractVector{XD}) where {T, X, XD}
-    S = promote_type(T, X, XD)
+@propagate_inbounds function joint_spatial_acceleration(jt::SinCosRevolute, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector, vd::AbstractVector)
+    S = promote_eltype(jt, q, v, vd)
     angular = convert(SVector{3, S}, jt.axis * vd[1])
     SpatialAcceleration(frame_after, frame_before, frame_after, angular, zero(angular))
 end
 
-@inline function motion_subspace(jt::SinCosRevolute{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function motion_subspace(jt::SinCosRevolute, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector)
+    S = promote_eltype(jt, q)
     angular = SMatrix{3, 1, S}(jt.axis)
     linear = zero(SMatrix{3, 1, S})
     GeometricJacobian(frame_after, frame_before, frame_after, angular, linear)
 end
 
-@inline function constraint_wrench_subspace(jt::SinCosRevolute{T}, joint_transform::Transform3D{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function constraint_wrench_subspace(jt::SinCosRevolute, joint_transform::Transform3D)
+    S = promote_eltype(jt, joint_transform)
     R = convert(RotMatrix3{S}, jt.rotation_from_z_aligned)
     Rcols12 = R[:, SVector(1, 2)]
     angular = hcat(Rcols12, zero(SMatrix{3, 3, S}))
diff --git a/src/joint_types/spquat_floating.jl b/src/joint_types/spquat_floating.jl
index 4db26b0b..f548b72b 100644
--- a/src/joint_types/spquat_floating.jl
+++ b/src/joint_types/spquat_floating.jl
@@ -30,7 +30,8 @@ isfloating(::Type{<:SPQuatFloating}) = true
     SPQuat(q[1], q[2], q[3])
 end
 
-@propagate_inbounds function set_rotation!(q::AbstractVector, jt::SPQuatFloating, rot::Rotation{3, T}) where T
+@propagate_inbounds function set_rotation!(q::AbstractVector, jt::SPQuatFloating, rot::Rotation{3})
+    T = eltype(rot)
     spq = convert(SPQuat{T}, rot)
     q[1] = spq.x
     q[2] = spq.y
@@ -73,30 +74,32 @@ end
     v
 end
 
-@propagate_inbounds function joint_transform(jt::SPQuatFloating, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D, q::AbstractVector)
+@propagate_inbounds function joint_transform(jt::SPQuatFloating, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector)
     Transform3D(frame_after, frame_before, rotation(jt, q), translation(jt, q))
 end
 
-@inline function motion_subspace(jt::SPQuatFloating{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function motion_subspace(jt::SPQuatFloating, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector)
+    S = promote_eltype(jt, q)
     angular = hcat(one(SMatrix{3, 3, S}), zero(SMatrix{3, 3, S}))
     linear = hcat(zero(SMatrix{3, 3, S}), one(SMatrix{3, 3, S}))
     GeometricJacobian(frame_after, frame_before, frame_after, angular, linear)
 end
 
-@inline function constraint_wrench_subspace(jt::SPQuatFloating{T}, joint_transform::Transform3D{X}) where {T, X}
-    S = promote_type(T, X)
+@inline function constraint_wrench_subspace(jt::SPQuatFloating, joint_transform::Transform3D)
+    S = promote_eltype(jt, joint_transform)
     WrenchMatrix(joint_transform.from, zero(SMatrix{3, 0, S}), zero(SMatrix{3, 0, S}))
 end
 
-@propagate_inbounds function bias_acceleration(jt::SPQuatFloating{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@propagate_inbounds function bias_acceleration(jt::SPQuatFloating, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector)
+    S = promote_eltype(jt, q, v)
     zero(SpatialAcceleration{S}, frame_after, frame_before, frame_after)
 end
 
-@propagate_inbounds function configuration_derivative_to_velocity!(v::AbstractVector, jt::SPQuatFloating{T}, q::AbstractVector, q̇::AbstractVector) where {T}
+@propagate_inbounds function configuration_derivative_to_velocity!(v::AbstractVector, jt::SPQuatFloating,
+        q::AbstractVector, q̇::AbstractVector)
     spq = rotation(jt, q)
     spqdot = SVector(q̇[1], q̇[2], q̇[3])
     ω = angular_velocity_in_body(spq, spqdot)
@@ -107,7 +110,8 @@ end
     nothing
 end
 
-@propagate_inbounds function configuration_derivative_to_velocity_adjoint!(fq, jt::SPQuatFloating, q::AbstractVector, fv)
+@propagate_inbounds function configuration_derivative_to_velocity_adjoint!(fq, jt::SPQuatFloating,
+        q::AbstractVector, fv)
     spq = SPQuat(q[1], q[2], q[3])
     rot = velocity_jacobian(angular_velocity_in_body, spq)' * angular_velocity(jt, fv)
     trans = spq * linear_velocity(jt, fv)
@@ -116,7 +120,8 @@ end
     nothing
 end
 
-@propagate_inbounds function velocity_to_configuration_derivative!(q̇::AbstractVector, jt::SPQuatFloating, q::AbstractVector, v::AbstractVector)
+@propagate_inbounds function velocity_to_configuration_derivative!(q̇::AbstractVector, jt::SPQuatFloating,
+        q::AbstractVector, v::AbstractVector)
     spq = rotation(jt, q)
     ω = angular_velocity(jt, v)
     linear = linear_velocity(jt, v)
@@ -127,7 +132,8 @@ end
     nothing
 end
 
-@propagate_inbounds function velocity_to_configuration_derivative_jacobian(jt::SPQuatFloating, q::AbstractVector)
+@propagate_inbounds function velocity_to_configuration_derivative_jacobian(jt::SPQuatFloating,
+        q::AbstractVector)
     spq = rotation(jt, q)
     vj = velocity_jacobian(spquat_derivative, spq)
     R = RotMatrix(spq)
@@ -141,7 +147,8 @@ end
          0     0     0      R[3] R[6] R[9]])
 end
 
-@propagate_inbounds function configuration_derivative_to_velocity_jacobian(jt::SPQuatFloating, q::AbstractVector)
+@propagate_inbounds function configuration_derivative_to_velocity_jacobian(jt::SPQuatFloating,
+        q::AbstractVector)
     spq = rotation(jt, q)
     vj = velocity_jacobian(angular_velocity_in_body, spq)
     R_inv = RotMatrix(inv(spq))
@@ -169,17 +176,17 @@ end
     nothing
 end
 
-@propagate_inbounds function joint_twist(jt::SPQuatFloating{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}) where {T, X}
-    S = promote_type(T, X)
+@propagate_inbounds function joint_twist(jt::SPQuatFloating, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector)
+    S = promote_eltype(jt, q, v)
     angular = convert(SVector{3, S}, angular_velocity(jt, v))
     linear = convert(SVector{3, S}, linear_velocity(jt, v))
     Twist(frame_after, frame_before, frame_after, angular, linear)
 end
 
-@propagate_inbounds function joint_spatial_acceleration(jt::SPQuatFloating{T}, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
-        q::AbstractVector{X}, v::AbstractVector{X}, vd::AbstractVector{XD}) where {T, X, XD}
-    S = promote_type(T, X, XD)
+@propagate_inbounds function joint_spatial_acceleration(jt::SPQuatFloating, frame_after::CartesianFrame3D, frame_before::CartesianFrame3D,
+        q::AbstractVector, v::AbstractVector, vd::AbstractVector)
+    S = promote_eltype(jt, q, v, vd)
     angular = convert(SVector{3, S}, angular_velocity(jt, vd))
     linear = convert(SVector{3, S}, linear_velocity(jt, vd))
     SpatialAcceleration(frame_after, frame_before, frame_after, angular, linear)
diff --git a/src/spatial/Spatial.jl b/src/spatial/Spatial.jl
index f537d075..bd68dfa0 100644
--- a/src/spatial/Spatial.jl
+++ b/src/spatial/Spatial.jl
@@ -49,6 +49,8 @@ using StaticArrays
 using Rotations
 using DocStringExtensions
 
+using Base: promote_eltype
+
 include("frame.jl")
 include("util.jl")
 include("transform3d.jl")
diff --git a/src/spatial/motion_force_interaction.jl b/src/spatial/motion_force_interaction.jl
index fb9c46a2..91c6db05 100644
--- a/src/spatial/motion_force_interaction.jl
+++ b/src/spatial/motion_force_interaction.jl
@@ -48,8 +48,9 @@ Construct a `SpatialInertia` by specifying:
 
 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)
+@inline function SpatialInertia(frame::CartesianFrame3D, moment::AbstractMatrix, cross_part::AbstractVector, mass)
+    T = promote_eltype(moment, cross_part, mass)
+    SpatialInertia{T}(frame, moment, cross_part, mass)
 end
 
 """
@@ -263,7 +264,8 @@ end
 @inline torque!(τ::AbstractVector, jac::GeometricJacobian, wrench::Wrench) = mul!(τ, transpose(jac), wrench)
 
 @inline function torque(jac::GeometricJacobian, wrench::Wrench)
-    τ = Vector{promote_type(eltype(jac), eltype(wrench))}(undef, size(jac, 2))
+    T = promote_eltype(jac, wrench)
+    τ = Vector{T}(undef, size(jac, 2))
     torque!(τ, jac, wrench)
     τ
 end
diff --git a/src/spatial/spatialforce.jl b/src/spatial/spatialforce.jl
index 78302a2e..121945ec 100644
--- a/src/spatial/spatialforce.jl
+++ b/src/spatial/spatialforce.jl
@@ -114,8 +114,9 @@ end
 for ForceSpaceElement in (:Momentum, :Wrench)
     @eval begin
         # Construct with possibly eltype-heterogeneous inputs
-        @inline function $ForceSpaceElement(frame::CartesianFrame3D, angular::AbstractVector{T1}, linear::AbstractVector{T2}) where {T1, T2}
-            $ForceSpaceElement{promote_type(T1, T2)}(frame, angular, linear)
+        @inline function $ForceSpaceElement(frame::CartesianFrame3D, angular::AbstractVector, linear::AbstractVector)
+            T = promote_eltype(angular, linear)
+            $ForceSpaceElement{T}(frame, angular, linear)
         end
 
         """
diff --git a/src/spatial/spatialmotion.jl b/src/spatial/spatialmotion.jl
index d66d2eb9..f39a38c4 100644
--- a/src/spatial/spatialmotion.jl
+++ b/src/spatial/spatialmotion.jl
@@ -157,8 +157,9 @@ for MotionSpaceElement in (:Twist, :SpatialAcceleration)
     @eval begin
         # Construct with possibly eltype-heterogeneous inputs
         @inline function $MotionSpaceElement(body::CartesianFrame3D, base::CartesianFrame3D, frame::CartesianFrame3D,
-                angular::AbstractVector{T1}, linear::AbstractVector{T2}) where {T1, T2}
-            $MotionSpaceElement{promote_type(T1, T2)}(body, base, frame, angular, linear)
+                angular::AbstractVector, linear::AbstractVector)
+            T = promote_eltype(angular, linear)
+            $MotionSpaceElement{T}(body, base, frame, angular, linear)
         end
 
         # Construct given FreeVector3Ds
diff --git a/src/spatial/transform3d.jl b/src/spatial/transform3d.jl
index b15630bc..692e0f455 100644
--- a/src/spatial/transform3d.jl
+++ b/src/spatial/transform3d.jl
@@ -17,7 +17,7 @@ end
 Base.eltype(::Type{Transform3D{T}}) where {T} = T
 
 @inline function Transform3D(from::CartesianFrame3D, to::CartesianFrame3D, rot::Rotation{3}, trans::SVector{3})
-    T = promote_type(eltype(typeof(rot)), eltype(typeof(trans)))
+    T = promote_eltype(rot, trans)
     R = convert(RotMatrix3{T}, rot)
     @inbounds mat = @SMatrix [R[1] R[4] R[7] trans[1];
                               R[2] R[5] R[8] trans[2];
diff --git a/src/spatial/util.jl b/src/spatial/util.jl
index 47d37f34..0276652c 100644
--- a/src/spatial/util.jl
+++ b/src/spatial/util.jl
@@ -25,7 +25,7 @@ colwise(f, A::AbstractMatrix, b::AbstractVector) = mapslices(x -> f(x, b), A, di
 end
 
 @inline function _colwise(f, ::Val{0}, a::StaticVector, B::StaticMatrix)
-    zero(similar_type(B, promote_type(eltype(a), eltype(B))))
+    zero(similar_type(B, promote_eltype(a, B)))
 end
 
 @inline function _colwise(f, M::Val, a::StaticVector, B::StaticMatrix)
@@ -41,7 +41,7 @@ end
 end
 
 @inline function _colwise(f, ::Val{0}, A::StaticMatrix, b::StaticVector)
-    zero(similar_type(A, promote_type(eltype(A), eltype(b))))
+    zero(similar_type(A, promote_eltype(A, b)))
 end
 
 @inline function _colwise(f, M::Val, A::StaticMatrix, b::StaticVector)