-
Notifications
You must be signed in to change notification settings - Fork 8
/
runtests.jl
128 lines (115 loc) · 4.48 KB
/
runtests.jl
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
using Test
using MeshCat
using MeshCatMechanisms
using RigidBodyDynamics
using RigidBodyDynamics.OdeIntegrators
using CoordinateTransformations: Translation
using ValkyrieRobot
using NBInclude
using StaticArrays
vis = Visualizer()
@testset "MeshCatMechanisms" begin
@testset "URDF mechanism" begin
urdf = joinpath(@__DIR__, "urdf", "Acrobot.urdf")
robot = parse_urdf(Float64, urdf)
mvis = MechanismVisualizer(robot, URDFVisuals(urdf), vis)
if !haskey(ENV, "CI")
open(mvis)
wait(mvis)
end
set_configuration!(mvis, [0, -0.5])
set_configuration!(mvis, findjoint(robot, "shoulder"), 1.0)
@test configuration(mvis) == [1.0, -0.5]
@test configuration(mvis, findjoint(robot, "shoulder")) == [1.0]
copyto!(mvis, [0.1, -0.6, 0.0, 0.0])
@test configuration(mvis) == [0.1, -0.6]
setelement!(mvis, default_frame(bodies(robot)[end]))
setelement!(mvis, Point3D(default_frame(bodies(robot)[3]), 0.2, 0.2, 0.2))
@testset "simulation and animation" begin
state = MechanismState(robot, randn(2), randn(2))
t, q, v = simulate(state, 5.0);
animate(mvis, t, q)
setanimation!(mvis, t, q)
end
end
@testset "URDF with fixed joints" begin
urdf = joinpath(@__DIR__, "urdf", "Acrobot_fixed.urdf")
robot = parse_urdf(Float64, urdf)
RigidBodyDynamics.remove_fixed_tree_joints!(robot)
delete!(vis)
mvis = MechanismVisualizer(robot, URDFVisuals(urdf), vis)
set_configuration!(mvis, [0.5])
@testset "simulation and animation" begin
state = MechanismState(robot, randn(1), randn(1))
t, q, v = simulate(state, 5.0);
animate(mvis, t, q)
setanimation!(mvis, t, q)
end
end
@testset "Valkyrie" begin
val = Valkyrie();
delete!(vis)
mvis = MechanismVisualizer(
val.mechanism,
URDFVisuals(ValkyrieRobot.urdfpath(), package_path=[ValkyrieRobot.packagepath()]),
vis[:val_urdf])
end
@testset "valkyrie inertias" begin
val = Valkyrie();
mvis = MechanismVisualizer(
val.mechanism,
Skeleton(),
vis[:val_inertia])
settransform!(vis[:val_inertia], Translation(0, 2, 0))
end
@testset "visualization during simulation" begin
urdf = joinpath(@__DIR__, "urdf", "Acrobot.urdf")
robot = parse_urdf(Float64, urdf)
mvis = MechanismVisualizer(robot, URDFVisuals(urdf), vis[:acrobot][:robot])
settransform!(vis[:acrobot], Translation(0, -2, 0))
result = DynamicsResult{Float64}(robot)
function damped_dynamics!(vd::AbstractArray, sd::AbstractArray, t, state)
damping = 2.
τ = -damping * velocity(state)
dynamics!(result, state, τ)
copyto!(vd, result.v̇)
copyto!(sd, result.ṡ)
nothing
end
state = MechanismState(robot, randn(2), randn(2))
integrator = MuntheKaasIntegrator(state, damped_dynamics!, runge_kutta_4(Float64), MeshCatSink(mvis))
integrate(integrator, 10., 1e-3, max_realtime_rate = 1.)
end
@testset "position bounds resolution" begin
j = Joint("foo", Revolute(SVector(1., 0, 0)))
RigidBodyDynamics.position_bounds(j) .= RigidBodyDynamics.Bounds(-1.234, 0.0)
MeshCatMechanisms.Manipulate.sliders(j)
MeshCatMechanisms.Manipulate.widget(j)
end
@testset "manipulation" begin
delete!(vis)
mechanism = rand_chain_mechanism(Float64, [QuaternionFloating{Float64}; [Revolute{Float64} for i = 1:5]]...)
mvis = MechanismVisualizer(mechanism)
widget = manipulate!(mvis)
end
@testset "manipulation - negative bounds issue" begin
mechanism = rand_chain_mechanism(Float64, Revolute{Float64})
joint = first(joints(mechanism))
joint.position_bounds .= RigidBodyDynamics.Bounds(-2.0, -1.0)
mvis = MechanismVisualizer(mechanism)
widget = manipulate!(mvis)
end
@testset "examples" begin
dir = joinpath(@__DIR__, "..", "examples")
for file in readdir(dir)
base, ext = splitext(file)
if ext == ".jl"
println("Running $file")
include(joinpath(dir, file))
elseif ext == ".ipynb"
println("Running notebook $file")
@nbinclude(joinpath(dir, file))
end
end
end
end