Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix most errors and warnings on Julia nightly #176

Merged
merged 6 commits into from
Feb 21, 2017
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions REQUIRE
Original file line number Diff line number Diff line change
Expand Up @@ -4,3 +4,4 @@ Rotations 0.3.4
DataStructures 0.4.6
LightXML 0.4.0
DocStringExtensions 0.3.1
Compat 0.19.0
5 changes: 2 additions & 3 deletions src/RigidBodyDynamics.jl
Original file line number Diff line number Diff line change
Expand Up @@ -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")
Expand Down
2 changes: 1 addition & 1 deletion src/cache_element.jl
Original file line number Diff line number Diff line change
@@ -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)
Expand Down
4 changes: 2 additions & 2 deletions src/dynamics_result.jl
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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

Expand Down
27 changes: 19 additions & 8 deletions src/frames.jl
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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}))
Expand All @@ -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)
Expand Down Expand Up @@ -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)
Expand All @@ -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)

"""
Expand Down
8 changes: 4 additions & 4 deletions src/joint_types.jl
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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

"""
Expand Down Expand Up @@ -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

"""
Expand Down
4 changes: 2 additions & 2 deletions src/mechanism.jl
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
14 changes: 7 additions & 7 deletions src/mechanism_state.jl
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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}}()
Expand All @@ -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

Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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}(…)")

Expand Down
23 changes: 12 additions & 11 deletions src/ode_integrators.jl
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@ module OdeIntegrators
using RigidBodyDynamics
using StaticArrays
using DocStringExtensions
using Compat

export runge_kutta_4,
MuntheKaasIntegrator,
Expand All @@ -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)
Expand Down Expand Up @@ -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")

Expand All @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -144,15 +145,15 @@ 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)...)
ϕs = SVector{N, Vector{T}}((Vector{T}() for i in 1 : N)...)
ϕ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)
Expand Down Expand Up @@ -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

Expand Down
8 changes: 4 additions & 4 deletions src/rigid_body.jl
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
Loading