The Cram Plan Language — Plan-based
Control of Autonomous Robots
Lorenz Mösenlechner ([email protected])
Intelligent Autonomous Systems Group
Technische Universität München
14 June 2012
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Outline
1. Motivation
2. The Language
3. Reasoning about Plan Execution
4. Plan Parameterization and Inference of Locations
5. Lab session
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Motivation
I
Goal: perform complex activity
in a human household
I
Implementing reliable robot
control programs is hard
I
Complex failure handling is
required
I
Tasks synchronization, parallel
execution, resource
management, ...
CPL
Lorenz Mösenlechner
Plan Parameterization
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Cognitive Robot Abstract Machine
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
The CRAM Core
Goals/
Reasoning
on Plans
Designators
Execution
trace
Process
Modules
CRAM Language
Common Lisp
CPL
Lorenz Mösenlechner
Knowrob
...
Motivation
CPL
Reasoning about Plan Execution
High-Level Robot Control
Task execution
I Parallel
I Synchronization
I Robust and flexible
I Failure handling
CPL
Lorenz Mösenlechner
Plan Parameterization
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
High-Level Robot Control
Task execution
I Parallel
I Synchronization
I Robust and flexible
I Failure handling
Requirements for the Language
I Expressive
I Easy to use
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
High-Level Robot Control
Task execution
I Parallel
I Synchronization
I Robust and flexible
I Failure handling
Requirements for the Language
I Expressive
I Easy to use
⇒ CPL is a Domain Specific Language fulfilling these requirements
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Outline
1. Motivation
2. The Language
3. Reasoning about Plan Execution
4. Plan Parameterization and Inference of Locations
5. Lab session
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Overview of the CRAM Language
I
Implemented in Common Lisp.
I
Compiles down to multithreaded programs.
I
Programs are in native machine code.
I
Provides control structures for parallel and sequential evaluation of
expressions.
I
Reactive control programs.
I
Exception handling, also across threads.
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Example: Picking up an object
Example
( let * (( obj-pose ( find-object obj ))
( pre-grasp-pos ( c a l c u l a t e - p r e - g r a s p obj-pose ))
( grasp-vector ( cl-transforms : make-3d- vector 0 0 -0.1))
( lift-vector ( cl-transforms : ma ke-3d-ve ctor 0 0 0.1)))
( open-gripper side )
( t a k e- c o l l i s i o n - m a p )
( with-failure-handling
(( no-ik- solution ( e )
( move-to-different-place )
( retry ))
( link-in-collision (e)
( setf pre-grasp-pos ( new-pre-grasp ))
( retry ))
( trajectory-controller-failed (e)
( retry )))
( m o ve - a r m - t o - p o i n t side pre-grasp-pos )))
...)
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Basic Lisp Syntax
I
Parenthesis around complete expression:
foo(bar, 123) ⇒ (foo bar 123)
I
Prefix notation for operators:
1 + 2 + 3 + 4 + 5 ⇒ (+ 1 2 3 4 5)
I
Expressions in “blocks”:
with open("foo.txt", "w") as f:
f.write("bar\n")
⇓
( with−open−file ( f ” foo . txt ” : d i r e c t i o n
( f o r m a t f ” b a r˜%” ) )
CPL
Lorenz Mösenlechner
: output )
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Functions in Common Lisp and CRAM
I
Define common lisp functions with
( d e f u n <function−name > (< p a r a m e t e r s ∗>)
< l i s p − e x p r e s s i o n s >∗)
Example:
( defun f a c t o r i a l (n)
( d e c l a r e ( type fixnum n ))
( i f (> n 1 )
( ∗ n ( f a c t o r i a l (1− n ) ) )
1))
I
Define CRAM “functions” with def-plan and
def-top-level-plan to make them visible in the task tree.
I
Lisp functions can be called from CRAM.
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Control structures in Common Lisp
Conditional evaluation
I
One “if” and one “else” expression:
( i f (> n 0 )
( foo )
( bar )
I
Only an “if” or only an “else” block:
( when (> n 0 )
( foo ))
( u n l e s s (> n 0 )
( bar ))
I
Complex conditionals:
( cond (( > n 0 )
( foo ))
(t
( bar ) ) )
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Control structures in Common Lisp
Return values
I
The return value of the last expression that is evaluated is used as
return value.
I
prog1 returns the first return value, prog2 the second, progn the
last.
I
Multiple return values with values.
( defun foo () ( v a l u e s 1 2 3))
( multiple−value−bind ( a b c ) ( foo )
( f o r m a t t ” ˜ a ˜ a ˜ a˜%” a b c ) )
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Variables
I
Create local, lexically scoped variables with let or let*.
( l e t ( ( a 1)
(b 2))
...)
I
Create global, dynamically scoped variables with defvar and
defparameter.
( d e f v a r ∗ foo ∗ 1)
( de fpa ram ete r ∗ bar ∗ 2)
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Data
Built-in data types
I
Simple data types: symbols, strings, numbers:
’ foo
I
: bar
” baz ”
123
123.45
1 2 3 . 4 5 6 d0
Built-in support for lists:
( l i s t 1 2 3 4)
’(1 2 3 4)
‘ ( 1 2 3 , a , @b c )
( f i r s t ( l i s t 1 2 3 4))
( car ( l i s t 1 2 3 4))
( nth 5 some−list )
( e l t some−list 5)
I
Built-in support for arrays:
( l e t ( ( m a t r i x ( make−array
’(2 2) : element−type ’ d o u b l e − f l o a t ) ) )
( d e c l a r e ( type simple−array matrix ))
( aref matrix 0 1))
I
Built-in support for hash-tables:
( l e t ( ( c a c h e ( make−hash−table ) ) )
( gethash ’ foo cache ))
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Data
User-defined data types
I
Structs with defstruct:
( d e f s t r u c t 2 d−vector
( x 0 . 0 d0 : t y p e d o u b l e − f l o a t )
( y 0 . 0 d0 : t y p e d o u b l e − f l o a t ) )
( l e t ( ( v e c t o r ( make−2d−vector : x 2 . 0 d0 ) ) )
( 2 d−vector−x v e c t o r ) )
I
Classes (support for inheritance) with defclass:
( d e f c l a s s parent ()
(( parent−slot : reader parent−slot
: initform nil
: i n i t a r g : parent−slot )))
( d e f c l a s s c h i l d ( parent ) ())
( make−instance ’ c h i l d : p a r e n t − s l o t 5)
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Side effects
I
Lisp is not a purely functional language, it supports side effects.
I
Set values with setq or setf. setf is more general.
( s e t f foo 123)
( s e t f ( a r e f m a t r i x 0 1 ) 0 . 1 d0 )
( s e t f ( gethash : foo cache ) 5)
I
Various forms for in-place updates: incf, decf, push, pushnew,
pop, ...
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Function objects and mappers
I
Function pointer with #’: #’factorial
I
Lambda functions:
( lambda ( x ) (+ x 1 0 ) )
I
Calling a function pointer:
( f u n c a l l #’ f a c t o r i a l 5 )
I
Apply a function to each element of a list and get the list of return
values:
( mapcar #’ f a c t o r i a l ( 1 2 3 4 5 ) )
( mapcar ( lambda ( x ) (+ x 1 0 ) ) ( 1 2 3 4 5 ) )
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Common Lisp and SBCL provide much
more
I
Macros
I
Object oriented programming (defmethod, defclass)
I
File IO
I
The loop macro
I
Sockets
I
Multithreading
I
Foreign Function Interface
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Overview CRAM Language
I
Fluents
I
Sequential evaluation
I
Parallel evaluation
I
Exceptions and failure handling
I
Task suspension
CPL
Lorenz Mösenlechner
Plan Parameterization
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Fluents
I
Fluents are objects that contain a value and provide synchronized
access.
I
Create with
I
Wait (block thread) until a fluent becomes true:
(make-fluent :name ’fl :value 1)
(wait-for fl)
I
Execute whenever a fluent becomes true:
(whenever fl)
I
Can be combined to fluent networks that update their value when
one fluent changes its value.
(wait-for (> x 20))
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Fluent networks
hand−position(x, y, z)
position−distance
<
cup−handle−position(x, y, z)
position−tolerance
hand−orientation(ax, ay, az)
angle−distance
<
cup−handle−orientation(ax, ay, az)
angle−tolerance
left−finger−hand−force
>
right−finger−hand−force
>
min−hand−force
CPL
Lorenz Mösenlechner
and
cup−gripped?
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
CRAM Control Flow
Sequential Evaluation
I Execute expressions sequentially:
(seq
(do a)
(do b)
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
CRAM Control Flow
Sequential Evaluation
I Execute expressions sequentially:
(seq
(do a)
(do b)
I Execute expressions sequentially until one succeeds:
(try-in-order
(do a)
(do b)
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
CRAM Control Flow
Parallel Evaluation
I Execute in parallel, succeed when all succeed, fail if one fails:
...)
Examples:
(par
(open-right-gripper)
(open-left-gripper)
CPL
Lorenz Mösenlechner
(par
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
CRAM Control Flow
Parallel Evaluation
I Execute in parallel, succeed when all succeed, fail if one fails: (par
...)
I Execute in parallel, succeed when one succeeds, fail if one fails:
(pursue ...)
Examples:
(par
(open-right-gripper)
(open-left-gripper)
CPL
Lorenz Mösenlechner
(pursue
(wait-for (< (distance robot p) 5))
(update-nav-cmd x)
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
CRAM Control Flow
Parallel Evaluation
I Execute in parallel, succeed when all succeed, fail if one fails: (par
...)
I Execute in parallel, succeed when one succeeds, fail if one fails:
(pursue ...)
I Try in parallel, succeed when one succeeds, fail if all fail: (try-all
...)
Examples:
(par
(open-right-gripper)
(open-left-gripper)
CPL
Lorenz Mösenlechner
(pursue
(wait-for (< (distance robot p) 5))
(update-nav-cmd x)
Motivation
CPL
Reasoning about Plan Execution
Failure Handling
I
Create exception class:
(define-condition nav-failed (plan-error) ())
CPL
Lorenz Mösenlechner
Plan Parameterization
Lab session
Motivation
CPL
Reasoning about Plan Execution
Failure Handling
I
Create exception class:
I
(define-condition nav-failed (plan-error) ())
Throw exception: (fail ’nav-failed)
CPL
Lorenz Mösenlechner
Plan Parameterization
Lab session
Motivation
CPL
Reasoning about Plan Execution
Failure Handling
I
I
I
Create exception class:
(define-condition nav-failed (plan-error) ())
Throw exception: (fail ’nav-failed)
Handle exceptions:
(with-failure-handling
((obj-not-reachable (e)
(move-to-better-location)
(retry)))
(pursue
(seq
(sleep timeout)
(fail timeout)
(grasp-obj obj)
CPL
Lorenz Mösenlechner
Plan Parameterization
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Failure Handling
I
I
I
Create exception class:
(define-condition nav-failed (plan-error) ())
Throw exception: (fail ’nav-failed)
Handle exceptions:
(with-failure-handling
((obj-not-reachable (e)
(move-to-better-location)
(retry)))
(pursue
(seq
(sleep timeout)
(fail timeout)
(grasp-obj obj)
I
Execute expressions even on exceptions (finally):
(unwind-protect
(grasp-object)
(move-arms-to-save-position))
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Tagging, Suspension, Protection forms
I
Name sub-expressions and bind them to variables in the current
lexical scope:
(:tag var
(move-to x y)
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Tagging, Suspension, Protection forms
I
I
Name sub-expressions and bind them to variables in the current
lexical scope: (:tag var ...)
Execute expressions with a parallel task suspended:
(pursue
(whenever c
(with-task-suspended nav
...))
(:tag nav
(move-to x y)
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Tagging, Suspension, Protection forms
I
I
I
Name sub-expressions and bind them to variables in the current
lexical scope: (:tag var ...)
Execute expressions with a parallel task suspended:
(pursue
(whenever c
(with-task-suspended nav
...))
(:tag nav
(move-to x y)
Execute code just before a task is suspended:
(suspend-protect
(move-to x y)
(stop-motors)
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Outline
1. Motivation
2. The Language
3. Reasoning about Plan Execution
4. Plan Parameterization and Inference of Locations
5. Lab session
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Reasoning based on Execution Traces
CPL
Lorenz Mösenlechner
I
Why did you leave the cup on
the table while clearing it?
I
Where did you stand while
performing a task?
I
What did you see?
I
How did you move?
I
How did you move the arm
while grasping the bottle?
I
...
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Our approach
1. Record execution trace
2. Provide an interface to the execution trace through a first-order
representation
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Our approach
1. Record execution trace
I
I
Belief state
State of plan execution, tasks, activation, deactivation, results
2. Provide an interface to the execution trace through a first-order
representation
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Our approach
1. Record execution trace
I
I
Belief state
State of plan execution, tasks, activation, deactivation, results
2. Provide an interface to the execution trace through a first-order
representation
I
I
I
Symbolic annotations of plans
Causal relations through plan hierarchy
Symbolic representation of objects in plans
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Recording of Execution Trace
Achieve(Loc(bottle, table))
Achieve(ObjectOpened(fridge))
Achieve(ObjPlacedAt(bottle, table))
Achieve(ObjInHand(bottle))
Achieve(ObjectClosed(fridge))
t
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Recording of Execution Trace
Achieve(Loc(bottle, table))
Achieve(ObjectOpened(fridge))
Achieve(ObjPlacedAt(bottle, table))
Achieve(ObjInHand(bottle))
Achieve(ObjectClosed(fridge))
t
Action:
I
Move to fridge
Log:
CPL
Lorenz Mösenlechner
I
Achieve(Loc(bottle,table)) running
I
Achieve(Loc(Robot, l)) running
I
Trajectory of robot
I
...
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Recording of Execution Trace
Achieve(Loc(bottle, table))
Achieve(ObjectOpened(fridge))
Achieve(ObjPlacedAt(bottle, table))
Achieve(ObjInHand(bottle))
Achieve(ObjectClosed(fridge))
t
Action:
I
Open fridge
Log:
CPL
Lorenz Mösenlechner
I
Achieve(Loc(Robot, l)) succeeded
I
Achieve(ObjectOpened(fridge))
running
I
Trajectory of arm
I
...
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Recording of Execution Trace
Achieve(Loc(bottle, table))
Achieve(ObjectOpened(fridge))
Achieve(ObjPlacedAt(bottle, table))
Achieve(ObjInHand(bottle))
Achieve(ObjectClosed(fridge))
t
Action:
I
Grasp the bottle
Log:
CPL
Lorenz Mösenlechner
I
Achieve(ObjectOpended(fridge))
succeeded
I
Achieve(ObjInHand(bottle)) running
I
Perceived properties of bottle (object
designator)
I
...
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Recording of Execution Trace
Achieve(Loc(bottle, table))
Achieve(ObjectOpened(fridge))
Achieve(ObjPlacedAt(bottle, table))
Achieve(ObjInHand(bottle))
Achieve(ObjectClosed(fridge))
t
Action:
I
Close the fridge
Log:
I
CPL
Lorenz Mösenlechner
...
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Recording of Execution Trace
Achieve(Loc(bottle, table))
Achieve(ObjectOpened(fridge))
Achieve(ObjPlacedAt(bottle, table))
Achieve(ObjInHand(bottle))
Achieve(ObjectClosed(fridge))
t
Action:
I
Put down bottle
Log:
I
CPL
Lorenz Mösenlechner
...
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Recording of Execution Trace
Achieve(Loc(bottle, table))
Achieve(ObjectOpened(fridge))
Achieve(ObjPlacedAt(bottle, table))
Achieve(ObjInHand(bottle))
Achieve(ObjectClosed(fridge))
t
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Goals and Reasoning
I
I
Reasoning about programs is complex.
We annotate only the interesting parts to infer the semantics of a
plan.
I
I
I
achieve: Make true if not already true
perceive: Try to find object and return a information
at-location: Execute code at a specific location
CPL
Lorenz Mösenlechner
about it
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Reasoning about Plan Execution
I The plate is on the cupboard at the beginning.
1
2
1
2
1
2
1
3
Time
216.41 s
3
1
3
I The plate is supported by the table.
I The robot placed a plate on the table
I The robot cannot see the plate on the
table.
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Reasoning about Plan Execution
I Holds(Loc(Plate, on(Cupboard)), At(0s))
1
2
1
2
1
2
1
3
Time
216.41 s
3
1
3
I The plate is supported by the table.
I The robot placed a plate on the table
I The robot cannot see the plate on the
table.
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Reasoning about Plan Execution
I Holds(Loc(Plate, on(Cupboard)), At(0s))
1
2
1
2
1
2
1
3
Time
216.41 s
3
1
I Holds(Loc(Plate, on(Table)), At(TaskEnd(tsk)))
∧ Task(tsk)
∧ TaskGoal(tsk,
Achieve(Loc(Plate, on(Table))))
CPL
Lorenz Mösenlechner
3
I The plate is supported by the table.
I The robot cannot see the plate on the
table.
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Reasoning about Plan Execution
I Holds(Loc(Plate, on(Cupboard)), At(0s))
1
2
1
2
1
2
1
3
Time
216.41 s
3
1
3
I Holds(Loc(Plate, on(Table)), At(TaskEnd(tsk)))
∧ Task(tsk)
∧ TaskGoal(tsk,
Achieve(Loc(Plate, on(Table))))
CPL
Lorenz Mösenlechner
I Holds(Supporting (Table, Plate), At(t))
I ¬Holds(ObjectVisible(Plate), At(t))
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Annotating Plans
I
Achieve goals:
(def-goal (achieve (loc ?obj ?loc))
(achieve ‘(object-in-hand ,?obj :right))
(achieve ‘(object-placed-at ,?obj ,?loc)))
I
Perceive:
(def-goal (perceive ?obj-name)
(find-obj ?obj-name))
I
At-location:
(at-location (?loc)
(achieve ‘(object-in-hand ,?obj :right)))
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Plan Representation
Achieve(Loc(Obj, Loc))
Perceive(Loc(Obj, Loc))
Achieve(ObjPlacedAt(Obj, Loc))
Achieve(ObjInHand(Obj))
..
.
..
.
CPL
Lorenz Mösenlechner
..
.
..
.
..
.
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Predicates for reasoning on execution
traces
(task ?tsk)
(task-goal ?tsk ?goal)
(task-start ?tsk ?t)
(task-end ?tsk ?t)
(subtask ?tsk ?subtsk)
(subtask+ ?tsk ?subtsk)
(task-outcome ?tsk ?status)
(task-result ?tsk ?result)
CPL
Lorenz Mösenlechner
?tsk is a task on the interpretation
stack.
Unifies the goal of the task.
Unifies the start time of the task.
Unifies the end time of the task.
Asserts that subtask is a direct subtask of task.
Assets that subtask is a subtask of
task.
Unifies the final status of a task
(Failed, Done or Evaporated).
Unifies the result of a task.
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Outline
1. Motivation
2. The Language
3. Reasoning about Plan Execution
4. Plan Parameterization and Inference of Locations
5. Lab session
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Plan parameterizations: Designators
I
I
Symbolic descriptions of Objects, Locations and Actions
Examples:
I
I
I
I
(location (on table) (reachable-by Rosie))
(object (type cup))
(action (to reach) (the object (name Cup1)))
Example plan:
( with−designators
( ( c u p − l o c ( l o c a t i o n ‘ ( on c o u n t e r − t o p ) ) )
( cup ( o b j e c t ‘ ( t y p e cup ) ( a t , c u p − l o c ) ) )
( p i c k − u p − l o c ‘ ( l o c a t i o n ( t o r e a c h ) ( o b j cup ) ) ) )
( a t − l o c a t i o n pick−up−loc
( a c h i e v e ‘ ( o b j − g r a s p e d , cup ) ) ) )
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Example: Location to put-down pancake
mix
(a location
(for pancake-mix)
(on counter)
(reachable-for Rosie)
(visible-for James)
(not-hindering (the activity (type pancake-making))))
Concepts
I Generative model
I Stability reasoning
I Reachability reasoning
I Visibility reasoning and perspective taking
I Light-weight temporal projection
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Example: Location to put-down pancake
mix
(a location
(for pancake-mix)
(on counter)
(reachable-for Rosie)
(visible-for James)
(not-hindering (the activity (type pancake-making))))
Concepts
I Generative model
I Stability reasoning
I Reachability reasoning
I Visibility reasoning and perspective taking
I Light-weight temporal projection
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Example: Location to put-down pancake
mix
(a location
(for pancake-mix)
(on counter)
(reachable-for Rosie)
(visible-for James)
(not-hindering (the activity (type pancake-making))))
Concepts
I Generative model
I Stability reasoning
I Reachability reasoning
I Visibility reasoning and perspective taking
I Light-weight temporal projection
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Example: Location to put-down pancake
mix
(a location
(for pancake-mix)
(on counter)
(reachable-for Rosie)
(visible-for James)
(not-hindering (the activity (type pancake-making))))
Concepts
I Generative model
I Stability reasoning
I Reachability reasoning
I Visibility reasoning and perspective taking
I Light-weight temporal projection
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Example: Location to put-down pancake
mix
(a location
(for pancake-mix)
(on counter)
(reachable-for Rosie)
(visible-for James)
(not-hindering (the activity (type pancake-making))))
Concepts
I Generative model
I Stability reasoning
I Reachability reasoning
I Visibility reasoning and perspective taking
I Light-weight temporal projection
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
First-order representation of Designators
(a location
(for pancake-mix)
(on counter)
(reachable-for Rosie)
(visible-for James)
(not-hindering (the activity (type pancake-making))))
⇓
posesOn(Counter, PancakeMix, ?Poses) ∧ member(?P, ?Poses)
∧ assertPose(PancakeMix, ?P) ∧ stable(PancakeMix)
∧ reachable(Rosie, PancakeMix) ∧ blockingObjects(Rosie, PancakeMix, ∅)
∧ visible(James, PancakeMix)
∧ projectPlan(MakePancakes, ?Tl) ∧ bagof(?F, flawsInTimeline(?Tl, ?F), ∅)
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
First-order representation of Designators
(a location
(for pancake-mix)
(on counter)
(reachable-for Rosie)
(visible-for James)
(not-hindering (the activity (type pancake-making))))
⇓
posesOn(Counter, PancakeMix, ?Poses) ∧ member(?P, ?Poses)
∧ assertPose(PancakeMix, ?P) ∧ stable(PancakeMix)
∧ reachable(Rosie, PancakeMix) ∧ blockingObjects(Rosie, PancakeMix, ∅)
∧ visible(James, PancakeMix)
∧ projectPlan(MakePancakes, ?Tl) ∧ bagof(?F, flawsInTimeline(?Tl, ?F), ∅)
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
First-order representation of Designators
(a location
(for pancake-mix)
(on counter)
(reachable-for Rosie)
(visible-for James)
(not-hindering (the activity (type pancake-making))))
⇓
posesOn(Counter, PancakeMix, ?Poses) ∧ member(?P, ?Poses)
∧ assertPose(PancakeMix, ?P) ∧ stable(PancakeMix)
∧ reachable(Rosie, PancakeMix) ∧ blockingObjects(Rosie, PancakeMix, ∅)
∧ visible(James, PancakeMix)
∧ projectPlan(MakePancakes, ?Tl) ∧ bagof(?F, flawsInTimeline(?Tl, ?F), ∅)
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
First-order representation of Designators
(a location
(for pancake-mix)
(on counter)
(reachable-for Rosie)
(visible-for James)
(not-hindering (the activity (type pancake-making))))
⇓
posesOn(Counter, PancakeMix, ?Poses) ∧ member(?P, ?Poses)
∧ assertPose(PancakeMix, ?P) ∧ stable(PancakeMix)
∧ reachable(Rosie, PancakeMix) ∧ blockingObjects(Rosie, PancakeMix, ∅)
∧ visible(James, PancakeMix)
∧ projectPlan(MakePancakes, ?Tl) ∧ bagof(?F, flawsInTimeline(?Tl, ?F), ∅)
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
First-order representation of Designators
(a location
(for pancake-mix)
(on counter)
(reachable-for Rosie)
(visible-for James)
(not-hindering (the activity (type pancake-making))))
⇓
posesOn(Counter, PancakeMix, ?Poses) ∧ member(?P, ?Poses)
∧ assertPose(PancakeMix, ?P) ∧ stable(PancakeMix)
∧ reachable(Rosie, PancakeMix) ∧ blockingObjects(Rosie, PancakeMix, ∅)
∧ visible(James, PancakeMix)
∧ projectPlan(MakePancakes, ?Tl) ∧ bagof(?F, flawsInTimeline(?Tl, ?F), ∅)
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Inference algorithm
posesOn(Counter, Cup, ?Poses) ∧ member(?P, ?Poses)
∧ assertPose(Cup, ?P) ∧ stable(Cup)
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Inference algorithm
posesOn(Counter, Cup, ?Poses) ∧ member(?P, ?Poses)
∧ assertPose(Cup, ?P) ∧ stable(Cup)
Create distribution for sampling
poses
1. posesOn(Counter, Cup,
?Poses)
2. member(?P, ?Poses)
3. assertPose(Cup, ?P)
4. stable(Cup)
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Inference algorithm
posesOn(Counter, Cup, ?Poses) ∧ member(?P, ?Poses)
∧ assertPose(Cup, ?P) ∧ stable(Cup)
Draw a pose sample
1. posesOn(Counter, Cup,
?Poses)
2. member(?P, ?Poses)
3. assertPose(Cup, ?P)
4. stable(Cup)
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Inference algorithm
posesOn(Counter, Cup, ?Poses) ∧ member(?P, ?Poses)
∧ assertPose(Cup, ?P) ∧ stable(Cup)
Place the mug
1. posesOn(Counter, Cup,
?Poses)
2. member(?P, ?Poses)
3. assertPose(Cup, ?P)
4. stable(Cup)
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Inference algorithm
posesOn(Counter, Cup, ?Poses) ∧ member(?P, ?Poses)
∧ assertPose(Cup, ?P) ∧ stable(Cup)
Simulate for 50ms, fail!
1. posesOn(Counter, Cup,
?Poses)
2. member(?P, ?Poses)
3. assertPose(Cup, ?P)
4. stable(Cup)
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Inference algorithm
posesOn(Counter, Cup, ?Poses) ∧ member(?P, ?Poses)
∧ assertPose(Cup, ?P) ∧ stable(Cup)
Backtrack, draw another pose
sample
1. posesOn(Counter, Cup,
?Poses)
2. member(?P, ?Poses)
3. assertPose(Cup, ?P)
4. stable(Cup)
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Inference algorithm
posesOn(Counter, Cup, ?Poses) ∧ member(?P, ?Poses)
∧ assertPose(Cup, ?P) ∧ stable(Cup)
Place the mug
1. posesOn(Counter, Cup,
?Poses)
2. member(?P, ?Poses)
3. assertPose(Cup, ?P)
4. stable(Cup)
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Inference algorithm
posesOn(Counter, Cup, ?Poses) ∧ member(?P, ?Poses)
∧ assertPose(Cup, ?P) ∧ stable(Cup)
Simulate for 50ms, succeed!
1. posesOn(Counter, Cup,
?Poses)
2. member(?P, ?Poses)
3. assertPose(Cup, ?P)
4. stable(Cup)
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Built-in Predicates of CRAM Reasoning
Stability
Contact between objects
Stability of object
Visibility
visible(P, O)
Object visible from pose P
occluding(P, O1 , O2 )
Object O2 occludes object O1
Reachability
reachable(R, O)
Object O is reachable by robot R
blockingObjects(R, O, B) B is the list of objects that “block” O
contact(O1 , O2 )
stable(O)
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Built-in Predicates
ofreasoning
CRAM Reasoning
Stability
I
Bullet physics engine
Infer collisions and stabilityStability
of scenes
Icontact(O
Simulate1 ,for
of time between
(5s)
O2a) short periodContact
objects
Stability
of object
Istable(O)
Example: mugs can be placed
on plates
but not the other way
Visibility
around
visible(P, O)
Object visible from pose P
occluding(P, O1 , O2 )
Object O2 occludes object O1
5s
Reachability
reachable(R, O)
Object O is reachable by robot R
blockingObjects(R, O, B) B is the list of objects that “block” O
5s
I
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Built-in Predicates of CRAM Reasoning
Visibility reasoning
OpenGL to infer visibility ofStability
objects
cameraContact
frame, each
object
in a different color
between
objects
and
count
pixels
stable(O)
Stability of object
I Infer visibility of objects and
occluding objects
Visibility
visible(P, O)
Object visible from pose P
occluding(P, O1 , O2 )
Object O2 occludes object O1
Reachability
reachable(R, O)
Object O is reachable by robot R
blockingObjects(R, O, B) B is the list of objects that “block” O
I
Icontact(O
Render scene
1 , O2 )from
See also “Geometric Tools for Perspective Taking for Human-Robot Interaction” [Marin-Urias 2008]
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Built-in Predicates of CRAM Reasoning
Reachability
Stability
Icontact(O
Use inverse
to infer
reachability
Contact
between objects
1 , Okinematics
2)
Istable(O)
Stability
object
No grasp planning, reach for
objectofcenter
Blocking objects = objectsVisibility
in collision with IK solution
visible(P, O)
Object visible from pose P
occluding(P, O1 , O2 )
Object O2 occludes object O1
Reachability
reachable(R, O)
Object O is reachable by robot R
blockingObjects(R, O, B) B is the list of objects that “block” O
I
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Temporal projection
(a location ...
(not-hindering (the activity (type (pick-up Cup1)))))
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Temporal projection
(a location ...
(not-hindering (the activity (type (pick-up Cup1)))))
⇓
projectPlan(PickUp(Cup1), ?Tl) ∧ bagof(?F, flawsInTimeline(?F), ∅)
1. Execute plan in projection mode
2. Projection generates a timeline
3. Match pre-defined flaws on the timeline
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Plan projection and Timeline generation
Achieve(ObjInHand(Cup1))
Perceive(Cup1)
AtLocation(ReachLoc)
..
.
..
.
ObjectGrasped(Cup1)
ObjectLifted(Cup1)
t
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Plan projection and Timeline generation
Achieve(ObjInHand(Cup1))
Perceive(Cup1)
AtLocation(ReachLoc)
..
.
..
.
ObjectGrasped(Cup1)
ObjectLifted(Cup1)
t
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Plan projection and Timeline generation
Achieve(ObjInHand(Cup1))
Perceive(Cup1)
AtLocation(ReachLoc)
..
.
..
.
ObjectGrasped(Cup1)
ObjectLifted(Cup1)
t
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Plan projection and Timeline generation
Achieve(ObjInHand(Cup1))
Perceive(Cup1)
AtLocation(ReachLoc)
..
.
..
.
ObjectGrasped(Cup1)
ObjectLifted(Cup1)
t
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Plan projection and Timeline generation
Achieve(ObjInHand(Cup1))
Perceive(Cup1)
AtLocation(ReachLoc)
..
.
..
.
ObjectGrasped(Cup1)
ObjectLifted(Cup1)
t
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Inferring behavior flaws
I
Projection creates a timeline
I
Discrete transitions in world
I
Execution trace of plan execution
Predicates:
I
I
I
I
holds(occ, at(t)), holds(occ, during(t1 , t2 )),
holds(occ, throughout(t1 , t2 )): Assert occasions (states)
occurs(ev, t): Assert occurrence of an event
Example: Robot collides with an object
TaskGoal(?tsk, Achieve(ObjectInHand(Cup1)))
∧ TaskStartedAt(?tsk, ?t1 ) ∧ TaskEndedAt(?tsk, ?t2 )
∧ holds(Contact(Robot, ?o), during(?t1 , ?t2 )) ∧ (?o 6= Cup1)
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Other applications of the system
Integration into executive to:
I
keep a consistent representation of the world
I
generate (artificial) sensor data
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Other applications of the system
Integration into executive to:
I
keep a consistent representation of the world
I
generate (artificial) sensor data
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Outline
1. Motivation
2. The Language
3. Reasoning about Plan Execution
4. Plan Parameterization and Inference of Locations
5. Lab session
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
Tutorial setup
I
Make sure that Emacs is installed
I
Make sure that
installed.
I
Execute
ros-electric-roslisp-common
and the
cram pl
stack are
cp ‘rospack find roslisp_repl‘/swank.lisp ~/.swank.lisp
I
You can run a LISP REPL with: rosrun roslisp\_repl repl
CPL
Lorenz Mösenlechner
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
The Lisp REPL
I
REPL = Read-Eval-Print-Loop
I
Interactive development environment
I
Inspection of variables
Important commands
I Ctrl-up and Ctrl-down for moving in history
I Change package with
(in-package :roslisp)
I When in debugger, press number of restart Abort to abort debugging
I Enter in debugger opens stack frames or calls the inspector
I ‘‘l’’ to go back in inspector
I ‘‘q’’ to exit inspector
CPL
Lorenz Mösenlechner
Lab session
Motivation
CPL
Reasoning about Plan Execution
Plan Parameterization
Lab session
CRAM Tutorials
Go through tutorials 1 to 4 at
http://ros.org/wiki/cram_pl/Tutorials.
Then solve the following task: we want to simulate two turtles in
Turtlesim. One is controlled by the keyboard teleoperation node and
one by CRAM. The CRAM controlled turtle should behave as follows:
1. It should drive along a “rectangular” path with the corners being
close to the window corners. The exact coordinates do not matter
too much, the turtle should just turn towards the next corner when
it gets close to a corner.
2. If the turtle controlled by the keyboard comes too close, the CRAM
turtle should stop. If the distance increases again, the CRAM turtle
should continue driving on its rectangular path.
CPL
Lorenz Mösenlechner