An analytical diabolo model for robotic learning and control (ICRA ‘21)

Felix von Drigalski, Devwrat Joshi, Takayuki Murooka, Kazutoshi Tanaka, Masashi Hamaya, Yoshihisa Ijiri, "An analytical diabolo model for robotic learning and control", ICRA2021

blog: https://medium.com/sinicx/an-analytical-diabolo-model-for-robotic-learning-and-control-c8d8a8384d6d

Transcript

1. © 2021 OMRON SINIC X Corporation. All Rights Reserved. An analytical diabolo model for robotic learning and control (ICRA ‘21) Felix von Drigalski, Devwrat Joshi*, Takayuki Murooka**, Kazutoshi Tanaka, Masashi Hamaya, Yoshihisa Ijiri (OMRON SINIC X) *(Osaka University) **(University of Tokyo) International Conference on Robotics and Automation (ICRA) 2021 Jun. 1, 2021
2. OMRON SINIC X An analytical diabolo model for robotic learning and control ICRA 2021 Felix von Drigalski*, Devwrat Joshi*, Takayuki Murooka, Kazutoshi Tanaka, Masashi Hamaya, Yoshihisa Ijiri
3. 2 © OMRON SINIC X Corporation All Rights Reserved • Robots that perform human tricks are inspiring and push the limits of technology • Hardly any previous work on diabolo play Difficult control à Machine learning? Objectives: 1. Play diabolo with a robot 2. Create a diabolo learning environment with long curriculum of incremental challenges and high difficulty ceiling 1. Motivation Video: Jacob Sharpe Video: Hiroki Kamei Video: Chris Atkeson
4. 3 © OMRON SINIC X Corporation All Rights Reserved 1. Motivation 2. Diabolo model (simulation) 3. Diabolo play 1. Trajectory generation 2. Robot implementation 4. Results 5. Conclusion Agenda
5. 4 © OMRON SINIC X Corporation All Rights Reserved - A diabolo is played using a string between two sticks - The friction of the string accelerates the diabolo - Unstable system! Pitch and yaw need to be controlled via stick positions 2.0. Diabolo Basics Moving the sticks along the rotation axis causes change in tilt (pitch) Uneven pitch causes slow change in yaw Accelerating the diabolo in different ways
6. 5 © OMRON SINIC X Corporation All Rights Reserved - Diabolo = a point mass with a rotation speed, pitch and yaw - Gravity and basic physics (free motion) are trivial to simulate - Simulating the string is complex à use auxiliary ellipsoid defined by stick tips 2.1. Diabolo model Ellipse The points that the diabolo can reach when the string is taut are described by an ellipsoid (a 3D rotated ellipse)
7. 6 © OMRON SINIC X Corporation All Rights Reserved • At each time step, we obtain the next diabolo state with these 4 operations • For simplicity, we choose to ignore forces and accelerations (except for gravity) • We will walk through the calculations for two example cases: 1. No stick motion (diabolo swings) 2. Stick motion (diabolo is thrown) • Please refer to the paper for the mathematical equations. 2.1.0. Simulation Intro Velocity vector 𝒗 Step Done Calculate free motion Constrain position Add velocity Constrain velocity
8. 7 © OMRON SINIC X Corporation All Rights Reserved End of time step t-1 Diabolo on the string Velocity vector in ellipse tangent plane All calculation steps finished Velocity vector 𝒗 2.1. Simulation (stationary sticks) Step Done Calculate free motion ✔ Constrain position ✔ Add velocity ✔ Constrain velocity ✔
9. 8 © OMRON SINIC X Corporation All Rights Reserved 2.1.1. Calculate free motion Velocity Verlet algorithm to calculate next diabolo position First velocity estimate 𝒗𝟎 (time t) First diabolo position estimate 𝒑𝟎 (time t) 𝒑 (time t-1) Step Done Calculate free motion ✔ Constrain position _ Add velocity _ Constrain velocity _
10. 9 © OMRON SINIC X Corporation All Rights Reserved 2.1.2. Constrain position Diabolo may not move outside of the ellipse à move towards ellipsoid center until on the ellipsoid Step Done Calculate free motion ✔ Constrain position _ Add velocity _ Constrain velocity _ 𝒑𝟎 First velocity estimate 𝒗𝟎 (t)
11. 10 © OMRON SINIC X Corporation All Rights Reserved 2.1.2. Constrain position Diabolo may not move outside of the ellipse à move towards ellipsoid center until on the ellipsoid First velocity estimate 𝒗𝟎 (t) Step Done Calculate free motion ✔ Constrain position ✔ Add velocity _ Constrain velocity _ 𝒑𝒏𝒆𝒘 𝒑𝟎
12. 11 © OMRON SINIC X Corporation All Rights Reserved 2.1.3. Add velocity First velocity estimate 𝒗𝟎 (t) Step Done Calculate free motion ✔ Constrain position ✔ Add velocity ✔ Constrain velocity _ Sticks stationary à No velocity added 𝒑𝒏𝒆𝒘
13. 12 © OMRON SINIC X Corporation All Rights Reserved 2.1.4. Constrain velocity Velocity cannot point outside of ellipse (physically infeasible) à project onto ellipse tangent plane New velocity vector 𝒗𝒏𝒆𝒘 (t) Ellipse normal Step Done Calculate free motion ✔ Constrain position ✔ Add velocity ✔ Constrain velocity ✔ 𝒗𝟎
14. 13 © OMRON SINIC X Corporation All Rights Reserved 2.1.5. Time step t (done) Step Done Calculate free motion ✔ Constrain position ✔ Add velocity ✔ Constrain velocity ✔
15. 14 © OMRON SINIC X Corporation All Rights Reserved Next, we consider what happens when the sticks move and add velocity to the diabolo. 2.2. Simulation (moving sticks) Step Done Calculate free motion _ Constrain position _ Add velocity _ Constrain velocity _
16. 15 © OMRON SINIC X Corporation All Rights Reserved Next, we consider what happens when the sticks move and add velocity to the diabolo. 2.2. Simulation (moving sticks) Step Done Calculate free motion _ Constrain position _ Add velocity _ Constrain velocity _
17. 16 © OMRON SINIC X Corporation All Rights Reserved The start state is the same. 2.2. Time step t-1 Velocity vector 𝒗 Step Done Calculate free motion _ Constrain position _ Add velocity _ Constrain velocity _
18. 17 © OMRON SINIC X Corporation All Rights Reserved Apply velocity + gravity 2.2.1. Calculate free motion First velocity estimate 𝒗𝟎 (t) First diabolo position estimate 𝒑𝟎 (time t) (from free motion) Step Done Calculate free motion ✔ Constrain position _ Add velocity _ Constrain velocity _
19. 18 © OMRON SINIC X Corporation All Rights Reserved At time t, the sticks have moved to new locations. The ellipsoid has moved. 2.2.1. Calculate free motion First velocity estimate 𝒗𝟎 (t) First diabolo position estimate 𝒑𝟎 (time t) (from free motion) Step Done Calculate free motion ✔ Constrain position _ Add velocity _ Constrain velocity _
20. 19 © OMRON SINIC X Corporation All Rights Reserved We “pull” the diabolo to the ellipsoid center until the string length constraint is satisfied. 2.2.2. Constrain position Step Done Calculate free motion ✔ Constrain position ✔ Add velocity _ Constrain velocity _ 𝒑𝟎 𝒗𝟎 𝒑𝒏𝒆𝒘 (Corrected position)
21. 20 © OMRON SINIC X Corporation All Rights Reserved 2.2.3. Add velocity To arrive at the new position, the diabolo must have moved at velocity 𝒗𝒑𝒖𝒍𝒍 , which we add to the diabolo. Step Done Calculate free motion ✔ Constrain position ✔ Add velocity ✔ Constrain velocity _ 𝒗𝒑𝒖𝒍𝒍 𝒗𝟎 𝒗𝒏𝒆𝒘 𝒑𝒏𝒆𝒘 𝒑𝟎
22. 21 © OMRON SINIC X Corporation All Rights Reserved 2.2.4. Constrain velocity In this example, 𝒗𝒏𝒆𝒘 points inside the ellipse, so the velocity vector is already feasible. Step Done Calculate free motion ✔ Constrain position ✔ Add velocity ✔ Constrain velocity ✔ 𝒗𝒏𝒆𝒘
23. 22 © OMRON SINIC X Corporation All Rights Reserved 2.2. Time steps t, t+1… If the sticks stopped moving, the diabolo would detach from the string and enter free fall as if it was thrown (FLYING state)
24. 23 © OMRON SINIC X Corporation All Rights Reserved - To allow throwing and catching, we define these states: (s: distance from diabolo to ellipsoid; 𝒄𝑳 , 𝒄𝑭 : distance values ≅ 2, 5 cm) - ON_STRING: string keeps diabolo inside ellipsoid string friction affects rotation speed - ON_STRING_LOOSE: string does not affect rot. speed - FLYING: no effect on diabolo (freefall) - To catch, the string needs to be taut 2.3. Diabolo States ON_STRING <-> ON_STRING_LOOSE ON_STRING <-> FLYING
25. 24 © OMRON SINIC X Corporation All Rights Reserved - Stick trajectory splines are defined via keypoints with timings via GUI - Before the current trajectory finishes, the next one is calculated - The optimizer uses the diabolo model to predict the diabolo trajectories from stick trajectories - The stick trajectory resulting in the diabolo trajectory closest to the goal(s) is chosen - Diabolo goal states are hand-crafted for each type of motion 3.1. Stick Trajectory Generation Goal state
26. 25 © OMRON SINIC X Corporation All Rights Reserved - As the stick position does not impose an orientation constraint, following it with circular motions would lead to drift - To avoid this and other IK failures, we define joint states at the corners of cuboids, and interpolate linearly inside the cuboid’s volume - The interpolated seed is passed to BioIK (genetic optimization) 3.2. Robot Implementation: Seeded IK Joint poses used to interpolate seeds for the IK
27. 26 © OMRON SINIC X Corporation All Rights Reserved To validate the model and seed learning approaches, we recorded ~60 minutes of human demonstration, using motion capture and IR markers to track the diabolos and sticks. 4. Results Diabolos with different sizes and characteristics. IR-reflective markers attached to the inside to withstand centrifugal force. Non-reflective dummy markers avoid symmetry.
28. 27 © OMRON SINIC X Corporation All Rights Reserved Model accuracy 4.1. Results Our diabolo model outperforms a neural network predicting its trajectory. Red: Ours Cyan: Neural Network Yellow: Ground Truth
29. 28 © OMRON SINIC X Corporation All Rights Reserved Simulation 4.2. Results Green: Diabolo goal states Red: auxiliary ellipsoid, Yellow (top): stick trajectory Yellow (bottom): predicted diabolo path Gazebo
30. 29 © OMRON SINIC X Corporation All Rights Reserved Real robot experiments 4.3. Results Accelerating & vertical throwing Sideways throwing & passing Limitations: - No pitch/yaw compensation - Diabolo can fall “out of phase” - Quick motions can cause protective stops (especially for throwing) System diagram Robot control Stick trajectory Joint trajectory Control node Robot IK Player Diabolo predictor State estimation Joint targets Camera images Diabolo state 2 Hz 100 Hz 500 Hz 2 Hz 100 Hz Target diabolo trajectory
31. 30 © OMRON SINIC X Corporation All Rights Reserved - A physical model to simulate a diabolo on a string - A method to play diabolo on two robot arms - A dataset of human demonstrations Future work: - Model diabolo-string interactions (pitch/yaw, wrapping around axle) - OpenAI Gym Environment - Time-stretch stick trajectories in optimization to maintain phase Conclusion