1. 1 MARS PI Meeting JSC 8/2004 Interaction Lab USC – Interaction Lab Skill Learning by Primitives-Based Demonstration & Imitation Maja Matarić - PI Marcelo Kallmann, Chad Jenkins - postdocs Amit Ramesh - PhD student Nathan Miller - subcontracted engineer

2. 2 MARS PI Meeting JSC 8/2004 Interaction Lab Overall Goals Goal: –xx

3. 3 MARS PI Meeting JSC 8/2004 Interaction Lab Major Contributions Contributions: –Development of the USC motion suit for low cost, lightweight, wireless, real-time motion collection –Cross-kinematics metrics for imitation learning, allowing for comparison and mapping of motion data across various kinematic structures –Prediction of expected sensory information for a successful grasping, from the analysis and interpolation of collected Robonaut sensory data –The use of randomized motion planning for reaching and for motion sequencing

4. 4 MARS PI Meeting JSC 8/2004 Interaction Lab Major Contributions Demonstrations/Applications: –Teleoperation of Robonaut –The implementation of human-robot cooperation applications for a variety of tasks. An application example was demonstrated using Robosim (NASA’s Robonaut simulator)

5. 5 MARS PI Meeting JSC 8/2004 Interaction Lab Architecture Overview USC Motion Suit Cross-kinematics metrics for imitation learning are used for collecting motion from various types of kinematics Other sources of motion collection Robot-Ready Motion Database Derivation of motion controllers based on collected motion and sensory data Robonaut Control Learned skills are represented as parameterized motion controllers Derivation of motion controllers based on collected motion and randomized roadmaps

6. 6 MARS PI Meeting JSC 8/2004 Interaction Lab USC Motion Suit Applications:

7. 7 MARS PI Meeting JSC 8/2004 Interaction Lab Cross-Kinematics Metric Applications:

8. 8 MARS PI Meeting JSC 8/2004 Interaction Lab Learning Sensory Structures Applications:

9. 9 MARS PI Meeting JSC 8/2004 Interaction Lab Collision-Free Reaching Goal: Efficient collision-free motion planning for humanoid arms Applications: –manipulation tasks in environments with obstacles –relocating grasped objects in the workspace –maintenance tasks when the tools, controls, handles, etc, to be reached are situated in difficult locations

10. 10 MARS PI Meeting JSC 8/2004 Interaction Lab Potential Application Example Truss Assembling –Collision-free reaching for tools and for grasping bars

11. 11 MARS PI Meeting JSC 8/2004 Interaction Lab Approach Our tests indicate that sampling-based motion planners are more efficient than other methods, e.g. such as IK with collision avoidance (Drumwright, Kallmann, Mataric, “Towards Single-Arm Reaching for Humanoids in Dynamic Environments”, submitted to Humanoids’04 ) On-Line Arms Motion Planning –We have applied the on-line Rapidly-exploring Random Trees (RRT) on the composite configuration space of the two arms –Motions can be achieved in 1 or 2 secs in simple scenarios Dynamic Roadmaps –When the workspace has few changes, dynamic roadmaps can be very efficient. Joins the advantages of multi-query methods (PRMs, PRTs, VGs) and single-query methods (RRTs, Exp. Spaces, SBLs).

12. 12 MARS PI Meeting JSC 8/2004 Interaction Lab On-line Arms Motion Planning Example collision-free motions to pre-grasping targets –Visualization geometry: 23930 triangles –Collision geometry: 1016 triangles –Computation time: 1 to 2s, including optimization (smoothing) –Optimization takes about 0.3s (Pentium III 2.8 GHz)

13. 13 MARS PI Meeting JSC 8/2004 Interaction Lab Path Optimization (1/2) Incremental path linearization –Simple and efficient in most cases –May be time-consuming as collision detection must be invoked before each local linearization.

14. 14 MARS PI Meeting JSC 8/2004 Interaction Lab Path Optimization (2/2) Decoupled, sub-configuration linearization can be applied: –In the example, the top arm in the left video makes an unnecessary motion –This can be only corrected when smoothing arms independently (right video)

15. 15 MARS PI Meeting JSC 8/2004 Interaction Lab Dynamic Roadmaps: Overview Evaluation of dynamic roadmaps for finding collision-free arm motions in changing environments Comparison with on-line bi-directional RRT 4 DOFs 7 DOFs 17 DOFs

16. 16 MARS PI Meeting JSC 8/2004 Interaction Lab Roadmap Computation A grid is defined over the workspace Desired number of configurations are sampled

17. 17 MARS PI Meeting JSC 8/2004 Interaction Lab Roadmap Computation Sampled nodes are connected to the k neighbors if the connection is valid, until linking all valid connections

18. 18 MARS PI Meeting JSC 8/2004 Interaction Lab Roadmap Computation Cell localization in respect to the roadmap nodes: –For each cell, test all edges and nodes which are invalid in respect to that cell, ie, which “collide” with that cell –Each cell keeps references to the invalidated edges/nodes –More efficient methods: Hierarchical tests of cells Exploiting cell adjacency coherence (Leven and Hutchinson 2000) [2] Using graphics hardware

19. 19 MARS PI Meeting JSC 8/2004 Interaction Lab Roadmap Computation Cells are localized and roadmap nodes and edges intersecting with objects are invalidated

20. 20 MARS PI Meeting JSC 8/2004 Interaction Lab Roadmap Maintenance Whenever obstacles in the workspace are detected to change position, the affected cells are either liberated or occupied Reference counting management: –All roadmap nodes/edges maintain reference counters –When cells are occupied/liberated, the associated nodes/edges have their counters incremented/decremented

21. 21 MARS PI Meeting JSC 8/2004 Interaction Lab Roadmap Maintenance Examples: 24x322218 nodes64x702406 nodes48x646661 nodes

22. 22 MARS PI Meeting JSC 8/2004 Interaction Lab Roadmap Maintenance Examples: 20 2 3146 nodes40 2 2898 nodes

23. 23 MARS PI Meeting JSC 8/2004 Interaction Lab Roadmap Maintenance Examples: 24 3 3575 nodes24 3 5144 nodes

24. 24 MARS PI Meeting JSC 8/2004 Interaction Lab Roadmap Maintenance Examples: 24 3 3575 nodes24 3 5144 nodes

25. 25 MARS PI Meeting JSC 8/2004 Interaction Lab Roadmap Query Find motion from initial configuration q i to goal configuration q g Retrieve nearest nodes N(q i ), N(q g ) in the roadmap Retrieve a path from N(q i ) to N(q g ): A* search, however: –Path may not exist –q i to N(q i ) may not be valid –q g to N(q g ) may not be valid In case of failure, a Bi-RRT is used as on-line planner –to fix the path if one of the tests fails –Compute the full path

26. 26 MARS PI Meeting JSC 8/2004 Interaction Lab Experiments 100 random problems for each scenario –Random obstacles, initial and goal configurations –Failure considered after 10 seconds 7 Dynamic scenarios with: One arm Two armsRobonaut

27. 27 MARS PI Meeting JSC 8/2004 Interaction Lab Two arm planar manipulator, planar workspace, 7 DOFs Robonaut model, 17 DOFs: 7 in each arm + 3 at the base (Pentium III 2.8 GHz) Experiments: Example Motions

28. 28 MARS PI Meeting JSC 8/2004 Interaction Lab Up to 8 times faster in the planar workspaces Roadmap/Grid size vs. maintenance cost tradeoff Modest speed gains in 3d workspaces Experiments ScenarioGridNodesLinksDRM Time(s)RRT Time(s)Comparison One arm24x322218746712.059.34.9 One arm64x702406806917.1146.88.6 One arm48x6466612243145.972.41.6 Two arm20 2 31461062918.2151.38.3 Two arm40 2 2898983817.1146.88.6 Robonaut24 3 35751231247.963.11.3 Robonaut24 3 51441770745.851.81.1

29. 29 MARS PI Meeting JSC 8/2004 Interaction Lab Integration with Robonaut Motions generated on our Robonaut model can be easily transferred to Robonaut Same mapping mechanism as the one used for the teleoperation with the motion suit

30. 30 MARS PI Meeting JSC 8/2004 Interaction Lab Sequencing Parameterized Motion Motion control for complex operations require mixed type of controllers Sequencing of primitive movement controllers is required Ex: coordination of bimanual manipulations, of multiple robots, or of biped locomotion

31. 31 MARS PI Meeting JSC 8/2004 Interaction Lab Example with Biped Locomotion We propose a search method for sequencing movement primitives The method is based on randomized roadmaps and discrete search methods The method was applied to obtain statically stable biped locomotion among obstacles

32. 32 MARS PI Meeting JSC 8/2004 Interaction Lab Movement Primitive: Definition Any controller with a simpler parameterization over the configuration space: P i s : S i s  C where: s is the start configuration of primitive P i s S i s is the parameter space of primitive P i s C is the configuration space Primitives generate postures, not motions

33. 33 MARS PI Meeting JSC 8/2004 Interaction Lab Total of 9 DOFs Movement control only allowed trough motion primitives parameterization, ensuring: correct support, balance, limits, collision-free Primitives: Description PRPR PLPL PBPB

34. 34 MARS PI Meeting JSC 8/2004 Interaction Lab Primitives: Description Movement Primitive Instantiation Condition Primitive MotionParametric Space Dim. PLPL support in left foot moves right leg articulations and body rotation 4 PBPB support in both feet moves body, feet fixed with IK 3 PRPR support in right foot moves left leg articulations and body rotation 4 (Robot has 9 DOFs)

35. 35 MARS PI Meeting JSC 8/2004 Interaction Lab Primitives: Description Movement Primitive Instantiation Condition Primitive MotionParametric Space Dim. PLPL support in left foot moves right leg articulations and body rotation 4 PBPB support in both feet moves body, feet fixed with IK 3 PRPR support in right foot moves left leg articulations and body rotation 4 (Robot has 9 DOFs)

36. 36 MARS PI Meeting JSC 8/2004 Interaction Lab Primitives: Description Movement Primitive Instantiation Condition Primitive MotionParametric Space Dim. PLPL support in left foot moves right leg articulations and body rotation 4 PBPB support in both feet moves body, feet fixed with IK 3 PRPR support in right foot moves left leg articulations and body rotation 4 (Robot has 9 DOFs)

37. 37 MARS PI Meeting JSC 8/2004 Interaction Lab Primitives: Description Movement Primitive Instantiation Condition Primitive MotionParametric Space Dim. PLPL support in left foot moves right leg articulations and body rotation 4 PBPB support in both feet moves body, feet fixed with IK 3 PRPR support in right foot moves left leg articulations and body rotation 4 (Robot has 9 DOFs)

38. 38 MARS PI Meeting JSC 8/2004 Interaction Lab Sequencing: Problem Definition Given: –Primitives P i –Task completion test function t(q) : C  {0,1} Determine a sequence of concatenated valid paths in configuration space, such that: –Each path is generated by a single primitive –The first point is the current position –The last point satisfies completion test

39. 39 MARS PI Meeting JSC 8/2004 Interaction Lab Sequencing: Search Tree Expand a roadmap in the parametric space of the motion primitive associated with c search tree

40. 40 MARS PI Meeting JSC 8/2004 Interaction Lab Sequencing: Search Tree Determine paths leading to configurations in a different support mode search tree

41. 41 MARS PI Meeting JSC 8/2004 Interaction Lab Sequencing: Search Tree Select lowest cost leaf c cost(c) = length(root,c) + dist(c,goal) search tree …

42. 42 MARS PI Meeting JSC 8/2004 Interaction Lab Sequencing: Search Tree Expand a roadmap in the parametric space of the new motion primitive Continue the process until close to goal point search tree … …

43. 43 MARS PI Meeting JSC 8/2004 Interaction Lab Obtained Examples

44. 44 MARS PI Meeting JSC 8/2004 Interaction Lab Integrated demo with Robosim Cooperative human-robot example application Demonstration of example motions using the USC motion suit Segmentation into meaningful example Derivation of a primitive controller, which is able to interpolate the examples in order to obtain a scooping motion in any position inside the tray New tools can be reached using our motion planner

45. 45 MARS PI Meeting JSC 8/2004 Interaction Lab Integrated demo with Robosim Example of scooping motions to different instructed locations

46. 46 MARS PI Meeting JSC 8/2004 Interaction Lab Integrated demo with Robosim Example of sending scooping motions to Robosim

47. 47 MARS PI Meeting JSC 8/2004 Interaction Lab Summary a b

48. 48 MARS PI Meeting JSC 8/2004 Interaction Lab Thanks Doug JSC Robonaut Team