Skip to content

Commit 1362707

Browse files
Larbino1Larbino1
committed
Doc fixes, separate out ros example.
1 parent 8419757 commit 1362707

File tree

6 files changed

+71
-87
lines changed

6 files changed

+71
-87
lines changed

‎README.md‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -67,5 +67,5 @@ include("./docs/make.jl")
6767
```
6868
and host them using `LiveServer.jl`:
6969
```julia
70-
include("./docs/make.jl")
70+
include("./docs/host.jl")
7171
```

‎docs/make.jl‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,14 @@
1+
module_dir = joinpath(splitpath(Base.source_dir())[1:end-1])
2+
if pwd() != module_dir
3+
@info "Changing directory to folder $(module_dir)"
4+
cd(module_dir)
5+
end
6+
7+
# Ensure that VMRobotControl is devved.
8+
using Pkg
9+
Pkg.develop(PackageSpec(path=pwd()))
10+
Pkg.instantiate()
11+
112
using Revise
213

314
using

‎ros/ROS.jl‎

Lines changed: 0 additions & 77 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,3 @@
1-
using Pkg; Pkg.activate("VMRobotControlEnv"; shared=true)
2-
31
using Printf
42
using Sockets
53
using StaticArrays
@@ -397,78 +395,3 @@ function ros_vm_controller(
397395
(E = stored_energy(control_cache)) > E_max && error("Initial stored energy exceeds $(E_max)J, was $(E)J")
398396
warmup_and_activate(connection, control_func!; )
399397
end
400-
401-
#=
402-
# Enable debug logging
403-
using Logging
404-
debuglogger = ConsoleLogger(stderr, Logging.Debug)
405-
global_logger(debuglogger)
406-
=#
407-
408-
#=
409-
# Test connection with python only
410-
with_rospy_connection(Sockets.localhost, ROSPY_LISTEN_PORT+1, 7, 14) do connection
411-
warmup_and_activate(connection, test_control_func; )
412-
end
413-
=#
414-
415-
#=
416-
# Test on franka
417-
module_path = joinpath(splitpath(splitdir(pathof(VMRobotControl))[1])[1:end-1])
418-
using FileIO, UUIDs
419-
try
420-
FileIO.add_format(format"DAE", (), ".dae", [:DigitalAssetExchangeFormatIO => UUID("43182933-f65b-495a-9e05-4d939cea427d")])
421-
catch
422-
end
423-
robot = parseRSON(joinpath(module_path, "RSONs/rsons/franka_panda/pandaSurgicalV2.rson"))
424-
add_gravity_compensation!(robot, 1.1*DEFAULT_GRAVITY)
425-
426-
vms = compile(VirtualMechanismSystem("franka", robot))
427-
qᵛ = Float64[]
428-
429-
with_rospy_connection(Sockets.localhost, ROSPY_LISTEN_PORT, 7, 14) do connection
430-
ros_vm_controller(connection, vms, qᵛ)
431-
end
432-
=#
433-
434-
# Test on sciurus
435-
# Test on franka
436-
module_path = joinpath(splitpath(splitdir(pathof(VMRobotControl))[1])[1:end-1])
437-
robot = parseURDF(joinpath(module_path, "URDFs/sciurus17_description/urdf/sciurus17.urdf"))
438-
add_gravity_compensation!(robot, DEFAULT_GRAVITY)
439-
440-
vms = VirtualMechanismSystem("sciurus", robot)
441-
# add_gravity_compensation!(vms, -0.2*DEFAULT_GRAVITY)
442-
add_coordinate!(vms, ReferenceCoord(Ref(SVector(.3, 0.1, 0.2))); id="l_tgt")
443-
add_coordinate!(vms, FramePoint(".robot.l_link7", SVector(0., 0., 0.)); id="l_hand")
444-
add_coordinate!(vms, CoordDifference("l_tgt", "l_hand"), id="l_err")
445-
add_component!(vms, TanhSpring("l_err"; stiffness=200.0, max_force=5.0); id="l_spring")
446-
add_component!(vms, LinearDamper(5., "l_err"); id="l_damper")
447-
448-
add_coordinate!(vms, ReferenceCoord(Ref(SVector(.3, 0.1, 0.2))); id="r_tgt")
449-
add_coordinate!(vms, FramePoint(".robot.r_link7", SVector(0., 0., 0.)); id="r_hand")
450-
add_coordinate!(vms, CoordDifference("r_tgt", "r_hand"), id="r_err")
451-
add_component!(vms, TanhSpring("r_err"; stiffness=200.0, max_force=5.0); id="r_spring")
452-
add_component!(vms, LinearDamper(5., "r_err"); id="r_damper")
453-
454-
455-
456-
function f_setup(cache)
457-
l_ref_coord_id = get_compiled_coordID(cache, "l_tgt")
458-
r_ref_coord_id = get_compiled_coordID(cache, "r_tgt")
459-
return (l_ref_coord_id, r_ref_coord_id)
460-
end
461-
462-
function f_control(cache, t, setup_ret, extra)
463-
l_ref_coord_id, r_ref_coord_id = setup_ret
464-
coord = cache[l_ref_coord_id].coord_data.val[] = SVector(0.3, .1 + 0.1*sin(t), 0.2)
465-
coord = cache[r_ref_coord_id].coord_data.val[] = SVector(0.3 + 0.1*cos(t), -.1, 0.2 + 0.1*sin(t))
466-
nothing
467-
end
468-
469-
cvms = compile(vms)
470-
471-
qᵛ = Float64[]
472-
with_rospy_connection(Sockets.localhost, ROSPY_LISTEN_PORT, 21, 42) do connection
473-
ros_vm_controller(connection, cvms, qᵛ; f_control, f_setup, E_max=2.0)
474-
end

‎ros/ros_example.jl‎

Lines changed: 43 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
# Load ROS communication code
2+
include("ROS.jl")
3+
4+
# Load robot model
5+
module_path = joinpath(splitpath(splitdir(pathof(VMRobotControl))[1])[1:end-1])
6+
robot = parseURDF(joinpath(module_path, "URDFs/sciurus17_description/urdf/sciurus17.urdf"))
7+
# Include gravity compensation done by robot software (not done by our controller)
8+
add_gravity_compensation!(robot, DEFAULT_GRAVITY)
9+
10+
# Build our virtual mechanism controller
11+
vms = VirtualMechanismSystem("sciurus", robot)
12+
add_coordinate!(vms, ReferenceCoord(Ref(SVector(.3, 0.1, 0.2))); id="l_tgt")
13+
add_coordinate!(vms, FramePoint(".robot.l_link7", SVector(0., 0., 0.)); id="l_hand")
14+
add_coordinate!(vms, CoordDifference("l_tgt", "l_hand"), id="l_err")
15+
add_component!(vms, TanhSpring("l_err"; stiffness=200.0, max_force=5.0); id="l_spring")
16+
add_component!(vms, LinearDamper(5., "l_err"); id="l_damper")
17+
18+
add_coordinate!(vms, ReferenceCoord(Ref(SVector(.3, 0.1, 0.2))); id="r_tgt")
19+
add_coordinate!(vms, FramePoint(".robot.r_link7", SVector(0., 0., 0.)); id="r_hand")
20+
add_coordinate!(vms, CoordDifference("r_tgt", "r_hand"), id="r_err")
21+
add_component!(vms, TanhSpring("r_err"; stiffness=200.0, max_force=5.0); id="r_spring")
22+
add_component!(vms, LinearDamper(5., "r_err"); id="r_damper")
23+
24+
function f_setup(cache)
25+
l_ref_coord_id = get_compiled_coordID(cache, "l_tgt")
26+
r_ref_coord_id = get_compiled_coordID(cache, "r_tgt")
27+
return (l_ref_coord_id, r_ref_coord_id)
28+
end
29+
30+
function f_control(cache, t, setup_ret, extra)
31+
l_ref_coord_id, r_ref_coord_id = setup_ret
32+
coord = cache[l_ref_coord_id].coord_data.val[] = SVector(0.3, .1 + 0.1*sin(t), 0.2)
33+
coord = cache[r_ref_coord_id].coord_data.val[] = SVector(0.3 + 0.1*cos(t), -.1, 0.2 + 0.1*sin(t))
34+
nothing
35+
end
36+
37+
# Compile the virtual mechanism system, and run the controller via ROS
38+
# Make sure rospy_client.py is running first.
39+
cvms = compile(vms)
40+
qᵛ = Float64[]
41+
with_rospy_connection(Sockets.localhost, ROSPY_LISTEN_PORT, 21, 42) do connection
42+
ros_vm_controller(connection, cvms, qᵛ; f_control, f_setup, E_max=2.0)
43+
end

‎test/energy_test.jl‎

Lines changed: 13 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,15 @@
1+
function is_dissipative_system(system)
2+
dissipative = false
3+
if system isa CompiledMechanism
4+
dissipative = !isempty(dissipations(system))
5+
else
6+
dissipative = dissipative || !isempty(dissipations(system))
7+
dissipative = dissipative || !isempty(dissipations(system.robot))
8+
dissipative = dissipative || !isempty(dissipations(system.virtual_mechanism))
9+
end
10+
return dissipative
11+
end
12+
113
"""
214
test_energy(immut_mechanism)
315
@@ -54,14 +66,7 @@ function test_energy(system)
5466
dynamics!(cache, T, q, q̇, gravity)
5567
E2 = stored_energy(cache)
5668

57-
dissipative = false
58-
if system isa CompiledMechanism
59-
dissipative = !isempty(dissipations(system))
60-
else
61-
dissipative = dissipative || !isempty(dissipations(system))
62-
dissipative = dissipative || !isempty(dissipations(system.robot))
63-
dissipative = dissipative || !isempty(dissipations(system.virtual_mechanism))
64-
end
69+
dissipative = is_dissipative_system(system)
6570
if dissipative
6671
@test E2 <= E1
6772
else

‎test/runtests.jl‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,8 @@
1+
using Pkg
2+
13
if (dir = splitpath(pwd())[end]) != "test" # If we are not in the test directory, cd to it
24
if dir == "VMRobotControl.jl"
5+
Pkg.activate(".")
36
@info "Changing directory to ./test, and activating test environment"
47
cd("test")
58
using TestEnv
@@ -9,7 +12,6 @@ if (dir = splitpath(pwd())[end]) != "test" # If we are not in the test directory
912
end
1013
end
1114

12-
using Pkg
1315
# TEST_ENZYME = "Enzyme" ∈ keys(Pkg.project().dependencies)
1416
TEST_ENZYME = false
1517
# TEST_ENZYME = true

0 commit comments

Comments
 (0)