by Simon DanischAug 08 2019
Julia & graphic enthusiast. Interested in high performance computing, GPUs and machine learning.

# 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
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
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
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
@time solve(problem, Tsit5(), abs_tol = 1e-8, dt = 1e-6);

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