Real-Time control of Humanoid Robots using OpenJDK

Jesper Smith IHMC

40 South Alcaniz St Pensacola, Florida

32502 jsmith@ihmc.us

Jerry Pratt

IHMC40 South Alcaniz St

Pensacola, Florida 32502

jpratt@ihmc.us

Douglas Stephen

IHMC40 South Alcaniz St

Pensacola, Florida 32502

dstephen@ihmc.us

Alex LesmanMIT

77 Massachusetts AveCambridge, MA 02139

lesman@mit.edu

Florida Institute for Human and Machine Cognition

Outline

• Control using Java• Computational Considerations• Existing real-time VM• Real-time control using the

OpenJDK• Results• Conclusion

Java simulation and controls

Simulation Robot

Why Java

• Fast• Relatively easy learning curve

– Mechanical/Control engineers– Extensive standard library

• Quick deployment– Compared to statically compiled languages– Significantly increases number of tests we

can do

• Mature IDE support– Refactor codebase

Computational Considerations

• Architecture• Force Control• State Estimation• Whole body optimization• Data logging• Computational cost• Threading architecture

Architecture

Force control

• Compliant system– Avoid high gain

feedback

• Increased stability• Rapid response

Force control

• Requirements on control system

• F = m * a– Delays lead to long lasting accelerations– Overshoot control target

• Maximum delay of 10ms– Based on previous work

State Estimation

• Incoming data at 1000 Hz• Kalman Filter

– Joint state– Internal Measurement Unit (IMU)

• Orientation• Rotational velocity• Linear Acceleration

– Kinematic data

• Estimate state of the robot– Position in world– Hand, feet, and head location

11

Whole-body motion control framework

Motion constraints

Rate of change of momentum objective

J v̇= p

A v̇  = b

Robot’s state

High level controller

Quadratic program solver

Inverse dynamics calculator

Robot

Available contacts

W iWrenches on

contacting bodies

v̇Joint accelerations

τJoint torques

Computational cost

• State estimation– Average: 0.15 ms– Maximum: 0.26 ms

• Whole body optimization and motion planning– Average: 2.5 ms– Maximum: 4.6 ms

• 2.6GHz Intel i7-4960HQ with 16GB RAM

Data logging and visualization

• Complete internal state– ~8000 variables– At estimator (1ms) rate

• Non-blocking• Networked

Control architecture

Exisiting solutions

• Requirements• Previous solution• Evaluated solutions• Micro benchmarks

Requirements

• 64 bit VM– Native drivers for Atlas

• Throughput comparable to OpenJDK– Complexity of control code

• Compilation and startup less than 5 min– Delays in testing

• Java 1.6 compatible– Preferably a road map to 1.7/1.8

compatibility

Previous solution

• Previous Robots– M2V2– Mina Exo– FastRunner

• Sun Java Real-Time System– 64 bit– Java 1.5

Evaluated Solutions

• Atego PERC– Java 1.6– 32 bit– AOT/JIT compliation

• Aicas Jamaica VM– Java 1.6– 32/64 bit– AOT compilation

• Sun JTRS– Java 1.5– 32/64 bit– JIT compilation

• Websphere RT for RT Linux– Java 1.7– 32 bit– AOT/JIT compilation– Rejected for non-

technical reasons

Microbenchmarks

Microbenchmarks

• Sun JRTS is the fastest Realtime VM– But unsupported, Java 1.5 only

• Other RT VMs are significantly slower– Also introduce large compilation time

• Nothing near OpenJDK performance– Results even worse on full controller

Real-Time control using the OpenJDK

• Real-time priority• Garbage collection• JIT Compilation• Synchronization• Processor Affinity

Real-time priority

• JNI Wrapper– POSIX Thread– clock_gettime(CLOCK_MONOTONIC, &ts)– clock_nanosleep

• Attach to Java VM– Calls the Runnable interface

Garbage Collection

• Realtime Garbage collection is hard– Outside project scope

• Competition rules limit runtime to 30 minutes– Force GC cycle after initialization– Increase heap size– Use predictable Serial collector– Avoid object allocation

JIT Compilation

• Cyclic code path– No significant JIT compilation after

initialization and warmup

• Low compile treshold• Warmup period

– Disable torque output

Synchronization

• Lockless synchronization– Avoids priority inversion– Increased troughput

• Built-inn primitives result in object allocation– ConcurrentLinkedQueue

• Implemented custom synchronization primitives– ConcurrentRingBuffer– ConcurrentCopier

ConcurrentRingBuffer

• Single reader, single writer– Volatile read/write

index

• Re-use objects– Allocate data

structures on construction

• High troughput– Reduce false

sharing with padding on x86

ConcurrentCopier

• Communication between state estimator and controller– Only latest data is of interest– Older data can be discarded

• Internal three element buffer– Atomic read/writes– Pre-allocated data– Old data is overwritten

Processor Affinity

• Thread migration between cores introduces jitter– Especially prevelant on multi-cpu

systems

• Pin real-time threads to single core– Isolate cores from scheduler

• JNI Wrapper– sched_setaffinity

RESULTS

Missed DeadlinesController lags estimator

Controller leads estimator

Non-synchronized events

Run 1 0 2 2

Run 2 0 0 0

Run 3 1 1 2

DelayMinimum Average Maximum

State Estimator computation time

0.12ms 0.13ms 0.20ms

Controller computation time

1.3ms 1.4ms 2.0ms

Total Delay 2.24ms 2.51ms 3.34ms

Conclusion

• Real-time control using the OpenJDK– High troughput– Minimum amount of missed deadlines

• Lack of GC is not critical for our application– Large heap– Minimal object allocation

• Lockless synchronization– Improves performance even in non real-time

sims

Conclusion

• OpenJDK– Reference implementation of latests

Java version– Performance optimized

• Freely availabe runtime– Collabration with other teams– Enables open source release of our

software

Open Source

• IHMCRealTime– POSIX Real-time threads– Lockless Synchronization primitives– Affinity settings

• Apache 2.0 license

• Available on bitbucket– https://bitbucket.org/ihmcrobotics/

ihmcrealtime

IHMCRealTime

PriorityParameters priority = new PriorityParameters(PriorityParameters.getMaximumPriority());

Runnable runnable = …;RealtimeThread thread = new RealtimeThread(priority, runnable);thread.start();

thread.join();

IHMCRealTime

PriorityParameters priority = …;MonotonicTime period = new MonotonicTime(0, 1000000);PeriodicParameters param = new PeriodicParameters(period);

Runnable runnable = …;

PeriodicRealtimeThread thread = new PeriodicRealtimeThread(priority, param, runnable);

IHMCRealTime

ConcurrentRingBuffer<?> foo = ...;  public void run()  {

while(true)    {

 Bar bar = foo.next();

 bar.set(data);      foo.commit();    }  }

ConcurrentRingBuffer<?> foo = A.foo;  public void run({ while(true) { if(foo.poll())

{ while((bar = foo.read())

!= null) {

process(bar)  }  Thread.sleep(1);

   }  }

IHMCRealTime

CPUTopology topology = new CPUTopology();System.out.println(topology.toString());

if(topology.isHyperThreadingEnabled()) System.err.println(“Bad things happen”);

Processor processor = new Processor(2);RealtimeThread thread = …;Affinity.setAffinity(thread, processor);

Open Source

• Planned Open Source release of– State estimator– Walking Algorithm– Physics simulation

• GPLv3 license

Future work

• Garbage collection– 1 hour runtime– Freeze joint positions while

collecting– Controlled by user

Questions