:- object capability_lib.

var perceptionDelay = 500.
var rightHand = 1.0.
var leftHand = -1.0.
var maxStepLength = 1.0.
 
%%% watch tv %%%
capability(demo1, [], Actions, []) :-
  Actions = [
     switch(tv),
     sit(on(couch2)),
     position(tv, X,_,Z),
     get_direction(X,Z, R),
     rotation(R)
   ],
   !.
   
 capability(demo2, [], Actions, []) :-
  Actions = [
     grab(book),
     sit(on(deskchair)),
     release(book)
   ],
   !.
   
 capability(demo3, [], Actions, []) :-
   Actions = [
     lay(on(bed))
   ],
   !.

   
 %%% Pak het object bij een grabObject, bv handvat %%%
 
 capability(grab(Object), Conditions, Actions, Effects) :-
   Conditions = [
     property(belief, [grab,Object,GrabObject], fail),
     property(belief, not([grabbed,_Object,_,_]), tell(['I already carry an object']) ),
     property(Object, [position,XO,YO,ZO], try(find(Object)) ),
     property(Object, [rotation,RO]),
     child_property(Object, GrabObject, position(Xgrab,Ygrab,Zgrab))
   ],
   Actions = [
     get_in_room(Object),
     get_in_reach(GrabObject, XO,YO,ZO, Xgrab,Ygrab,Zgrab, RO, 1, leftHand),
     grab_object_position(Xgrab,Ygrab,Zgrab, NewXO,NewYO,NewZO),
     arm_rotation(YO, Ygrab, Rarm, RarmUp, 0.2),
     get_node(Object, choice, ObjectRef),
     action([grab_left,ObjectRef,Object,NewXO,NewYO,NewZO,Rarm,RarmUp])
   ],
   Effects = [ add(belief, [grabbed,Object,Xgrab,Zgrab]) ].
 
 %%% Pak het object als de afmetingen niet te groot zijn %%%
     
 capability(grab(Object), Conditions, Actions, Effects) :-
   Conditions = [
     property(belief, not([grabbed,_,_,_]), tell(['I already carry an object']) ),
     property(Object, [position,XO,YO,ZO], try(find(Object)) ),
     property(Object, [rotation,RO]),
     property(Object, [size,Xsize,Ysize,Zsize]),
     bool(Xsize + Ysize + Zsize < 1.0, tell(['I don\'t know how to grab ', Object]))
   ],
   Actions = [
     get_in_room(Object),
     get_grab_side(Xsize,Ysize,Zsize, Xgrab,Ygrab,Zgrab),
     get_in_reach(Object, XO,YO,ZO, Xgrab,Ygrab,Zgrab, RO, 1, leftHand),
     arm_rotation(YO, Ygrab, Rarm, RarmUp, 0.2),
     get_node(Object, choice, ObjectRef),
     action([grab_left,ObjectRef,Object,Rarm,RarmUp])
   ],
   Effects = [ add(belief, [grabbed,Object,Xgrab,Zgrab]) ],
   !.
 
 capability(switch(Object), Conditions, Actions, Effects) :-
   Conditions = [
     property(Object, [position,XO,YO,ZO], try(find(Object)) ),
     property(Object, [rotation,RO]),
     property(belief, [switch,Object,SwitchObject], tell(['I don\'t know how to switch the ', Object]) ),
     child_property(Object, SwitchObject, status(Status), tell(['I can\'t change ', Object]) ),
     child_property(Object, SwitchObject, position(Xswitch,Yswitch,Zswitch))
   ],
   Actions = [
     get_in_room(Object),
     get_in_reach(SwitchObject, XO,YO,ZO, Xswitch,Yswitch,Zswitch, RO, 1, rightHand),
     arm_rotation(YO, Yswitch, Rarm, _, 0.2),
     atom_list_concat([Object, '_', SwitchObject], Switch),
     action([switch, Switch, Rarm]),
     switch_status(Status, NewStatus)
   ],
   Effects = [
     change(Object, [status,Status], [status,NewStatus])
   ],
   !.
 
 capability(switch(Object,Status), Conditions, Actions, []) :-
   Conditions = [
     property(Object, [position,_X,_Y,_Z], try(find(Object)) ),
     property(belief, [switch,Object,SwitchObject], tell(['I don\'t know how to switch the ', Object]) ),
     child_property(Object, SwitchObject, status(Status), tell([Object, ' is switched already']) )
   ],
   Actions = [
     switch(Object)
   ],
   !.
 
 capability(start(Object), Conditions, Actions, []) :-
   Conditions = [
     property(Object, [position,_,_,_], try(find(Object)) ),
     property(belief, [start, Object, StartObject], tell(['I don\'t know how to start the ', Object]) )
   ],
   Actions = [
     push(Object, StartObject)
   ],
   !.
 
 capability(start(with(StartObject),Object), Conditions, Actions, Effects) :-
   Conditions = [
     property(Object, [position,_,_,_], try(find(Object)) )
   ],
   Actions = [
     push(Object, StartObject)
   ],
   Effects = [
     change(belief, [start, Object, _StartObject], [start, Object, StartObject])
   ],
   !.
 
 capability(push(Object, PushObject), Conditions, Actions, []) :-
   Conditions = [
     property(Object, [position,XO,YO,ZO], try(find(Object)) ),
     property(Object, [rotation,RO]),
     child_property(Object, PushObject, position(Xpush,Ypush,Zpush))
   ],
   Actions = [
     get_in_room(Object),
     get_in_reach(PushObject, XO,YO,ZO, Xpush,Ypush,Zpush, RO, 1, rightHand),
     arm_rotation(YO, Ypush, Rarm, _, 0.2),
     atom_list_concat([Object, '_', PushObject], Push),
     action([push_button, Push, Rarm])
   ],
   !.
 
 capability(release(Object), Conditions, Actions, Effects) :-
   Conditions = [
     property(belief, [grabbed,Object,Xgrab,Zgrab], tell(['I don\'t carry ',Object]) ),
     property(belief, [position,X,Y,Z]),
     property(belief, [rotation,R])
   ],
   Actions = [
     get_node(Object, choice, ObjectRef),
     release_object(X,Y,Z, R, leftHand, XO,_,ZO, Xgrab,Zgrab, RO),
     action([release, ObjectRef, Object, XO,1.0,ZO,-1.0,0.0,RO])
   ],
   Effects = [ 
     delete(belief, [grabbed,Object,_,_]),
     change(Object, [position,_,_,_], [position,XO,1.0,ZO])
   ],
   !.
 
 capability(sit, Conditions, Actions, []) :-
   Conditions = [
     property(belief, [sit,Object])
   ],
   Actions = [
     sit(on(Object))
   ],
   !.
 
 %%% Agent ligt op het object %%%
 
 capability(sit(on(Object)), Conditions, Actions, []) :-
   Conditions = [
     property(belief, [laying,Object], fail)
   ],
   Actions = [
     get(up(Object))
   ].
 
 %%% Agent sit al op het object %%%
 
 capability(sit(on(Object)), Conditions, Actions, []) :-
   Conditions = [
     property(belief, [sitting,Object], fail)
   ],
   Actions = [
     tell(['I already sit on ',Object])
   ].
 
 capability(sit(on(Object)), Conditions, Actions, []) :-
   Conditions = [
     property(belief, or([sitting,Object1],[laying,Object1]), fail),
     bool(Object \== Object1, [])
   ],
   Actions = [
     get(of(Object1)),
     sit(on(Object))
   ].
     
 capability(sit(on(Object)), Conditions, Actions, Effects) :-
   Conditions = [
     property(belief, [standing]),
     property(Object, [position,XO,YO,ZO], try(find(Object)) ),
     property(Object, [rotation,RO]),
     property(belief, [sit,Object], tell(['I don\'t know if I can sit on ', Object]) ),
     property(Object, [size,_Xsize,_Ysize,Zsize])
   ],
   Actions = [
     get_in_room(Object),
     Z is Zsize/2 + 0.3,
     get_in_sit_reach(Object, XO,ZO, RO, 0.0,Z),
     sit_position(0.0,YO,Z, RO, Xsit,Ysit,Zsit, -1),
     position(belief, XA,YA,ZA),
     action([sit, Xsit,Ysit,Zsit])
   ],
   Effects = [
     change(belief, [standing], [sitting,Object]),
     increment(belief, [position,XA,YA,ZA], [position,Xsit,Ysit,Zsit])
   ],
   !.
   
 capability(lay(on(Object)), Conditions, Actions, Effects) :-
   Conditions = [
     property(belief, [sitting,Object], fail)
   ],
   Actions = [
     rotation(Object, RO),
     R is RO + 1.57,
     sit_rotation(R),
     action([lay,Object])
   ],
   Effects = [
     change(belief, [sitting,Object],[laying,Object])
   ].
 
 %%% Agent staat of zit of ligt op een ander object %%%
   
 capability(lay(on(Object)), Conditions, Actions, []) :-
   Conditions = [
     property(belief, or([standing], or([sitting,Object1], [laying,Object1])), fail),
     property(Object, [position,_X,_Y,_Z], try(find(Object)) ),
     property(belief, [sit,Object], tell(['I don\'t know if I can sit on ', Object]) )
   ],
   Actions = [
     Object1 \== Object,
     sit(on(Object)),
     lay(on(Object))
   ],
   !.
 
 capability(get_of, Conditions, Actions, []) :-
   Conditions = [
     property(belief, or([sitting,Object],[laying,Object]))
   ],
   Actions = [
     get(of(Object))
   ],
   !.
 
 capability(get(of(Object)), Conditions, Actions, Effects) :-
   Conditions = [
     property(belief, [sitting,Object], fail),
     property(belief, [position,XA,YA,ZA]),
     property(Object, [position,_XO,YO,_ZO], try(find(Object))),
     property(Object, [rotation,RO]),
     property(Object, [size,_Xsize,_Ysize,Zsize])
   ],
   Actions = [
     Z is Zsize/2 + 0.3,
     sit_position(0.0,YO,Z, RO, Xstand,Ystand,Zstand, 1),
     sit_rotation(RO),
     action([get_of, Xstand,Ystand,Zstand])
   ],
   Effects = [
     change(belief, [sitting,Object], [standing]),
     increment(belief, [position,XA,YA,ZA], [position,Xstand,Ystand,Zstand])
   ].
 
 capability(get(of(Object)), Conditions, Actions, []) :-
   Conditions = [
     property(belief, [laying,Object], tell(['I\'m not sitting on ', Object]))
   ],
   Actions = [
     get(up(Object)),
     get(of(Object))
   ],
   !.
 
 capability(get(up(Object)), Conditions, Actions, Effects) :-
   Conditions = [
     property(belief, [laying,Object], tell(['I\'m not laying on ',Object]) ),
     property(Object, [rotation,R], [])
   ],
   Actions = [
     action([get_up, Object]),
     sit_rotation(R)
   ],
   Effects = [
     change(belief, [laying,Object], [sitting,Object])
   ],
   !.
 
 capability(open(Object), Conditions, Actions, Effects) :-
   Conditions = [
     property(Object, [position,XO,YO,ZO], try(find(Object)) ),
     property(Object, [rotation,RO]),
     property(belief, [open,Object,OpenObject], tell(['I don\'t know how to open the ', Object]) ),
     child_property(Object, OpenObject, position(Xopen,Yopen,Zopen)),
     child_property(Object, OpenObject, status(Status), tell(['I can\'t change ',Object]) ),
 
     bool(Status == false, tell([Object, ' is open already']) )
   ],
   Actions = [
     get_in_door_room(Object),
     get_side(Object, Side),
     SideZopen is Zopen + 0.15 * Side,
     RSide is Side * -1,
     get_in_reach(OpenObject, XO,YO,ZO, Xopen,Yopen,SideZopen, RO, Side, rightHand),
     move_back(RO, RSide, -0.4, Xstep,Zstep, Rfront,Rback, NewX,NewZ, _R),
     arm_rotation(YO, 1.0, Rarm,RarmDown, -0.2),
     atom_list_concat([Object, '_', OpenObject], Open),
     action([open, Open, r_shoulder, Xstep,Zstep, Rfront,Rback, Rarm, RarmDown]),
     position(belief, XA,YA,ZA)
   ],
   Effects = [
     increment(belief, [position,XA,YA,ZA], [position,NewX,YA,NewZ])
   ],
   !.
 
 capability(close1(Object), Conditions, Actions, Effects) :-
   Conditions = [
     property(Object, [position,XO,YO,ZO], try(find(Object)) ),
     property(Object, [rotation,RO]),
     property(belief, [open,Object,CloseObject], tell(['I don\'t know how to close the ', Object]) ),
     child_property(Object, CloseObject, status(true), tell(['door is open already']) ),
     child_property(Object, CloseObject, position(Xclose,Yclose,Zclose)),
     child_property(Object, object, rotation(Rdoor))
   ],
   Actions = [
     get_in_door_room(Object),
     get_side(Object, Side),
     SideZclose is Zclose + 0.15 * Side,
     R is RO - Rdoor,
     get_in_reach(CloseObject, XO,YO,ZO, Xclose,Yclose,SideZclose, RO, Side, rightHand),
     move_back(R, Side, 1.0, Xstep,Zstep, Rfront,Rback, NewX,NewZ, NewR),
     arm_rotation(YO, Yclose, Rarm,RarmDown, -0.2),
     atom_list_concat([Object, '_', CloseObject], Close),
     action([close, Close, r_shoulder, Xstep,Zstep, Rfront, Rback, NewR, Rarm,RarmDown]),
     position(belief, XA,YA,ZA)
   ],
   Effects = [
     increment(belief, [position,XA,YA,ZA], [position,NewX,0.0,NewZ])
   ],
   !.
 
 capability(walk(to(Room)), [], Actions, []) :-
  Actions = [
    goto(Room)
  ],
  !.

 capability(walk(through(Object)), Conditions, Actions, []) :-
   Conditions = [
     property(Object, [position,XO,YO,ZO], try(find(Object)) ),
     property(Object, [rotation,RO]),
     property(belief, [open,Object, OpenObject], tell(['I don\'t know how to open the ', Object]) ),
     child_property(Object, OpenObject, status(Status)),
     child_property(Object, OpenObject, position(Xopen,Yopen,Zopen))
   ],
   Actions = [
     get_in_door_room(Object),
     get_side(Object, Side),
     if_then_else(
       and(bool(Status = true,[]),bool(Side = 1, [])),
       [
         SideZopen is Zopen + 0.1 * Side,
         get_in_reach(OpenObject, XO,YO,ZO, Xopen,Yopen,SideZopen, RO, Side, rightHand)
       ],
       [ open(Object) ]
     ),
     sleep(1000),
     through(Object),
     close1(Object)
   ],
   !.
       
 capability(through(Object), Conditions, Actions, Effects) :-
   Conditions = [
     property(belief, [inside,CurrentRoom], tell(['I don\'t know in wich room I am']) ),
     property(fact, or([door,Object, CurrentRoom, Room], [door,Object, Room, CurrentRoom])),
     property(belief, [position,XA,_Y,ZA]),
     property(Object, [rotation,RO])
   ],
   Actions = [
     get_side(Object, Side),
     if_then_else(
       bool(Side = 1, []),
       [
         R is RO - 1.5,
         X is XA + sin(R) * -1.8,
         Z is ZA + cos(R) * -1.8,
         walking(X,Z)
       ],
       []
     ),
     if_then_else(
       property(belief, grabbed(Object,_,_)),
       [ ChangeObject = [change(belief, [inside,Object, CurrentRoom], [inside,Object, Room])] ],
       [ ChangeObject = [] ]
     )
   ],
   Effects = [
     change(belief, [inside,CurrentRoom], [inside,Room])|
     ChangeObject
   ],
   !.
 
 capability(get_in_sit_reach(_Object, XO,ZO, RO, Xsit,Zsit), [], Actions, []) :-
   Actions = [
     touch_position(XO,ZO, Xsit,Zsit, RO, Xdest,Zdest),
     walk(Xdest,Zdest),
     rotation(RO)
   ].
 
 capability(get_in_reach(Object, Side, Hand), Conditions, Actions, []) :-
   Conditions = [
     property(Object, [position,X,Y,Z], try(find(Object))),
     property(Object, [rotation,R])
   ],
   Actions = [
     get_in_reach(Object, X,Y,Z, 0.0,0.0,0.0, R, Side, Hand)
   ],
   !.
 
 %%% Beperking in de reach rotatie van het object (grabR) %%%
 
 capability(get_in_reach(Object, X,Y,Z, XT,YT,ZT, RO, Side, Hand), Conditions, Actions, []) :-
   Conditions = [
     property(belief, [grabR,Object,Rmin,Rmax], fail)
   ],
   Actions = [
     get_in_reach(X,Y,Z, XT,YT,ZT, RO,Rmin,Rmax, Side, Hand)
   ].
 
 %%% Geen beperking gevonden, gebruik maximale beperking %%%
 
 capability(get_in_reach(_Object, X,Y,Z, XT,YT,ZT, RO, Side, Hand), [], Actions, []) :-
   Actions = [
     Rmin is 0.0,
     Rmax is 0.0,
     get_in_reach(X,Y,Z, XT,YT,ZT, RO,Rmin,Rmax, Side, Hand)
   ],
   !.
   
 capability(get_in_reach(XO,_YO,ZO, XT,_,ZT, RO,Rmin,Rmax, Side, Hand), [], Actions, []) :-
   Actions = [
     touch_position(XO,ZO, XT,ZT, RO, Xtouch,Ztouch),
     object_rotation(RO, Rmin,Rmax, Side, NewRmin,NewRmax),
     touch_destination(Xtouch,Ztouch, NewRmin, NewRmax, Hand, Xdest,Zdest, Rdest),
     change_rotation(Rdest, RA, -1),
     walk(Xdest,Zdest),
     rotation(RA)
   ],
   !.
 
 capability(get_side(Object, 1), Conditions, [], []) :-
   Conditions = [
     property(belief, [inside,CurrentRoom]), 
     property(fact, [door,Object, CurrentRoom, _Room], fail)
   ].
   
 capability(get_side(Object, -1), Conditions, [], []) :-
   Conditions = [
     property(belief, [inside,CurrentRoom]), 
     property(fact, [door,Object, _Room, CurrentRoom])
   ],
   !.
 
 capability(get_in_room(Object), Conditions, Actions, []) :-
   Conditions = [
     property(belief, [inside,Object,Room])
   ],
   Actions = [
     goto(Room)
   ],
   !.
 
 capability(get_in_door_room(Door), Conditions, Actions, []) :-
   Conditions = [
     property(belief, [inside,Room])
   ],
   Actions = [
     goto(Door,Room)
   ],
   !.
 
 capability(goto(Door,Room), Conditions, [], []) :-
   Conditions = [
     property(fact, or([door,Door,Room,_Room2],[door,Door,_Room1,Room]), fail)
   ].
 
 capability(goto(Door,Room), Conditions, Actions, []) :-
   Conditions = [
     property(fact, or([door,Door1,Room,Room1],[door,Door1,Room1,Room]), fail)
   ],
   Actions = [
     walk(through(Door1)),
     goto(Door,Room1)
   ],
   !.
 
 capability(goto(Room), Conditions, [], []) :-
   Conditions = [
     property(belief, [inside,CurrentRoom]),
     bool(CurrentRoom == Room, fail)
   ].
   
 capability(goto(Room), Conditions, Actions, []) :-
   Conditions = [
     property(belief, [inside,CurrentRoom]),
     property(fact, or([door,Door,CurrentRoom,Room], [door,Door,Room,CurrentRoom]), fail)
   ],
   Actions = [
     walk(through(Door))
   ].
   
 capability(goto(Room), Conditions, Actions, []) :-
   Conditions = [
     property(belief, [inside,CurrentRoom]),
     property(fact, or([door,Door,BetweenRoom,Room], [door,Door,Room,BetweenRoom])),
     bool(CurrentRoom \== BetweenRoom, fail)
   ],
   Actions = [
     goto(BetweenRoom),
     goto(Room)
   ],
   !.
 
 capability(look_around(Property, Tell), [], Actions, []) :-
   Actions = [
     look(-0.6),
     sleep(perceptionDelay),
     if_then_else(Property, [],
       [
         look(0.6), 
         sleep(perceptionDelay),
         if_then_else(Property, [],
           [
             add_rotation(3.14),
             sleep(perceptionDelay),
             if_then_else(Property, [], [Tell])
           ]
         )
       ]
     )
   ],
   !.
 
 capability(find(Object), Conditions, Actions, []) :-
   Conditions = [
     property(belief, [inside,CurrentRoom]),
     property(belief, [inside,Object, Room], fail)
   ],
   Actions = [
     CurrentRoom \== Room,
     get_in_room(Object),
     find(Object)
   ].
   
 capability(find(Object), [], Actions, []) :-
   Actions = [
     look_around(property(Object, [position,_,_,_]), tell(['I don\'t know where ', Object, ' is']))
   ],
   !.
  
 capability(avoid_obstacles(X,Z, Xdest,Zdest), Conditions, Actions, []) :-
   Conditions = [
     property(module, [avoidance,true], fail),
     property(object, ObjectList)
   ],
   Actions = [
     get_object_corners(ObjectList, Corners),
     get_crosspoints(X,Z, Xdest,Zdest, Corners, Crosspoints),
     nearest_crosspoint(X,Z, Crosspoints, Crosspoint),
     nearest_corner(Xdest,Zdest, Crosspoint, Destination),
     if_then_else(
       bool(Destination = [Xcorner,Zcorner], []),
       [
         walking(Xcorner,Zcorner),
         walk(Xdest, Zdest)
       ],
       [
         walking(Xdest,Zdest)
       ]
     )
   ].
 
 capability(avoid_obstacles(_X,_Z, Xdest,Zdest), Conditions, Actions, []) :-
   Conditions = [
     property(module, [avoidance,false])
   ],
   Actions = [
     walking(Xdest,Zdest)
   ],
   !.
 
 capability(add_rotation(Radd), Conditions, Actions, []) :-
   Conditions = [ 
     property(belief, [rotation,R])
   ],
   Actions = [ 
     NewR is R + Radd,
     valid_rotation(NewR, FinalR),
     rotation(FinalR)
   ],
   !.
         
 capability(rotation(R), Conditions, [], []) :-
   Conditions = [
     property(belief, [rotation,RA]),
     bool(R == RA, fail)
   ].
         
 capability(rotation(R), Conditions, Actions, Effects) :-
   Conditions = [
     property(belief, [rotation,RA]),
     bool(R > RA, fail),
     property(belief, [standing], try(get_of))
   ],
   Actions = [
     action([rotate_left, R, 0.2,-0.2])
   ],
   Effects = [ 
     change(belief, [rotation,_], [rotation,R]),
     change(belief, [head,_], [head,0.0])
   ].
 
 capability(rotation(R), Conditions, Actions, Effects) :-
   Conditions = [
     property(belief, [rotation,RA]),
     bool(R < RA, []),
     property(belief, [standing], try(get_of))
   ],
   Actions = [
     action([rotate_right, R, 0.2,-0.2])
   ],
   Effects = [ 
     change(belief, [rotation,_], [rotation,R]), 
     change(belief, [head,_], [head,0.0])
   ],
   !.
 
 capability(sit_rotation(R), [], Actions, Effects) :-
   Actions = [
     action([rotate, R])
   ],
   Effects = [ 
     change(belief, [rotation,_], [rotation,R]), 
     change(belief, [head,_], [head,0.0])
   ],
   !.
   
 capability(look(at(Object)), Conditions, Actions, []) :-
   Conditions = [
     property(Object, [position,XO,_YO,ZO], try(find(Object))),
     property(belief, [position,XA,_Y,ZA]),
     property(belief, [rotation,RA])
   ],
   Actions = [
     get_direction(XA,ZA, RA, XO,ZO, R),
     rotation_difference(R, RA, Rdif),
     look(Rdif)
   ],
   !.
   
 capability(look(R), Conditions, Actions, Effects) :-
   Conditions = [
     and(bool(R =< 0.6, fail), bool(R >= -0.6, fail))
   ],
   Actions = [
       action([look,R])
   ],
   Effects = [ change(belief, [head,_], [head,R]) ].
 
 capability(look(R), [], Actions, []) :-
   Actions = [
     add_rotation(R)
   ],
   !.
 
 capability(walk(Xdest,Zdest), Conditions, Actions, []) :-
   Conditions = [
     property(belief, [standing], try(get_of)),
     property(belief, [position,X,_Y,Z]),
     property(belief, [rotation,R])
   ],
   Actions = [
     get_direction(X,Z, R, Xdest,Zdest, R1),
     rotation_difference(R1,R,Rdif),
     look(Rdif),
     sleep(perceptionDelay),
     avoid_obstacles(X,Z, Xdest,Zdest)
   ].
 
capability(walking(X,Z), [], Actions, []) :-
   Actions = [
     get_direction(X,Z, R),
     rotation(R),
     repeat([step(X,Z,Reached)], bool(Reached, []))
   ],
   !.
 
 capability(step(Xdest,Zdest,Reached), Conditions, Actions, Effects) :-
   Conditions = [
     property(belief, [position,X,Y,Z]),
     property(belief, [rotation,R])
   ],
   Actions = [
     distance(X,Z, Xdest,Zdest, Distance),
     get_stepLength(Distance, StepLength, Reached),
     next_step(R, StepLength, Xstep,Zstep, Rfront,Rback),
     action([walk, Xstep,Y,Zstep, Rfront,Rback]),
     NewX is X + Xstep * 2,
     NewZ is Z + Zstep * 2
   ],
   Effects = [
     change(belief, [position,X,Y,Z], [position,NewX,Y,NewZ])
   ],
   !.
 
 :- end_object capability_lib.