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

QuickTime™ et undécompresseur MPEG-4 Videosont requis pour visionner cette image.

5

GromitATRV Junior

Sonars

Stereo Camera

Pan and Tilt unit

WheelsSpeed control (max 50cm/s)

Attitude sensor

2 Pentium III 1.3GhzLinux RH 7.23

Computer display

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

Overall Gromit OrganizationSupervisor/Execution control

Request

Cyclical

USRFLEX

P3D3D reactive

Motion Planner

Speed

CameraImagespos-tag

SCorrelCor. Impos-tag

POMPosition Manager

Pos

SMU Pos-ang

STEOStereo Odometry

PosLANELocal Env.

Env

Platine PlaPos

N

S

W E

AWAYSonars logical

bumpers

Speed

Pos

8

Supervisor/Execution control V2 (Propice)

9

Real Procedure....

10

Rflex and SMU

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

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

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

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

Robot Robot-red

Platine

Left cam Right cam

18

Internal Data Flow and Frequency

RFLEX Pos

P3D Speed

Camera Images

SCorrel Cor. Im

POM Pos

SMU Pos-ang

STEO PosLANE Env

Platine PlaPos

25 Hz

1,8 Hz

Upon Rq10 Hz

2,5 HzUpon Rq

Upon RqUpon Rq

25 Hz

25 Hz

1,8HzUpon Rq

Upon Rq

Upon Rq

2,5 Hz

Upon Rq

2,5 Hz

2,5 Hz

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

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

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

New Organization

PRS/Propice

IDEA

Relay

ComLib/CsLib/PosterLib

Init: connect to mpSendCom: send messageReply: from select in

thread

CommandsParameters mappingother 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))) )))

USRFLEX

P3D3D reactive

Motion Planner

Speed

CameraImagespos-tag

SCorrelCor. Impos-tag

POMPosition Manager

Pos

SMU Pos-ang

STEOStereo Odometry

PosLANELocal Env.

Env

Platine PlaPos

N

S

W E

AWAYSonars logical

bumpers

Speed

Pos

26

“Partial” Current Model

LANE

CAMERA shot shotidle idle

SCORREL scorrel scorrelidleidle

STEO read steo_posidle idle

read fuseidle idle

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-idleread met by pos

LANE: read meets scorrel, read contained-by scorrel-idleread met by fuse

27

SCORREL scorrel idle scorrelidle

STEO readsteo_posidle idle

LANE read fuseidle idle

CAMERA shot shot shotidle idle idle idleshot idle

Complete Model V 0.1

28

SCORREL scorrel idle scorrelidle

STEO readsteo_posidle idle

LANE read fuseidle idle

CAMERA shot shot shotidle idle idle idleshot idle

cntrlcntrl cntrlstopRFLEX

speed_cntl stop stop

Complete Model V1.0(equivalent to propice)

speed_refidleP3D

speed_ref speed_ref speed_ref speed_ref idle

add_way_pointidleidleP3Dplan_traj

add_way_point

idleAWAY away_monitor idle away_monitor

idle idlePLATINE moveidlemove

29

Complete Model V2.0

SCORREL scorrel idle scorrelidle

STEO readsteo_posidle idle

LANE read fuseidle idle

CAMERA shot shot shotidle idle idle idleshot idle

cntrlcntrl cntrlstopRFLEX

speed_cntl stop stop

POM robot_pos robot_pos robot_pos robot_pos robot_pos robot_posidle

speed_refidleP3D

speed_ref speed_ref speed_ref speed_ref idle

add_way_pointidleidleP3Dplan_traj

add_way_point

idleAWAY away_monitor idle away_monitor

idle idlePLATINE moveidlemove

RFLEXpos

idle POS

SMUpos

idle POS

PLATINEpos

idle POS

30

Ultimate Model...

RFLEX

Camera

SCorrel STEO LANE POM

P3D Goal

Exec

Goal

Exec

shotidle idle shot

scorrel idleidle

Goal

Exec

steo idlereadidle

Goal

Exec

fuse idlereadidle

Goal

Exec

pomidle

Goal

Exec

add_wpidle

speed_ref idle sped_refidle

idle

trackidle

Goal

idle ts ts ts ts ts ts

idle rflex_pos

steo_posidle steo_pos steo_posidle

idle ....robot_pos robot_pos

idle ....robot_pos robot_pos

idle ....stereo_pair stereo_pair

idle image_pair image_pair

stereo_imageidlester_im

stereo_imageidlester_im stereo_imageidlester_im

lane idle laneidlesteo_pos steo_pos

speed_refspeed_refidle

idle robot_pos robot_pos

lane idle lane

idle rflex_pos

Exec

SCORRELscorrel idle scorrelidle

STEOreadsteo_posidle idle

LANEread fuseidle idle

CAMERAshot shot shotidle idle idle idleshot idle

cntrlcntrl cntrlstopRFLEXspeed_cntl

stop stop

POM robot_pos robot_pos robot_pos robot_pos robot_pos robot_posidle

speed_refidleP3Dspeed_ref

speed_ref speed_ref speed_ref idle

add_way_pointidleidleP3Dplan_traj

add_way_point

idleAWAY

away_monitor idle away_monitor

idle idlePLATINEmoveidlemove

RFLEXpos

idle

POS

SMUpos

idle

POS

PLATINEpos

idlePOS

31

... or even

RFLEX

Camera

SCorrel STEO LANE POM

P3D Goal

Exec

Goal

Exec

shotidle idle shot

scorrel idleidle

Goal

Exec

steo idlereadidle

Goal

Exec

fuse idlereadidle

Goal

Exec

pomidle

Goal

Exec

add_wpidle

speed_ref idle sped_refidle

idle

trackidle

Goal

idle ts ts ts ts ts ts

idle rflex_pos

steo_posidle steo_pos steo_posidle

idle ....robot_pos robot_pos

idle ....robot_pos robot_pos

idle ....stereo_pair stereo_pair

idle image_pair image_pair

stereo_imageidlester_im

stereo_imageidlester_im stereo_imageidlester_im

lane idle laneidlesteo_pos steo_pos

speed_refspeed_refidle

idle robot_pos robot_pos

lane idle lane

idle rflex_pos

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

Long Horizon

SCORREL idle scorrel

STEO read steo_posidle idle

LANE read fuse idleidle

CAMERA idle shot idle

GoalLocalize

Localizeidle idleLocalize

GoalMap

Mapidle idleMap

read fuse

steo_pos

idle

met by

contained by

36

Long Horizon

• Problem– generation management

• Solution– common argument to the ith generation– this argument is strictly incremented at each

generation

37

Long Horizon

SCORREL idle scorrel

STEO read steo_posidle idle

LANE read fuse idleidle

CAMERA idle shot idle

GoalLocalize

Localizeidle idleLocalize

GoalMap

Mapidle idleMap

read fuse

steo_pos

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

idle

idle

scorrel

read steo_pos idle

read fuse idle

shot idle

Localize

Map

idle

idle

idle

Long Horizon

SCORREL scorrel

STEO read steo_pos idle

LANE read fuse idle

CAMERA shot idle

GoalLocalize

Localize

GoalMap

Map

idle

idle

idle

scorrel

read steo_pos idle

read fuse idle

idle

Localize

Map

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

idle

idle

scorrel

read steo_pos idle

read fuse idle

shot idle

Localize

Map

idle

idle

idle

Long Horizon(problem)

SCORREL scorrel

STEO read steo_pos idle

LANE read fuse idle

CAMERA shot idle

GoalLocalize

Localize

GoalMap

Map

idle

idle

idle

scorrel

read steo_pos idle

read fuse idle

idle

Localize

Map

idle

42

idle

idle

scorrel

read steo_pos idle

read fuse idle

shot idle

Localize

Map

idle

Long Horizon(repair)

SCORREL scorrel

STEO readsteo_pos

LANE read

CAMERA shot idle

GoalLocalize

GoalMap

idle

idle

idle

scorrel

read steo_pos idle

read fuse idle

idle

Localize

Map

idle scorrel

shot

idle

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