Use promote_eltype instead of promote_type.

Ref: #570.
JuliaRobotics · Sep 19, 2019 · cb1977f · cb1977f
1 parent 1f6986b
commit cb1977f
Show file tree

Hide file tree

Showing 16 changed files with 167 additions and 140 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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)