diff --git a/REQUIRE b/REQUIRE index b8b413e0..1cab2ed8 100644 --- a/REQUIRE +++ b/REQUIRE @@ -4,3 +4,4 @@ Rotations 0.3.4 DataStructures 0.4.6 LightXML 0.4.0 DocStringExtensions 0.3.1 +Compat 0.19.0 diff --git a/src/RigidBodyDynamics.jl b/src/RigidBodyDynamics.jl index d89e8569..1d31f0c0 100644 --- a/src/RigidBodyDynamics.jl +++ b/src/RigidBodyDynamics.jl @@ -10,12 +10,11 @@ using StaticArrays using Rotations using LightXML using DocStringExtensions +using Compat const noalloc_doc = """This method does its computation in place, performing no dynamic memory allocation.""" -if isdefined(Base, :Iterators) - import Base.Iterators: filter -end +import Compat.Iterators: filter include("util.jl") include("third_party_addendum.jl") diff --git a/src/cache_element.jl b/src/cache_element.jl index 472aa217..cbd9a1f3 100644 --- a/src/cache_element.jl +++ b/src/cache_element.jl @@ -1,7 +1,7 @@ type CacheElement{T} dirty::Bool data::T - CacheElement() = new(true) + (::Type{CacheElement{T}}){T}() = new{T}(true) end @inline function update!{T}(element::CacheElement{T}, data::T) diff --git a/src/dynamics_result.jl b/src/dynamics_result.jl index 001fe46c..d538fb8b 100644 --- a/src/dynamics_result.jl +++ b/src/dynamics_result.jl @@ -23,7 +23,7 @@ type DynamicsResult{M<:Number, T<:Number} z::Vector{T} Y::Matrix{T} - function DynamicsResult(mechanism::Mechanism{M}) + function (::Type{DynamicsResult{M, T}}){M<:Number, T<:Number}(mechanism::Mechanism{M}) nq = num_positions(mechanism) nv = num_velocities(mechanism) nconstraints = isempty(mechanism.nonTreeEdges)? 0 : sum(num_constraints, edge.joint for edge in mechanism.nonTreeEdges) @@ -43,7 +43,7 @@ type DynamicsResult{M<:Number, T<:Number} z = Vector{T}(nv) Y = Matrix{T}(nconstraints, nv) - new(massMatrix, dynamicsBias, constraintJacobian, constraintBias, v̇, λ, accelerations, jointWrenches, L, A, z, Y) + new{M, T}(massMatrix, dynamicsBias, constraintJacobian, constraintBias, v̇, λ, accelerations, jointWrenches, L, A, z, Y) end end diff --git a/src/frames.jl b/src/frames.jl index f5c39868..47f7b8b5 100644 --- a/src/frames.jl +++ b/src/frames.jl @@ -35,7 +35,7 @@ immutable CartesianFrame3D end name(frame::CartesianFrame3D) = get(frame_names, frame.id, "anonymous") -show(io::IO, frame::CartesianFrame3D) = print(io, "CartesianFrame3D: \"$(name(frame))\" (id = $(frame.id))") +Base.show(io::IO, frame::CartesianFrame3D) = print(io, "CartesianFrame3D: \"$(name(frame))\" (id = $(frame.id))") """ $(SIGNATURES) @@ -75,9 +75,17 @@ immutable Transform3D{T<:Number} rot::RotMatrix3{T} trans::SVector{3, T} - Transform3D(from::CartesianFrame3D, to::CartesianFrame3D, rot::Rotation{3, T}, trans::SVector{3, T}) = new(from, to, rot, trans) - Transform3D(from::CartesianFrame3D, to::CartesianFrame3D) = new(from, to, eye(RotMatrix3{T}), zeros(SVector{3, T})) - Transform3D(frame::CartesianFrame3D) = new(frame, frame, eye(RotMatrix3{T}), zeros(SVector{3, T})) + function (::Type{Transform3D{T}}){T<:Number}(from::CartesianFrame3D, to::CartesianFrame3D, rot::Rotation{3, T}, trans::SVector{3, T}) + new{T}(from, to, rot, trans) + end + + function (::Type{Transform3D{T}}){T<:Number}(from::CartesianFrame3D, to::CartesianFrame3D) + new{T}(from, to, eye(RotMatrix3{T}), zeros(SVector{3, T})) + end + + function (::Type{Transform3D{T}}){T<:Number}(frame::CartesianFrame3D) + new{T}(frame, frame, eye(RotMatrix3{T}), zeros(SVector{3, T})) + end 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})) @@ -88,7 +96,7 @@ Transform3D{T}(::Type{T}, frame::CartesianFrame3D) = Transform3D{T}(frame, frame 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)) -function show(io::IO, t::Transform3D) +function Base.show(io::IO, t::Transform3D) println(io, "Transform3D from \"$(name(t.from))\" to \"$(name(t.to))\":") angleAxis = AngleAxis(t.rot) angle = rotation_angle(angleAxis) @@ -121,11 +129,14 @@ end # whereas a Point3D is also translated for VectorType in (:FreeVector3D, :Point3D) @eval begin - type $VectorType{V <:AbstractVector} + type $VectorType{V<:AbstractVector} frame::CartesianFrame3D v::V - $VectorType(frame::CartesianFrame3D, v::V) = begin @boundscheck length(v) == 3; new(frame, v) end + function (::Type{$VectorType{V}}){V<:AbstractVector}(frame::CartesianFrame3D, v::V) + @boundscheck length(v) == 3 + new{V}(frame, v) + end end $VectorType{V}(frame::CartesianFrame3D, v::V) = $VectorType{V}(frame, v) @@ -139,7 +150,7 @@ for VectorType in (:FreeVector3D, :Point3D) (*){S<:Number}(s::S, p::$VectorType) = $VectorType(p.frame, s * p.v) Random.rand{T}(::Type{$VectorType}, ::Type{T}, frame::CartesianFrame3D) = $VectorType(frame, rand(SVector{3, T})) - Base.show(io::IO, p::$VectorType) = print(io, "$($(VectorType).name.name) in \"$(name(p.frame))\": $(p.v)") + Base.show(io::IO, p::$VectorType) = print(io, "$($(string(VectorType))) in \"$(name(p.frame))\": $(p.v)") Base.isapprox(x::$VectorType, y::$VectorType; atol::Real = 1e-12) = x.frame == y.frame && isapprox(x.v, y.v; atol = atol) """ diff --git a/src/joint_types.jl b/src/joint_types.jl index 2fe83549..d57e8461 100644 --- a/src/joint_types.jl +++ b/src/joint_types.jl @@ -1,6 +1,6 @@ # TODO: put in separate module -abstract JointType{T<:Number} +@compat abstract type JointType{T<:Number} end Base.eltype{T}(::Union{JointType{T}, Type{JointType{T}}}) = T # Default implementations @@ -198,7 +198,7 @@ end #= OneDegreeOfFreedomFixedAxis =# -abstract OneDegreeOfFreedomFixedAxis{T<:Number} <: JointType{T} +@compat abstract type OneDegreeOfFreedomFixedAxis{T<:Number} <: JointType{T} end num_positions(::OneDegreeOfFreedomFixedAxis) = 1 num_velocities(::OneDegreeOfFreedomFixedAxis) = 1 @@ -240,7 +240,7 @@ immutable Prismatic{T<:Number} <: OneDegreeOfFreedomFixedAxis{T} axis::SVector{3, T} rotationFromZAligned::RotMatrix{3, T} - Prismatic(axis::SVector{3, T}) = new(axis, rotation_between(SVector(zero(T), zero(T), one(T)), axis)) + (::Type{Prismatic{T}}){T<:Number}(axis::SVector{3, T}) = new{T}(axis, rotation_between(SVector(zero(T), zero(T), one(T)), axis)) end """ @@ -302,7 +302,7 @@ immutable Revolute{T<:Number} <: OneDegreeOfFreedomFixedAxis{T} axis::SVector{3, T} rotationFromZAligned::RotMatrix{3, T} - Revolute(axis::SVector{3, T}) = new(axis, rotation_between(SVector(zero(T), zero(T), one(T)), axis)) + (::Type{Revolute{T}}){T<:Number}(axis::SVector{3, T}) = new{T}(axis, rotation_between(SVector(zero(T), zero(T), one(T)), axis)) end """ diff --git a/src/mechanism.jl b/src/mechanism.jl index a72a9e77..8ea0c223 100644 --- a/src/mechanism.jl +++ b/src/mechanism.jl @@ -16,11 +16,11 @@ type Mechanism{T<:Number} nonTreeEdges::Vector{NonTreeEdge{T}} gravitationalAcceleration::FreeVector3D{SVector{3, T}} # TODO: consider removing - function Mechanism(rootBody::RigidBody{T}; gravity::SVector{3, T} = SVector(zero(T), zero(T), T(-9.81))) + function (::Type{Mechanism{T}}){T<:Number}(rootBody::RigidBody{T}; gravity::SVector{3, T} = SVector(zero(T), zero(T), T(-9.81))) tree = Tree{RigidBody{T}, Joint{T}}(rootBody) nonTreeEdges = Vector{NonTreeEdge{T}}() gravitationalAcceleration = FreeVector3D(default_frame(rootBody), gravity) - new(toposort(tree), nonTreeEdges, gravitationalAcceleration) + new{T}(toposort(tree), nonTreeEdges, gravitationalAcceleration) end end diff --git a/src/mechanism_state.jl b/src/mechanism_state.jl index 8ee4fdc1..fb6ee291 100644 --- a/src/mechanism_state.jl +++ b/src/mechanism_state.jl @@ -8,12 +8,12 @@ immutable JointState{X<:Number, M<:Number, C<:Number} biasAcceleration::CacheElement{SpatialAcceleration{C}} motionSubspace::CacheElement{MotionSubspace{C}} - function JointState(joint::Joint{M}, q::VectorSegment{X}, v::VectorSegment{X}) + function (::Type{JointState{X, M, C}}){X<:Number, M<:Number, C<:Number}(joint::Joint{M}, q::VectorSegment{X}, v::VectorSegment{X}) jointTransform = CacheElement{Transform3D{C}}() twist = CacheElement{Twist{C}}() biasAcceleration = CacheElement{SpatialAcceleration{C}}() motionSubspace = CacheElement{MotionSubspace{C}}() - new(joint, q, v, jointTransform, twist, biasAcceleration, motionSubspace) + new{X, M, C}(joint, q, v, jointTransform, twist, biasAcceleration, motionSubspace) end end JointState{X, M}(joint::Joint{M}, q::VectorSegment{X}, v::VectorSegment{X}) = JointState{X, M, promote_type(M, X)}(joint, q, v) @@ -46,7 +46,7 @@ immutable RigidBodyState{M<:Number, C<:Number} inertia::CacheElement{SpatialInertia{C}} crbInertia::CacheElement{SpatialInertia{C}} - function RigidBodyState(body::RigidBody{M}, isroot::Bool) + function (::Type{RigidBodyState{M, C}}){M<:Number, C<:Number}(body::RigidBody{M}, isroot::Bool) transformToWorld = CacheElement{Transform3D{C}}() twist = CacheElement{Twist{C}}() biasAcceleration = CacheElement{SpatialAcceleration{C}}() @@ -61,7 +61,7 @@ immutable RigidBodyState{M<:Number, C<:Number} update!(biasAcceleration, zero(SpatialAcceleration{C}, frame, frame, frame)) end - new(body, transformToWorld, twist, biasAcceleration, motionSubspace, inertia, crbInertia) + new{M, C}(body, transformToWorld, twist, biasAcceleration, motionSubspace, inertia, crbInertia) end end @@ -97,7 +97,7 @@ immutable MechanismState{X<:Number, M<:Number, C<:Number} nonRootTopoSortedStateVertices::VectorSegment{TreeVertex{RigidBodyState{M, C}, JointState{X, M, C}}} # because of https://github.com/JuliaLang/julia/issues/14955 constraintJacobianStructure::SparseMatrixCSC{Int64,Int64} # TODO: consider just using a Vector{Vector{Pair{Int64, Int64}}} - function MechanismState(::Type{X}, mechanism::Mechanism{M}) + function (::Type{MechanismState{X, M, C}}){X<:Number, M<:Number, C<:Number}(mechanism::Mechanism{M}) q = Vector{X}(num_positions(mechanism)) v = zeros(X, num_velocities(mechanism)) rootBodyState = RigidBodyState(root_body(mechanism), X, true) @@ -125,10 +125,10 @@ immutable MechanismState{X<:Number, M<:Number, C<:Number} end vertices = toposort(tree) constraintJacobianStructure = constraint_jacobian_structure(mechanism) - new(mechanism, q, v, vertices, view(vertices, 2 : length(vertices)), constraintJacobianStructure) + new{X, M, C}(mechanism, q, v, vertices, view(vertices, 2 : length(vertices)), constraintJacobianStructure) end end -MechanismState{X, M}(t::Type{X}, mechanism::Mechanism{M}) = MechanismState{X, M, promote_type(X, M)}(t, mechanism) +MechanismState{X, M}(::Type{X}, mechanism::Mechanism{M}) = MechanismState{X, M, promote_type(X, M)}(mechanism) Base.show{X, M, C}(io::IO, ::MechanismState{X, M, C}) = print(io, "MechanismState{$X, $M, $C}(…)") diff --git a/src/ode_integrators.jl b/src/ode_integrators.jl index 3a929508..e116e3b2 100644 --- a/src/ode_integrators.jl +++ b/src/ode_integrators.jl @@ -3,6 +3,7 @@ module OdeIntegrators using RigidBodyDynamics using StaticArrays using DocStringExtensions +using Compat export runge_kutta_4, MuntheKaasIntegrator, @@ -26,14 +27,14 @@ immutable ButcherTableau{N, T<:Number, L} c::SVector{N, T} explicit::Bool - function ButcherTableau(a::AbstractMatrix{T}, b::AbstractVector{T}) + function (::Type{ButcherTableau{N, T, L}}){N, T<:Number, L}(a::AbstractMatrix{T}, b::AbstractVector{T}) @assert N > 0 @assert size(a, 1) == N @assert size(a, 2) == N @assert length(b) == N c = vec(sum(a, 2)) explicit = all(triu(a) .== 0) - new(SMatrix{N, N}(a), SVector{N}(b), SVector{N}(c), explicit) + new{N, T, L}(SMatrix{N, N}(a), SVector{N}(b), SVector{N}(c), explicit) end end ButcherTableau{T}(a::Matrix{T}, b::Vector{T}) = ButcherTableau{length(b), T, length(b)^2}(a, b) @@ -62,7 +63,7 @@ visualizing, etc.). Subtypes must implement: * `initialize(sink, state)`: called with the initial state when integration begins. * `process(sink, t, state)`: called at every integration time step with the current state and time. """ -abstract OdeResultsSink +@compat abstract type OdeResultsSink end initialize(::OdeResultsSink, state) = error("concrete subtypes must implement") process(::OdeResultsSink, t, state) = error("concrete subtypes must implement") @@ -78,11 +79,11 @@ type RingBufferStorage{T} <: OdeResultsSink vs::Vector{Vector{T}} lastIndex::Int64 - function RingBufferStorage(n::Int64) + function (::Type{RingBufferStorage{T}}){T}(n::Int64) ts = Vector{T}(n) qs = [Vector{T}() for i in 1 : n] vs = [Vector{T}() for i in 1 : n] - new(ts, qs, vs, 0) + new{T}(ts, qs, vs, 0) end end Base.eltype{T}(storage::RingBufferStorage{T}) = T @@ -116,11 +117,11 @@ type ExpandingStorage{T} <: OdeResultsSink qs::Vector{Vector{T}} vs::Vector{Vector{T}} - function ExpandingStorage(n::Int64) + function (::Type{ExpandingStorage{T}}){T}(n::Int64) ts = Vector{T}(); sizehint!(ts, n) qs = Vector{T}[]; sizehint!(qs, n) vs = Vector{T}[]; sizehint!(vs, n) - new(ts, qs, vs) + new{T}(ts, qs, vs) end end Base.eltype{T}(storage::ExpandingStorage{T}) = T @@ -144,7 +145,7 @@ type MuntheKaasStageCache{N, T<:Number} ϕstep::Vector{T} # local coordinates around q0 after complete step vstep::Vector{T} # velocity after complete step - function MuntheKaasStageCache() + function (::Type{MuntheKaasStageCache{N, T}}){N, T<:Number}() q0 = Vector{T}() vs = SVector{N, Vector{T}}((Vector{T}() for i in 1 : N)...) vds = SVector{N, Vector{T}}((Vector{T}() for i in 1 : N)...) @@ -152,7 +153,7 @@ type MuntheKaasStageCache{N, T<:Number} ϕds = SVector{N, Vector{T}}((Vector{T}() for i in 1 : N)...) ϕstep = Vector{T}() vstep = Vector{T}() - new(q0, vs, vds, ϕs, ϕds, ϕstep, vstep) + new{N, T}(q0, vs, vds, ϕs, ϕds, ϕstep, vstep) end end set_num_positions!(cache::MuntheKaasStageCache, n::Int64) = resize!(cache.q0, n) @@ -192,10 +193,10 @@ immutable MuntheKaasIntegrator{N, T<:Number, F, S<:OdeResultsSink, L} sink::S stages::MuntheKaasStageCache{N, T} - function MuntheKaasIntegrator(dynamics!::F, tableau::ButcherTableau{N, T, L}, sink::S) + function (::Type{MuntheKaasIntegrator{N, T, F, S, L}}){N, T<:Number, F, S<:OdeResultsSink, L}(dynamics!::F, tableau::ButcherTableau{N, T, L}, sink::S) @assert isexplicit(tableau) stages = MuntheKaasStageCache{N, T}() - new(dynamics!, tableau, sink, stages) + new{N, T, F, S, L}(dynamics!, tableau, sink, stages) end end diff --git a/src/rigid_body.jl b/src/rigid_body.jl index 00341f00..564fa291 100644 --- a/src/rigid_body.jl +++ b/src/rigid_body.jl @@ -13,14 +13,14 @@ type RigidBody{T<:Number} frameDefinitions::Set{Transform3D{T}} # inertia undefined; can be used for the root of a kinematic tree - function RigidBody(name::String) + function (::Type{RigidBody{T}}){T<:Number}(name::String) frame = CartesianFrame3D(name) - new(name, Nullable{SpatialInertia{T}}(), Set([Transform3D{T}(frame)])) + new{T}(name, Nullable{SpatialInertia{T}}(), Set([Transform3D{T}(frame)])) end # other bodies - function RigidBody(name::String, inertia::SpatialInertia{T}) - new(name, Nullable(inertia), Set([Transform3D{T}(inertia.frame)])) + function (::Type{RigidBody{T}}){T<:Number}(name::String, inertia::SpatialInertia{T}) + new{T}(name, Nullable(inertia), Set([Transform3D{T}(inertia.frame)])) end end diff --git a/src/spatial.jl b/src/spatial.jl index 8500e23d..a2b385c0 100644 --- a/src/spatial.jl +++ b/src/spatial.jl @@ -143,11 +143,11 @@ immutable GeometricJacobian{A<:AbstractMatrix} angular::A linear::A - function GeometricJacobian(body::CartesianFrame3D, base::CartesianFrame3D, frame::CartesianFrame3D, angular::A, linear::A) + function (::Type{GeometricJacobian{A}}){A<:AbstractMatrix}(body::CartesianFrame3D, base::CartesianFrame3D, frame::CartesianFrame3D, angular::A, linear::A) @boundscheck size(angular, 1) == 3 || error("size mismatch") @boundscheck size(linear, 1) == 3 || error("size mismatch") @boundscheck size(angular, 2) == size(linear, 2) || error("size mismatch") - new(body, base, frame, angular, linear) + new{A}(body, base, frame, angular, linear) end end @@ -157,11 +157,11 @@ for ForceSpaceMatrix in (:MomentumMatrix, :WrenchMatrix) angular::A linear::A - function $ForceSpaceMatrix(frame::CartesianFrame3D, angular::A, linear::A) + function (::Type{$ForceSpaceMatrix{A}}){A<:AbstractMatrix}(frame::CartesianFrame3D, angular::A, linear::A) @assert size(angular, 1) == 3 @assert size(linear, 1) == 3 @assert size(angular, 2) == size(linear, 2) - new(frame, angular, linear) + new{A}(frame, angular, linear) end end end @@ -297,7 +297,7 @@ for MotionSpaceElement in (:Twist, :SpatialAcceleration) StaticArrays.similar_type{T1, T2}(::Type{$MotionSpaceElement{T1}}, ::Type{T2}) = $MotionSpaceElement{T2} function Base.show(io::IO, m::$MotionSpaceElement) - print(io, "$($(MotionSpaceElement).name.name) of \"$(name(m.body))\" w.r.t \"$(name(m.base))\" in \"$(name(m.frame))\":\nangular: $(m.angular), linear: $(m.linear)") + print(io, "$($(string(MotionSpaceElement))) of \"$(name(m.body))\" w.r.t \"$(name(m.base))\" in \"$(name(m.frame))\":\nangular: $(m.angular), linear: $(m.linear)") end function Base.isapprox(x::$MotionSpaceElement, y::$MotionSpaceElement; atol = 1e-12) @@ -511,14 +511,17 @@ for ForceSpaceElement in (:Momentum, :Wrench) Base.eltype{T}(::Type{$ForceSpaceElement{T}}) = T StaticArrays.similar_type{T1, T2}(::Type{$ForceSpaceElement{T1}}, ::Type{T2}) = $ForceSpaceElement{T2} - Base.show(io::IO, f::$ForceSpaceElement) = print(io, "$($(ForceSpaceElement).name.name) expressed in \"$(name(f.frame))\":\nangular: $(f.angular), linear: $(f.linear)") + function Base.show(io::IO, f::$ForceSpaceElement) + print(io, "$($(string(ForceSpaceElement))) expressed in \"$(name(f.frame))\":\nangular: $(f.angular), linear: $(f.linear)") + end + Base.zero{T}(::Type{$ForceSpaceElement{T}}, frame::CartesianFrame3D) = $ForceSpaceElement(frame, zeros(SVector{3, T}), zeros(SVector{3, T})) Random.rand{T}(::Type{$ForceSpaceElement{T}}, frame::CartesianFrame3D) = $ForceSpaceElement(frame, rand(SVector{3, T}), rand(SVector{3, T})) """ $(SIGNATURES) - Transform the $($(ForceSpaceElement).name.name) to a different frame. + Transform the $($(string(ForceSpaceElement))) to a different frame. """ function transform(f::$ForceSpaceElement, transform::Transform3D) @framecheck(f.frame, transform.from) @@ -545,7 +548,7 @@ for ForceSpaceElement in (:Momentum, :Wrench) end # WrenchSubspace is the return type of e.g. constraint_wrench_subspace(::Joint, ...) -typealias WrenchSubspace{T} WrenchMatrix{ContiguousSMatrixColumnView{3, 6, T, 18}} +@compat const WrenchSubspace{T} = WrenchMatrix{ContiguousSMatrixColumnView{3, 6, T, 18}} function WrenchSubspace(frame::CartesianFrame3D, angular, linear) WrenchMatrix(frame, smatrix3x6view(angular), smatrix3x6view(linear)) end @@ -557,7 +560,7 @@ function GeometricJacobian{A<:AbstractMatrix}(body::CartesianFrame3D, base::Cart end # MotionSubspace is the return type of motion_subspace(::Joint, ...) -typealias MotionSubspace{T} GeometricJacobian{ContiguousSMatrixColumnView{3, 6, T, 18}} +@compat const MotionSubspace{T} = GeometricJacobian{ContiguousSMatrixColumnView{3, 6, T, 18}} function MotionSubspace(body::CartesianFrame3D, base::CartesianFrame3D, frame::CartesianFrame3D, angular, linear) GeometricJacobian(body, base, frame, smatrix3x6view(angular), smatrix3x6view(linear)) end @@ -624,7 +627,9 @@ for ForceSpaceMatrix in (:MomentumMatrix, :WrenchMatrix) angular_part(mat::$ForceSpaceMatrix) = mat.angular linear_part(mat::$ForceSpaceMatrix) = mat.linear - Base.show(io::IO, m::$ForceSpaceMatrix) = print(io, "$($(ForceSpaceMatrix).name.name) expressed in \"$(name(m.frame))\":\n$(Array(m))") + function Base.show(io::IO, m::$ForceSpaceMatrix) + print(io, "$($(string(ForceSpaceMatrix))) expressed in \"$(name(m.frame))\":\n$(Array(m))") + end function Base.hcat(mats::$ForceSpaceMatrix...) frame = mats[1].frame diff --git a/src/tree.jl b/src/tree.jl index e31b6115..30ae3d36 100644 --- a/src/tree.jl +++ b/src/tree.jl @@ -1,10 +1,10 @@ module TreeDataStructure +using Compat + import Base: parent, findfirst, map!, insert!, copy -if isdefined(Base, :Iterators) - import Base.Iterators: filter -end +import Compat.Iterators: filter export # types @@ -35,12 +35,15 @@ type TreeVertex{V, E} children::Vector{TreeVertex{V, E}} parentAndEdgeData::Nullable{Pair{TreeVertex{V, E}, E}} - TreeVertex(vertexData::V) = new(vertexData, [], Nullable{Pair{TreeVertex{V, E}, E}}()) - TreeVertex{V, E}(vertexData::V, parent::TreeVertex{V, E}, edgeData::E) = new(vertexData, [], parent => edgeData) + (::Type{TreeVertex{V, E}}){V, E}(vertexData::V) = new{V, E}(vertexData, [], Nullable{Pair{TreeVertex{V, E}, E}}()) + + function (::Type{TreeVertex{V, E}}){V, E}(vertexData::V, parent::TreeVertex{V, E}, edgeData::E) + new{V, E}(vertexData, [], parent => edgeData) + end end TreeVertex{V, E}(vertexData::V, parent::TreeVertex{V, E}, edgeData::E) = TreeVertex{V, E}(vertexData, parent, edgeData) -typealias Tree{V, E} TreeVertex{V, E} +@compat const Tree{V, E} = TreeVertex{V, E} vertex_data(v::TreeVertex) = v.vertexData children(v::TreeVertex) = v.children diff --git a/src/util.jl b/src/util.jl index ff9596c6..e4e30b21 100644 --- a/src/util.jl +++ b/src/util.jl @@ -19,7 +19,7 @@ Base.linearindexing{T}(::Type{NullVector{T}}) = Base.LinearFast() # type of a view of a vector # TODO: a bit too specific -typealias VectorSegment{T} SubArray{T,1,Array{T, 1},Tuple{UnitRange{Int64}},true} +@compat const VectorSegment{T} = SubArray{T,1,Array{T, 1},Tuple{UnitRange{Int64}},true} # non-allocating, unsafe vector view # from https://github.com/mlubin/ReverseDiffSparse.jl/commit/8e3ade867581aad6ade7c898ada2ed58e0ad42bb @@ -87,8 +87,8 @@ macro rtti_dispatch(typeTuple, signature) :($(esc(ret))) end -typealias ContiguousSMatrixColumnView{S1, S2, T, L} SubArray{T,2,SMatrix{S1, S2, T, L},Tuple{Colon,UnitRange{Int64}},true} -typealias RotMatrix3{T} RotMatrix{3, T, 9} +@compat const ContiguousSMatrixColumnView{S1, S2, T, L} = SubArray{T,2,SMatrix{S1, S2, T, L},Tuple{Colon,UnitRange{Int64}},true} +@compat const RotMatrix3{T} = RotMatrix{3, T, 9} # TODO: use fusing broadcast instead of these functions in 0.6, where they don't allocate. function sub!(out, a, b) diff --git a/test/REQUIRE b/test/REQUIRE index 09f3c46b..bab6741e 100644 --- a/test/REQUIRE +++ b/test/REQUIRE @@ -1,5 +1,4 @@ ForwardDiff 0.3.1 BenchmarkTools 0.0.5 -ODE 0.2.1 SymPy 0.4.0 NBInclude 1.1.0 diff --git a/test/runtests.jl b/test/runtests.jl index dd25ca07..6a240505 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -4,12 +4,9 @@ using RigidBodyDynamics using RigidBodyDynamics.TreeDataStructure using Rotations using StaticArrays -using ODE using ForwardDiff -if isdefined(Base, :Iterators) - import Base.Iterators: filter -end +import Compat.Iterators: filter # useful utility function for computing time derivatives. create_autodiff(x, dx) = [ForwardDiff.Dual(x[i], dx[i]) for i in 1 : length(x)] diff --git a/test/test_mechanism_algorithms.jl b/test/test_mechanism_algorithms.jl index cb8c691f..58d8055c 100644 --- a/test/test_mechanism_algorithms.jl +++ b/test/test_mechanism_algorithms.jl @@ -324,6 +324,23 @@ @test isapprox(τ, zeros(num_velocities(mechanism)); atol = 1e-10) end + @testset "dynamics ode method" begin + mechanism = rand_tree_mechanism(Float64, [QuaternionFloating{Float64}; [Revolute{Float64} for i = 1 : 10]; [Prismatic{Float64} for i = 1 : 10]]...) + x = MechanismState(Float64, mechanism) + rand!(x) + torques = rand(num_velocities(mechanism)) + externalWrenches = Dict(body => rand(Wrench{Float64}, root_frame(mechanism)) for body in non_root_bodies(mechanism)) + + result1 = DynamicsResult(Float64, mechanism) + ẋ = Vector{Float64}(length(state_vector(x))) + dynamics!(ẋ, result1, x, state_vector(x), torques, externalWrenches) + + result2 = DynamicsResult(Float64, mechanism) + dynamics!(result2, x, torques, externalWrenches) + + @test isapprox([configuration_derivative(x); result2.v̇], ẋ) + end + @testset "power flow" begin mechanism = rand_chain_mechanism(Float64, [QuaternionFloating{Float64}; [Revolute{Float64} for i = 1 : 10]; [Prismatic{Float64} for i = 1 : 10]]...) # what really matters is that there's a floating joint first x = MechanismState(Float64, mechanism) @@ -401,15 +418,8 @@ total_energy_after = gravitational_potential_energy(x) + kinetic_energy(x) @test isapprox(total_energy_after, total_energy_before, atol = 1e-3) - # use standard integrator (fine when q̇ == v) total_energy_before = gravitational_potential_energy(x) + kinetic_energy(x) - x0 = state_vector(x) result = DynamicsResult(Float64, acrobot) - ẋ = Vector{Float64}(length(x0)) - odefun(t, y) = dynamics!(ẋ, result, x, y) - times, states = ODE.ode45(odefun, x0, linspace(0., 1., 1e4)) - set!(x, states[end]) - @test isapprox(gravitational_potential_energy(x) + kinetic_energy(x), total_energy_before, atol = 1e-1) # use RingBufferStorage using RigidBodyDynamics.OdeIntegrators @@ -421,7 +431,8 @@ storage = RingBufferStorage{Float64}(3) integrator = MuntheKaasIntegrator(passive_dynamics!, runge_kutta_4(Float64), storage) integrate(integrator, x, 1., 1e-4) - set!(x, states[storage.lastIndex]) + set_configuration!(x, storage.qs[storage.lastIndex]) + set_velocity!(x, storage.vs[storage.lastIndex]) @test isapprox(gravitational_potential_energy(x) + kinetic_energy(x), total_energy_before, atol = 1e-1) end end