55 LinearAlgebra,
66 StaticArrays,
77 VMRobotControl
8+ # begin
9+ # X = SVector{3, Float64}(1.0, 0.0, 0.0)
10+ # Y = SVector{3, Float64}(0.0, 1.0, 0.0)
11+ # Z = SVector{3, Float64}(0.0, 0.0, 1.0)
12+
13+ # T1 = zero(Transform{Float64})
14+ # J1 = Revolute(Y, T1)
15+
16+ # T2 = Transform(Z)
17+ # J2 = Revolute(Y, T2)
18+
19+ # T3 = Transform(Z)
20+ # J3 = Rigid(T3)
21+
22+ # mech = Mechanism{Float64}("2Link")
23+ # cart_frame = add_frame!(mech, "Cart")
24+ # L2_frame = add_frame!(mech, "L2")
25+ # EE_frame = add_frame!(mech, "EE")
26+
27+ # add_joint!(mech, J1; parent="root_frame", child=cart_frame, id="J1")
28+ # add_joint!(mech, J2; parent=cart_frame, child=L2_frame, id="J2")
29+ # add_joint!(mech, J3, parent=L2_frame, child=EE_frame; id="J3")
30+
31+ # # Then, we add a coordinate to the mechanism to represent the tip of the pendulum, and a
32+ # # coordinate to represent the position of the cart. These are used to add point masses to
33+ # # the mechanism.
34+ # add_coordinate!(mech, FrameOrigin(EE_frame); id="tip_pos")
35+ # add_coordinate!(mech, FrameOrigin(cart_frame); id="cart_pos")
36+ # add_component!(mech, PointMass(1.0, "cart_pos"); id="cart_mass")
37+ # add_component!(mech, PointMass(1.0, "tip_pos"); id="pendulum_mass")
38+ # m = compile(mech)
39+ # end
40+
41+ # We compile the mechanism, and setup an ODE problem to simulate the dynamics of the rail robot.
42+ # m = compiled_mechanisms[5]
43+ m = compile (parseRSON (" ./RSONs/rsons/trivial2link.rson"
44+ cache = Observable (new_inverse_dynamics_cache (m))
45+ # @testset test_inverse_dynamics(m)
846
9- X = SVector {3, Float64} (1.0 , 0.0 , 0.0 )
10- Y = SVector {3, Float64} (0.0 , 1.0 , 0.0 )
11- Z = SVector {3, Float64} (0.0 , 0.0 , 1.0 )
47+ bundle = cache[]
1248
13- T1 = zero (Transform{Float64})
14- J1 = Revolute (Y, T1)
49+ begin
1550
16- T2 = Transform (Z)
17- J2 = Revolute (Y, T2)
1851
19- T3 = Transform (Z)
20- J3 = Rigid (T3)
52+ t = 0.0
53+ q = [0.0 , π/ 2 ]
54+ q̇ = zero_q̇ (m)
55+ g = SVector {3, Float64} (0.0 , 0.0 , 0.0 )
2156
22- mech = Mechanism {Float64} (" 2Link"
23- cart_frame = add_frame! (mech, " Cart"
24- L2_frame = add_frame! (mech, " L2"
25- EE_frame = add_frame! (mech, " EE"
57+ # rng = MersenneTwister(1234)
58+ # t = 0.0
59+ # q = rand!(rng, zero_q(Float64, m))
60+ # q̇ = rand!(rng, zero_q̇(Float64, m))
61+ # g = randn(rng, SVector{3, Float64})
2662
27- add_joint! (mech, J1; parent= " root_frame" = cart_frame, id= " J1"
28- add_joint! (mech, J2; parent= cart_frame, child= L2_frame, id= " J2"
29- add_joint! (mech, J3, parent= L2_frame, child= EE_frame; id= " J3"
63+ u = zero_u (m)
3064
31- # Then, we add a coordinate to the mechanism to represent the tip of the pendulum, and a
32- # coordinate to represent the position of the cart. These are used to add point masses to
33- # the mechanism.
34- add_coordinate! (mech, FrameOrigin (EE_frame); id= " tip_pos"
35- add_coordinate! (mech, FrameOrigin (cart_frame); id= " cart_pos"
36- add_component! (mech, PointMass (1.0 , " cart_pos" = " cart_mass"
37- add_component! (mech, PointMass (1.0 , " tip_pos" = " pendulum_mass"
65+ # dcache = new_dynamics_cache(m)
66+ # dynamics!(dcache, t, q, q̇, g, u)
67+ # q̈ = get_q̈(dcache)
68+ onehot_q̈ = [1.0 , 0.0 ]
69+ q̈ = onehot_q̈
3870
39- # We compile the mechanism, and setup an ODE problem to simulate the dynamics of the rail robot.
40- m = compile (mech)
41- cache = Observable (new_inverse_dynamics_cache (m))
4271
72+ VMRobotControl. _inverse_dynamics_set_inputs! (bundle, t, q, q̇, q̈, g)
73+ VMRobotControl. _inverse_dynamics_forward_pass! (bundle)
74+ VMRobotControl. _inverse_dynamics_zero! (bundle)
75+ VMRobotControl. _inverse_dynamics_backward_pass_a! (bundle)
76+ VMRobotControl. _inverse_dynamics_backward_pass_b! (bundle)
77+ # VMRobotControl._inverse_dynamics_backward_pass_c!(bundle)
78+ # VMRobotControl._inverse_dynamics_backward_pass_d!(bundle)
4379
44- begin
45- t = 0.0
46- q = [0.0 , π/ 2 ]
47- q̇ = zero_q̇ (m)
48- q̈ = zero_q̈ (m)
49- g = SVector {3, Float64} (0.0 , 0.0 , - 9.81 )
50- inverse_dynamics! (cache[], t, q, q̇, q̈, g)
80+ # @show get_u(bundle)
81+
82+ # @show inverse_dynamics!(bundle, t, q, zero_q̇(m), onehot_q̈, 0*g)
5183
84+ # _test_RNE_inertance_matrix(bundle, dcache, t, q)
85+
86+ # q̈_out = dynamics!(dcache, t, q, q̇, g, get_u(bundle))
87+
88+ # @test get_u(cache[]) ≈ u atol=1e-7 rtol=1e-7
89+ # @test q̈ ≈ q̈_out atol=1e-7 rtol=1e-7
90+ end
91+
92+
93+ begin
5294fig = Figure ()
5395display (fig)
5496ls = LScene (fig[1 , 1 ]; show_axis= false )
55- robotsketch! (ls, cache; linewidth= 3 )
97+ robotsketch! (ls, cache; linewidth= 3 , scale= 0.05 )
98+ robotvisualize! (ls, cache)
5699
57100frameIDs = get_compiled_frameID .((cache[],), frames (cache[]))
58101tfs = map (cache) do cache
@@ -65,20 +108,24 @@ fs = map(cache) do cache
65108 fs = map (id -> VMRobotControl. get_frame_force (cache, id), frameIDs)
66109end
67110f_scale = map (fs) do fs
68- 0.1 / norm ( maximum (fs))
111+ 0.2 / maximum ( norm (fs))
69112end
70113τs = map (cache) do cache
71114 map (id -> VMRobotControl. get_frame_torque (cache, id), frameIDs)
72115end
116+ τ_scale = map (τs) do τs
117+ 0.4 / maximum (norm (τs))
118+ end
119+
73120
74- arrows! (ls, positions, fs; lengthscale= f_scale, arrowsize= 0.1 , color= :red , )
75- arrows! (ls, positions, τs; lengthscale= f_scale , arrowsize= 0.1 , color= :red , )
121+ arrows! (ls, positions, fs; lengthscale= f_scale, arrowsize= map (f -> * (f, 50 ), f_scale) , color= :red , )
122+ arrows! (ls, positions, τs; lengthscale= τ_scale , arrowsize= 0.04 , color= :blue , )
76123
77124
78125end
79126
80- using Test, Random
81- Revise. includet (" ../test/inverse_dynamics_test.jl"
127+ # using Test, Random
128+ # Revise.includet("../test/inverse_dynamics_test.jl")
82129@testset test_inverse_dynamics (m)
83130
84131# function create_rotated_cylinder_mesh(N1, N2, r1, r2, ϕ)
0 commit comments