notebooks/Standing controller.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "using Pkg\n",
    "Pkg.activate(@__DIR__)\n",
    "Pkg.instantiate()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "using LinearAlgebra\n",
    "using QPControl\n",
    "using RigidBodyDynamics\n",
    "using RigidBodyDynamics.PDControl\n",
    "using RigidBodyDynamics.Contact\n",
    "using StaticArrays\n",
    "using AtlasRobot\n",
    "using BenchmarkTools\n",
    "using Test\n",
    "using RigidBodySim"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# load URDF\n",
    "mechanism = AtlasRobot.mechanism()\n",
    "remove_fixed_tree_joints!(mechanism);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# add environment\n",
    "rootframe = root_frame(mechanism)\n",
    "ground = HalfSpace3D(Point3D(rootframe, 0., 0., 0.), FreeVector3D(rootframe, 0., 0., 1.))\n",
    "add_environment_primitive!(mechanism, ground);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# create optimizer\n",
    "using MathOptInterface\n",
    "using OSQP\n",
    "using OSQP.MathOptInterfaceOSQP: OSQPSettings\n",
    "const MOI = MathOptInterface\n",
    "optimizer = OSQP.Optimizer()\n",
    "MOI.set(optimizer, OSQPSettings.Verbose(), false)\n",
    "MOI.set(optimizer, OSQPSettings.EpsAbs(), 1e-5)\n",
    "MOI.set(optimizer, OSQPSettings.EpsRel(), 1e-5)\n",
    "MOI.set(optimizer, OSQPSettings.MaxIter(), 5000)\n",
    "MOI.set(optimizer, OSQPSettings.AdaptiveRhoInterval(), 25) # required for deterministic behavior"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "# create low level controller\n",
    "const num_basis_vectors = 4\n",
    "lowlevel = MomentumBasedController{num_basis_vectors}(mechanism, optimizer,\n",
    "    floatingjoint = findjoint(mechanism, \"pelvis_to_world\"));\n",
    "for body in bodies(mechanism)\n",
    "    for point in RigidBodyDynamics.contact_points(body)\n",
    "        position = location(point)\n",
    "        normal = FreeVector3D(default_frame(body), 0.0, 0.0, 1.0)\n",
    "        μ = point.model.friction.μ\n",
    "        contact = addcontact!(lowlevel, body, position, normal, μ)\n",
    "        contact.maxnormalforce[] = 1e6 # TODO\n",
    "        contact.weight[] = 1e-3\n",
    "    end\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# state initialization\n",
    "function initialize!(state::MechanismState)\n",
    "    mechanism = state.mechanism\n",
    "    zero!(state)\n",
    "    kneebend = 1.1\n",
    "    hipbendextra = 0.1\n",
    "    for sideprefix in ('l', 'r')\n",
    "        knee = findjoint(mechanism, \"$(sideprefix)_leg_kny\")\n",
    "        hippitch = findjoint(mechanism, \"$(sideprefix)_leg_hpy\")\n",
    "        anklepitch = findjoint(mechanism, \"$(sideprefix)_leg_aky\")\n",
    "        set_configuration!(state, knee, [kneebend])\n",
    "        set_configuration!(state, hippitch, [-kneebend / 2 + hipbendextra])\n",
    "        set_configuration!(state, anklepitch, [-kneebend / 2 - hipbendextra])\n",
    "    end\n",
    "    floatingjoint = first(out_joints(root_body(mechanism), mechanism))\n",
    "    set_configuration!(state, floatingjoint, [1; 0; 0; 0; 0; 0; 0.85])\n",
    "    state\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# create standing controller\n",
    "feet = findbody.(Ref(mechanism), [\"l_foot\", \"r_foot\"])\n",
    "pelvis = findbody(mechanism, \"pelvis\")\n",
    "nominalstate = MechanismState(mechanism)\n",
    "initialize!(nominalstate)\n",
    "controller = StandingController(lowlevel, feet, pelvis, nominalstate);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "state = MechanismState(mechanism)\n",
    "initialize!(state)\n",
    "τ = similar(velocity(state));\n",
    "benchresult = @benchmark $controller($τ, 0.0, $state)\n",
    "@show benchresult.allocs\n",
    "@test benchresult.allocs <= 24\n",
    "benchresult"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# function test(τ, state, n)\n",
    "#     for i = 1 : n\n",
    "#         controller(τ, 0.0, state)\n",
    "#     end\n",
    "# end\n",
    "# test(τ, state, 1)\n",
    "# Profile.clear()\n",
    "# @profile test(τ, state, 10000)\n",
    "# using ProfileView\n",
    "# ProfileView.view()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#NBSKIP\n",
    "# set up visualizer\n",
    "using MeshCat\n",
    "using MeshCatMechanisms\n",
    "if !@isdefined(vis) || !any(isopen, vis.core.scope.pool.connections)\n",
    "    vis = Visualizer()[:atlas]\n",
    "    visuals = URDFVisuals(AtlasRobot.urdfpath(); package_path = [AtlasRobot.packagepath()])\n",
    "    mvis = MechanismVisualizer(mechanism, visuals, vis)\n",
    "    set_configuration!(mvis, configuration(nominalstate))\n",
    "    open(mvis)\n",
    "    wait(mvis)\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# simulate\n",
    "state = MechanismState(mechanism)\n",
    "initialize!(state)\n",
    "Δt = 1 / 500\n",
    "pcontroller = PeriodicController(similar(velocity(state)), Δt, controller)\n",
    "# TODO: add damping\n",
    "dynamics = Dynamics(mechanism, pcontroller)\n",
    "problem = ODEProblem(dynamics, state, (0., 10.))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "sol = solve(problem, Tsit5(), abs_tol = 1e-8, dt = 1e-6)\n",
    "@time sol = solve(problem, Tsit5(), abs_tol = 1e-8, dt = 1e-6)\n",
    "@test sol.retcode == :Success\n",
    "copyto!(state, last(sol.u))\n",
    "@test norm(velocity(state)) ≈ 0 atol=1e-8\n",
    "@test center_of_mass(state).v[3] > 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#NBSKIP\n",
    "setanimation!(mvis, sol)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "@webio": {
   "lastCommId": "0c004afddc5c403686132dfef876e225",
   "lastKernelId": "f3249b6f-b037-4dc2-b14d-a2e5ab980676"
  },
  "kernelspec": {
   "display_name": "Julia 1.1.0",
   "language": "julia",
   "name": "julia-1.1"
  },
  "language_info": {
   "file_extension": ".jl",
   "mimetype": "application/julia",
   "name": "julia",
   "version": "1.1.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}