search

KavrakiLab / tmkit

A Task-Motion Planning Framework
http://tmkit.kavrakilab.org
BSD 3-Clause "New" or "Revised" License
48 stars 12 forks source link

How to use tmkit for my own robot and domain? #14

Open Reluigi opened 5 years ago

Reluigi commented 5 years ago

Hello again, I'm trying to use tmkit for a four-legged robot domain. Could you briefly tell me the key steps to be followed to generate the appropriate scene description and domain semantics files? Thank you for your help

ndantam commented 5 years ago

You need three parts: the task domain (PDDL files), the motion domain (URDF/Scene files), and the domain semantics. You can test the task domain and motion domain independently. To test the task domain, you can run tmkit in pure task planning mode (tmsmt ops.pddl facts.pddl -o plan.tmp). To test the the motion domain, you can visualize the scene through the scene compiler (aarxc --gui robot.urdf). Note that you don't need to call the scene compiler explicity for TMKit; the planner will compile the scene on-demand.

For the domain semantics, it is probably easiest to start with one of the demo/tutorial files and adapt it to your domain. You'll (presumably) need to modify the functions to map from scenes to states and goals and the functions to call the motion planner for task actions.

Reluigi commented 5 years ago

Thank you, I changed the pddl files and it worked. I changed urdf model and python file tm-block-extend.py but I keep getting this error: 


> Unhandled SIMPLE-TYPE-ERROR in thread #<SB-THREAD:THREAD "main thread" RUNNING
>                                          {100660CB43}>:
>   Argument Y is not a NUMBER: NIL
> 
> Backtrace for: #<SB-THREAD:THREAD "main thread" RUNNING {100660CB43}>
> 0: ((LAMBDA NIL :IN SB-DEBUG::FUNCALL-WITH-DEBUG-IO-SYNTAX))
> 1: (SB-IMPL::CALL-WITH-SANE-IO-SYNTAX #<CLOSURE (LAMBDA NIL :IN SB-DEBUG::FUNCALL-WITH-DEBUG-IO-SYNTAX) {100993C9CB}>)
> 2: (SB-IMPL::%WITH-STANDARD-IO-SYNTAX #<CLOSURE (LAMBDA NIL :IN SB-DEBUG::FUNCALL-WITH-DEBUG-IO-SYNTAX) {100993C99B}>)
> 3: (PRINT-BACKTRACE :STREAM #<SYNONYM-STREAM :SYMBOL SB-SYS:*STDERR* {10001AB0F3}> :START 0 :FROM :INTERRUPTED-FRAME :COUNT NIL :PRINT-THREAD T :PRINT-FRAME-SOURCE NIL :METHOD-FRAME-STYLE NIL)
> 4: (SB-DEBUG::DEBUGGER-DISABLED-HOOK #<SIMPLE-TYPE-ERROR expected-type: NUMBER datum: NIL> #<unavailable argument>)
> 5: (SB-DEBUG::RUN-HOOK *INVOKE-DEBUGGER-HOOK* #<SIMPLE-TYPE-ERROR expected-type: NUMBER datum: NIL>)
> 6: (INVOKE-DEBUGGER #<SIMPLE-TYPE-ERROR expected-type: NUMBER datum: NIL>)
> 7: (ERROR SIMPLE-TYPE-ERROR :DATUM NIL :EXPECTED-TYPE NUMBER :FORMAT-CONTROL "~@<Argument ~A is not a ~S: ~2I~_~S~:>" :FORMAT-ARGUMENTS (SB-KERNEL::Y NUMBER NIL))
> 8: (SB-KERNEL:TWO-ARG-+ 0 NIL)
> 9: ((:METHOD TMSMT::TRANSLATE-OP ((EQL :MOTION-PLAN) T T T)) #<unavailable argument> #.(SB-SYS:INT-SAP #X0068A790) #<ROBRAY:SCENE-GRAPH ("block_a" "block_b" "block_c" "block_d" "body-collision" "body-visual" "bookshelf" "curtain" "end_effector_grasp" "fakejoint1" "fakejoint2" "fakejoint3" ...)> #<unavailable argument>) [fast-method]
> 10: ((LAMBDA (TMSMT::CONTEXT TMSMT::OP) :IN "/home/luigi/tmkit/lisp/foreign-tmplan.lisp") #<unavailable argument> #.(SB-SYS:INT-SAP #X0068A790))
> 11: ((LAMBDA (SB-ALIEN::ARGS-POINTER SB-ALIEN::RESULT-POINTER FUNCTION) :IN "/home/luigi/.cache/common-lisp/sbcl-1.3.1.debian-linux-x64/home/luigi/amino/lisp/rx/rx-win.fasl") 70368629226164 70368629226160 #<FUNCTION (LAMBDA (TMSMT::CONTEXT TMSMT::OP) :IN "/home/luigi/tmkit/lisp/foreign-tmplan.lisp") {1000CCEF3B}>)
> 12: ("foreign function: call_into_lisp")
> 13: ("foreign function: funcall3")
> 14: ("foreign function: callback_wrapper_trampoline")
> 15: ("foreign function: #x20100E97")
> 16: ("foreign function: #x68A510")
> 17: (TMSMT::TRANSLATE-TMPLAN #<ROBRAY:SCENE-GRAPH ("block_a" "block_b" "block_c" "block_d" "body-collision" "body-visual" "bookshelf" "curtain" "end_effector_grasp" "fakejoint1" "fakejoint2" "fakejoint3" ...)> #.(SB-SYS:INT-SAP #X0068A750))
> 18: (TMSMT::PARSE-TMPLAN #<ROBRAY:SCENE-GRAPH ("block_a" "block_b" "block_c" "block_d" "body-collision" "body-visual" "bookshelf" "curtain" "end_effector_grasp" "fakejoint1" "fakejoint2" "fakejoint3" ...)> "q0.tmp")
> 19: (TMSMT:TMP-DRIVER :TIMES-FILE NIL :START-SCENE ("new_robot.urdf" "sussman-extend.robray" "allowed-collision.robray") :GOAL-SCENE ("space-1.robray") :PDDL ("tm-blocks-extend.pddl" "extended-facts.pddl") :GUI NIL :SCRIPTS ("tm-blocks-extend.py") :VERBOSE NIL :MAX-STEPS 10 :OUTPUT "plan.tmp" :WRITE-FACTS "file.txt" :MOTION-TIMEOUT NIL :START-PLAN "q0.tmp" :START NIL :PREFIX-CACHE T :CONSTRAINTS :STATE)
> 20: (SB-INT:SIMPLE-EVAL-IN-LEXENV (TMSMT::TMP-COMMAND) #<NULL-LEXENV>)
> 21: (EVAL (TMSMT::TMP-COMMAND))
> 22: (SB-IMPL::PROCESS-EVAL/LOAD-OPTIONS ((:EVAL . "(require :tmsmt)") (:EVAL . "(tmsmt::tmp-command)") (:QUIT)))
> 23: (SB-IMPL::TOPLEVEL-INIT)
> 24: ((FLET #:WITHOUT-INTERRUPTS-BODY-82 :IN SAVE-LISP-AND-DIE))
> 25: ((LABELS SB-IMPL::RESTART-LISP :IN SAVE-LISP-AND-DIE))
>

Can you help me to understand what could cause it? Thank you again

ndantam commented 5 years ago

I'm not sure if this problem is with the URDF or the domains semantics, though it does look like a plan is found.

  1. Can you try displaying / compiling the URDF separately (via aarxc)?
  2. Can you output the plan to a file (tmsmt -o FILE.tmp) and check whether it looks reasonable?
Reluigi commented 5 years ago
  1. I tried to compile with aarxc and it works fine, with apparently no errors since I can visualise the robot. Here is the compilation output: 
aarxc -R new_robot.urdf 
  /usr/local/blender-2.77a-linux-glibc211-x86_64/blender -b -P /home/luigi/amino/lisp/../share/exec/meshconv --/usr/local/blender-2.77a-linux-glibc/home/luigi/master_thesis/rosWs/src/new_robot/meshes/hand.STL  -o-b  /tmp/amino-luigi/po--Py/home/luigi/master_thesis/rosWs/src/new_r  ot/meshes/hand.STL.obj /home/l--up=Zmino/lisp/../share/exec/meshconv--forward=Y ---  
/home/luigi/master_thesis/rosWs/src/new_robot/meshes/lowerarm.STL  -o-o  /tmp/amino-luigi/povray/home/luigi/maste/tmp/amino-luigi/povray/home/luigi/master_thesis/rosWs/src/new_robot/meshes/wrist.S L.obj --up=Z--up=Z  --forward=Y--forward=Y
  /usr/local/blender-2.77a-linux-glibc211-x86_64/blender -b -P /home/luigi/amino/lisp/../share/exec/meshconv --/usr/local/blender-2.77a-linux-glibc/home/luigi/master_thesis/rosWs/src/new_robot/meshes/hand.STL  -o-b  /tmp/amino-luigi/po--Py/home/luigi/master_thesis/rosWs/src/new_r  ot/meshes/hand.STL.obj /home/l--up=Zmino/lisp/../share/exec/meshconv--forward=Y ---  
/home/luigi/master_thesis/rosWs/src/new_robot/meshes/lowerarm.STL  -o-o  /tmp/amino-luigi/povray/home/luigi/maste/tmp/amino-luigi/povray/home/luigi/master_thesis/rosWs/src/new_robot/meshes/wrist.S L.obj --up=Z--up=Z  --forward=Y--forward=Y
  /usr/local/blender-2.77a-linux-glibc211-x86_64/blender -b -P /home/luigi/amino/lisp/../share/exec/meshconv --/usr/local/blender-2.77a-linux-glibc/home/luigi/master_thesis/rosWs/src/new_robot/meshes/hand.STL  -o-b  /tmp/amino-luigi/po--Py/home/luigi/master_thesis/rosWs/src/new_r  ot/meshes/hand.STL.obj /home/l--up=Zmino/lisp/../share/exec/meshconv--forward=Y ---  
/home/luigi/master_thesis/rosWs/src/new_rob  /usr/local/blender-2.77a-linux-glibc211-x86_64/blender -b -P /home/luigi/amino/lisp/../share/exec/meshconv --/usr/local/blender-2.77a-linux-glibc/home/luigi/master_thesis/rosWs/src/new_robot/meshes/hand.STL  -o-b  /tmp/amino-luigi/po--Py/home/luigi/master_thesis/rosWs/src/new_r  ot/meshes/hand.STL.obj /home/l--up=Zmino/lisp/../share/exec/meshconv--forward=Y ---  
/home/luigi/master_thesis/rosWs/src/new_robot/meshes/lowerarm.STL  -o-o  /tmp/amino-luigi/povray/home/luigi/maste/tmp/amino-luigi/povray/home/luigi/master_thesis/rosWs/src/new_robot/meshes/wrist.S L.obj --up=Z--up=Z  --forward=Y--forward=Y
  /usr/local/blender-2.77a-linux-glibc211-x86_64/blender -b -P /home/luigi/amino/lisp/../share/exec/meshconv -- /home/luigi/master_thesis/rosWs/src/new_robot/meshes/upperarm.STL -o /tmp/amino-luigi/povray/home/luigi/master_thesis/rosWs/src/new_robot/meshes/upperarm.STL.obj --up=Z --forward=Y
  /usr/local/blender-2.77a-linux-glibc211-x86_64/blender -b -P /home/luigi/amino/lisp/../share/exec/meshconv --/usr/local/blender-2.77a-linux-glibc/home/luigi/master_thesis/rosWs/src/new_robot/meshes/hand.STL  -o-b  /tmp/amino-luigi/po--Py/home/luigi/master_thesis/rosWs/src/new_r  ot/meshes/hand.STL.obj /home/l--up=Zmino/lisp/../share/exec/meshconv--forward=Y ---  
/home/luigi/master_thesis/rosWs/src/new_robot/meshes/lowerarm.STL  -o-o  /tmp/amino-luigi/povray/home/luigi/maste/tmp/amino-luigi/povray/home/luigi/master_thesis/rosWs/src/new_robot/meshes/wrist.S L.obj --up=Z--up=Z  --forward=Y--forward=Y
  /usr/local/blender-2.77a-linux-glibc211-x86_64/blender -b -P /home/luigi/amino/lisp/../share/exec/meshconv -- /home/luigi/master_thesis/rosWs/src/new_robot/meshes/upperarm.STL -o /tmp/amino-luigi/povray/home/luigi/master_thesis/rosWs/src/new_robot/meshes/upperarm.STL.obj --up=Z --forward=Y
  /usr/local/blender-2.77a-linux-glibc211-x86_64/blender -b -P /home/luigi/amino/lisp/../share/exec/meshconv -- /home/luigi/master_thesis/rosWs/src/new_robot/meshes/body.STL -o /tmp/amino-luigi/povray/home/luigi/master_thesis/rosWs/src/new_robot/meshes/body.STL.obj --up=Z --forward=Y
  1. It seems that a plan is not found because the file is never created or written. However if I run just the PDDL planning I get a result that is fine.

I'm attaching my URDF package if you want to check it. new_robot.zip

Thanks again for your help