Real-Time control of Humanoid Robots using OpenJDK

1. Real-Time control of Humanoid Robots using OpenJDK Douglas Stephen IHMC40 South Alcaniz St Pensacola, Florida 32502 dstephen@ihmc.us Alex Lesman MIT 77 Massachusetts Ave Cambridge, MA 02139 lesman@mit.edu Jesper Smith IHMC40 South Alcaniz St Pensacola, Florida 32502 jsmith@ihmc.us Jerry Pratt IHMC40 South Alcaniz St Pensacola, Florida 32502 jpratt@ihmc.us

2. Florida Institute for Human and Machine Cognition

3. Outline • Control using Java • Computational Considerations • Existing real-time VM • Real-time control using the OpenJDK • Results • Conclusion

4. Java simulation and controls Simulation Robot

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

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

7. Architecture

8. Force control • Compliant system • Avoid high gain feedback • Increased stability • Rapid response

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

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

11. Whole-body motion control framework Wrenches on contacting bodies Available contacts Robot’s state Joint torques Joint accelerations High level controller Quadratic program solver Inverse dynamics calculator Robot Motion constraints Rate of change of momentum objective

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

13. Data logging and visualization • Complete internal state • ~8000 variables • At estimator (1ms) rate • Non-blocking • Networked

14. Control architecture

15. Exisiting solutions • Requirements • Previous solution • Evaluated solutions • Micro benchmarks

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

17. Previous solution • Previous Robots • M2V2 • Mina Exo • FastRunner • Sun Java Real-Time System • 64 bit • Java 1.5

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

19. Microbenchmarks

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

21. Real-Time control using the OpenJDK • Real-time priority • Garbage collection • JIT Compilation • Synchronization • Processor Affinity

22. Real-time priority • JNI Wrapper • POSIX Thread • clock_gettime(CLOCK_MONOTONIC, &ts) • clock_nanosleep • Attach to Java VM • Calls the Runnable interface

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

24. JIT Compilation • Cyclic code path • No significant JIT compilation after initialization and warmup • Low compile treshold • Warmup period • Disable torque output

25. Synchronization • Lockless synchronization • Avoids priority inversion • Increased troughput • Built-inn primitives result in object allocation • ConcurrentLinkedQueue • Implemented custom synchronization primitives • ConcurrentRingBuffer • ConcurrentCopier

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

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

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

29. RESults

31. Missed Deadlines

32. Delay

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

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

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

36. IHMCRealTime PriorityParameters priority = new PriorityParameters(PriorityParameters.getMaximumPriority()); Runnable runnable = …; RealtimeThread thread = new RealtimeThread(priority, runnable); thread.start(); thread.join();

37. IHMCRealTime PriorityParameters priority = …; MonotonicTime period = new MonotonicTime(0, 1000000); PeriodicParameters param = new PeriodicParameters(period); Runnable runnable = …; PeriodicRealtimeThread thread = new PeriodicRealtimeThread(priority, param, runnable);

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);    }  }

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

40. Open Source • Planned Open Source release of • State estimator • Walking Algorithm • Physics simulation • GPLv3 license

41. Future work • Garbage collection • 1 hour runtime • Freeze joint positions while collecting • Controlled by user

42. Questions