1. 1 Gromit has an IDEA Felix Ingrand (LAAS/CAAR) NASA/IDEA Team LAAS Eden Team

2. 2 Justification Functional level and functional modules interactions have become increasingly complex No temporal model of functional level No temporal model of internal functional modules

3. 3 Plan

4. 4 Old LAAS Gromit Demonstration

5. 5 Gromit ATRV Junior Sonars Stereo Camera Pan and Tilt unit Wheels Speed control (max 50cm/s) Attitude sensor 2 Pentium III 1.3Ghz Linux RH 7.23 Computer display

6. 6 What does Gromit do? Visit a number of waypoints –specify a speed ref. to reach them considering obstacles and traversability Iteratively –Take a pair of images –Stereo Correlation // Visual odometry // Environment mapping Use the US to detect interesting object –stop and take a picture of them

7. 7 Overall Gromit Organization Supervisor/ Execution control Request Cyclical US RFLEX P3D 3D reactive Motion Planner Speed Camera Images pos-tag SCorrel Cor. Im pos-tag POM Position Manager Pos SMU Pos-ang STEO Stereo Odometry Pos LANE Local Env. Env Platine PlaPos N S WE AWAY Sonars logical bumpers Speed Pos

8. 8 Supervisor/Execution control V2 (Propice)

9. 9 Real Procedure....

10. 10 Rflex and SMU RFLEX Requests –SetMode AUTO/JOYSTICK –BrakeOn/BrakeOff –SonarOn/SonarOff –TrackSpeed (SpeedRef-Poster) Posters –Position (odometry) –Sonars –Battery level SMU Compass, Pan & Tilt provide a poster with: –absolute heading, pitch and roll –temperature ;-)

11. 11 Platine and Camera Platine Request –CmdPosCoord (pan,tilt) Poster –PlatinePosition Camera Take pictures pair (rectified and calibrated) Resolution divided by 2 (480x640 =>240x320) Tagged with POM RobotPos, PlatinePos, SMU Request –OneShot –SaveImage (save on disk for checking) Poster –ImagesPair

12. 12 Scorrel Stereo Correlation No cyclic task Compute a stereo correlated image on request Tagged by POM (the same tag than the images pair)

13. 13 Steo: Stereo Odometry

14. 14 Steo Upon request, compute the displacement between the new stereo correlated image and the previous “valid” one Only update its position when confident enough...

15. 15 Lane Build a local “map” from Scorrel Upon request, produce a poster with the 3d points Forget the “past” after a while, but not moving obstacles

16. 16 P3D 3D Reactive Motion Planer produce a tree of arc trajectories evaluate the quality of the rover attitude on discrete positions A* search to find the best trajectory 1 Task (1,8 Hz, 0,6 s) –to compute and update the SpeedRef –the processing often takes more :-( Still, has to produce/update/refresh the SpeedRef

17. 17 POM Position Manager Manage the position of the robot Has a model of the robot frames and their relationship Check Rflex and Steo position poster Prefer steo over rflex ;-) Robot Robot-red Platine Left camRight cam

18. 18 Internal Data Flow and Frequency RFLEX Pos P3D Speed Camera Images SCorrel Cor. Im POM Pos SMU Pos-ang STEO Pos LANE Env Platine PlaPos 25 Hz 1,8 Hz Upon Rq 10 Hz 2,5 Hz Upon Rq 25 Hz 1,8Hz Upon Rq 2,5 Hz Upon Rq 2,5 Hz

19. 19 ran by this...

20. 20 Potential Problems... subtle contention for resources (e.g. camera images) speed ref has to be updated at a rate consistent with the speed and field of view pom position has to be properly updated (steo) make sure position estimation chain of processing is “temporally valid”... in a context where # of CPU and power of CPU can change CPU load can change the OS may not be hard- RT (linux in our case) etc

21. 21 Other approaches TDL –Nice task description language (procedural executive) –many C++ constructs to program tasks –handle time (but not in a constraint network) –no high level semaphores / monitors /resources –no temporal logic CLARAty –no temporal model of the functional level –no clear model of modules interaction

22. 22 Temporal model...... should resolve resource contention issue –only one camera shot, at a time –no simultaneous posters read and write access (without explicit semaphore)

23. 23 Temporal model...... should control Robot Speed –w.r.t. the time taken by scorrel/steo/lane (those have a more or less predictable duration, but still it can depend of the load on the CPU) –and their respective success... –according to the CPU power/load

24. 24 Temporal model...... should catch: –POM-Platine-Camera pb POM reads platine-pos poster at 4Hz Camera can take an image at any time... Platine could be moved at any time too if images are tagged with POM pos, then 0,25s can elapse between the platine motion and the tag.

25. 25 New Organization PRS/Propice IDEA Relay ComLib/CsLib/PosterLib Init: connect to mp SendCom: send message Reply: from select in thread Commands Parameters mapping other activities (Define_Member_Values ((Gromit_Class SCorrel_SV)) (SCorrel_idle SCorrel_scorrel SCorrel_steo_wait ) (Define_Compatibility (SINGLE ((Gromit_Class SCorrel_SV)) ((SCorrel_scorrel (OK True ?steo_state)))) :compatibility_spec (?steo_state OR (Simultaneous AND (ends_before [0 0]........ AND (meets (SINGLE ((Gromit_Class SCorrel_SV)) (SCorrel_idle))) ))) US RFLEX P3D 3D reactive Motion Planner Speed Camera Images pos-tag SCorrel Cor. Im pos-tag POM Position Manager Pos SMU Pos-ang STEO Stereo Odometry Pos LANE Local Env. Env Platine PlaPos N S WE AWAY Sonars logical bumpers Speed Pos

26. 26 “Partial” Current Model LANE CAMERA shot idle SCORREL scorrel idle STEO read steo_pos idle read fuseidle CAMERA: shot, idle, shot, idle idle contained-by meets idle read steo_pos read fuse shot scorrel SCORREL: scorrel meets shot, scorrel contained-by camera_idle STEO: read meets scorrel, read contained-by camera_idle and scorrel-idle read met by pos LANE: read meets scorrel, read contained-by scorrel-idle read met by fuse

27. 27 SCORREL scorrelidlescorrel idle STEO read steo_posidle LANE read fuseidle CAMERA shot idle shot idle Complete Model V 0.1

28. 28 SCORREL scorrelidlescorrel idle STEO read steo_posidle LANE read fuseidle CAMERA shot idle shot idle cntrl stop RFLEX speed_cntl stop Complete Model V1.0 (equivalent to propice) speed_ref idle P3D speed_ref idle add_way_point idle P3D plan_traj add_way_point idle AWAY away_monitor idle away_monitor idle PLATINE moveidlemove

29. 29 Complete Model V2.0 SCORREL scorrelidlescorrel idle STEO read steo_posidle LANE read fuseidle CAMERA shot idle shot idle cntrl stop RFLEX speed_cntl stop POM robot_pos idle speed_ref idle P3D speed_ref idle add_way_point idle P3D plan_traj add_way_point idle AWAY away_monitor idle away_monitor idle PLATINE moveidlemove RFLEX pos idle POS SMU pos idle POS PLATINE pos idle POS

30. 30 Ultimate Model... RFLEX Camera SCorrelSTEOLANEPOM P3D Goal Exec Goal Exec shot idle shot scorrelidle Goal Exec steoidlereadidle Goal Exec fuseidlereadidle Goal Exec pom idle Goal Exec add_wpidle speed_refidlesped_refidle track idle Goal idle ts idle rflex_pos steo_posidlesteo_pos idle.... robot_pos idle.... robot_pos idle.... stereo_pair idle image_pair stereo_imageidlester_imstereo_imageidlester_imstereo_imageidlester_imlaneidlelaneidlesteo_pos speed_ref idle robot_pos laneidlelane idle rflex_pos Exec SCORREL scorrelidlescorrel idle STEO read steo_posidle LANE read fuseidle CAMERA shot idle shot idle cntrl stop RFLEX speed_cntl stop POM robot_pos idle speed_ref idle P3D speed_ref idle add_way_point idle P3D plan_traj add_way_point idle AWAY away_monitor idle away_monitor idle PLATINE moveidlemove RFLEX pos idle POS SMU pos idle POS PLATINE pos idle POS

31. 31... or even RFLEX Camera SCorrelSTEOLANEPOM P3D Goal Exec Goal Exec shot idle shot scorrelidle Goal Exec steoidlereadidle Goal Exec fuseidlereadidle Goal Exec pom idle Goal Exec add_wpidle speed_refidlesped_refidle track idle Goal idle ts idle rflex_pos steo_posidlesteo_pos idle.... robot_pos idle.... robot_pos idle.... stereo_pair idle image_pair stereo_imageidlester_imstereo_imageidlester_imstereo_imageidlester_imlaneidlelaneidlesteo_pos speed_ref idle robot_pos laneidlelane idle rflex_pos Exec

32. 32 Current Integration :-) Forget the past does the job right Timescale 0.5 on the real robot with all the functional modules Integration thru Propice make it easy to access all the functional modules

33. 33 Integration :-( One “tick” horizon model is a big pain in the  –One need to fold back in the next tick all the possible choices the planner could explore Writing a “one token” horizon model should “help”

34. 34 Longer Horizon Model Put a “goal” token further down the road (not on the next tick) Have this token being met somehow by other tokens which will establish it, and so on

35. 35 Long Horizon SCORREL idlescorrel STEO read steo_posidle LANE read fuseidle CAMERA idle shot idle Goal Localize Localize idle Localize Goal Map Map idle Map read fuse steo_pos idle met by contained by

36. 36 Long Horizon Problem –generation management Solution –common argument to the ith generation –this argument is strictly incremented at each generation

37. 37 Long Horizon SCORREL idlescorrel STEO read steo_posidle LANE read fuseidle CAMERA idle shot idle Goal Localize Localize idle Localize Goal Map Map idle Map read fuse steo_pos idle

38. 38 Long Horizon –squeeze the token as much as we can (leave no slack) –sub-plans interleaved –but still leave some temporal flexibility

39. 39 idle scorrel read steo_posidle read fuseidle shot idle Localize Map idle Long Horizon SCORREL scorrel STEO read steo_posidle LANE read fuseidle CAMERA shot idle Goal Localize Localize Goal Map Map idle scorrel read steo_posidle read fuseidle Localize Map idle

40. 40 Long Horizon Problem –how do we handle non nominal return values and branching? scorrel fails, reinsert a camera_shot and a scorrel to meet the lane_read, steo_read Solution –repair the plan to reestablish the met-by...

41. 41 idle scorrel read steo_posidle read fuseidle shot idle Localize Map idle Long Horizon (problem) SCORREL scorrel STEO read steo_posidle LANE read fuseidle CAMERA shot idle Goal Localize Localize Goal Map Map idle scorrel read steo_posidle read fuseidle Localize Map idle

42. 42 idle scorrel read steo_posidle read fuseidle shot idle Localize Map idle Long Horizon (repair) SCORREL scorrel STEO read steo_pos LANE read CAMERA shot idle Goal Localize Goal Map idle scorrel read steo_posidle read fuseidle Localize Map idlescorrel shot idle

43. 43 Long Horizon Expected advantages: –cleaner/simper model –better compositionality –expect the model/search to find common sub-goals (shot and scorrel)... without expliciting them

44. 44 IDEA (model based) vs Procedural Model based approach to execution control gives us a more robust solution with more flexibility Better for non nominal case (put the system back in a safe state) Resource management Easier integration with the Mission Planning IDEA architecture for different parts –mission planning, diagnosis, execution control) facilitate the integration process.-

45. 45 Remain to be done each module becomes an IDEA agent... –encapsulate them in the IDEA VM –require to also model the internal state of the module and their request interactions (a very interesting pb in fact). Better dynamic horizon handling Controllability issues