Simon Danisch / Aug 08 2019

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:

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

) 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
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: