Real-Time control of
Humanoid Robots using
OpenJDK
Jesper Smith
IHMC
40 South Alcaniz St
Pensacola, Florida
32502
jsmith@ihmc.us
Douglas Stephen
IHMC
40 South Alcaniz St
Pensacola, Florida
32502
dstephen@ihmc.us
Jerry Pratt
IHMC
40 South Alcaniz St
Pensacola, Florida
32502
jpratt@ihmc.us
Alex Lesman
MIT
77 Massachusetts Ave
Cambridge, 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
F i gur e 2: H i gh l ev el ar ch i t ect u r e of our com m and and cont r ol set up for t h e A t l as r ob ot .
T ab l e 1: E x ecu t i on t i m e of t h e st at e est i m at or and
cont r ol l er i s si m u l at i on t aken on a 2.6G H z I nt el i 74960H Q w i t h 16G B of R A M .
T ab l e 2: M ai n t hr ead s r un ni ng t he cont r ol soft war e
on t he r ob ot .
T hread descript ion Priorit y
Execut ion rat e
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
Whole-body motion control
framework
Robot’s state
Available Qρ = ∑Wi
contacts ρ ≥ 0
High level
controller
Motion constraints
Wrenches on
W
contacting bodies i
Quadratic
program solver
Jv = p
Rate of change of
momentum objective Av = b
Inverse dynamics
calculator
Joint
τ
torques
Robot
Joint
v
accelerations
11
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
St at e est imat or
Cont roller
Logging
Networking
Real-t ime
Real-t ime
Non-real-t ime
Non-real-t ime
1ms
6ms
1ms
on-demand
Control architecture
Real-time threads
Atlas Robot
State Estimator
Copier
Ring Buffer
Logger
Controller
Ring Buffer
F i gur e 3: B l ock di agr am of t he t h r eadi n g fl ow .
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 nontechnical reasons
Microbenchmarks
SciMark2-BenchmarksMFlops/Second-(Higher-is-Be5 er)-
3000"
2500"
2000"
1500"
OpenJDK"7"
Perc"VM"
1000"
JamaicaBuilder"(parallel,"full)"
RTSJ"1.5"
500"
0"
Composite"
FFT"
SOR"
Monte"Carlo" Sparse"
LU"
(1048576)" (1000x1000)"
matmult" (1000x1000)"
(N=100000,"
nz=1000000)"
F i gu r e 5: O p en J D K J ava 7 com par ed t o o↵ er i n gs fr om P er c, t he ful l ahead-of-t i m e J am ai ca com p i l e w i t h
p ar al l el i sm , and t h e Su n RT SJ 1.5. I t i s cl ear t hat t he p er for m an ce of t he avai l ab l e r eal -t i m e v i r t ual m achi n es
l ags t he p er for m ance of t he O p en J D K by a si gni fi cant am ount .
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 Deadlines
Controller lags
estimator
Controller leads
estimator
Non-synchronized
events
Run 1
0
2
2
Run 2
0
0
0
Run 3
1
1
2
Delay
Minimum
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.getMaximumPrio
rity());
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