initial commit

Manifest.toml

@@ -123,10 +123,10 @@ uuid = "da5c29d0-fa7d-589e-88eb-ea29b0a81949"
 version = "0.4.0"
 
 [[EzXML]]
-deps = ["BinaryProvider", "Libdl", "Printf"]
-git-tree-sha1 = "469de9cb996a9c03f31905619ff3c33e905711f3"
+deps = ["Printf", "XML2_jll"]
+git-tree-sha1 = "0fa3b52a04a4e210aeb1626def9c90df3ae65268"
 uuid = "8f5d6c58-4d21-5cfd-889c-e3ad7ee6a615"
-version = "0.9.5"
+version = "1.1.0"
 
 [[FillArrays]]
 deps = ["LinearAlgebra", "Random", "SparseArrays"]
@@ -225,9 +225,9 @@ uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 uuid = "56ddb016-857b-54e1-b83d-db4d58db5568"
 
 [[LyceumBase]]
-deps = ["Adapt", "BenchmarkTools", "Dates", "Distributions", "ElasticArrays", "EllipsisNotation", "Future", "InteractiveUtils", "JLSO", "LibGit2", "LinearAlgebra", "Logging", "MacroTools", "Parameters", "Pkg", "Random", "Shapes", "StaticArrays", "Statistics", "Test", "TimeZones", "UnicodePlots", "UniversalLogger", "UnsafeArrays"]
-git-tree-sha1 = "ead2c7f87eac34745aff097c37883dce06e5f9fa"
-repo-rev = "master"
+deps = ["Adapt", "BenchmarkTools", "Dates", "Distributions", "ElasticArrays", "EllipsisNotation", "Future", "InteractiveUtils", "JLSO", "LibGit2", "LinearAlgebra", "Logging", "MacroTools", "Parameters", "Pkg", "Random", "Shapes", "StaticArrays", "Statistics", "Test", "UnicodePlots", "UniversalLogger", "UnsafeArrays"]
+git-tree-sha1 = "86a46f1f4604a33f4bec485b9a039f4957ae659a"
+repo-rev = "testingtests"
 repo-url = "https://github.com/Lyceum/LyceumBase.jl.git"
 uuid = "db31fed1-ca1e-4084-8a49-12fae1996a55"
 version = "0.1.0"
@@ -264,8 +264,8 @@ version = "0.7.0"
 
 [[MuJoCo]]
 deps = ["AxisArrays", "BangBang", "Base64", "CEnum", "LinearAlgebra", "MacroTools", "MuJoCo_jll", "Reexport", "StaticArrays", "UnsafeArrays"]
-git-tree-sha1 = "f3db7f65558508cc8cd15abee8c52fa0dfe639a5"
-repo-rev = "master"
+git-tree-sha1 = "d15312f8f0a9cad7f1865357f3c061618a0c7fcd"
+repo-rev = "util"
 repo-url = "https://github.com/Lyceum/MuJoCo.jl.git"
 uuid = "93189219-7048-461c-94ec-443a161ed927"
 version = "0.1.1"
@@ -455,9 +455,9 @@ uuid = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [[TimeZones]]
 deps = ["Dates", "EzXML", "Mocking", "Printf", "Serialization", "Unicode"]
-git-tree-sha1 = "29c7ae3f50f291358043e21db47b3e1a516df891"
+git-tree-sha1 = "0271dc890dd1447da1568510937bd5b3a6b51b58"
 uuid = "f269a46b-ccf7-5d73-abea-4c690281aa53"
-version = "0.10.3"
+version = "0.9.2"
 
 [[TranscodingStreams]]
 deps = ["Random", "Test"]

src/LyceumMuJoCo.jl

@@ -123,6 +123,7 @@ end
 ####
 
 include("suite/pointmass.jl")
+include("suite/armhand/armhandpickup.jl")
 
 include("gym/swimmer-v2.jl")
 include("gym/hopper-v2.jl")

src/suite/armhand/armhandpickup.jl

@@ -0,0 +1,130 @@
+struct ArmHandPickup{S<:MJSim, O} <: AbstractMuJoCoEnvironment
+    sim::S
+    osp::O
+    ball::Int
+    palm::Int
+    thumb::Int
+    index::Int
+    middle::Int
+    ring::Int
+    pinky::Int
+    goal::SVector{3, Float64}
+
+    function ArmHandPickup(sim::MJSim)
+        m  = sim.m
+        mn, dn = sim.mn, sim.dn
+
+        osp = MultiShape(d_thumb  = ScalarShape(Float64),
+                         d_index  = ScalarShape(Float64),
+                         d_middle = ScalarShape(Float64),
+                         d_ring   = ScalarShape(Float64),
+                         d_pinky  = ScalarShape(Float64),
+                         a_thumb  = ScalarShape(Float64),
+                         a_index  = ScalarShape(Float64),
+                         a_middle = ScalarShape(Float64),
+                         a_ring   = ScalarShape(Float64),
+                         a_pinky  = ScalarShape(Float64),
+                         a_close  = ScalarShape(Float64),
+                         handball = ScalarShape(Float64),
+                         ballgoal = ScalarShape(Float64),
+                         ball     = VectorShape(Float64, 3),
+                         palm     = VectorShape(Float64, 3))
+
+        ball = jl_name2id(sim.m, MuJoCo.MJCore.mjOBJ_SITE, "ball")
+        palm = jl_name2id(sim.m, MuJoCo.MJCore.mjOBJ_SITE, "palm")
+
+        thumb  = jl_name2id(sim.m, MuJoCo.MJCore.mjOBJ_SITE, "thumb_IMU")
+        index  = jl_name2id(sim.m, MuJoCo.MJCore.mjOBJ_SITE, "index_IMU")
+        middle = jl_name2id(sim.m, MuJoCo.MJCore.mjOBJ_SITE, "middle_IMU")
+        ring   = jl_name2id(sim.m, MuJoCo.MJCore.mjOBJ_SITE, "ring_IMU")
+        pinky  = jl_name2id(sim.m, MuJoCo.MJCore.mjOBJ_SITE, "pinky_IMU")
+
+        goal = SA_F64[0.0, 0.2, 0.5]
+
+        new{typeof(sim), typeof(osp)}(sim, osp,
+                                      ball, palm,
+                                      thumb, index, middle, ring, pinky,
+                                      goal)
+    end
+end
+
+function tconstruct(::Type{ArmHandPickup}, n::Integer)
+    modelpath = joinpath(@__DIR__, "armhand.xml")
+    return Tuple(ArmHandPickup(s) for s in LyceumBase.tconstruct(MJSim, n, modelpath, skip = 3))
+end
+ArmHandPickup() = first(tconstruct(ArmHandPickup, 1))
+
+@inline getsim(env::ArmHandPickup) = env.sim
+@inline obsspace(env::ArmHandPickup) = env.osp
+
+@propagate_inbounds function randreset!(rng::Random.AbstractRNG, env::ArmHandPickup)
+    fastreset_nofwd!(env.sim)
+    # randomize object starting position
+    env.sim.d.qpos[1] = rand(rng, Uniform(-0.15, 0.15))
+    env.sim.d.qpos[2] = rand(rng, Uniform(-0.1, 0.1))
+    forward!(env.sim)
+    env
+end
+
+@inline _sitedist(s1, s2, dmin) = min(euclidean(s1, s2), dmin)
+
+@propagate_inbounds function getobs!(obs, env::ArmHandPickup)
+    m, d = env.sim.m, env.sim.d
+    osp  = obsspace(env)
+    sx   = d.site_xpos
+    dmin = 0.5
+
+    _ball = siteSA(sx, env.ball)
+    _palm = siteSA(sx, env.palm)
+    _thumb  = siteSA(sx, env.thumb)
+    _index  = siteSA(sx, env.index)
+    _middle = siteSA(sx, env.middle)
+    _ring   = siteSA(sx, env.ring)
+    _pinky  = siteSA(sx, env.pinky)
+
+    _goal = _ball - env.goal
+
+    @uviews obs @inbounds begin
+        shaped = osp(obs)
+
+        shaped.ball    .= _ball
+        shaped.palm    .= _palm .- _ball
+
+        shaped.handball = _sitedist(_palm, _ball, dmin)
+        shaped.ballgoal = _sitedist(_ball, _goal, dmin)
+
+        shaped.d_thumb  = _sitedist(_thumb,  _ball, dmin)
+        shaped.d_index  = _sitedist(_index,  _ball, dmin)
+        shaped.d_middle = _sitedist(_middle, _ball, dmin)
+        shaped.d_ring   = _sitedist(_ring,   _ball, dmin)
+        shaped.d_pinky  = _sitedist(_pinky,  _ball, dmin)
+        shaped.a_thumb  = cosine_dist(_goal, _thumb)
+        shaped.a_index  = cosine_dist(_goal, _index)
+        shaped.a_middle = cosine_dist(_goal, _middle)
+        shaped.a_ring   = cosine_dist(_goal, _ring)
+        shaped.a_pinky  = cosine_dist(_goal, _pinky)
+        shaped.a_close  = cosine_dist(_middle, _thumb)
+    end
+    obs
+end
+
+@propagate_inbounds function getreward(state, action, obs, env::ArmHandPickup)
+    os = obsspace(env)(obs)
+    _handball = os.handball / 0.5
+    _ballgoal = os.ballgoal / 0.5
+
+    reward = -_handball
+    if _handball < 0.06
+        reward = 2.0 - 2 * _ballgoal
+        reward -= ( os.d_thumb + os.d_index + os.d_middle + os.d_ring + os.d_pinky ) * 0.1
+        #reward += os.ball[3]
+    end
+    reward
+end
+
+@propagate_inbounds function geteval(state, action, obs, env::ArmHandPickup)
+    os = obsspace(env)(obs)
+    os.ball[3]
+end
+
+

test/runtests.jl

@@ -3,7 +3,8 @@ using Test, Random, Shapes, Pkg, LinearAlgebra, BenchmarkTools
 using MuJoCo: TESTMODELXML
 
 const LYCEUM_SUITE = [
-    (LyceumMuJoCo.PointMass, (), ())
+    (LyceumMuJoCo.PointMass, (), ()),
+    (LyceumMuJoCo.ArmHandPickup, (), ()),
 ]
 
 const GYM = [