Taking your Robot for a walk

Robots are great fun! But they're also expensive which makes it a big investment to play around with them. But why not just simulate a robot? Well, that can also easily take several weeks of figuring out how to install which software.

At JuliaCon 2019 I had the pleasure to sit together with Robin Deits, who set me up with all the packages from Julia Robotics, that are needed to make a Robot walk. The software is mainly written by Twan Koolen and Robin. It's fairly trivial to set up if you know what's needed - and it offers competitive speed to established, complex C++ based software.

To make the whole thing a bit more appealing, I'm visualizing the robot with my own plotting software, namely Makie.

The great news is, that this notebook is fully reproducible! You can just press remix, run this notebook online, et voilà: you have your own robot that you can take for a walk.

Let's cut the chit-chat and set up our robot:

robot_utils.jl

0.7s

Julia

using RigidBodySim, MathOptInterface, MechanismGeometries, QPControl
using OSQP, QPWalkingControl, RigidBodyDynamics, AtlasRobot, LinearAlgebra

include(robot_utils.jl
)

function initialize_robot(tspan = (0.0, 18.0))

  mechanism = AtlasRobot.mechanism(add_flat_ground = true)
  # static contact information
  soleframes = add_sole_frames!(mechanism)
  foot_polygons = make_foot_polygons(
    mechanism, soleframes; num_extreme_points=4
  )

  # body lookup
  feet = findbody.(Ref(mechanism), ["l_foot", "r_foot"])
  pelvis = findbody(mechanism, "pelvis");
  # create low level controller
  optimizer = create_optimizer()
  lowlevel = MomentumBasedController{4}(
      mechanism, optimizer,
      floatingjoint = findjoint(mechanism, "pelvis_to_world")
  )
  for body in bodies(mechanism)
    for point in RigidBodyDynamics.contact_points(body)
      position = RigidBodyDynamics.location(point)
      normal = FreeVector3D(default_frame(body), 0.0, 0.0, 1.0)
      μ = point.model.friction.μ
      contact = addcontact!(lowlevel, body, position, normal, μ)
      contact.maxnormalforce[] = 1e6 # TODO
      contact.weight[] = 1e-3
    end
  end

  nominalstate = MechanismState(mechanism)
  AtlasRobot.setnominal!(nominalstate)

  zdes = center_of_mass(nominalstate).v[3] - 0.05
  gz = norm(mechanism.gravitational_acceleration)
  ω = sqrt(gz / zdes)
  optimizer = create_optimizer(
    epsabs = 1e-6, epsrel = 1e-8, maxiter = 10000
  )
  max_polygon_sides = 6
  num_segments = 15
  icptraj = ICPTrajectoryGenerator{Float64, max_polygon_sides}(
    optimizer, num_segments, ω
  )
  linear_momentum_controller = ICPController(mechanism, icptraj, zdes);
  # walking state machine
  contacts = Dict(
    (BodyID(body) => contact for (body, contact) in lowlevel.contacts)
  )
  statemachine = ICPWalkingStateMachine(mechanism, contacts, icptraj)

  QPWalkingControl.init_footstep_plan!(
    statemachine, nominalstate, foot_polygons
  )

  # create high level controller
  controller = PushRecoveryController(
    lowlevel, pelvis, nominalstate,
    statemachine,
    collect(values(statemachine.end_effector_controllers)),
    linear_momentum_controller
  )

  state = MechanismState(mechanism)
  copyto!(state, nominalstate)
  Δt = 1 / 500
  pcontroller = PeriodicController(similar(velocity(state)), Δt, controller)
  damping = JointDamping{Float64}(mechanism, AtlasRobot.urdfpath())
  dynamics = Dynamics(
    mechanism, SumController(similar(velocity(state)), (pcontroller, damping))
  )
  # create a callback, that updates the state & updates the visualization.
  # Visualization gets created initially in the callback
  visual, callback = TransformPublisher(mechanism)
  callback = CallbackSet(RealtimeRateLimiter(poll_interval = pi/100), callback)

  problem = ODEProblem(dynamics, state, tspan; callback=callback)
  QPWalkingControl.init_footstep_plan!(
    statemachine, nominalstate, foot_polygons
  )
  visual, problem
end

0.7s

Julia

initialize_robot (generic function with 2 methods)

Create our robot and visualize is:

JSServe.__init__() # init the visualization library 
visual, problem = initialize_robot()
visual # Safari doesn't support WebGL 2.0, so you won't see any output

6.9s

Julia

@time solve(problem, Tsit5(), abs_tol = 1e-8, dt = 1e-6);

58.6s

Julia

For anyone without the patience to remix & run this article, here is your ticket to still see the robot walk: