Page 1
Real-Time control of Humanoid Robots using OpenJDK
Jesper Smith IHMC
40 South Alcaniz St Pensacola, Florida
32502 [email protected]
Jerry Pratt
IHMC40 South Alcaniz St
Pensacola, Florida 32502
[email protected]
Douglas Stephen
IHMC40 South Alcaniz St
Pensacola, Florida 32502
[email protected]
Alex LesmanMIT
77 Massachusetts AveCambridge, MA 02139
[email protected]
Page 2
Florida Institute for Human and Machine Cognition
Page 3
Outline
• Control using Java• Computational Considerations• Existing real-time VM• Real-time control using the
OpenJDK• Results• Conclusion
Page 4
Java simulation and controls
Simulation Robot
Page 5
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
Page 6
Computational Considerations
• Architecture• Force Control• State Estimation• Whole body optimization• Data logging• Computational cost• Threading architecture
Page 8
Force control
• Compliant system– Avoid high gain
feedback
• Increased stability• Rapid response
Page 9
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
Page 10
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
Page 11
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
Page 12
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
Page 13
Data logging and visualization
• Complete internal state– ~8000 variables– At estimator (1ms) rate
• Non-blocking• Networked
Page 14
Control architecture
Page 15
Exisiting solutions
• Requirements• Previous solution• Evaluated solutions• Micro benchmarks
Page 16
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
Page 17
Previous solution
• Previous Robots– M2V2– Mina Exo– FastRunner
• Sun Java Real-Time System– 64 bit– Java 1.5
Page 18
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
Page 20
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
Page 21
Real-Time control using the OpenJDK
• Real-time priority• Garbage collection• JIT Compilation• Synchronization• Processor Affinity
Page 22
Real-time priority
• JNI Wrapper– POSIX Thread– clock_gettime(CLOCK_MONOTONIC, &ts)– clock_nanosleep
• Attach to Java VM– Calls the Runnable interface
Page 23
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
Page 24
JIT Compilation
• Cyclic code path– No significant JIT compilation after
initialization and warmup
• Low compile treshold• Warmup period
– Disable torque output
Page 25
Synchronization
• Lockless synchronization– Avoids priority inversion– Increased troughput
• Built-inn primitives result in object allocation– ConcurrentLinkedQueue
• Implemented custom synchronization primitives– ConcurrentRingBuffer– ConcurrentCopier
Page 26
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
Page 27
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
Page 28
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
Page 31
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
Page 32
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
Page 33
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
Page 34
Conclusion
• OpenJDK– Reference implementation of latests
Java version– Performance optimized
• Freely availabe runtime– Collabration with other teams– Enables open source release of our
software
Page 35
Open Source
• IHMCRealTime– POSIX Real-time threads– Lockless Synchronization primitives– Affinity settings
• Apache 2.0 license
• Available on bitbucket– https://bitbucket.org/ihmcrobotics/
ihmcrealtime
Page 36
IHMCRealTime
PriorityParameters priority = new PriorityParameters(PriorityParameters.getMaximumPriority());
Runnable runnable = …;RealtimeThread thread = new RealtimeThread(priority, runnable);thread.start();
thread.join();
Page 37
IHMCRealTime
PriorityParameters priority = …;MonotonicTime period = new MonotonicTime(0, 1000000);PeriodicParameters param = new PeriodicParameters(period);
Runnable runnable = …;
PeriodicRealtimeThread thread = new PeriodicRealtimeThread(priority, param, runnable);
Page 38
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);
} }
Page 39
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);
Page 40
Open Source
• Planned Open Source release of– State estimator– Walking Algorithm– Physics simulation
• GPLv3 license
Page 41
Future work
• Garbage collection– 1 hour runtime– Freeze joint positions while
collecting– Controlled by user