1 / 44

Gromit has an IDEA

Gromit has an IDEA. Felix Ingrand (LAAS/CAAR) NASA/IDEA Team LAAS Eden Team. Justification. Functional level and functional modules interactions have become increasingly complex No temporal model of functional level No temporal model of internal functional modules. Plan.

thuyet
Download Presentation

Gromit has an IDEA

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

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

  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. Plan

  4. Old LAAS Gromit Demonstration

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

  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. Platine PlaPos AWAYSonars logicalbumpers Speed Camera Imagespos-tag SMU Pos-ang POM Position Manager Pos SCorrel Cor. Impos-tag US RFLEX P3D3D reactive Motion Planner LANELocal Env. STEOStereo Odometry Pos Speed Env Pos N W E S Supervisor/Execution control Overall Gromit Organization Request Cyclical

  8. Supervisor/Execution control V2 (Propice)

  9. Real Procedure....

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

  11. 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 Platine and Camera

  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. Steo:Stereo Odometry

  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. LaneBuild 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. P3D3D 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. Left cam Right cam Platine Robot Robot-red POMPosition 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 ;-)

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

  19. ran by this...

  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. 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 Other approaches

  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. 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. 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. Platine PlaPos AWAYSonars logicalbumpers Speed Camera Imagespos-tag SMU Pos-ang POM Position Manager Pos SCorrel Cor. Impos-tag US RFLEX P3D3D reactive Motion Planner LANELocal Env. STEOStereo Odometry Pos Speed Env Pos N W E S New Organization Relay PRS/Propice Commands Parameters mappingother activities Init: connect to mp SendCom: send message Reply: from select in thread ComLib/CsLib/PosterLib IDEA (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))) )))

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

  27. CAMERA idle shot idle shot idle shot idle shot idle SCORREL idle scorrel idle scorrel STEO idle read steo_pos idle LANE idle read fuse idle Complete Model V 0.1

  28. RFLEXspeed_cntl stop cntrl stop cntrl stop cntrl P3D speed_ref idle speed_ref speed_ref speed_ref idle speed_ref P3D plan_traj idle add_way_point idle add_way_point CAMERA idle shot idle shot idle shot idle shot idle SCORREL idle scorrel idle scorrel STEO idle read steo_pos idle LANE idle read fuse idle AWAY idle away_monitor idle away_monitor PLATINE idle move idle move idle Complete Model V1.0(equivalent to propice)

  29. RFLEXspeed_cntl stop cntrl stop cntrl stop cntrl P3D speed_ref idle speed_ref speed_ref speed_ref idle speed_ref P3D plan_traj idle add_way_point idle add_way_point RFLEXpos POS idle SMUpos POS idle CAMERA idle shot idle shot idle shot idle shot idle SCORREL idle scorrel idle scorrel STEO idle read steo_pos idle LANE idle read fuse idle POM idle robot_pos robot_pos robot_pos robot_pos robot_pos robot_pos AWAY idle away_monitor idle away_monitor PLATINE idle move idle move idle Complete Model V2.0 PLATINE pos POS idle

  30. RFLEXspeed_cntl stop cntrl stop cntrl stop cntrl idle speed_ref idle sped_ref idle robot_pos robot_pos P3D speed_ref idle speed_ref speed_ref speed_ref idle speed_ref POM LANE Camera RFLEX SCorrel STEO idle speed_ref speed_ref P3D plan_traj idle add_way_point idle add_way_point idle RFLEXpos POS idle track idle SMUpos POS idle ts ts ts ts ts ts idle shot idle shot idle shot idle shot idle CAMERA idle scorrel idle scorrel SCORREL .... idle shot idle shot idle stereo_pair idle stereo_pair read steo_pos idle STEO idle read fuse idle LANE PLATINE pos idle POS idle robot_pos robot_pos robot_pos robot_pos robot_pos robot_pos POM idle read steo idle idle read fuse idle idle away_monitor idle away_monitor idle idle AWAY steo_pos idle steo_pos lane lane idle idle lane lane ster_im ster_im ster_im idle idle idle stereo_image stereo_image stereo_image idle steo_pos steo_pos idle steo_pos rflex_pos rflex_pos .... .... idle move idle move idle PLATINE idle image_pair image_pair idle idle robot_pos robot_pos robot_pos robot_pos P3D Ultimate Model... Goal idle add_wp idle Exec Goal Exec Goal Exec Goal Goal Goal Goal idle pom idle scorrel idle Exec Exec Exec Exec

  31. idle speed_ref idle sped_ref idle robot_pos robot_pos POM STEO LANE Camera RFLEX SCorrel idle speed_ref speed_ref idle track idle ts ts ts ts ts ts .... idle shot idle shot idle stereo_pair stereo_pair idle read steo idle idle read fuse idle idle idle steo_pos idle steo_pos lane lane idle idle lane lane ster_im ster_im ster_im idle idle idle stereo_image stereo_image stereo_image idle steo_pos steo_pos idle steo_pos rflex_pos rflex_pos .... .... idle image_pair image_pair idle idle robot_pos robot_pos robot_pos robot_pos P3D ... or even Goal idle add_wp idle Exec Goal Exec Goal Exec Goal Goal Goal Goal idle pom idle scorrel idle Exec Exec Exec Exec

  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. 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. 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. met by Long Horizon contained by GoalMap idle Map idle Map read fuse idle read fuse idle LANE GoalLocalize idle Localize idle Localize STEO steo_pos idle read steo_pos idle SCORREL idle scorrel idle CAMERA idle shot idle

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

  37. Long Horizon GoalMap idle Map idle Map read fuse idle read fuse idle LANE GoalLocalize idle Localize idle Localize STEO steo_pos idle read steo_pos idle SCORREL idle scorrel idle CAMERA idle shot idle

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

  39. Long Horizon GoalMap Map idle Map idle Map idle read fuse idle read fuse idle read fuse idle LANE GoalLocalize Localize idle Localize idle Localize idle STEO read steo_pos idle read steo_pos idle read steo_pos idle SCORREL scorrel idle scorrel idle scorrel idle CAMERA idle shot idle shot idle

  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. Long Horizon(problem) GoalMap Map idle Map idle Map idle read fuse idle read fuse idle read fuse idle LANE GoalLocalize Localize idle Localize idle Localize idle STEO read steo_pos idle read steo_pos idle read steo_pos idle SCORREL scorrel idle scorrel idle scorrel idle CAMERA idle shot idle shot idle

  42. Long Horizon(repair) GoalMap Map idle Map idle read fuse idle read fuse idle read LANE GoalLocalize Localize idle Localize idle STEO read steo_pos idle read steo_pos idle read steo_pos SCORREL scorrel idle scorrel idle scorrel idle scorrel idle CAMERA idle shot idle shot idle shot idle

  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. 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.-

More Related