2.6 - 0.7 - 1.9

using IntervalArithmetic

i = Interval(1.0, 2.0)

dump(i)

i + i

sin(i)

i = @interval(2.9)
@show i.lo === i.hi
dump(i)

i = Interval(2.9)
@show i.lo === i.hi
dump(i)

i = @interval(2.6) - @interval(0.7) - @interval(1.9)

using Compat.Test
@test (2.6 - 0.7 - 1.9) ∈ i

using RigidBodyDynamics

const T = Interval{Float64}
const mechanism = parse_urdf(T, Pkg.dir("RigidBodyDynamics", "test", "urdf", "Acrobot.urdf"))
remove_fixed_tree_joints!(mechanism)
state = MechanismState(mechanism)

shoulder, elbow = joints(mechanism)
set_configuration!(state, shoulder, @interval(1))
set_configuration!(state, elbow, @interval(2));

M = mass_matrix(state)

err = map(x -> x.hi - x.lo, M)

@test maximum(abs, err) ≈ 0 atol = 1e-14

set_configuration!(state, shoulder, @interval(0.95, 1.05))
set_configuration!(state, elbow, @interval(1.95, 2.05));

center_of_mass(state)