From aef329ee5bbab9dcc5ae88288918a9734fb04064 Mon Sep 17 00:00:00 2001 From: Robin Deits Date: Wed, 16 Aug 2017 18:22:20 -0400 Subject: [PATCH 01/11] WIP: complementarity with articulations --- complementarity.ipynb | 202 ++++----------------- examples/articulated_complementarity.jl | 229 ++++++++++++++++++++++++ 2 files changed, 260 insertions(+), 171 deletions(-) create mode 100644 examples/articulated_complementarity.jl diff --git a/complementarity.ipynb b/complementarity.ipynb index 7c6b0ff..35d9df7 100644 --- a/complementarity.ipynb +++ b/complementarity.ipynb @@ -6,7 +6,7 @@ "metadata": {}, "outputs": [], "source": [ - "using ProfileView\n", + "using Polyhedra\n", "using DrakeVisualizer, CoordinateTransformations\n", "DrakeVisualizer.any_open_windows() || DrakeVisualizer.new_window()" ] @@ -17,11 +17,9 @@ "metadata": {}, "outputs": [], "source": [ - "using Polyhedra\n", - "using StaticArrays\n", - "using CoordinateTransformations\n", - "using JuMP, ConditionalJuMP, Cbc\n", - "using Base.Test" + "vis = Visualizer()[:hopper]\n", + "setgeometry!(vis, HyperRectangle(Vec(-0.1, -0.1, 0), Vec(0.2, 0.2, 0.2)))\n", + "setgeometry!(vis[:foot], HyperRectangle(Vec(-0.05, -0.05, 0), Vec(0.1, 0.1, 0.1)))" ] }, { @@ -30,7 +28,7 @@ "metadata": {}, "outputs": [], "source": [ - "rot2(θ) = SMatrix{2, 2}(cos(θ), -sin(θ), sin(θ), cos(θ))" + "include(\"examples/articulated_complementarity.jl\")" ] }, { @@ -39,165 +37,32 @@ "metadata": {}, "outputs": [], "source": [ - "struct Obstacle{N, T, H <: HRepresentation{N, T}}\n", - " interior::H\n", - " contact_face::HalfSpace{N, T}\n", - "end\n", - "\n", - "struct Environment{N, T, H1 <: HRepresentation{N, T}, H2 <: HRepresentation{N, T}}\n", - " obstacles::Vector{Obstacle{N, T, H1}}\n", - " free_regions::Vector{H2}\n", - "end\n", - "\n", - "function contact_basis(face::HalfSpace{2})\n", - " θ = atan(μ)\n", - " R = rot2(θ)\n", - " hcat(R * face.a, R' * face.a)\n", - "end\n", - " \n", "\n", - "contact_basis(obs::Obstacle) = contact_basis(obs.contact_face)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "env = Environment(\n", + "env = Complementarity.Environment(\n", " [\n", - " Obstacle(\n", + " Complementarity.Obstacle(\n", " SimpleHRepresentation{2, Float64}([0 1], [0]),\n", " HalfSpace{2, Float64}([0, 1], 0)\n", + " ),\n", + " Complementarity.Obstacle(\n", + " SimpleHRepresentation{2, Float64}([-1 0], [-0.2]),\n", + " HalfSpace{2, Float64}([-1, 0], -0.2)\n", " )\n", " ],\n", " [\n", - " SimpleHRepresentation{2, Float64}([0 -1], [0])\n", + " SimpleHRepresentation{2, Float64}(\n", + " [0 -1;\n", + " 1 0],\n", + " [0, 0.2])\n", " ]\n", - ")\n" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# A simple complementarity-based time-stepping rigid body simulation. All\n", - "# notation is taken from Stewart & Trinkle \"An Implicit Time-Stepping Scheme for\n", - "# Rigid Body Dynamics with Coulomb Friction\". This particular example solves\n", - "# for all N timesteps simultaneously. That's not actually necessary, but it makes\n", - "# the code a bit simpler to read. \n", - "#\n", - "# The model consists of a point mass (visualized as a brick) moving in two dimensions\n", - "# with gravity and a single planar surface at y = 0. \n", - "\n", - "h = 0.05\n", - "μ = 0.5\n", - "n = [0, 1]\n", - "mass = 1.0\n", - "g = [0, -9.81]\n", - "\n", - "struct ContactResult{T, Tf}\n", - " β::Vector{T}\n", - " λ::T\n", - " c_n::T\n", - " contact_force::Tf\n", - "end\n", - "\n", - "JuMP.getvalue(c::ContactResult) = ContactResult(getvalue.((c.β, c.λ, c.c_n, c.contact_force))...)\n", - "\n", - "struct LCPUpdate{T, Tf}\n", - " q::Vector{T}\n", - " v::Vector{T}\n", - " contacts::Vector{ContactResult{T, Tf}}\n", - "end\n", - "\n", - "JuMP.getvalue(up::LCPUpdate) =\n", - " LCPUpdate(getvalue.((up.q, up.v))..., getvalue.(up.contacts))\n", + ")\n", "\n", - "function contact_force(qnext, vnext, obstacle::Obstacle, model::Model)\n", - " n = obstacle.contact_face.a\n", - " D = contact_basis(obstacle)\n", - " k = size(D, 2)\n", - " \n", - " β = @variable(model, [1:k], lowerbound=0, basename=\"β\", upperbound=100)\n", - " λ = @variable(model, lowerbound=0, basename=\"λ\", upperbound=100)\n", - " c_n = @variable(model, lowerbound=0, basename=\"c_n\", upperbound=100)\n", - " \n", - " @constraints model begin\n", - " λ .+ D' * vnext .>= 0 # (8)\n", - " μ * c_n .- sum(β) >= 0 # (9)\n", - " end\n", - " \n", - " @disjunction(model, (n' * qnext == 0), (c_n == 0)) # (10)\n", - " Dtv = D' * vnext\n", - " for j in 1:k\n", - " @disjunction(model, ((λ + Dtv[j]) == 0), β[j] == 0) # (11)\n", - " end\n", - " @disjunction(model, (μ * c_n - sum(β) == 0), (λ == 0)) # (12)\n", - " \n", - " contact_force = c_n * n .+ D * β\n", - " ContactResult(β, λ, c_n, contact_force)\n", - "end\n", - "\n", - "function update(q, v, env::Environment, model::Model)\n", - " qnext = @variable(model, [1:length(q)], lowerbound=-10, basename=\"qnext\", upperbound=10)\n", - " vnext = @variable(model, [1:length(v)], lowerbound=-10, basename=\"vnext\", upperbound=10)\n", - " \n", - " contacts = [contact_force(qnext, vnext, obs, model) for obs in env.obstacles]\n", - " total_force = mass * g + sum([c.contact_force for c in contacts])\n", - " \n", - " ConditionalJuMP.disjunction!(\n", - " model, \n", - " [@?(qnext ∈ P) for P in env.free_regions])\n", - " \n", - " @constraints model begin\n", - " mass * (vnext - v) .== h * total_force # (5)\n", - " qnext - q .== h .* vnext # (6)\n", - " qnext[2] >= 0 # (7)\n", - " end\n", - "\n", - " LCPUpdate(qnext, vnext, contacts)\n", - "end\n", - "\n", - "function simulate(q0, v0, env::Environment, N)\n", - " q, v = q0, v0\n", - " results = LCPUpdate{Float64}[]\n", - " for i in 1:N\n", - " m = Model(solver=CbcSolver())\n", - " up = update(q, v, env, m)\n", - " solve(m)\n", - " push!(results, getvalue(up))\n", - " q = results[end].q\n", - " v = results[end].v\n", - " end\n", - " results\n", - "end\n", - "\n", - "function optimize(q0, v0, env::Environment, N)::Vector{LCPUpdate{Float64}}\n", - " q, v = q0, v0\n", - " m = Model(solver=CbcSolver())\n", - " results = []\n", - " for i in 1:N\n", - " up = update(q, v, env, m)\n", - " push!(results, up)\n", - " q = results[end].q\n", - " v = results[end].v\n", - " end\n", - " solve(m)\n", - " getvalue.(results)\n", - "end " - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "hs = env.obstacles[1].contact_face" + "q0 = [-0.5, 1.25, 1.0]\n", + "v0 = [0, 0, 0.0]\n", + "kp = 10\n", + "kd = 0.1 * kp\n", + "controller = (q, v) -> kp * (1 - q[3]) - kd * v[3]\n", + "N = 20" ] }, { @@ -206,27 +71,22 @@ "metadata": {}, "outputs": [], "source": [ - "q0 = [-1, 0.5]\n", - "v0 = [2, 0.5]\n", - "N = 40" + "results = Complementarity.simulate(q0, v0, controller, env, 40);" ] }, { "cell_type": "code", "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "results = simulate(q0, v0, env, N)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, + "metadata": { + "collapsed": true + }, "outputs": [], "source": [ - "optimize(q0, v0, env, N)" + "for r in results\n", + " settransform!(vis, Translation(r.q[1], 0, r.q[2]))\n", + " settransform!(vis[:foot], Translation(0, 0, -r.q[3]))\n", + " sleep(0.1)\n", + "end" ] }, { diff --git a/examples/articulated_complementarity.jl b/examples/articulated_complementarity.jl new file mode 100644 index 0000000..1878109 --- /dev/null +++ b/examples/articulated_complementarity.jl @@ -0,0 +1,229 @@ +module Complementarity + +using Polyhedra +using StaticArrays +using JuMP, ConditionalJuMP, Cbc +using Base.Test + +rot2(θ) = SMatrix{2, 2}(cos(θ), -sin(θ), sin(θ), cos(θ)) + +struct Obstacle{N, T, H <: HRepresentation{N, T}} + interior::H + contact_face::HalfSpace{N, T} +end + +struct Environment{N, T, H1 <: HRepresentation{N, T}, H2 <: HRepresentation{N, T}} + obstacles::Vector{Obstacle{N, T, H1}} + free_regions::Vector{H2} +end + +function contact_basis(face::HalfSpace{2}) + θ = atan(μ) + R = rot2(θ) + hcat(R * face.a, R' * face.a) +end + + +contact_basis(obs::Obstacle) = contact_basis(obs.contact_face) + +function ConditionalJuMP.Conditional(op::typeof(in), x::AbstractVector, P::HRepresentation) + ConditionalJuMP.Conditional(&, [@?(P.A[i, :]' * x <= P.b[i]) for i in 1:length(P)]...) +end + + +# A simple complementarity-based time-stepping rigid body simulation. All +# notation is taken from Stewart & Trinkle "An Implicit Time-Stepping Scheme for +# Rigid Body Dynamics with Coulomb Friction". + +const h = 0.05 +const μ = 0.5 +const n = [0, 1] +const mass = 1.0 +const g = [0, -9.81] +const min_leg_len = 0.5 +const max_leg_len = 1.5 + +struct ContactResult{T, Tf} + β::Vector{T} + λ::T + c_n::T + contact_force::Tf +end + +JuMP.getvalue(c::ContactResult) = ContactResult(getvalue.((c.β, c.λ, c.c_n, c.contact_force))...) + +struct LCPUpdate{T, Tf} + q::Vector{T} + v::Vector{T} + contacts::Vector{ContactResult{T, Tf}} +end + +JuMP.getvalue(up::LCPUpdate) = + LCPUpdate(getvalue.((up.q, up.v))..., getvalue.(up.contacts)) + +function leg_position_in_world(q) + q[1:2] + [0, -1] * q[3] +end + +function leg_velocity_in_world(v) + v[1:2] + [0, -1] * v[3] +end + +function contact_force(qnext, vnext, obstacle::Obstacle, model::Model) + n = obstacle.contact_face.a + D = contact_basis(obstacle) + k = size(D, 2) + + β = @variable(model, [1:k], lowerbound=0, basename="β", upperbound=100) + λ = @variable(model, lowerbound=0, basename="λ", upperbound=100) + c_n = @variable(model, lowerbound=0, basename="c_n", upperbound=100) + + separation = n' * leg_position_in_world(qnext) - obstacle.contact_face.β + contact_velocity = leg_velocity_in_world(vnext) + + @constraints model begin + λ .+ D' * contact_velocity .>= 0 # (8) + μ * c_n .- sum(β) >= 0 # (9) + end + + @disjunction(model, (separation == 0), (c_n == 0)) # (10) + Dtv = D' * contact_velocity + for j in 1:k + @disjunction(model, ((λ + Dtv[j]) == 0), β[j] == 0) # (11) + end + @disjunction(model, (μ * c_n - sum(β) == 0), (λ == 0)) # (12) + + contact_force = c_n * n .+ D * β + ContactResult(β, λ, c_n, contact_force) +end + +function update(q, v, u, env::Environment, model::Model) + qnext = @variable(model, [1:length(q)], lowerbound=-10, basename="qnext", upperbound=10) + vnext = @variable(model, [1:length(v)], lowerbound=-10, basename="vnext", upperbound=10) + + contacts = [contact_force(qnext, vnext, obs, model) for obs in env.obstacles] + total_force = sum([c.contact_force for c in contacts]) + + @constraints model begin + mass * (vnext[1:2] - v[1:2]) .== h * mass * g .- [0, -1] * u # (5) + mass * (vnext[3] - v[3]) == [0, -1]' * total_force + u + qnext - q .== h .* vnext # (6) + end + + # @constraints model begin + # mass * (vnext - v) .== h * total_force # (5) + # qnext - q .== h .* vnext # (6) + # end + + ConditionalJuMP.disjunction!( + model, + [@?(leg_position_in_world(qnext) ∈ P) for P in env.free_regions]) # (7) + + LCPUpdate(qnext, vnext, contacts) +end + +function simulate(q0, v0, controller, env::Environment, N) + q, v = q0, v0 + results = LCPUpdate{Float64}[] + for i in 1:N + m = Model(solver=CbcSolver()) + u = controller(q, v) + up = update(q, v, u, env, m) + solve(m) + push!(results, getvalue(up)) + q = results[end].q + v = results[end].v + end + results +end + +function optimize(q0, v0, env::Environment, N::Integer)::Vector{LCPUpdate{Float64}} + q, v = q0, v0 + m = Model(solver=CbcSolver()) + results = [] + for i in 1:N + up = update(q, v, env, m) + push!(results, up) + q = results[end].q + v = results[end].v + end + solve(m) + getvalue.(results) +end + +function JuMP.setvalue(contact::ContactResult{<:JuMP.AbstractJuMPScalar}, seed::ContactResult{<:Number}) + setvalue(contact.β, seed.β) + setvalue(contact.λ, seed.λ) + setvalue(contact.c_n, seed.c_n) + @assert getvalue(contact.contact_force) ≈ seed.contact_force +end + +function JuMP.setvalue(up::LCPUpdate{<:JuMP.AbstractJuMPScalar}, seed::LCPUpdate{<:Number}) + setvalue(up.q, seed.q) + setvalue(up.v, seed.v) + setvalue.(up.contacts, seed.contacts) +end + +function optimize(q0, v0, env::Environment, seed::Vector{<:LCPUpdate}) + q, v = q0, v0 + m = Model(solver=CbcSolver()) + results = [] + for i in 1:N + up = update(q, v, env, m) + setvalue(up, seed[i]) + push!(results, up) + q = results[end].q + v = results[end].v + end + warmstart!(m, true) + @assert sum(m.colCat .== :Bin) == 0 + solve(m) + getvalue.(results) +end + + +# env = Environment( +# [ +# Obstacle( +# SimpleHRepresentation{2, Float64}([0 1], [0]), +# HalfSpace{2, Float64}([0, 1], 0) +# ), +# Obstacle( +# SimpleHRepresentation{2, Float64}([-1 0], [-0.2]), +# HalfSpace{2, Float64}([-1, 0], -0.2) +# ) +# ], +# [ +# SimpleHRepresentation{2, Float64}( +# [0 -1; +# 1 0], +# [0, 0.2]) +# ] +# ) + +# q0 = [-0.5, 0.5] +# v0 = [3, -1.5] +# N = 20 +# results1 = simulate(q0, v0, env, N) +# results2 = optimize(q0, v0, env, N) +# @test all([r1.q ≈ r2.q for (r1, r2) in zip(results1, results2)]) + +# results_seeded = optimize(q0, v0, env, results1) +# @test all([r1.q ≈ r2.q for (r1, r2) in zip(results1, results_seeded)]) + +# if Pkg.installed("DrakeVisualizer") !== nothing +# @eval using DrakeVisualizer; +# @eval using CoordinateTransformations +# DrakeVisualizer.any_open_windows() || DrakeVisualizer.new_window() + +# vis = Visualizer()[:block] +# setgeometry!(vis, HyperRectangle(Vec(-0.1, -0.1, 0), Vec(0.2, 0.2, 0.2))) + +# q = [r.q for r in results1] +# for qi in q +# settransform!(vis, Translation(qi[1], 0, qi[2])) +# sleep(h) +# end +# end + +end \ No newline at end of file From 07493d2c7f661a402cb272701856ef46b252cc88 Mon Sep 17 00:00:00 2001 From: Robin Deits Date: Thu, 17 Aug 2017 15:00:59 -0400 Subject: [PATCH 02/11] add joint limit support with hacked-up dynamics --- complementarity.ipynb | 22 ++++++++- examples/articulated_complementarity.jl | 65 ++++++++++++++++++------- 2 files changed, 67 insertions(+), 20 deletions(-) diff --git a/complementarity.ipynb b/complementarity.ipynb index 35d9df7..051b965 100644 --- a/complementarity.ipynb +++ b/complementarity.ipynb @@ -11,6 +11,15 @@ "DrakeVisualizer.any_open_windows() || DrakeVisualizer.new_window()" ] }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "using Plots; gr()" + ] + }, { "cell_type": "code", "execution_count": null, @@ -59,8 +68,8 @@ "\n", "q0 = [-0.5, 1.25, 1.0]\n", "v0 = [0, 0, 0.0]\n", - "kp = 10\n", - "kd = 0.1 * kp\n", + "kp = 1.25\n", + "kd = 0.01 * kp\n", "controller = (q, v) -> kp * (1 - q[3]) - kd * v[3]\n", "N = 20" ] @@ -89,6 +98,15 @@ "end" ] }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "plot([r.q[2] for r in results])" + ] + }, { "cell_type": "code", "execution_count": null, diff --git a/examples/articulated_complementarity.jl b/examples/articulated_complementarity.jl index 1878109..b20b268 100644 --- a/examples/articulated_complementarity.jl +++ b/examples/articulated_complementarity.jl @@ -51,15 +51,40 @@ struct ContactResult{T, Tf} end JuMP.getvalue(c::ContactResult) = ContactResult(getvalue.((c.β, c.λ, c.c_n, c.contact_force))...) +function JuMP.setvalue(contact::ContactResult{<:JuMP.AbstractJuMPScalar}, seed::ContactResult{<:Number}) + setvalue(contact.β, seed.β) + setvalue(contact.λ, seed.λ) + setvalue(contact.c_n, seed.c_n) + @assert getvalue(contact.contact_force) ≈ seed.contact_force +end + + +struct JointLimitResult{T, Tf <: AbstractVector} + λ::T + generalized_force::Tf +end + +JuMP.getvalue(r::JointLimitResult) = JointLimitResult(getvalue.((r.λ, r.generalized_force))...) +function JuMP.setvalue(r::JointLimitResult{<:JuMP.AbstractJuMPScalar}, seed::JointLimitResult{<:Number}) + setvalue(r.λ, seed.λ) + @assert getvalue(r.generalized_force) ≈ seed.generalized_force +end struct LCPUpdate{T, Tf} q::Vector{T} v::Vector{T} contacts::Vector{ContactResult{T, Tf}} + joint_contacts::Vector{JointLimitResult{T, Tf}} end JuMP.getvalue(up::LCPUpdate) = - LCPUpdate(getvalue.((up.q, up.v))..., getvalue.(up.contacts)) + LCPUpdate(getvalue.((up.q, up.v))..., getvalue.(up.contacts), getvalue.(up.joint_contacts)) +function JuMP.setvalue(up::LCPUpdate{<:JuMP.AbstractJuMPScalar}, seed::LCPUpdate{<:Number}) + setvalue(up.q, seed.q) + setvalue(up.v, seed.v) + setvalue.(up.contacts, seed.contacts) + setvalue.(up.joint_contacts, seed.joint_contacts) +end function leg_position_in_world(q) q[1:2] + [0, -1] * q[3] @@ -97,16 +122,33 @@ function contact_force(qnext, vnext, obstacle::Obstacle, model::Model) ContactResult(β, λ, c_n, contact_force) end +function joint_limit(qnext, vnext, a::AbstractVector, b::Number, model::Model) + λ = @variable(model, lowerbound=0, upperbound=100, basename="λ") + separation = a' * qnext - b + @constraint model separation <= 0 + @disjunction(model, separation == 0, λ == 0) + + JointLimitResult(λ, -λ * a) +end + +function join_limits(qnext, vnext, limits::SimpleHRepresentation, model::Model) + [joint_limit(qnext, vnext, limits.A[i, :], limits.b[i], model) for i in 1:length(limits)] +end + function update(q, v, u, env::Environment, model::Model) qnext = @variable(model, [1:length(q)], lowerbound=-10, basename="qnext", upperbound=10) vnext = @variable(model, [1:length(v)], lowerbound=-10, basename="vnext", upperbound=10) contacts = [contact_force(qnext, vnext, obs, model) for obs in env.obstacles] - total_force = sum([c.contact_force for c in contacts]) + external_force = sum([c.contact_force for c in contacts]) + + join_limit_results = join_limits(qnext, vnext, SimpleHRepresentation([0. 0 1; 0 0 -1], [1.5, -0.5]), model) + + internal_force = u + sum([r.generalized_force[3] for r in join_limit_results]) @constraints model begin - mass * (vnext[1:2] - v[1:2]) .== h * mass * g .- [0, -1] * u # (5) - mass * (vnext[3] - v[3]) == [0, -1]' * total_force + u + mass * (vnext[1:2] - v[1:2]) .== h * mass * g .- [0, -1] * internal_force # (5) + mass * (vnext[3] - v[3]) == [0, -1]' * external_force + internal_force qnext - q .== h .* vnext # (6) end @@ -119,7 +161,7 @@ function update(q, v, u, env::Environment, model::Model) model, [@?(leg_position_in_world(qnext) ∈ P) for P in env.free_regions]) # (7) - LCPUpdate(qnext, vnext, contacts) + LCPUpdate(qnext, vnext, contacts, join_limit_results) end function simulate(q0, v0, controller, env::Environment, N) @@ -151,19 +193,6 @@ function optimize(q0, v0, env::Environment, N::Integer)::Vector{LCPUpdate{Float6 getvalue.(results) end -function JuMP.setvalue(contact::ContactResult{<:JuMP.AbstractJuMPScalar}, seed::ContactResult{<:Number}) - setvalue(contact.β, seed.β) - setvalue(contact.λ, seed.λ) - setvalue(contact.c_n, seed.c_n) - @assert getvalue(contact.contact_force) ≈ seed.contact_force -end - -function JuMP.setvalue(up::LCPUpdate{<:JuMP.AbstractJuMPScalar}, seed::LCPUpdate{<:Number}) - setvalue(up.q, seed.q) - setvalue(up.v, seed.v) - setvalue.(up.contacts, seed.contacts) -end - function optimize(q0, v0, env::Environment, seed::Vector{<:LCPUpdate}) q, v = q0, v0 m = Model(solver=CbcSolver()) From 3f3cb5750ed5df77109c4206ffba50f4ac47434b Mon Sep 17 00:00:00 2001 From: Robin Deits Date: Thu, 17 Aug 2017 19:01:45 -0400 Subject: [PATCH 03/11] first working version with RBD models --- complementarity.ipynb | 111 ++++++++---- examples/articulated_complementarity.jl | 229 +++++++++++++----------- 2 files changed, 195 insertions(+), 145 deletions(-) diff --git a/complementarity.ipynb b/complementarity.ipynb index 051b965..39a7213 100644 --- a/complementarity.ipynb +++ b/complementarity.ipynb @@ -7,8 +7,11 @@ "outputs": [], "source": [ "using Polyhedra\n", - "using DrakeVisualizer, CoordinateTransformations\n", - "DrakeVisualizer.any_open_windows() || DrakeVisualizer.new_window()" + "using DrakeVisualizer\n", + "DrakeVisualizer.any_open_windows() || DrakeVisualizer.new_window()\n", + "using RigidBodyDynamics\n", + "using StaticArrays\n", + "using RigidBodyTreeInspector" ] }, { @@ -17,18 +20,18 @@ "metadata": {}, "outputs": [], "source": [ - "using Plots; gr()" + "vis = Visualizer()[:hopper]" ] }, { "cell_type": "code", "execution_count": null, - "metadata": {}, + "metadata": { + "collapsed": true + }, "outputs": [], "source": [ - "vis = Visualizer()[:hopper]\n", - "setgeometry!(vis, HyperRectangle(Vec(-0.1, -0.1, 0), Vec(0.2, 0.2, 0.2)))\n", - "setgeometry!(vis[:foot], HyperRectangle(Vec(-0.05, -0.05, 0), Vec(0.1, 0.1, 0.1)))" + "using JuMP, Cbc" ] }, { @@ -46,32 +49,54 @@ "metadata": {}, "outputs": [], "source": [ + "g = -9.81\n", + "world = RigidBody{Float64}(\"world\")\n", + "hopper = Mechanism(world; gravity=SVector(0, 0, g))\n", + "\n", + "frame = CartesianFrame3D(\"core\")\n", + "mass = 1.\n", + "inertia = SpatialInertia(frame, 0.01 * eye(SMatrix{3, 3}), SVector(0., 0, 0), mass)\n", + "core = RigidBody(inertia)\n", + "\n", + "floating_base = Joint(\"core_to_world\", QuaternionFloating{Float64}())\n", + "attach!(hopper, world, floating_base, eye(Transform3D, frame_before(floating_base), default_frame(world)), core)\n", + "\n", + "frame = CartesianFrame3D(\"foot\")\n", + "mass = 1.\n", + "inertia = SpatialInertia(frame, 0.01 * eye(SMatrix{3, 3}), SVector(0., 0, 0), mass)\n", + "foot = RigidBody(inertia)\n", + "leg = Joint(\"leg\", Prismatic(SVector(0, 0, -1.)))\n", + "attach!(hopper, core, leg, eye(Transform3D, frame_before(leg), default_frame(core)), foot)\n", "\n", + "foot_contact = Point3D(default_frame(foot), SVector(0, 0, 0))\n", + "\n", + "setgeometry!(vis, hopper, create_geometry(hopper, show_inertias=true))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ "env = Complementarity.Environment(\n", - " [\n", - " Complementarity.Obstacle(\n", - " SimpleHRepresentation{2, Float64}([0 1], [0]),\n", - " HalfSpace{2, Float64}([0, 1], 0)\n", - " ),\n", - " Complementarity.Obstacle(\n", - " SimpleHRepresentation{2, Float64}([-1 0], [-0.2]),\n", - " HalfSpace{2, Float64}([-1, 0], -0.2)\n", + " Dict(foot => Complementarity.ContactEnvironment(\n", + " [foot_contact],\n", + " [\n", + " Complementarity.Obstacle(\n", + " SimpleHRepresentation{3, Float64}([0 0 1], [0]),\n", + " HalfSpace{3, Float64}([0, 0, 1], 0),\n", + " 0.5\n", + " ),\n", + " ],\n", + " [\n", + " SimpleHRepresentation{3, Float64}(\n", + " [0 0 -1],\n", + " [0])\n", + " ])\n", " )\n", - " ],\n", - " [\n", - " SimpleHRepresentation{2, Float64}(\n", - " [0 -1;\n", - " 1 0],\n", - " [0, 0.2])\n", - " ]\n", ")\n", - "\n", - "q0 = [-0.5, 1.25, 1.0]\n", - "v0 = [0, 0, 0.0]\n", - "kp = 1.25\n", - "kd = 0.01 * kp\n", - "controller = (q, v) -> kp * (1 - q[3]) - kd * v[3]\n", - "N = 20" + " " ] }, { @@ -80,22 +105,31 @@ "metadata": {}, "outputs": [], "source": [ - "results = Complementarity.simulate(q0, v0, controller, env, 40);" + "x0 = MechanismState(hopper, [0., 0, 0, 1, 0, 0, 1.25, 1.0], zeros(num_velocities(hopper)))\n", + "\n", + "u = zeros(num_velocities(x0))\n", + "\n", + "model = Model(solver=CbcSolver())\n", + "up = Complementarity.update(x0, u, env, 0.1, model)" ] }, { "cell_type": "code", "execution_count": null, "metadata": { - "collapsed": true + "scrolled": false }, "outputs": [], "source": [ - "for r in results\n", - " settransform!(vis, Translation(r.q[1], 0, r.q[2]))\n", - " settransform!(vis[:foot], Translation(0, 0, -r.q[3]))\n", - " sleep(0.1)\n", - "end" + "controller = (x) -> begin\n", + " kp = 40\n", + " kd = 0.1 * kp\n", + " u = zeros(num_velocities(x))\n", + " u[end] = kp * (1 - configuration(x)[end]) - kd * velocity(x)[end]\n", + " u\n", + "end\n", + "\n", + "results = Complementarity.simulate(x0, controller, env, 0.1, 20);" ] }, { @@ -104,7 +138,10 @@ "metadata": {}, "outputs": [], "source": [ - "plot([r.q[2] for r in results])" + "for r in results\n", + " settransform!(vis, r.state)\n", + " sleep(0.1)\n", + "end" ] }, { diff --git a/examples/articulated_complementarity.jl b/examples/articulated_complementarity.jl index b20b268..15c405b 100644 --- a/examples/articulated_complementarity.jl +++ b/examples/articulated_complementarity.jl @@ -4,45 +4,39 @@ using Polyhedra using StaticArrays using JuMP, ConditionalJuMP, Cbc using Base.Test +using RigidBodyDynamics +using RigidBodyDynamics: colwise +using Rotations +using ForwardDiff -rot2(θ) = SMatrix{2, 2}(cos(θ), -sin(θ), sin(θ), cos(θ)) - -struct Obstacle{N, T, H <: HRepresentation{N, T}} +struct Obstacle{T, H <: HRepresentation{3, T}} interior::H - contact_face::HalfSpace{N, T} + contact_face::HalfSpace{3, T} + μ::T +end + +struct ContactEnvironment{P <: Point3D, T, H <: HRepresentation{3, T}} + points::Vector{P} + obstacles::Vector{Obstacle{T, H}} + free_regions::Vector{H} end -struct Environment{N, T, H1 <: HRepresentation{N, T}, H2 <: HRepresentation{N, T}} - obstacles::Vector{Obstacle{N, T, H1}} - free_regions::Vector{H2} +struct Environment{B <: RigidBody, C <: ContactEnvironment} + contacts::Dict{B, C} end -function contact_basis(face::HalfSpace{2}) +function contact_basis(face::HalfSpace{3}, μ) θ = atan(μ) - R = rot2(θ) + R = RotY(θ) hcat(R * face.a, R' * face.a) end - -contact_basis(obs::Obstacle) = contact_basis(obs.contact_face) +contact_basis(obs::Obstacle) = contact_basis(obs.contact_face, obs.μ) function ConditionalJuMP.Conditional(op::typeof(in), x::AbstractVector, P::HRepresentation) ConditionalJuMP.Conditional(&, [@?(P.A[i, :]' * x <= P.b[i]) for i in 1:length(P)]...) end - -# A simple complementarity-based time-stepping rigid body simulation. All -# notation is taken from Stewart & Trinkle "An Implicit Time-Stepping Scheme for -# Rigid Body Dynamics with Coulomb Friction". - -const h = 0.05 -const μ = 0.5 -const n = [0, 1] -const mass = 1.0 -const g = [0, -9.81] -const min_leg_len = 0.5 -const max_leg_len = 1.5 - struct ContactResult{T, Tf} β::Vector{T} λ::T @@ -58,6 +52,7 @@ function JuMP.setvalue(contact::ContactResult{<:JuMP.AbstractJuMPScalar}, seed:: @assert getvalue(contact.contact_force) ≈ seed.contact_force end +JuMP.getvalue(f::FreeVector3D) = FreeVector3D(f.frame, getvalue(f.v)) struct JointLimitResult{T, Tf <: AbstractVector} λ::T @@ -70,18 +65,23 @@ function JuMP.setvalue(r::JointLimitResult{<:JuMP.AbstractJuMPScalar}, seed::Joi @assert getvalue(r.generalized_force) ≈ seed.generalized_force end -struct LCPUpdate{T, Tf} - q::Vector{T} - v::Vector{T} +function JuMP.getvalue(x::MechanismState{<:JuMP.AbstractJuMPScalar}) + MechanismState(x.mechanism, getvalue(configuration(x)), getvalue(velocity(x)), getvalue(additional_state(x))) +end + +struct LCPUpdate{T, M <: MechanismState{T}, Tf} + state::M contacts::Vector{ContactResult{T, Tf}} - joint_contacts::Vector{JointLimitResult{T, Tf}} + # joint_contacts::Vector{JointLimitResult{T, Tf}} end +# JuMP.getvalue(up::LCPUpdate) = +# LCPUpdate(getvalue(up.state), getvalue.(up.contacts), getvalue.(up.joint_contacts)) JuMP.getvalue(up::LCPUpdate) = - LCPUpdate(getvalue.((up.q, up.v))..., getvalue.(up.contacts), getvalue.(up.joint_contacts)) + LCPUpdate(getvalue(up.state), getvalue.(up.contacts)) + function JuMP.setvalue(up::LCPUpdate{<:JuMP.AbstractJuMPScalar}, seed::LCPUpdate{<:Number}) - setvalue(up.q, seed.q) - setvalue(up.v, seed.v) + setvalue(up.state, seed.state) setvalue.(up.contacts, seed.contacts) setvalue.(up.joint_contacts, seed.joint_contacts) end @@ -94,7 +94,12 @@ function leg_velocity_in_world(v) v[1:2] + [0, -1] * v[3] end -function contact_force(qnext, vnext, obstacle::Obstacle, model::Model) +struct ContactPoint{Tb <: RigidBody, Tp <: Point3D} + body::Tb + point::Tp +end + +function resolve_contact(xnext::MechanismState, contact_point::ContactPoint, obstacle::Obstacle, model::Model, x_dynamics::MechanismState{<:Number}) n = obstacle.contact_face.a D = contact_basis(obstacle) k = size(D, 2) @@ -103,12 +108,29 @@ function contact_force(qnext, vnext, obstacle::Obstacle, model::Model) λ = @variable(model, lowerbound=0, basename="λ", upperbound=100) c_n = @variable(model, lowerbound=0, basename="c_n", upperbound=100) - separation = n' * leg_position_in_world(qnext) - obstacle.contact_face.β - contact_velocity = leg_velocity_in_world(vnext) + function separation_in_world(x) + q = x[1:num_positions(xnext)] + v = x[(num_positions(xnext)+1):end] + x_diff = MechanismState(xnext.mechanism, q, v) + point_in_world = transform_to_root(x_diff, contact_point.point.frame) * contact_point.point + separation = n' * point_in_world.v - obstacle.contact_face.β + end + + separation = separation_in_world(state_vector(x_dynamics)) + (state_vector(xnext) - state_vector(x_dynamics))' * ForwardDiff.gradient(separation_in_world, state_vector(x_dynamics)) + + function contact_velocity_in_world(x) + q = x[1:num_positions(xnext)] + v = x[(num_positions(xnext)+1):end] + x_diff = MechanismState(xnext.mechanism, q, v) + point_in_world = transform_to_root(x_diff, contact_point.point.frame) * contact_point.point + contact_velocity = point_velocity(twist_wrt_world(x_diff, contact_point.body), point_in_world).v + end + + contact_velocity = contact_velocity_in_world(state_vector(x_dynamics)) + ForwardDiff.jacobian(contact_velocity_in_world, state_vector(x_dynamics)) * (state_vector(xnext) - state_vector(x_dynamics)) @constraints model begin λ .+ D' * contact_velocity .>= 0 # (8) - μ * c_n .- sum(β) >= 0 # (9) + obstacle.μ * c_n .- sum(β) >= 0 # (9) end @disjunction(model, (separation == 0), (c_n == 0)) # (10) @@ -116,9 +138,9 @@ function contact_force(qnext, vnext, obstacle::Obstacle, model::Model) for j in 1:k @disjunction(model, ((λ + Dtv[j]) == 0), β[j] == 0) # (11) end - @disjunction(model, (μ * c_n - sum(β) == 0), (λ == 0)) # (12) + @disjunction(model, (obstacle.μ * c_n - sum(β) == 0), (λ == 0)) # (12) - contact_force = c_n * n .+ D * β + contact_force = FreeVector3D(root_frame(xnext.mechanism), c_n * n .+ D * β) ContactResult(β, λ, c_n, contact_force) end @@ -135,46 +157,82 @@ function join_limits(qnext, vnext, limits::SimpleHRepresentation, model::Model) [joint_limit(qnext, vnext, limits.A[i, :], limits.b[i], model) for i in 1:length(limits)] end -function update(q, v, u, env::Environment, model::Model) - qnext = @variable(model, [1:length(q)], lowerbound=-10, basename="qnext", upperbound=10) - vnext = @variable(model, [1:length(v)], lowerbound=-10, basename="vnext", upperbound=10) +function update(x::MechanismState{X, M}, u, env::Environment, Δt::Real, model::Model, x_dynamics=x) where {X, M} + mechanism = x.mechanism + world = root_body(mechanism) + qnext = @variable(model, [1:num_positions(x)], lowerbound=-10, basename="qnext", upperbound=10) + vnext = @variable(model, [1:num_velocities(x)], lowerbound=-10, basename="vnext", upperbound=10) + xnext = MechanismState(mechanism, qnext, vnext) + + contact_results = [] + externalwrenches_list = [] + for (body, contact_env) in env.contacts + geo_jac = geometric_jacobian(x_dynamics, path(mechanism, body, world)) + wrenches = [] + + for contact_point in contact_env.points + for obs in contact_env.obstacles + result = resolve_contact(xnext, ContactPoint(body, contact_point), obs, model, x_dynamics) + push!(contact_results, result) + push!(wrenches, Wrench( + transform_to_root(x_dynamics, contact_point.frame) * contact_point, + result.contact_force)) + end + + function _point_in_world(x) + q = x[1:num_positions(xnext)] + v = x[(num_positions(xnext)+1):end] + x_diff = MechanismState(xnext.mechanism, q, v) + point_in_world = transform_to_root(x_diff, contact_point.frame) * contact_point + point_in_world.v + end + + point_in_world = _point_in_world(state_vector(x_dynamics)) + ForwardDiff.jacobian(_point_in_world, state_vector(x_dynamics)) * (state_vector(xnext) - state_vector(x_dynamics)) + + ConditionalJuMP.disjunction!(model, + [@?(point_in_world ∈ P) for P in contact_env.free_regions]) # (7) + end + push!(externalwrenches_list, (body => sum(wrenches))) + end + externalwrenches = Dict(externalwrenches_list) + contact_results_type = typeof(contact_results[1]) + contact_results_typed = convert(Vector{contact_results_type}, contact_results) - contacts = [contact_force(qnext, vnext, obs, model) for obs in env.obstacles] - external_force = sum([c.contact_force for c in contacts]) - join_limit_results = join_limits(qnext, vnext, SimpleHRepresentation([0. 0 1; 0 0 -1], [1.5, -0.5]), model) + H = mass_matrix(x_dynamics) + bias = dynamics_bias(x_dynamics, externalwrenches) - internal_force = u + sum([r.generalized_force[3] for r in join_limit_results]) + @constraint(model, H * (vnext - velocity(x)) .== Δt * (u .- bias)) # (5) - @constraints model begin - mass * (vnext[1:2] - v[1:2]) .== h * mass * g .- [0, -1] * internal_force # (5) - mass * (vnext[3] - v[3]) == [0, -1]' * external_force + internal_force - qnext - q .== h .* vnext # (6) + function _config_derivative(v) + q = oftype(v, configuration(x_dynamics)) + x_diff = MechanismState(mechanism, q, v) + configuration_derivative(x_diff) end - # @constraints model begin - # mass * (vnext - v) .== h * total_force # (5) - # qnext - q .== h .* vnext # (6) - # end + config_derivative = ForwardDiff.jacobian(_config_derivative, velocity(x_dynamics)) * velocity(xnext) + @constraint(model, qnext .- configuration(x) .== Δt .* config_derivative) + + LCPUpdate(xnext, contact_results_typed) - ConditionalJuMP.disjunction!( - model, - [@?(leg_position_in_world(qnext) ∈ P) for P in env.free_regions]) # (7) + # joint_limit_results = join_limits(qnext, vnext, SimpleHRepresentation([0. 0 1; 0 0 -1], [1.5, -0.5]), model) - LCPUpdate(qnext, vnext, contacts, join_limit_results) + # internal_force = u + sum([r.generalized_force[3] for r in joint_limit_results]) + + # LCPUpdate(qnext, vnext, contact_results, joint_limit_results) end -function simulate(q0, v0, controller, env::Environment, N) - q, v = q0, v0 - results = LCPUpdate{Float64}[] +function simulate(x0::MechanismState, controller, env::Environment, Δt::Real, N::Integer) + results = [] # todo: type this + x = x0 for i in 1:N m = Model(solver=CbcSolver()) - u = controller(q, v) - up = update(q, v, u, env, m) + u = controller(x) + up = update(x, u, env, Δt, m) + # up = update(q, v, u, env, m) solve(m) push!(results, getvalue(up)) - q = results[end].q - v = results[end].v + x = results[end].state end results end @@ -210,49 +268,4 @@ function optimize(q0, v0, env::Environment, seed::Vector{<:LCPUpdate}) getvalue.(results) end - -# env = Environment( -# [ -# Obstacle( -# SimpleHRepresentation{2, Float64}([0 1], [0]), -# HalfSpace{2, Float64}([0, 1], 0) -# ), -# Obstacle( -# SimpleHRepresentation{2, Float64}([-1 0], [-0.2]), -# HalfSpace{2, Float64}([-1, 0], -0.2) -# ) -# ], -# [ -# SimpleHRepresentation{2, Float64}( -# [0 -1; -# 1 0], -# [0, 0.2]) -# ] -# ) - -# q0 = [-0.5, 0.5] -# v0 = [3, -1.5] -# N = 20 -# results1 = simulate(q0, v0, env, N) -# results2 = optimize(q0, v0, env, N) -# @test all([r1.q ≈ r2.q for (r1, r2) in zip(results1, results2)]) - -# results_seeded = optimize(q0, v0, env, results1) -# @test all([r1.q ≈ r2.q for (r1, r2) in zip(results1, results_seeded)]) - -# if Pkg.installed("DrakeVisualizer") !== nothing -# @eval using DrakeVisualizer; -# @eval using CoordinateTransformations -# DrakeVisualizer.any_open_windows() || DrakeVisualizer.new_window() - -# vis = Visualizer()[:block] -# setgeometry!(vis, HyperRectangle(Vec(-0.1, -0.1, 0), Vec(0.2, 0.2, 0.2))) - -# q = [r.q for r in results1] -# for qi in q -# settransform!(vis, Translation(qi[1], 0, qi[2])) -# sleep(h) -# end -# end - end \ No newline at end of file From 5b5e0ed4532ae01d17c80fe31d2934dea036fbcb Mon Sep 17 00:00:00 2001 From: Robin Deits Date: Thu, 17 Aug 2017 19:25:22 -0400 Subject: [PATCH 04/11] clean up RBD complementarity --- complementarity.ipynb | 4 ++-- examples/articulated_complementarity.jl | 31 +++++++++---------------- 2 files changed, 13 insertions(+), 22 deletions(-) diff --git a/complementarity.ipynb b/complementarity.ipynb index 39a7213..72ecfa7 100644 --- a/complementarity.ipynb +++ b/complementarity.ipynb @@ -105,7 +105,7 @@ "metadata": {}, "outputs": [], "source": [ - "x0 = MechanismState(hopper, [0., 0, 0, 1, 0, 0, 1.25, 1.0], zeros(num_velocities(hopper)))\n", + "x0 = MechanismState(hopper, [0., 0, 0, 1, 0, 0, 1.5, 1.0], zeros(num_velocities(hopper)))\n", "\n", "u = zeros(num_velocities(x0))\n", "\n", @@ -129,7 +129,7 @@ " u\n", "end\n", "\n", - "results = Complementarity.simulate(x0, controller, env, 0.1, 20);" + "results = Complementarity.simulate(x0, controller, env, 0.05, 20);" ] }, { diff --git a/examples/articulated_complementarity.jl b/examples/articulated_complementarity.jl index 15c405b..3ef9bca 100644 --- a/examples/articulated_complementarity.jl +++ b/examples/articulated_complementarity.jl @@ -3,6 +3,7 @@ module Complementarity using Polyhedra using StaticArrays using JuMP, ConditionalJuMP, Cbc +using JuMP: GenericAffExpr using Base.Test using RigidBodyDynamics using RigidBodyDynamics: colwise @@ -94,12 +95,7 @@ function leg_velocity_in_world(v) v[1:2] + [0, -1] * v[3] end -struct ContactPoint{Tb <: RigidBody, Tp <: Point3D} - body::Tb - point::Tp -end - -function resolve_contact(xnext::MechanismState, contact_point::ContactPoint, obstacle::Obstacle, model::Model, x_dynamics::MechanismState{<:Number}) +function resolve_contact(xnext::MechanismState, body::RigidBody, point::Point3D, obstacle::Obstacle, model::Model, x_dynamics::MechanismState{<:Number}) n = obstacle.contact_face.a D = contact_basis(obstacle) k = size(D, 2) @@ -112,7 +108,7 @@ function resolve_contact(xnext::MechanismState, contact_point::ContactPoint, obs q = x[1:num_positions(xnext)] v = x[(num_positions(xnext)+1):end] x_diff = MechanismState(xnext.mechanism, q, v) - point_in_world = transform_to_root(x_diff, contact_point.point.frame) * contact_point.point + point_in_world = transform_to_root(x_diff, point.frame) * point separation = n' * point_in_world.v - obstacle.contact_face.β end @@ -122,8 +118,8 @@ function resolve_contact(xnext::MechanismState, contact_point::ContactPoint, obs q = x[1:num_positions(xnext)] v = x[(num_positions(xnext)+1):end] x_diff = MechanismState(xnext.mechanism, q, v) - point_in_world = transform_to_root(x_diff, contact_point.point.frame) * contact_point.point - contact_velocity = point_velocity(twist_wrt_world(x_diff, contact_point.body), point_in_world).v + point_in_world = transform_to_root(x_diff, point.frame) * point + contact_velocity = point_velocity(twist_wrt_world(x_diff, body), point_in_world).v end contact_velocity = contact_velocity_in_world(state_vector(x_dynamics)) + ForwardDiff.jacobian(contact_velocity_in_world, state_vector(x_dynamics)) * (state_vector(xnext) - state_vector(x_dynamics)) @@ -164,15 +160,14 @@ function update(x::MechanismState{X, M}, u, env::Environment, Δt::Real, model:: vnext = @variable(model, [1:num_velocities(x)], lowerbound=-10, basename="vnext", upperbound=10) xnext = MechanismState(mechanism, qnext, vnext) - contact_results = [] - externalwrenches_list = [] + contact_results = ContactResult{Variable,FreeVector3D{Vector{GenericAffExpr{M,Variable}}}}[] + externalwrenches = Dict{typeof(world), Wrench{GenericAffExpr{M,Variable}}}() for (body, contact_env) in env.contacts - geo_jac = geometric_jacobian(x_dynamics, path(mechanism, body, world)) - wrenches = [] + wrenches = Wrench{GenericAffExpr{M,Variable}}[] for contact_point in contact_env.points for obs in contact_env.obstacles - result = resolve_contact(xnext, ContactPoint(body, contact_point), obs, model, x_dynamics) + result = resolve_contact(xnext, body, contact_point, obs, model, x_dynamics) push!(contact_results, result) push!(wrenches, Wrench( transform_to_root(x_dynamics, contact_point.frame) * contact_point, @@ -192,12 +187,8 @@ function update(x::MechanismState{X, M}, u, env::Environment, Δt::Real, model:: ConditionalJuMP.disjunction!(model, [@?(point_in_world ∈ P) for P in contact_env.free_regions]) # (7) end - push!(externalwrenches_list, (body => sum(wrenches))) + externalwrenches[body] = sum(wrenches) end - externalwrenches = Dict(externalwrenches_list) - contact_results_type = typeof(contact_results[1]) - contact_results_typed = convert(Vector{contact_results_type}, contact_results) - H = mass_matrix(x_dynamics) bias = dynamics_bias(x_dynamics, externalwrenches) @@ -213,7 +204,7 @@ function update(x::MechanismState{X, M}, u, env::Environment, Δt::Real, model:: config_derivative = ForwardDiff.jacobian(_config_derivative, velocity(x_dynamics)) * velocity(xnext) @constraint(model, qnext .- configuration(x) .== Δt .* config_derivative) - LCPUpdate(xnext, contact_results_typed) + LCPUpdate(xnext, contact_results) # joint_limit_results = join_limits(qnext, vnext, SimpleHRepresentation([0. 0 1; 0 0 -1], [1.5, -0.5]), model) From 03b17c7a88e3ea93e3866559af39bae417003705 Mon Sep 17 00:00:00 2001 From: Robin Deits Date: Thu, 17 Aug 2017 19:35:44 -0400 Subject: [PATCH 05/11] minor cleanup --- complementarity.ipynb | 5 ++++- examples/articulated_complementarity.jl | 4 ++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/complementarity.ipynb b/complementarity.ipynb index 72ecfa7..77f42fe 100644 --- a/complementarity.ipynb +++ b/complementarity.ipynb @@ -96,7 +96,10 @@ " ])\n", " )\n", ")\n", - " " + "\n", + "limits = Dict(\n", + " leg => SimpleHRepresentation{1, Float64}([1 -1]', [1.5, -0.5])\n", + ")" ] }, { diff --git a/examples/articulated_complementarity.jl b/examples/articulated_complementarity.jl index 3ef9bca..aea4365 100644 --- a/examples/articulated_complementarity.jl +++ b/examples/articulated_complementarity.jl @@ -149,7 +149,7 @@ function joint_limit(qnext, vnext, a::AbstractVector, b::Number, model::Model) JointLimitResult(λ, -λ * a) end -function join_limits(qnext, vnext, limits::SimpleHRepresentation, model::Model) +function joint_limits(qnext, vnext, limits::SimpleHRepresentation, model::Model) [joint_limit(qnext, vnext, limits.A[i, :], limits.b[i], model) for i in 1:length(limits)] end @@ -206,7 +206,7 @@ function update(x::MechanismState{X, M}, u, env::Environment, Δt::Real, model:: LCPUpdate(xnext, contact_results) - # joint_limit_results = join_limits(qnext, vnext, SimpleHRepresentation([0. 0 1; 0 0 -1], [1.5, -0.5]), model) + # joint_limit_results = joint_limits(qnext, vnext, SimpleHRepresentation([0. 0 1; 0 0 -1], [1.5, -0.5]), model) # internal_force = u + sum([r.generalized_force[3] for r in joint_limit_results]) From f12da6b0bb54e4201c114e85a01d07baba0fe69c Mon Sep 17 00:00:00 2001 From: Robin Deits Date: Fri, 18 Aug 2017 18:05:49 -0400 Subject: [PATCH 06/11] WIP: joint limits (untested) --- complementarity.ipynb | 4 +-- examples/articulated_complementarity.jl | 42 ++++++++++++++++++++----- 2 files changed, 37 insertions(+), 9 deletions(-) diff --git a/complementarity.ipynb b/complementarity.ipynb index 77f42fe..efb4ae6 100644 --- a/complementarity.ipynb +++ b/complementarity.ipynb @@ -113,7 +113,7 @@ "u = zeros(num_velocities(x0))\n", "\n", "model = Model(solver=CbcSolver())\n", - "up = Complementarity.update(x0, u, env, 0.1, model)" + "up = Complementarity.update(x0, u, limits, env, 0.1, model)" ] }, { @@ -132,7 +132,7 @@ " u\n", "end\n", "\n", - "results = Complementarity.simulate(x0, controller, env, 0.05, 20);" + "results = Complementarity.simulate(x0, controller, limits, env, 0.05, 20);" ] }, { diff --git a/examples/articulated_complementarity.jl b/examples/articulated_complementarity.jl index aea4365..2bd5a18 100644 --- a/examples/articulated_complementarity.jl +++ b/examples/articulated_complementarity.jl @@ -140,20 +140,41 @@ function resolve_contact(xnext::MechanismState, body::RigidBody, point::Point3D, ContactResult(β, λ, c_n, contact_force) end -function joint_limit(qnext, vnext, a::AbstractVector, b::Number, model::Model) +# function joint_limit(qnext, vnext, a::AbstractVector, b::Number, model::Model) +# λ = @variable(model, lowerbound=0, upperbound=100, basename="λ") +# separation = a' * qnext - b +# @constraint model separation <= 0 +# @disjunction(model, separation == 0, λ == 0) + +# JointLimitResult(λ, -λ * a) +# end + +function resolve_joint_limit(xnext::MechanismState, joint::Joint, a::AbstractVector, b::Number, model::Model) λ = @variable(model, lowerbound=0, upperbound=100, basename="λ") - separation = a' * qnext - b + q = configuration(xnext, joint) + separation = a' * q - b @constraint model separation <= 0 @disjunction(model, separation == 0, λ == 0) JointLimitResult(λ, -λ * a) end -function joint_limits(qnext, vnext, limits::SimpleHRepresentation, model::Model) - [joint_limit(qnext, vnext, limits.A[i, :], limits.b[i], model) for i in 1:length(limits)] +# function joint_limits(qnext, vnext, limits::SimpleHRepresentation, model::Model) +# [joint_limit(qnext, vnext, limits.A[i, :], limits.b[i], model) for i in 1:length(limits)] +# end + +function resolve_joint_limits(xnext::MechanismState, joint::Joint, limits::HRepresentation, model::Model) + [resolve_joint_limit(xnext, joint, limits.A[i, :], limits.b[i], model) for i in 1:length(limits)] end -function update(x::MechanismState{X, M}, u, env::Environment, Δt::Real, model::Model, x_dynamics=x) where {X, M} + +function update(x::MechanismState{X, M}, + u, + joint_limits::Associative{<:RigidBody, <:HRepresentation}, + env::Environment, + Δt::Real, + model::Model, + x_dynamics=x) where {X, M} mechanism = x.mechanism world = root_body(mechanism) qnext = @variable(model, [1:num_positions(x)], lowerbound=-10, basename="qnext", upperbound=10) @@ -190,6 +211,8 @@ function update(x::MechanismState{X, M}, u, env::Environment, Δt::Real, model:: externalwrenches[body] = sum(wrenches) end + joint_limit_results = Dict([joint => resolve_joint_limits(xnext, joint, limits, model) for (joint, limits) in joint_limits]) + H = mass_matrix(x_dynamics) bias = dynamics_bias(x_dynamics, externalwrenches) @@ -213,13 +236,18 @@ function update(x::MechanismState{X, M}, u, env::Environment, Δt::Real, model:: # LCPUpdate(qnext, vnext, contact_results, joint_limit_results) end -function simulate(x0::MechanismState, controller, env::Environment, Δt::Real, N::Integer) +function simulate(x0::MechanismState, + controller, + limits::Associative{<:RigidBody, <:HRepresentation}, + env::Environment, + Δt::Real, + N::Integer) results = [] # todo: type this x = x0 for i in 1:N m = Model(solver=CbcSolver()) u = controller(x) - up = update(x, u, env, Δt, m) + up = update(x, u, limits, env, Δt, m) # up = update(q, v, u, env, m) solve(m) push!(results, getvalue(up)) From 011fa9f921af84e8d80953352b620d8c7939adab Mon Sep 17 00:00:00 2001 From: Robin Deits Date: Mon, 21 Aug 2017 15:36:25 -0400 Subject: [PATCH 07/11] joint limit support in simulation and optimization --- complementarity.ipynb | 61 +++++++++++-- examples/articulated_complementarity.jl | 110 +++++++++++++----------- 2 files changed, 111 insertions(+), 60 deletions(-) diff --git a/complementarity.ipynb b/complementarity.ipynb index efb4ae6..5d827bf 100644 --- a/complementarity.ipynb +++ b/complementarity.ipynb @@ -14,6 +14,15 @@ "using RigidBodyTreeInspector" ] }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "using Plots; gr()" + ] + }, { "cell_type": "code", "execution_count": null, @@ -26,12 +35,10 @@ { "cell_type": "code", "execution_count": null, - "metadata": { - "collapsed": true - }, + "metadata": {}, "outputs": [], "source": [ - "using JuMP, Cbc" + "using JuMP, Cbc, Gurobi" ] }, { @@ -98,7 +105,7 @@ ")\n", "\n", "limits = Dict(\n", - " leg => SimpleHRepresentation{1, Float64}([1 -1]', [1.5, -0.5])\n", + " leg => SimpleHRepresentation{1, Float64}([1 -1]', [1.05, -0.5])\n", ")" ] }, @@ -125,14 +132,15 @@ "outputs": [], "source": [ "controller = (x) -> begin\n", - " kp = 40\n", + " kp = 50\n", " kd = 0.1 * kp\n", " u = zeros(num_velocities(x))\n", " u[end] = kp * (1 - configuration(x)[end]) - kd * velocity(x)[end]\n", " u\n", "end\n", "\n", - "results = Complementarity.simulate(x0, controller, limits, env, 0.05, 20);" + "Δt = 0.05\n", + "results = Complementarity.simulate(x0, controller, limits, env, Δt, 20);" ] }, { @@ -143,7 +151,44 @@ "source": [ "for r in results\n", " settransform!(vis, r.state)\n", - " sleep(0.1)\n", + " sleep(Δt)\n", + "end" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "plot([configuration(r.state, leg)[1] for r in results])" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "x0 = MechanismState(hopper, [0., 0, 0, 1, 0, 0, 1.0, 1.0], zeros(num_velocities(hopper)))\n", + "model, results_opt = Complementarity.optimize(x0, limits, env, Δt, 15, GurobiSolver())\n", + "@objective model Max configuration(results_opt[end].state, floating_base)[end]\n", + "for i in 1:length(results_opt)\n", + " @constraint(model, results_opt[i].state.q[1:6] .== [0, 0, 0, 1, 0, 0])\n", + "end\n", + "solve(model)\n", + "results_opt = getvalue.(results_opt);" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "for r in results_opt\n", + " settransform!(vis, r.state)\n", + " sleep(Δt)\n", "end" ] }, diff --git a/examples/articulated_complementarity.jl b/examples/articulated_complementarity.jl index 2bd5a18..6672f79 100644 --- a/examples/articulated_complementarity.jl +++ b/examples/articulated_complementarity.jl @@ -55,7 +55,7 @@ end JuMP.getvalue(f::FreeVector3D) = FreeVector3D(f.frame, getvalue(f.v)) -struct JointLimitResult{T, Tf <: AbstractVector} +struct JointLimitResult{T, Tf} λ::T generalized_force::Tf end @@ -70,29 +70,23 @@ function JuMP.getvalue(x::MechanismState{<:JuMP.AbstractJuMPScalar}) MechanismState(x.mechanism, getvalue(configuration(x)), getvalue(velocity(x)), getvalue(additional_state(x))) end -struct LCPUpdate{T, M <: MechanismState{T}, Tf} +struct LCPUpdate{T, M <: MechanismState{T}, Tf, D <: Dict{<:Joint, <:Vector{<:JointLimitResult{T}}}} state::M contacts::Vector{ContactResult{T, Tf}} - # joint_contacts::Vector{JointLimitResult{T, Tf}} + joint_contacts::D end -# JuMP.getvalue(up::LCPUpdate) = -# LCPUpdate(getvalue(up.state), getvalue.(up.contacts), getvalue.(up.joint_contacts)) +JuMP.getvalue(d::Dict{<:Joint, <:Vector{<:JointLimitResult}}) = Dict(zip(keys(d), [getvalue.(v) for v in values(d)])) + JuMP.getvalue(up::LCPUpdate) = - LCPUpdate(getvalue(up.state), getvalue.(up.contacts)) + LCPUpdate(getvalue(up.state), getvalue.(up.contacts), getvalue(up.joint_contacts)) + +JuMP.setvalue(d::Dict{<:Joint, <:Vector{<:JointLimitResult}}, seed::Dict) = [setvalue.(v1, v2) for (v1, v2) in zip(values(d), values(seed))] function JuMP.setvalue(up::LCPUpdate{<:JuMP.AbstractJuMPScalar}, seed::LCPUpdate{<:Number}) setvalue(up.state, seed.state) setvalue.(up.contacts, seed.contacts) - setvalue.(up.joint_contacts, seed.joint_contacts) -end - -function leg_position_in_world(q) - q[1:2] + [0, -1] * q[3] -end - -function leg_velocity_in_world(v) - v[1:2] + [0, -1] * v[3] + setvalue(up.joint_contacts, seed.joint_contacts) end function resolve_contact(xnext::MechanismState, body::RigidBody, point::Point3D, obstacle::Obstacle, model::Model, x_dynamics::MechanismState{<:Number}) @@ -114,6 +108,8 @@ function resolve_contact(xnext::MechanismState, body::RigidBody, point::Point3D, separation = separation_in_world(state_vector(x_dynamics)) + (state_vector(xnext) - state_vector(x_dynamics))' * ForwardDiff.gradient(separation_in_world, state_vector(x_dynamics)) + @show separation + function contact_velocity_in_world(x) q = x[1:num_positions(xnext)] v = x[(num_positions(xnext)+1):end] @@ -170,7 +166,7 @@ end function update(x::MechanismState{X, M}, u, - joint_limits::Associative{<:RigidBody, <:HRepresentation}, + joint_limits::Associative{<:Joint, <:HRepresentation}, env::Environment, Δt::Real, model::Model, @@ -211,34 +207,35 @@ function update(x::MechanismState{X, M}, externalwrenches[body] = sum(wrenches) end - joint_limit_results = Dict([joint => resolve_joint_limits(xnext, joint, limits, model) for (joint, limits) in joint_limits]) - - H = mass_matrix(x_dynamics) - bias = dynamics_bias(x_dynamics, externalwrenches) - - @constraint(model, H * (vnext - velocity(x)) .== Δt * (u .- bias)) # (5) - function _config_derivative(v) q = oftype(v, configuration(x_dynamics)) x_diff = MechanismState(mechanism, q, v) configuration_derivative(x_diff) end - config_derivative = ForwardDiff.jacobian(_config_derivative, velocity(x_dynamics)) * velocity(xnext) - @constraint(model, qnext .- configuration(x) .== Δt .* config_derivative) + jac_dq_wrt_v = ForwardDiff.jacobian(_config_derivative, velocity(x_dynamics)) - LCPUpdate(xnext, contact_results) + joint_limit_results = Dict([joint => resolve_joint_limits(xnext, joint, limits, model) for (joint, limits) in joint_limits]) + joint_limit_forces = zeros(GenericAffExpr{M, Variable}, num_velocities(x)) + for (joint, results) in joint_limit_results + for result in results + joint_limit_forces .+= (jac_dq_wrt_v')[:, parentindexes(configuration(x, joint))...] * result.generalized_force + end + end - # joint_limit_results = joint_limits(qnext, vnext, SimpleHRepresentation([0. 0 1; 0 0 -1], [1.5, -0.5]), model) + H = mass_matrix(x_dynamics) + bias = dynamics_bias(x_dynamics, externalwrenches) + config_derivative = jac_dq_wrt_v * velocity(xnext) - # internal_force = u + sum([r.generalized_force[3] for r in joint_limit_results]) + @constraint(model, H * (vnext - velocity(x)) .== Δt * (u .+ joint_limit_forces .- bias)) # (5) + @constraint(model, qnext .- configuration(x) .== Δt .* config_derivative) # (6) - # LCPUpdate(qnext, vnext, contact_results, joint_limit_results) + LCPUpdate(xnext, contact_results, joint_limit_results) end function simulate(x0::MechanismState, controller, - limits::Associative{<:RigidBody, <:HRepresentation}, + joint_limits::Associative{<:Joint, <:HRepresentation}, env::Environment, Δt::Real, N::Integer) @@ -247,7 +244,7 @@ function simulate(x0::MechanismState, for i in 1:N m = Model(solver=CbcSolver()) u = controller(x) - up = update(x, u, limits, env, Δt, m) + up = update(x, u, joint_limits, env, Δt, m) # up = update(q, v, u, env, m) solve(m) push!(results, getvalue(up)) @@ -256,35 +253,44 @@ function simulate(x0::MechanismState, results end -function optimize(q0, v0, env::Environment, N::Integer)::Vector{LCPUpdate{Float64}} - q, v = q0, v0 - m = Model(solver=CbcSolver()) - results = [] - for i in 1:N - up = update(q, v, env, m) - push!(results, up) - q = results[end].q - v = results[end].v +function optimize(x0::MechanismState, + joint_limits::Associative{<:Joint, <:HRepresentation}, + env::Environment, + Δt, + N::Integer, + solver=CbcSolver()) + x = x0 + m = Model(solver=solver) + results = map(1:N) do i + u = @variable(m, [1:num_velocities(x0)], basename="u_$i", lowerbound=-10, upperbound=10) + up = update(x, u, joint_limits, env, Δt, m, x0) + x = up.state + up end - solve(m) - getvalue.(results) + m, results + # solve(m) + # getvalue.(results) end -function optimize(q0, v0, env::Environment, seed::Vector{<:LCPUpdate}) - q, v = q0, v0 +function optimize(x0::MechanismState, + joint_limits::Associative{<:Joint, <:HRepresentation}, + env::Environment, + Δt, + seed::Vector{<:LCPUpdate}) + x = x0 m = Model(solver=CbcSolver()) - results = [] - for i in 1:N - up = update(q, v, env, m) + results = map(1:N) do i + u = @variable(m, [1:num_velocities(x0)], basename="u_$i", lowerbound=-10, upperbound=10) + up = update(x, u, joint_limits, env, Δt, m, x0) setvalue(up, seed[i]) - push!(results, up) - q = results[end].q - v = results[end].v + x = up.state + up end warmstart!(m, true) @assert sum(m.colCat .== :Bin) == 0 - solve(m) - getvalue.(results) + m, results + # solve(m) + # getvalue.(results) end end \ No newline at end of file From 7bc408fdeba8a69b5fe6ad1bed33bd17d8e73d2b Mon Sep 17 00:00:00 2001 From: Robin Deits Date: Mon, 21 Aug 2017 22:22:58 -0400 Subject: [PATCH 08/11] more frame safety and more sensible type params --- examples/articulated_complementarity.jl | 127 ++++++++++++++---------- 1 file changed, 72 insertions(+), 55 deletions(-) diff --git a/examples/articulated_complementarity.jl b/examples/articulated_complementarity.jl index 6672f79..364f40b 100644 --- a/examples/articulated_complementarity.jl +++ b/examples/articulated_complementarity.jl @@ -10,47 +10,75 @@ using RigidBodyDynamics: colwise using Rotations using ForwardDiff -struct Obstacle{T, H <: HRepresentation{3, T}} - interior::H +struct Obstacle{T} + frame::CartesianFrame3D + interior::SimpleHRepresentation{3, T} contact_face::HalfSpace{3, T} μ::T end -struct ContactEnvironment{P <: Point3D, T, H <: HRepresentation{3, T}} - points::Vector{P} - obstacles::Vector{Obstacle{T, H}} - free_regions::Vector{H} +struct FreeRegion{T} + frame::CartesianFrame3D + interior::SimpleHRepresentation{3, T} end -struct Environment{B <: RigidBody, C <: ContactEnvironment} - contacts::Dict{B, C} +struct ContactEnvironment{T} + points::Vector{Point3D{SVector{3, T}}} + obstacles::Vector{Obstacle{T}} + free_regions::Vector{FreeRegion{T}} end -function contact_basis(face::HalfSpace{3}, μ) - θ = atan(μ) +struct Environment{T} + contacts::Dict{RigidBody{T}, ContactEnvironment{T}} +end + +function contact_basis(obs::Obstacle) + θ = atan(obs.μ) R = RotY(θ) - hcat(R * face.a, R' * face.a) + SVector( + FreeVector3D(obs.frame, R * obs.contact_face.a), + FreeVector3D(obs.frame, R' * obs.contact_face.a)) end -contact_basis(obs::Obstacle) = contact_basis(obs.contact_face, obs.μ) +contact_normal(obs::Obstacle) = FreeVector3D(obs.frame, obs.contact_face.a) -function ConditionalJuMP.Conditional(op::typeof(in), x::AbstractVector, P::HRepresentation) - ConditionalJuMP.Conditional(&, [@?(P.A[i, :]' * x <= P.b[i]) for i in 1:length(P)]...) +function separation(obs::Obstacle, p::Point3D) + @framecheck obs.frame p.frame + n = contact_normal(obs) + n.v' * p.v - obs.contact_face.β end -struct ContactResult{T, Tf} +Base.@pure ConditionalJuMP.isjump(::Point3D{<:AbstractArray{<:JuMP.AbstractJuMPScalar}}) = true + +function ConditionalJuMP.Conditional(op::typeof(in), x::Point3D, P::FreeRegion) + @framecheck(x.frame, P.frame) + ConditionalJuMP.Conditional(&, [@?(P.interior.A[i, :]' * x.v <= P.interior.b[i]) for i in 1:length(P.interior)]...) +end + +struct ContactResult{T, M} β::Vector{T} λ::T c_n::T - contact_force::Tf + obs::Obstacle{M} +end + +_vec(f::FreeVector3D) = convert(Vector, f.v) +_vec(p::Point3D) = convert(Vector, p.v) + +function contact_force(r::ContactResult) + n = contact_normal(r.obs) + D = contact_basis(r.obs) + @framecheck(n.frame, D[1].frame) + # r.c_n * n .+ D * r.β + FreeVector3D(n.frame, r.c_n .* n.v .+ sum(broadcast.(*, _vec.(D), r.β))) end -JuMP.getvalue(c::ContactResult) = ContactResult(getvalue.((c.β, c.λ, c.c_n, c.contact_force))...) +JuMP.getvalue(c::ContactResult) = ContactResult(getvalue.((c.β, c.λ, c.c_n))..., c.obs) function JuMP.setvalue(contact::ContactResult{<:JuMP.AbstractJuMPScalar}, seed::ContactResult{<:Number}) + @assert contact.obs == seed.obs setvalue(contact.β, seed.β) setvalue(contact.λ, seed.λ) setvalue(contact.c_n, seed.c_n) - @assert getvalue(contact.contact_force) ≈ seed.contact_force end JuMP.getvalue(f::FreeVector3D) = FreeVector3D(f.frame, getvalue(f.v)) @@ -70,10 +98,10 @@ function JuMP.getvalue(x::MechanismState{<:JuMP.AbstractJuMPScalar}) MechanismState(x.mechanism, getvalue(configuration(x)), getvalue(velocity(x)), getvalue(additional_state(x))) end -struct LCPUpdate{T, M <: MechanismState{T}, Tf, D <: Dict{<:Joint, <:Vector{<:JointLimitResult{T}}}} - state::M - contacts::Vector{ContactResult{T, Tf}} - joint_contacts::D +struct LCPUpdate{T, M, S <: MechanismState{T, M}, J <: Joint, Tf} + state::S + contacts::Vector{ContactResult{T, M}} + joint_contacts::Dict{J, Vector{JointLimitResult{T, Tf}}} end JuMP.getvalue(d::Dict{<:Joint, <:Vector{<:JointLimitResult}}) = Dict(zip(keys(d), [getvalue.(v) for v in values(d)])) @@ -90,27 +118,24 @@ function JuMP.setvalue(up::LCPUpdate{<:JuMP.AbstractJuMPScalar}, seed::LCPUpdate end function resolve_contact(xnext::MechanismState, body::RigidBody, point::Point3D, obstacle::Obstacle, model::Model, x_dynamics::MechanismState{<:Number}) - n = obstacle.contact_face.a D = contact_basis(obstacle) - k = size(D, 2) + k = length(D) β = @variable(model, [1:k], lowerbound=0, basename="β", upperbound=100) λ = @variable(model, lowerbound=0, basename="λ", upperbound=100) c_n = @variable(model, lowerbound=0, basename="c_n", upperbound=100) - function separation_in_world(x) + function _separation(x) q = x[1:num_positions(xnext)] v = x[(num_positions(xnext)+1):end] x_diff = MechanismState(xnext.mechanism, q, v) point_in_world = transform_to_root(x_diff, point.frame) * point - separation = n' * point_in_world.v - obstacle.contact_face.β + separation(obstacle, point_in_world) end - separation = separation_in_world(state_vector(x_dynamics)) + (state_vector(xnext) - state_vector(x_dynamics))' * ForwardDiff.gradient(separation_in_world, state_vector(x_dynamics)) - - @show separation + separation_from_obstacle = _separation(state_vector(x_dynamics)) + (state_vector(xnext) - state_vector(x_dynamics))' * ForwardDiff.gradient(_separation, state_vector(x_dynamics)) - function contact_velocity_in_world(x) + function _contact_velocity(x) q = x[1:num_positions(xnext)] v = x[(num_positions(xnext)+1):end] x_diff = MechanismState(xnext.mechanism, q, v) @@ -118,33 +143,25 @@ function resolve_contact(xnext::MechanismState, body::RigidBody, point::Point3D, contact_velocity = point_velocity(twist_wrt_world(x_diff, body), point_in_world).v end - contact_velocity = contact_velocity_in_world(state_vector(x_dynamics)) + ForwardDiff.jacobian(contact_velocity_in_world, state_vector(x_dynamics)) * (state_vector(xnext) - state_vector(x_dynamics)) + contact_velocity = FreeVector3D(root_frame(xnext.mechanism), _contact_velocity(state_vector(x_dynamics)) + ForwardDiff.jacobian(_contact_velocity, state_vector(x_dynamics)) * (state_vector(xnext) - state_vector(x_dynamics))) + + D_transpose_times_v = [dot(d, contact_velocity) for d in D] @constraints model begin - λ .+ D' * contact_velocity .>= 0 # (8) + # λ .+ D' * contact_velocity .>= 0 # (8) + λ .+ D_transpose_times_v .>= 0 # (8) obstacle.μ * c_n .- sum(β) >= 0 # (9) end - @disjunction(model, (separation == 0), (c_n == 0)) # (10) - Dtv = D' * contact_velocity + @disjunction(model, (separation_from_obstacle == 0), (c_n == 0)) # (10) for j in 1:k - @disjunction(model, ((λ + Dtv[j]) == 0), β[j] == 0) # (11) + @disjunction(model, ((λ + D_transpose_times_v[j]) == 0), β[j] == 0) # (11) end @disjunction(model, (obstacle.μ * c_n - sum(β) == 0), (λ == 0)) # (12) - contact_force = FreeVector3D(root_frame(xnext.mechanism), c_n * n .+ D * β) - ContactResult(β, λ, c_n, contact_force) + ContactResult(β, λ, c_n, obstacle) end -# function joint_limit(qnext, vnext, a::AbstractVector, b::Number, model::Model) -# λ = @variable(model, lowerbound=0, upperbound=100, basename="λ") -# separation = a' * qnext - b -# @constraint model separation <= 0 -# @disjunction(model, separation == 0, λ == 0) - -# JointLimitResult(λ, -λ * a) -# end - function resolve_joint_limit(xnext::MechanismState, joint::Joint, a::AbstractVector, b::Number, model::Model) λ = @variable(model, lowerbound=0, upperbound=100, basename="λ") q = configuration(xnext, joint) @@ -155,10 +172,6 @@ function resolve_joint_limit(xnext::MechanismState, joint::Joint, a::AbstractVec JointLimitResult(λ, -λ * a) end -# function joint_limits(qnext, vnext, limits::SimpleHRepresentation, model::Model) -# [joint_limit(qnext, vnext, limits.A[i, :], limits.b[i], model) for i in 1:length(limits)] -# end - function resolve_joint_limits(xnext::MechanismState, joint::Joint, limits::HRepresentation, model::Model) [resolve_joint_limit(xnext, joint, limits.A[i, :], limits.b[i], model) for i in 1:length(limits)] end @@ -177,7 +190,7 @@ function update(x::MechanismState{X, M}, vnext = @variable(model, [1:num_velocities(x)], lowerbound=-10, basename="vnext", upperbound=10) xnext = MechanismState(mechanism, qnext, vnext) - contact_results = ContactResult{Variable,FreeVector3D{Vector{GenericAffExpr{M,Variable}}}}[] + contact_results = ContactResult{Variable, M}[] externalwrenches = Dict{typeof(world), Wrench{GenericAffExpr{M,Variable}}}() for (body, contact_env) in env.contacts wrenches = Wrench{GenericAffExpr{M,Variable}}[] @@ -188,7 +201,7 @@ function update(x::MechanismState{X, M}, push!(contact_results, result) push!(wrenches, Wrench( transform_to_root(x_dynamics, contact_point.frame) * contact_point, - result.contact_force)) + contact_force(result))) end function _point_in_world(x) @@ -199,10 +212,10 @@ function update(x::MechanismState{X, M}, point_in_world.v end - point_in_world = _point_in_world(state_vector(x_dynamics)) + ForwardDiff.jacobian(_point_in_world, state_vector(x_dynamics)) * (state_vector(xnext) - state_vector(x_dynamics)) + position = Point3D(root_frame(mechanism), _point_in_world(state_vector(x_dynamics)) + ForwardDiff.jacobian(_point_in_world, state_vector(x_dynamics)) * (state_vector(xnext) - state_vector(x_dynamics))) ConditionalJuMP.disjunction!(model, - [@?(point_in_world ∈ P) for P in contact_env.free_regions]) # (7) + [@?(position ∈ P) for P in contact_env.free_regions]) # (7) end externalwrenches[body] = sum(wrenches) end @@ -245,7 +258,6 @@ function simulate(x0::MechanismState, m = Model(solver=CbcSolver()) u = controller(x) up = update(x, u, joint_limits, env, Δt, m) - # up = update(q, v, u, env, m) solve(m) push!(results, getvalue(up)) x = results[end].state @@ -253,6 +265,11 @@ function simulate(x0::MechanismState, results end +# TODO: +# * record u in LCPUpdate +# * lots of cleanup +# * figure out a neater way to handle x_dynamics + function optimize(x0::MechanismState, joint_limits::Associative{<:Joint, <:HRepresentation}, env::Environment, From 72f19cd2c7f8f9cfe31e39a3fe35f7cd7bb0da46 Mon Sep 17 00:00:00 2001 From: Robin Deits Date: Mon, 21 Aug 2017 23:29:19 -0400 Subject: [PATCH 09/11] more cleanup --- complementarity.ipynb | 20 ++++++----- examples/articulated_complementarity.jl | 48 +++++++++++++------------ 2 files changed, 38 insertions(+), 30 deletions(-) diff --git a/complementarity.ipynb b/complementarity.ipynb index 5d827bf..f86d854 100644 --- a/complementarity.ipynb +++ b/complementarity.ipynb @@ -75,7 +75,7 @@ "leg = Joint(\"leg\", Prismatic(SVector(0, 0, -1.)))\n", "attach!(hopper, core, leg, eye(Transform3D, frame_before(leg), default_frame(core)), foot)\n", "\n", - "foot_contact = Point3D(default_frame(foot), SVector(0, 0, 0))\n", + "foot_contact = Point3D(default_frame(foot), SVector(0., 0, 0))\n", "\n", "setgeometry!(vis, hopper, create_geometry(hopper, show_inertias=true))" ] @@ -91,15 +91,19 @@ " [foot_contact],\n", " [\n", " Complementarity.Obstacle(\n", + " default_frame(world),\n", " SimpleHRepresentation{3, Float64}([0 0 1], [0]),\n", " HalfSpace{3, Float64}([0, 0, 1], 0),\n", " 0.5\n", " ),\n", " ],\n", " [\n", - " SimpleHRepresentation{3, Float64}(\n", - " [0 0 -1],\n", - " [0])\n", + " Complementarity.FreeRegion(\n", + " default_frame(world),\n", + " SimpleHRepresentation{3, Float64}(\n", + " [0 0 -1],\n", + " [0])\n", + " )\n", " ])\n", " )\n", ")\n", @@ -195,11 +199,11 @@ { "cell_type": "code", "execution_count": null, - "metadata": { - "collapsed": true - }, + "metadata": {}, "outputs": [], - "source": [] + "source": [ + "plot([configuration(r.state, leg)[1] for r in results_opt])" + ] } ], "metadata": { diff --git a/examples/articulated_complementarity.jl b/examples/articulated_complementarity.jl index 364f40b..0bb00d5 100644 --- a/examples/articulated_complementarity.jl +++ b/examples/articulated_complementarity.jl @@ -59,6 +59,7 @@ struct ContactResult{T, M} β::Vector{T} λ::T c_n::T + point::Point3D{SVector{3, M}} obs::Obstacle{M} end @@ -73,7 +74,7 @@ function contact_force(r::ContactResult) FreeVector3D(n.frame, r.c_n .* n.v .+ sum(broadcast.(*, _vec.(D), r.β))) end -JuMP.getvalue(c::ContactResult) = ContactResult(getvalue.((c.β, c.λ, c.c_n))..., c.obs) +JuMP.getvalue(c::ContactResult) = ContactResult(getvalue.((c.β, c.λ, c.c_n))..., c.point, c.obs) function JuMP.setvalue(contact::ContactResult{<:JuMP.AbstractJuMPScalar}, seed::ContactResult{<:Number}) @assert contact.obs == seed.obs setvalue(contact.β, seed.β) @@ -98,16 +99,21 @@ function JuMP.getvalue(x::MechanismState{<:JuMP.AbstractJuMPScalar}) MechanismState(x.mechanism, getvalue(configuration(x)), getvalue(velocity(x)), getvalue(additional_state(x))) end -struct LCPUpdate{T, M, S <: MechanismState{T, M}, J <: Joint, Tf} +struct LCPUpdate{T, M, S <: MechanismState{T, M}, I <:AbstractVector, J <: Joint, Tf} state::S - contacts::Vector{ContactResult{T, M}} + input::I + contacts::Dict{RigidBody{M}, Vector{ContactResult{T, M}}} joint_contacts::Dict{J, Vector{JointLimitResult{T, Tf}}} end -JuMP.getvalue(d::Dict{<:Joint, <:Vector{<:JointLimitResult}}) = Dict(zip(keys(d), [getvalue.(v) for v in values(d)])) +JuMP.getvalue(p::Pair{<:RigidBody, <:AbstractVector{<:ContactResult}}) = p.first => getvalue.(p.second) +JuMP.getvalue(p::Pair{<:Joint, <:AbstractVector{<:JointLimitResult}}) = p.first => getvalue.(p.second) + +_getvalue(x::AbstractVector{<:Number}) = x +_getvalue(x::AbstractVector{<:JuMP.AbstractJuMPScalar}) = getvalue(x) JuMP.getvalue(up::LCPUpdate) = - LCPUpdate(getvalue(up.state), getvalue.(up.contacts), getvalue(up.joint_contacts)) + LCPUpdate(getvalue(up.state), _getvalue(up.input), map(getvalue, up.contacts), map(getvalue, up.joint_contacts)) JuMP.setvalue(d::Dict{<:Joint, <:Vector{<:JointLimitResult}}, seed::Dict) = [setvalue.(v1, v2) for (v1, v2) in zip(values(d), values(seed))] @@ -148,7 +154,6 @@ function resolve_contact(xnext::MechanismState, body::RigidBody, point::Point3D, D_transpose_times_v = [dot(d, contact_velocity) for d in D] @constraints model begin - # λ .+ D' * contact_velocity .>= 0 # (8) λ .+ D_transpose_times_v .>= 0 # (8) obstacle.μ * c_n .- sum(β) >= 0 # (9) end @@ -159,7 +164,7 @@ function resolve_contact(xnext::MechanismState, body::RigidBody, point::Point3D, end @disjunction(model, (obstacle.μ * c_n - sum(β) == 0), (λ == 0)) # (12) - ContactResult(β, λ, c_n, obstacle) + ContactResult(β, λ, c_n, point, obstacle) end function resolve_joint_limit(xnext::MechanismState, joint::Joint, a::AbstractVector, b::Number, model::Model) @@ -176,7 +181,6 @@ function resolve_joint_limits(xnext::MechanismState, joint::Joint, limits::HRepr [resolve_joint_limit(xnext, joint, limits.A[i, :], limits.b[i], model) for i in 1:length(limits)] end - function update(x::MechanismState{X, M}, u, joint_limits::Associative{<:Joint, <:HRepresentation}, @@ -190,20 +194,21 @@ function update(x::MechanismState{X, M}, vnext = @variable(model, [1:num_velocities(x)], lowerbound=-10, basename="vnext", upperbound=10) xnext = MechanismState(mechanism, qnext, vnext) - contact_results = ContactResult{Variable, M}[] - externalwrenches = Dict{typeof(world), Wrench{GenericAffExpr{M,Variable}}}() - for (body, contact_env) in env.contacts - wrenches = Wrench{GenericAffExpr{M,Variable}}[] + contact_results = map(env.contacts) do item + body, contact_env = item + body => [resolve_contact(xnext, body, contact_point, obstacle, model, x_dynamics) + for contact_point in contact_env.points for obstacle in contact_env.obstacles] + end + + externalwrenches = map(contact_results) do item + body, results = item + body => sum([Wrench(transform_to_root(x_dynamics, result.point.frame) * result.point, + contact_force(result)) for result in results]) + end - for contact_point in contact_env.points - for obs in contact_env.obstacles - result = resolve_contact(xnext, body, contact_point, obs, model, x_dynamics) - push!(contact_results, result) - push!(wrenches, Wrench( - transform_to_root(x_dynamics, contact_point.frame) * contact_point, - contact_force(result))) - end + for (body, contact_env) in env.contacts + for contact_point in contact_env.points function _point_in_world(x) q = x[1:num_positions(xnext)] v = x[(num_positions(xnext)+1):end] @@ -217,7 +222,6 @@ function update(x::MechanismState{X, M}, ConditionalJuMP.disjunction!(model, [@?(position ∈ P) for P in contact_env.free_regions]) # (7) end - externalwrenches[body] = sum(wrenches) end function _config_derivative(v) @@ -243,7 +247,7 @@ function update(x::MechanismState{X, M}, @constraint(model, H * (vnext - velocity(x)) .== Δt * (u .+ joint_limit_forces .- bias)) # (5) @constraint(model, qnext .- configuration(x) .== Δt .* config_derivative) # (6) - LCPUpdate(xnext, contact_results, joint_limit_results) + LCPUpdate(xnext, u, contact_results, joint_limit_results) end function simulate(x0::MechanismState, From e43a6c9b3fdd90992c5baa21c01de0214dc98d51 Mon Sep 17 00:00:00 2001 From: Robin Deits Date: Mon, 21 Aug 2017 23:47:35 -0400 Subject: [PATCH 10/11] more cleanup --- examples/articulated_complementarity.jl | 38 ++++++++++++++----------- 1 file changed, 21 insertions(+), 17 deletions(-) diff --git a/examples/articulated_complementarity.jl b/examples/articulated_complementarity.jl index 0bb00d5..6ebdd03 100644 --- a/examples/articulated_complementarity.jl +++ b/examples/articulated_complementarity.jl @@ -181,6 +181,26 @@ function resolve_joint_limits(xnext::MechanismState, joint::Joint, limits::HRepr [resolve_joint_limit(xnext, joint, limits.A[i, :], limits.b[i], model) for i in 1:length(limits)] end +function add_free_region_constraints!(model::Model, xnext::MechanismState, env::Environment, x_dynamics::MechanismState) + for (body, contact_env) in env.contacts + for contact_point in contact_env.points + function _point_in_world(x) + q = x[1:num_positions(xnext)] + v = x[(num_positions(xnext)+1):end] + x_diff = MechanismState(xnext.mechanism, q, v) + point_in_world = transform_to_root(x_diff, contact_point.frame) * contact_point + point_in_world.v + end + + position = Point3D(root_frame(xnext.mechanism), _point_in_world(state_vector(x_dynamics)) + ForwardDiff.jacobian(_point_in_world, state_vector(x_dynamics)) * (state_vector(xnext) - state_vector(x_dynamics))) + + ConditionalJuMP.disjunction!(model, + [@?(position ∈ P) for P in contact_env.free_regions]) # (7) + end + end +end + + function update(x::MechanismState{X, M}, u, joint_limits::Associative{<:Joint, <:HRepresentation}, @@ -206,23 +226,7 @@ function update(x::MechanismState{X, M}, contact_force(result)) for result in results]) end - - for (body, contact_env) in env.contacts - for contact_point in contact_env.points - function _point_in_world(x) - q = x[1:num_positions(xnext)] - v = x[(num_positions(xnext)+1):end] - x_diff = MechanismState(xnext.mechanism, q, v) - point_in_world = transform_to_root(x_diff, contact_point.frame) * contact_point - point_in_world.v - end - - position = Point3D(root_frame(mechanism), _point_in_world(state_vector(x_dynamics)) + ForwardDiff.jacobian(_point_in_world, state_vector(x_dynamics)) * (state_vector(xnext) - state_vector(x_dynamics))) - - ConditionalJuMP.disjunction!(model, - [@?(position ∈ P) for P in contact_env.free_regions]) # (7) - end - end + add_free_region_constraints!(model, xnext, env, x_dynamics) function _config_derivative(v) q = oftype(v, configuration(x_dynamics)) From 81d102e57e67da2917b53e04402690d5b81ce5b6 Mon Sep 17 00:00:00 2001 From: Robin Deits Date: Tue, 22 Aug 2017 15:45:12 -0400 Subject: [PATCH 11/11] move some code out to MechanismComplementarity and update requirements --- REQUIRE | 5 +- complementarity.ipynb | 224 ----------------- examples/articulated_complementarity.jl | 321 ------------------------ src/ConditionalJuMP.jl | 1 - test/runtests.jl | 1 - 5 files changed, 2 insertions(+), 550 deletions(-) delete mode 100644 complementarity.ipynb delete mode 100644 examples/articulated_complementarity.jl diff --git a/REQUIRE b/REQUIRE index e9d9b39..439a68b 100644 --- a/REQUIRE +++ b/REQUIRE @@ -1,4 +1,3 @@ julia 0.6 -JuMP -IntervalArithmetic -MacroTools +JuMP 0.17 0.19 +IntervalArithmetic 0.11 diff --git a/complementarity.ipynb b/complementarity.ipynb deleted file mode 100644 index f86d854..0000000 --- a/complementarity.ipynb +++ /dev/null @@ -1,224 +0,0 @@ -{ - "cells": [ - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "using Polyhedra\n", - "using DrakeVisualizer\n", - "DrakeVisualizer.any_open_windows() || DrakeVisualizer.new_window()\n", - "using RigidBodyDynamics\n", - "using StaticArrays\n", - "using RigidBodyTreeInspector" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "using Plots; gr()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "vis = Visualizer()[:hopper]" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "using JuMP, Cbc, Gurobi" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "include(\"examples/articulated_complementarity.jl\")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "g = -9.81\n", - "world = RigidBody{Float64}(\"world\")\n", - "hopper = Mechanism(world; gravity=SVector(0, 0, g))\n", - "\n", - "frame = CartesianFrame3D(\"core\")\n", - "mass = 1.\n", - "inertia = SpatialInertia(frame, 0.01 * eye(SMatrix{3, 3}), SVector(0., 0, 0), mass)\n", - "core = RigidBody(inertia)\n", - "\n", - "floating_base = Joint(\"core_to_world\", QuaternionFloating{Float64}())\n", - "attach!(hopper, world, floating_base, eye(Transform3D, frame_before(floating_base), default_frame(world)), core)\n", - "\n", - "frame = CartesianFrame3D(\"foot\")\n", - "mass = 1.\n", - "inertia = SpatialInertia(frame, 0.01 * eye(SMatrix{3, 3}), SVector(0., 0, 0), mass)\n", - "foot = RigidBody(inertia)\n", - "leg = Joint(\"leg\", Prismatic(SVector(0, 0, -1.)))\n", - "attach!(hopper, core, leg, eye(Transform3D, frame_before(leg), default_frame(core)), foot)\n", - "\n", - "foot_contact = Point3D(default_frame(foot), SVector(0., 0, 0))\n", - "\n", - "setgeometry!(vis, hopper, create_geometry(hopper, show_inertias=true))" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "env = Complementarity.Environment(\n", - " Dict(foot => Complementarity.ContactEnvironment(\n", - " [foot_contact],\n", - " [\n", - " Complementarity.Obstacle(\n", - " default_frame(world),\n", - " SimpleHRepresentation{3, Float64}([0 0 1], [0]),\n", - " HalfSpace{3, Float64}([0, 0, 1], 0),\n", - " 0.5\n", - " ),\n", - " ],\n", - " [\n", - " Complementarity.FreeRegion(\n", - " default_frame(world),\n", - " SimpleHRepresentation{3, Float64}(\n", - " [0 0 -1],\n", - " [0])\n", - " )\n", - " ])\n", - " )\n", - ")\n", - "\n", - "limits = Dict(\n", - " leg => SimpleHRepresentation{1, Float64}([1 -1]', [1.05, -0.5])\n", - ")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "x0 = MechanismState(hopper, [0., 0, 0, 1, 0, 0, 1.5, 1.0], zeros(num_velocities(hopper)))\n", - "\n", - "u = zeros(num_velocities(x0))\n", - "\n", - "model = Model(solver=CbcSolver())\n", - "up = Complementarity.update(x0, u, limits, env, 0.1, model)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "scrolled": false - }, - "outputs": [], - "source": [ - "controller = (x) -> begin\n", - " kp = 50\n", - " kd = 0.1 * kp\n", - " u = zeros(num_velocities(x))\n", - " u[end] = kp * (1 - configuration(x)[end]) - kd * velocity(x)[end]\n", - " u\n", - "end\n", - "\n", - "Δt = 0.05\n", - "results = Complementarity.simulate(x0, controller, limits, env, Δt, 20);" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "for r in results\n", - " settransform!(vis, r.state)\n", - " sleep(Δt)\n", - "end" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "plot([configuration(r.state, leg)[1] for r in results])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "x0 = MechanismState(hopper, [0., 0, 0, 1, 0, 0, 1.0, 1.0], zeros(num_velocities(hopper)))\n", - "model, results_opt = Complementarity.optimize(x0, limits, env, Δt, 15, GurobiSolver())\n", - "@objective model Max configuration(results_opt[end].state, floating_base)[end]\n", - "for i in 1:length(results_opt)\n", - " @constraint(model, results_opt[i].state.q[1:6] .== [0, 0, 0, 1, 0, 0])\n", - "end\n", - "solve(model)\n", - "results_opt = getvalue.(results_opt);" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "for r in results_opt\n", - " settransform!(vis, r.state)\n", - " sleep(Δt)\n", - "end" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "plot([configuration(r.state, leg)[1] for r in results_opt])" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Julia 0.6.0", - "language": "julia", - "name": "julia-0.6" - }, - "language_info": { - "file_extension": ".jl", - "mimetype": "application/julia", - "name": "julia", - "version": "0.6.0" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/examples/articulated_complementarity.jl b/examples/articulated_complementarity.jl deleted file mode 100644 index 6ebdd03..0000000 --- a/examples/articulated_complementarity.jl +++ /dev/null @@ -1,321 +0,0 @@ -module Complementarity - -using Polyhedra -using StaticArrays -using JuMP, ConditionalJuMP, Cbc -using JuMP: GenericAffExpr -using Base.Test -using RigidBodyDynamics -using RigidBodyDynamics: colwise -using Rotations -using ForwardDiff - -struct Obstacle{T} - frame::CartesianFrame3D - interior::SimpleHRepresentation{3, T} - contact_face::HalfSpace{3, T} - μ::T -end - -struct FreeRegion{T} - frame::CartesianFrame3D - interior::SimpleHRepresentation{3, T} -end - -struct ContactEnvironment{T} - points::Vector{Point3D{SVector{3, T}}} - obstacles::Vector{Obstacle{T}} - free_regions::Vector{FreeRegion{T}} -end - -struct Environment{T} - contacts::Dict{RigidBody{T}, ContactEnvironment{T}} -end - -function contact_basis(obs::Obstacle) - θ = atan(obs.μ) - R = RotY(θ) - SVector( - FreeVector3D(obs.frame, R * obs.contact_face.a), - FreeVector3D(obs.frame, R' * obs.contact_face.a)) -end - -contact_normal(obs::Obstacle) = FreeVector3D(obs.frame, obs.contact_face.a) - -function separation(obs::Obstacle, p::Point3D) - @framecheck obs.frame p.frame - n = contact_normal(obs) - n.v' * p.v - obs.contact_face.β -end - -Base.@pure ConditionalJuMP.isjump(::Point3D{<:AbstractArray{<:JuMP.AbstractJuMPScalar}}) = true - -function ConditionalJuMP.Conditional(op::typeof(in), x::Point3D, P::FreeRegion) - @framecheck(x.frame, P.frame) - ConditionalJuMP.Conditional(&, [@?(P.interior.A[i, :]' * x.v <= P.interior.b[i]) for i in 1:length(P.interior)]...) -end - -struct ContactResult{T, M} - β::Vector{T} - λ::T - c_n::T - point::Point3D{SVector{3, M}} - obs::Obstacle{M} -end - -_vec(f::FreeVector3D) = convert(Vector, f.v) -_vec(p::Point3D) = convert(Vector, p.v) - -function contact_force(r::ContactResult) - n = contact_normal(r.obs) - D = contact_basis(r.obs) - @framecheck(n.frame, D[1].frame) - # r.c_n * n .+ D * r.β - FreeVector3D(n.frame, r.c_n .* n.v .+ sum(broadcast.(*, _vec.(D), r.β))) -end - -JuMP.getvalue(c::ContactResult) = ContactResult(getvalue.((c.β, c.λ, c.c_n))..., c.point, c.obs) -function JuMP.setvalue(contact::ContactResult{<:JuMP.AbstractJuMPScalar}, seed::ContactResult{<:Number}) - @assert contact.obs == seed.obs - setvalue(contact.β, seed.β) - setvalue(contact.λ, seed.λ) - setvalue(contact.c_n, seed.c_n) -end - -JuMP.getvalue(f::FreeVector3D) = FreeVector3D(f.frame, getvalue(f.v)) - -struct JointLimitResult{T, Tf} - λ::T - generalized_force::Tf -end - -JuMP.getvalue(r::JointLimitResult) = JointLimitResult(getvalue.((r.λ, r.generalized_force))...) -function JuMP.setvalue(r::JointLimitResult{<:JuMP.AbstractJuMPScalar}, seed::JointLimitResult{<:Number}) - setvalue(r.λ, seed.λ) - @assert getvalue(r.generalized_force) ≈ seed.generalized_force -end - -function JuMP.getvalue(x::MechanismState{<:JuMP.AbstractJuMPScalar}) - MechanismState(x.mechanism, getvalue(configuration(x)), getvalue(velocity(x)), getvalue(additional_state(x))) -end - -struct LCPUpdate{T, M, S <: MechanismState{T, M}, I <:AbstractVector, J <: Joint, Tf} - state::S - input::I - contacts::Dict{RigidBody{M}, Vector{ContactResult{T, M}}} - joint_contacts::Dict{J, Vector{JointLimitResult{T, Tf}}} -end - -JuMP.getvalue(p::Pair{<:RigidBody, <:AbstractVector{<:ContactResult}}) = p.first => getvalue.(p.second) -JuMP.getvalue(p::Pair{<:Joint, <:AbstractVector{<:JointLimitResult}}) = p.first => getvalue.(p.second) - -_getvalue(x::AbstractVector{<:Number}) = x -_getvalue(x::AbstractVector{<:JuMP.AbstractJuMPScalar}) = getvalue(x) - -JuMP.getvalue(up::LCPUpdate) = - LCPUpdate(getvalue(up.state), _getvalue(up.input), map(getvalue, up.contacts), map(getvalue, up.joint_contacts)) - -JuMP.setvalue(d::Dict{<:Joint, <:Vector{<:JointLimitResult}}, seed::Dict) = [setvalue.(v1, v2) for (v1, v2) in zip(values(d), values(seed))] - -function JuMP.setvalue(up::LCPUpdate{<:JuMP.AbstractJuMPScalar}, seed::LCPUpdate{<:Number}) - setvalue(up.state, seed.state) - setvalue.(up.contacts, seed.contacts) - setvalue(up.joint_contacts, seed.joint_contacts) -end - -function resolve_contact(xnext::MechanismState, body::RigidBody, point::Point3D, obstacle::Obstacle, model::Model, x_dynamics::MechanismState{<:Number}) - D = contact_basis(obstacle) - k = length(D) - - β = @variable(model, [1:k], lowerbound=0, basename="β", upperbound=100) - λ = @variable(model, lowerbound=0, basename="λ", upperbound=100) - c_n = @variable(model, lowerbound=0, basename="c_n", upperbound=100) - - function _separation(x) - q = x[1:num_positions(xnext)] - v = x[(num_positions(xnext)+1):end] - x_diff = MechanismState(xnext.mechanism, q, v) - point_in_world = transform_to_root(x_diff, point.frame) * point - separation(obstacle, point_in_world) - end - - separation_from_obstacle = _separation(state_vector(x_dynamics)) + (state_vector(xnext) - state_vector(x_dynamics))' * ForwardDiff.gradient(_separation, state_vector(x_dynamics)) - - function _contact_velocity(x) - q = x[1:num_positions(xnext)] - v = x[(num_positions(xnext)+1):end] - x_diff = MechanismState(xnext.mechanism, q, v) - point_in_world = transform_to_root(x_diff, point.frame) * point - contact_velocity = point_velocity(twist_wrt_world(x_diff, body), point_in_world).v - end - - contact_velocity = FreeVector3D(root_frame(xnext.mechanism), _contact_velocity(state_vector(x_dynamics)) + ForwardDiff.jacobian(_contact_velocity, state_vector(x_dynamics)) * (state_vector(xnext) - state_vector(x_dynamics))) - - D_transpose_times_v = [dot(d, contact_velocity) for d in D] - - @constraints model begin - λ .+ D_transpose_times_v .>= 0 # (8) - obstacle.μ * c_n .- sum(β) >= 0 # (9) - end - - @disjunction(model, (separation_from_obstacle == 0), (c_n == 0)) # (10) - for j in 1:k - @disjunction(model, ((λ + D_transpose_times_v[j]) == 0), β[j] == 0) # (11) - end - @disjunction(model, (obstacle.μ * c_n - sum(β) == 0), (λ == 0)) # (12) - - ContactResult(β, λ, c_n, point, obstacle) -end - -function resolve_joint_limit(xnext::MechanismState, joint::Joint, a::AbstractVector, b::Number, model::Model) - λ = @variable(model, lowerbound=0, upperbound=100, basename="λ") - q = configuration(xnext, joint) - separation = a' * q - b - @constraint model separation <= 0 - @disjunction(model, separation == 0, λ == 0) - - JointLimitResult(λ, -λ * a) -end - -function resolve_joint_limits(xnext::MechanismState, joint::Joint, limits::HRepresentation, model::Model) - [resolve_joint_limit(xnext, joint, limits.A[i, :], limits.b[i], model) for i in 1:length(limits)] -end - -function add_free_region_constraints!(model::Model, xnext::MechanismState, env::Environment, x_dynamics::MechanismState) - for (body, contact_env) in env.contacts - for contact_point in contact_env.points - function _point_in_world(x) - q = x[1:num_positions(xnext)] - v = x[(num_positions(xnext)+1):end] - x_diff = MechanismState(xnext.mechanism, q, v) - point_in_world = transform_to_root(x_diff, contact_point.frame) * contact_point - point_in_world.v - end - - position = Point3D(root_frame(xnext.mechanism), _point_in_world(state_vector(x_dynamics)) + ForwardDiff.jacobian(_point_in_world, state_vector(x_dynamics)) * (state_vector(xnext) - state_vector(x_dynamics))) - - ConditionalJuMP.disjunction!(model, - [@?(position ∈ P) for P in contact_env.free_regions]) # (7) - end - end -end - - -function update(x::MechanismState{X, M}, - u, - joint_limits::Associative{<:Joint, <:HRepresentation}, - env::Environment, - Δt::Real, - model::Model, - x_dynamics=x) where {X, M} - mechanism = x.mechanism - world = root_body(mechanism) - qnext = @variable(model, [1:num_positions(x)], lowerbound=-10, basename="qnext", upperbound=10) - vnext = @variable(model, [1:num_velocities(x)], lowerbound=-10, basename="vnext", upperbound=10) - xnext = MechanismState(mechanism, qnext, vnext) - - contact_results = map(env.contacts) do item - body, contact_env = item - body => [resolve_contact(xnext, body, contact_point, obstacle, model, x_dynamics) - for contact_point in contact_env.points for obstacle in contact_env.obstacles] - end - - externalwrenches = map(contact_results) do item - body, results = item - body => sum([Wrench(transform_to_root(x_dynamics, result.point.frame) * result.point, - contact_force(result)) for result in results]) - end - - add_free_region_constraints!(model, xnext, env, x_dynamics) - - function _config_derivative(v) - q = oftype(v, configuration(x_dynamics)) - x_diff = MechanismState(mechanism, q, v) - configuration_derivative(x_diff) - end - - jac_dq_wrt_v = ForwardDiff.jacobian(_config_derivative, velocity(x_dynamics)) - - joint_limit_results = Dict([joint => resolve_joint_limits(xnext, joint, limits, model) for (joint, limits) in joint_limits]) - joint_limit_forces = zeros(GenericAffExpr{M, Variable}, num_velocities(x)) - for (joint, results) in joint_limit_results - for result in results - joint_limit_forces .+= (jac_dq_wrt_v')[:, parentindexes(configuration(x, joint))...] * result.generalized_force - end - end - - H = mass_matrix(x_dynamics) - bias = dynamics_bias(x_dynamics, externalwrenches) - config_derivative = jac_dq_wrt_v * velocity(xnext) - - @constraint(model, H * (vnext - velocity(x)) .== Δt * (u .+ joint_limit_forces .- bias)) # (5) - @constraint(model, qnext .- configuration(x) .== Δt .* config_derivative) # (6) - - LCPUpdate(xnext, u, contact_results, joint_limit_results) -end - -function simulate(x0::MechanismState, - controller, - joint_limits::Associative{<:Joint, <:HRepresentation}, - env::Environment, - Δt::Real, - N::Integer) - results = [] # todo: type this - x = x0 - for i in 1:N - m = Model(solver=CbcSolver()) - u = controller(x) - up = update(x, u, joint_limits, env, Δt, m) - solve(m) - push!(results, getvalue(up)) - x = results[end].state - end - results -end - -# TODO: -# * record u in LCPUpdate -# * lots of cleanup -# * figure out a neater way to handle x_dynamics - -function optimize(x0::MechanismState, - joint_limits::Associative{<:Joint, <:HRepresentation}, - env::Environment, - Δt, - N::Integer, - solver=CbcSolver()) - x = x0 - m = Model(solver=solver) - results = map(1:N) do i - u = @variable(m, [1:num_velocities(x0)], basename="u_$i", lowerbound=-10, upperbound=10) - up = update(x, u, joint_limits, env, Δt, m, x0) - x = up.state - up - end - m, results - # solve(m) - # getvalue.(results) -end - -function optimize(x0::MechanismState, - joint_limits::Associative{<:Joint, <:HRepresentation}, - env::Environment, - Δt, - seed::Vector{<:LCPUpdate}) - x = x0 - m = Model(solver=CbcSolver()) - results = map(1:N) do i - u = @variable(m, [1:num_velocities(x0)], basename="u_$i", lowerbound=-10, upperbound=10) - up = update(x, u, joint_limits, env, Δt, m, x0) - setvalue(up, seed[i]) - x = up.state - up - end - warmstart!(m, true) - @assert sum(m.colCat .== :Bin) == 0 - m, results - # solve(m) - # getvalue.(results) -end - -end \ No newline at end of file diff --git a/src/ConditionalJuMP.jl b/src/ConditionalJuMP.jl index e543e8f..d300a1a 100644 --- a/src/ConditionalJuMP.jl +++ b/src/ConditionalJuMP.jl @@ -4,7 +4,6 @@ module ConditionalJuMP using JuMP using JuMP: AbstractJuMPScalar -using MacroTools: @capture using IntervalArithmetic: Interval import Base: <=, ==, >=, !, & diff --git a/test/runtests.jl b/test/runtests.jl index 37f9d8a..b2c499a 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -465,6 +465,5 @@ end @testset "complementarity" begin include("../examples/complementarity.jl") - include("../examples/multicontact_complementarity.jl") end end