Merge pull request #215 from tkoolen/transform-array-param

Use a single Array in Transform3D, parameterize Transform3D on array type

notebooks/Quickstart - double pendulum.ipynb

@@ -193,7 +193,7 @@
     }
    ],
    "source": [
-    "beforeShoulderToWorld = Transform3D{Float64}(frame_before(shoulder), default_frame(world)) # creates an identity transform"
+    "beforeShoulderToWorld = eye(Transform3D, frame_before(shoulder), default_frame(world))"
    ]
   },
   {

notebooks/Symbolic double pendulum.ipynb

@@ -118,7 +118,7 @@
     "inertia1 = SpatialInertia(CartesianFrame3D(\"upper_link\"), I_1 * axis * axis', SVector(0, 0, c_1), m_1)\n",
     "body1 = RigidBody(inertia1)\n",
     "joint1 = Joint(\"shoulder\", Revolute(axis))\n",
-    "joint1ToWorld = Transform3D{T}(frame_before(joint1), default_frame(world))\n",
+    "joint1ToWorld = eye(Transform3D, frame_before(joint1), default_frame(world))\n",
     "attach!(doublePendulum, world, joint1, joint1ToWorld, body1)\n",
     "\n",
     "# Attach the second (lower) link to the world via a revolute joint named 'elbow'\n",

src/RigidBodyDynamics.jl

@@ -37,6 +37,8 @@ export
     MechanismState,
     DynamicsResult,
     # functions
+    rotation,
+    translation,
     name, # TODO: remove?
     has_defined_inertia,
     default_frame,

src/frames.jl

@@ -69,66 +69,99 @@ $(TYPEDEF)
 A homogeneous transformation matrix representing the transformation from one
 three-dimensional Cartesian coordinate system to another.
 """
-immutable Transform3D{T<:Number}
+immutable Transform3D{A<:AbstractMatrix}
     from::CartesianFrame3D
     to::CartesianFrame3D
-    rot::RotMatrix3{T}
-    trans::SVector{3, T}
+    mat::A
 
-    function (::Type{Transform3D{T}}){T<:Number}(from::CartesianFrame3D, to::CartesianFrame3D, rot::Rotation{3, T}, trans::SVector{3, T})
-        new{T}(from, to, rot, trans)
+    function (::Type{Transform3D{A}}){A<:AbstractArray}(from::CartesianFrame3D, to::CartesianFrame3D, mat::A)
+        @boundscheck size(mat) == (4, 4) || throw(DimensionMismatch())
+        new{A}(from, to, mat)
     end
+end
 
-    function (::Type{Transform3D{T}}){T<:Number}(from::CartesianFrame3D, to::CartesianFrame3D)
-        new{T}(from, to, eye(RotMatrix3{T}), zeros(SVector{3, T}))
-    end
+@inline Transform3D{A}(from::CartesianFrame3D, to::CartesianFrame3D, mat::A) = Transform3D{A}(from, to, mat)
 
-    function (::Type{Transform3D{T}}){T<:Number}(frame::CartesianFrame3D)
-        new{T}(frame, frame, eye(RotMatrix3{T}), zeros(SVector{3, T}))
-    end
+Base.eltype{A}(::Type{Transform3D{A}}) = eltype(A)
+@compat const Transform3DS{T} = Transform3D{SMatrix{4, 4, T, 16}}
+
+@inline function Transform3D(from::CartesianFrame3D, to::CartesianFrame3D, rot::Rotation{3}, trans::SVector{3})
+    T = promote_type(eltype(typeof(rot)), eltype(typeof(trans)))
+    @inbounds mat = @SMatrix [rot[1] rot[4] rot[7] trans[1];
+                              rot[2] rot[5] rot[8] trans[2];
+                              rot[3] rot[6] rot[9] trans[3];
+                              zero(T) zero(T) zero(T) one(T)]
+   Transform3D(from, to, mat)
 end
-Transform3D{T}(from::CartesianFrame3D, to::CartesianFrame3D, rot::Rotation{3, T}, trans::SVector{3, T}) = Transform3D{T}(from, to, rot, trans)
-Transform3D{T}(from::CartesianFrame3D, to::CartesianFrame3D, rot::Rotation{3, T}) = Transform3D{T}(from, to, rot, zeros(SVector{3, T}))
-Transform3D{T}(from::CartesianFrame3D, to::CartesianFrame3D, trans::SVector{3, T}) = Transform3D{T}(from, to, eye(RotMatrix3{T}), trans)
-Transform3D{T}(::Type{T}, from::CartesianFrame3D, to::CartesianFrame3D) = Transform3D{T}(from, to, eye(RotMatrix3{T}), zeros(SVector{3, T}))
-Transform3D{T}(::Type{T}, frame::CartesianFrame3D) = Transform3D{T}(frame, frame, eye(RotMatrix3{T}), zeros(SVector{3, T}))
 
-Base.convert{T}(::Type{Transform3D{T}}, t::Transform3D{T}) = t
-Base.convert{T}(::Type{Transform3D{T}}, t::Transform3D) = Transform3D(t.from, t.to, convert(RotMatrix3{T}, t.rot), convert(SVector{3, T}, t.trans))
+@inline function Transform3D{T}(from::CartesianFrame3D, to::CartesianFrame3D, rot::Rotation{3, T})
+    @inbounds mat = @SMatrix [rot[1] rot[4] rot[7] zero(T);
+                              rot[2] rot[5] rot[8] zero(T);
+                              rot[3] rot[6] rot[9] zero(T);
+                              zero(T) zero(T) zero(T) one(T)]
+   Transform3D(from, to, mat)
+end
+
+@inline function Transform3D{T}(from::CartesianFrame3D, to::CartesianFrame3D, trans::SVector{3, T})
+    @inbounds mat = @SMatrix [one(T) zero(T) zero(T) trans[1];
+                              zero(T) one(T) zero(T) trans[2];
+                              zero(T) zero(T) one(T) trans[3];
+                              zero(T) zero(T) zero(T) one(T)]
+    Transform3D(from, to, mat)
+end
+
+@inline Base.convert{A}(::Type{Transform3D{A}}, t::Transform3D{A}) = t
+@inline Base.convert{A}(::Type{Transform3D{A}}, t::Transform3D) = Transform3D(t.from, t.to, convert(A, t.mat))
+
+@inline rotation(t::Transform3D) = @inbounds return RotMatrix(t.mat[1], t.mat[2], t.mat[3], t.mat[5], t.mat[6], t.mat[7], t.mat[9], t.mat[10], t.mat[11])
+@inline translation(t::Transform3D) = @inbounds return SVector(t.mat[13], t.mat[14], t.mat[15])
 
 function Base.show(io::IO, t::Transform3D)
     println(io, "Transform3D from \"$(name(t.from))\" to \"$(name(t.to))\":")
-    angleAxis = AngleAxis(t.rot)
+    angleAxis = AngleAxis(rotation(t))
     angle = rotation_angle(angleAxis)
     axis = rotation_axis(angleAxis)
-    println(io, "rotation: $(angle) rad about $(axis), translation: $(t.trans)") # TODO: use fixed Quaternions.jl version once it's updated
+    println(io, "rotation: $(angle) rad about $(axis), translation: $(translation(t))") # TODO: use fixed Quaternions.jl version once it's updated
 end
 
 @inline function *(t1::Transform3D, t2::Transform3D)
     @framecheck(t1.from, t2.to)
-    rot = t1.rot * t2.rot
-    trans = t1.trans + t1.rot * t2.trans
-    Transform3D(t2.from, t1.to, rot, trans)
+    mat = t1.mat * t2.mat
+    Transform3D(t2.from, t1.to, mat)
 end
 
 @inline function Base.inv(t::Transform3D)
-    rotinv = inv(t.rot)
-    Transform3D(t.to, t.from, rotinv, -(rotinv * t.trans))
+    rotinv = inv(rotation(t))
+    Transform3D(t.to, t.from, rotinv, -(rotinv * translation(t)))
 end
 
-function Random.rand{T}(::Type{Transform3D{T}}, from::CartesianFrame3D, to::CartesianFrame3D)
-    Transform3D(from, to, rand(RotMatrix3{T}), rand(SVector{3, T}))
+@inline Base.eye{A<:StaticArray}(::Type{Transform3D{A}}, from::CartesianFrame3D, to::CartesianFrame3D) = Transform3D(from, to, eye(A))
+@inline function Base.eye{A<:AbstractMatrix}(::Type{Transform3D{A}}, from::CartesianFrame3D, to::CartesianFrame3D)
+    T = eltype(A)
+    convert(Transform3D{A}, eye(Transform3DS{T}, from, to))
 end
+@inline Base.eye(::Type{Transform3D}, from::CartesianFrame3D, to::CartesianFrame3D) = eye(Transform3DS{Float64}, from, to)
+@inline Base.eye{T<:Transform3D}(::Type{T}, frame::CartesianFrame3D) = eye(T, frame, frame)
+
+function Random.rand{A}(::Type{Transform3D{A}}, from::CartesianFrame3D, to::CartesianFrame3D)
+    T = eltype(A)
+    rot = rand(RotMatrix3{T})
+    trans = rand(SVector{3, T})
+    convert(Transform3D{A}, Transform3D(from, to, rot, trans))
+end
+
+Random.rand(::Type{Transform3D}, from::CartesianFrame3D, to::CartesianFrame3D) = rand(Transform3DS{Float64}, from, to)
 
-function Base.isapprox{T}(x::Transform3D{T}, y::Transform3D{T}; atol::Real = 1e-12)
-    x.from == y.from && x.to == y.to && isapprox(x.rot, y.rot, atol = atol) && isapprox(x.trans, y.trans, atol = atol)
+function Base.isapprox(x::Transform3D, y::Transform3D; atol::Real = 1e-12)
+    x.from == y.from && x.to == y.to && isapprox(rotation(x), rotation(y), atol = atol) && isapprox(translation(x), translation(y), atol = atol)
 end
 
 
 # Point3D, FreeVector3D. The difference is that a FreeVector3D is only rotated when its frame is changed,
 # whereas a Point3D is also translated
 for VectorType in (:FreeVector3D, :Point3D)
     @eval begin
+        # TODO: consider storing as a homogeneous vector
         immutable $VectorType{V<:AbstractVector}
             frame::CartesianFrame3D
             v::V
@@ -173,7 +206,6 @@ for VectorType in (:FreeVector3D, :Point3D)
         invtransform(x::$VectorType, t::Transform3D) = t \ x
 
         Base.eltype{V}(::Type{$VectorType{V}}) = eltype(V)
-        Base.eltype(v::$VectorType) = eltype(typeof(v))
         StaticArrays.similar_type{V, T}(x::Type{$VectorType{V}}, ::Type{T}) = $VectorType{SVector{3, T}}
     end
 end
@@ -202,16 +234,16 @@ FreeVector3D
 
 # Point3D-specific
 (-)(p1::Point3D, p2::Point3D) = begin @framecheck(p1.frame, p2.frame); FreeVector3D(p1.frame, p1.v - p2.v) end
-(*)(t::Transform3D, point::Point3D) = begin @framecheck(t.from, point.frame); Point3D(t.to, t.rot * point.v + t.trans) end
-(\)(t::Transform3D, point::Point3D) = begin @framecheck point.frame t.to; Point3D(t.from, At_mul_B(t.rot, point.v - t.trans)) end
+(*)(t::Transform3D, point::Point3D) = begin @framecheck(t.from, point.frame); Point3D(t.to, rotation(t) * point.v + translation(t)) end
+(\)(t::Transform3D, point::Point3D) = begin @framecheck point.frame t.to; Point3D(t.from, At_mul_B(rotation(t), point.v - translation(t))) end
 
 # FreeVector3D-specific
 FreeVector3D(p::Point3D) = FreeVector3D(p.frame, p.v)
 (-)(v1::FreeVector3D, v2::FreeVector3D) = begin @framecheck(v1.frame, v2.frame); FreeVector3D(v1.frame, v1.v - v2.v) end
 Base.cross(v1::FreeVector3D, v2::FreeVector3D) = begin @framecheck(v1.frame, v2.frame); FreeVector3D(v1.frame, cross(v1.v, v2.v)) end
 Base.dot(v1::FreeVector3D, v2::FreeVector3D) = begin @framecheck(v1.frame, v2.frame); dot(v1.v, v2.v) end
-(*)(t::Transform3D, vector::FreeVector3D) = begin @framecheck(t.from, vector.frame); FreeVector3D(t.to, t.rot * vector.v) end
-(\)(t::Transform3D, point::FreeVector3D) = begin @framecheck point.frame t.to; FreeVector3D(t.from, At_mul_B(t.rot, point.v)) end
+(*)(t::Transform3D, vector::FreeVector3D) = begin @framecheck(t.from, vector.frame); FreeVector3D(t.to, rotation(t) * vector.v) end
+(\)(t::Transform3D, point::FreeVector3D) = begin @framecheck point.frame t.to; FreeVector3D(t.from, At_mul_B(rotation(t), point.v)) end
 Base.norm(v::FreeVector3D) = norm(v.v)
 Base.normalize(v::FreeVector3D, p = 2) = FreeVector3D(v.frame, normalize(v.v, p))
 

src/joint.jl

@@ -101,7 +101,7 @@ $(SIGNATURES)
 Return a `Transform3D` representing the homogeneous transform from the frame
 after the joint to the frame before the joint for joint configuration vector ``q``.
 """
-function joint_transform{M, X}(joint::Joint{M}, q::AbstractVector{X})::Transform3D{promote_type(M, X)}
+function joint_transform{M, X}(joint::Joint{M}, q::AbstractVector{X})::Transform3DS{promote_type(M, X)}
     @boundscheck check_num_positions(joint, q)
     @rtti_dispatch (QuaternionFloating{M}, Revolute{M}, Prismatic{M}, Fixed{M}) _joint_transform(joint.jointType, frame_after(joint), frame_before(joint), q)
 end
@@ -135,7 +135,7 @@ its successor.
 
 The constraint wrench subspace is orthogonal to the motion subspace.
 """
-function constraint_wrench_subspace{M, X}(joint::Joint{M}, jointTransform::Transform3D{X})#::WrenchSubspace{promote_type(M, X)} # FIXME: type assertion causes segfault! see https://github.com/JuliaLang/julia/issues/20034. should be fixed in 0.6
+function constraint_wrench_subspace{M, A}(joint::Joint{M}, jointTransform::Transform3D{A})#::WrenchSubspace{promote_type(M, X)} # FIXME: type assertion causes segfault! see https://github.com/JuliaLang/julia/issues/20034. should be fixed in 0.6
     @framecheck jointTransform.from frame_after(joint)
     @framecheck jointTransform.to frame_before(joint)
     @rtti_dispatch (QuaternionFloating{M}, Revolute{M}, Prismatic{M}, Fixed{M}) _constraint_wrench_subspace(joint.jointType, jointTransform)

src/joint_types.jl

@@ -72,10 +72,7 @@ end
 
 function _joint_transform(
         jt::QuaternionFloating, frameAfter::CartesianFrame3D, frameBefore::CartesianFrame3D, q::AbstractVector)
-    S = promote_type(eltype(jt), eltype(q))
-    rot = convert(Quat{S}, rotation(jt, q))
-    trans = convert(SVector{3, S}, translation(jt, q))
-    Transform3D{S}(frameAfter, frameBefore, rot, trans)
+    Transform3D(frameAfter, frameBefore, rotation(jt, q), translation(jt, q))
 end
 
 function _motion_subspace{T<:Number, X<:Number}(
@@ -86,8 +83,8 @@ function _motion_subspace{T<:Number, X<:Number}(
     MotionSubspace(frameAfter, frameBefore, frameAfter, angular, linear)
 end
 
-function _constraint_wrench_subspace{T<:Number, X<:Number}(jt::QuaternionFloating{T}, jointTransform::Transform3D{X})
-    S = promote_type(T, X)
+function _constraint_wrench_subspace{T<:Number, A<:AbstractMatrix}(jt::QuaternionFloating{T}, jointTransform::Transform3D{A})
+    S = promote_type(eltype(typeof((jt))), eltype(typeof(jointTransform)))
     WrenchSubspace(jointTransform.from, zeros(SMatrix{3, 0, S}), zeros(SMatrix{3, 0, S}))
 end
 
@@ -188,8 +185,8 @@ function _global_coordinates!(jt::QuaternionFloating, q::AbstractVector, q0::Abs
     ξ = Twist(frameAfter, frame0, frame0, ξrot, ξtrans)
     relative_transform = exp(ξ)
     t = t0 * relative_transform
-    rotation!(jt, q, t.rot)
-    translation!(jt, q, t.trans)
+    rotation!(jt, q, rotation(t))
+    translation!(jt, q, translation(t))
     nothing
 end
 
@@ -279,8 +276,8 @@ function _motion_subspace{T<:Number, X<:Number}(
     MotionSubspace(frameAfter, frameBefore, frameAfter, angular, linear)
 end
 
-function _constraint_wrench_subspace{T<:Number, X<:Number}(jt::Prismatic{T}, jointTransform::Transform3D{X})
-    S = promote_type(T, X)
+function _constraint_wrench_subspace{T<:Number, A<:AbstractMatrix}(jt::Prismatic{T}, jointTransform::Transform3D{A})
+    S = promote_type(eltype(typeof((jt))), eltype(typeof(jointTransform)))
     R = convert(RotMatrix{3, S}, jt.rotationFromZAligned)
     angular = hcat(R, zeros(SMatrix{3, 2, S}))
     linear = hcat(zeros(SMatrix{3, 3, S}), R[:, (1, 2)])
@@ -339,8 +336,8 @@ function _motion_subspace{T<:Number, X<:Number}(
     MotionSubspace(frameAfter, frameBefore, frameAfter, angular, linear)
 end
 
-function _constraint_wrench_subspace{T<:Number, X<:Number}(jt::Revolute{T}, jointTransform::Transform3D{X})
-    S = promote_type(T, X)
+function _constraint_wrench_subspace{T<:Number, A<:AbstractMatrix}(jt::Revolute{T}, jointTransform::Transform3D{A})
+    S = promote_type(eltype(typeof((jt))), eltype(typeof(jointTransform)))
     R = convert(RotMatrix{3, S}, jt.rotationFromZAligned)
     angular = hcat(R[:, (1, 2)], zeros(SMatrix{3, 3, S}))
     linear = hcat(zeros(SMatrix{3, 2, S}), R)
@@ -369,7 +366,7 @@ num_velocities(::Fixed) = 0
 
 function _joint_transform{T<:Number, X<:Number}(
         jt::Fixed{T}, frameAfter::CartesianFrame3D, frameBefore::CartesianFrame3D, q::AbstractVector{X})
-    Transform3D(promote_type(T, X), frameAfter, frameBefore)
+    eye(Transform3DS{promote_type(T, X)}, frameAfter, frameBefore)
 end
 
 function _joint_twist{T<:Number, X<:Number}(
@@ -383,8 +380,8 @@ function _motion_subspace{T<:Number, X<:Number}(
     MotionSubspace(frameAfter, frameBefore, frameAfter, zeros(SMatrix{3, 0, S}), zeros(SMatrix{3, 0, S}))
 end
 
-function _constraint_wrench_subspace{T<:Number, X<:Number}(jt::Fixed{T}, jointTransform::Transform3D{X})
-    S = promote_type(T, X)
+function _constraint_wrench_subspace{T<:Number, A<:AbstractMatrix}(jt::Fixed{T}, jointTransform::Transform3D{A})
+    S = promote_type(eltype(typeof((jt))), eltype(typeof(jointTransform)))
     angular = hcat(eye(SMatrix{3, 3, S}), zeros(SMatrix{3, 3, S}))
     linear = hcat(zeros(SMatrix{3, 3, S}), eye(SMatrix{3, 3, S}))
     WrenchSubspace(jointTransform.from, angular, linear)

src/mechanism.jl

@@ -227,8 +227,8 @@ Return the joint that is part of the mechanism's kinematic tree and has
 joint_to_parent(body::RigidBody, mechanism::Mechanism) = edge_to_parent(body, mechanism.tree)
 
 
-Base.@deprecate add_body_fixed_frame!{T}(mechanism::Mechanism{T}, body::RigidBody{T}, transform::Transform3D{T}) add_frame!(body, transform)
-function add_body_fixed_frame!{T}(mechanism::Mechanism{T}, transform::Transform3D{T})
+Base.@deprecate add_body_fixed_frame!{T}(mechanism::Mechanism{T}, body::RigidBody{T}, transform::Transform3D) add_frame!(body, transform)
+function add_body_fixed_frame!{T}(mechanism::Mechanism{T}, transform::Transform3D)
     add_frame!(body_fixed_frame_to_body(mechanism, transform.to), transform)
 end
 

src/mechanism_manipulation.jl

@@ -18,8 +18,8 @@ If `successor` is not yet a part of the `Mechanism`, it will be added to the
 `Mechanism`, effectively creating a loop constraint that will be enforced
 using Lagrange multipliers (as opposed to using recursive algorithms).
 """
-function attach!{T}(mechanism::Mechanism{T}, predecessor::RigidBody{T}, joint::Joint, jointToPredecessor::Transform3D{T},
-        successor::RigidBody{T}, successorToJoint::Transform3D{T} = Transform3D{T}(default_frame(successor), frame_after(joint)))
+function attach!{T}(mechanism::Mechanism{T}, predecessor::RigidBody{T}, joint::Joint{T}, jointToPredecessor::Transform3D,
+        successor::RigidBody{T}, successorToJoint::Transform3D = eye(Transform3DS{T}, default_frame(successor), frame_after(joint)))
     @assert jointToPredecessor.from == frame_before(joint)
     @assert successorToJoint.to == frame_after(joint)
 
@@ -64,7 +64,7 @@ belongs to `mechanism`).
 Note: gravitational acceleration for childmechanism is ignored.
 """
 function attach!{T}(mechanism::Mechanism{T}, parentbody::RigidBody{T}, childmechanism::Mechanism{T},
-        childroot_to_parent::Transform3D{T} = Transform3D(T, default_frame(root_body(childmechanism)), default_frame(parentbody)))
+        childroot_to_parent::Transform3D = eye(Transform3DS{T}, default_frame(root_body(childmechanism)), default_frame(parentbody)))
     # FIXME: test with cycles
 
     @assert mechanism != childmechanism # infinite loop otherwise
@@ -151,8 +151,8 @@ end
 
 Base.@deprecate(
 reattach!{T}(mechanism::Mechanism{T}, oldSubtreeRootBody::RigidBody{T},
-    parentBody::RigidBody{T}, joint::Joint, jointToParent::Transform3D{T},
-    newSubtreeRootBody::RigidBody{T}, newSubTreeRootBodyToJoint::Transform3D{T} = Transform3D{T}(default_frame(newSubtreeRootBody), frame_after(joint))),
+    parentBody::RigidBody{T}, joint::Joint, jointToParent::Transform3D,
+    newSubtreeRootBody::RigidBody{T}, newSubTreeRootBodyToJoint::Transform3D = eye(Transform3DS{T}, default_frame(newSubtreeRootBody), frame_after(joint))),
     begin
         attach!(mechanism, parentBody, joint, jointToParent, newSubtreeRootBody, newSubTreeRootBodyToJoint)
         remove_joint!(mechanism, joint_to_parent(oldSubtreeRootBody, mechanism))
@@ -180,7 +180,7 @@ function remove_fixed_tree_joints!(mechanism::Mechanism)
         succ = target(fixedjoint, graph)
 
         # Add identity joint transform as a body-fixed frame definition.
-        jointtransform = Transform3D{T}(frame_after(fixedjoint), frame_before(fixedjoint))
+        jointtransform = eye(Transform3DS{T}, frame_after(fixedjoint), frame_before(fixedjoint))
         add_frame!(pred, jointtransform)
 
         # Migrate body fixed frames to parent body.
@@ -251,7 +251,7 @@ function maximal_coordinates(mechanism::Mechanism)
         frameafter = default_frame(srcbody)
         body = bodymap[srcbody] = deepcopy(srcbody)
         floatingjoint = newfloatingjoints[body] = Joint(name(body), framebefore, frameafter, QuaternionFloating{T}())
-        attach!(ret, root, floatingjoint, Transform3D(T, framebefore), body, Transform3D(T, frameafter))
+        attach!(ret, root, floatingjoint, eye(Transform3DS{T}, framebefore), body, eye(Transform3DS{T}, frameafter))
     end
 
     # Copy input Mechanism's joints.
@@ -275,9 +275,9 @@ function rand_tree_mechanism{T}(::Type{T}, parentselector::Function, jointTypes.
     for i = 1 : length(jointTypes)
         @assert jointTypes[i] <: JointType{T}
         joint = Joint("joint$i", rand(jointTypes[i]))
-        jointToParentBody = rand(Transform3D{T}, frame_before(joint), default_frame(parentbody))
+        jointToParentBody = rand(Transform3DS{T}, frame_before(joint), default_frame(parentbody))
         body = RigidBody(rand(SpatialInertia{T}, CartesianFrame3D("body$i")))
-        body_to_joint = Transform3D{T}(default_frame(body), frame_after(joint))
+        body_to_joint = eye(Transform3DS{T}, default_frame(body), frame_after(joint))
         attach!(mechanism, parentbody, joint, jointToParentBody, body, body_to_joint)
         parentbody = parentselector(mechanism)
     end