Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
tutorials:advanced:bullet_world_robot [2015/09/11 16:23] – gkazhoyatutorials:advanced:bullet_world_robot [2015/09/17 15:54] (current) – [Boxy Prolog description] gkazhoya
Line 1: Line 1:
 ====== Setting up the Bullet world with a new robot ====== ====== Setting up the Bullet world with a new robot ======
  
-This tutorial will walk you though creating your own repo / metapackage from scratch that will use CRAM and Bullet world with your own robots. In this tutorial we will use the Boxy robot as an example.+This tutorial will walk you though creating your own repo / metapackage from scratch that will use CRAM and Bullet world with your own robots. In this tutorial we will use the [[http://wiki.ros.org/hector_quadrotor|Hector Quadrotor]] robot as an example.
  
 The starting point of this tutorial is an installation of CRAM including projection with PR2. If you followed the installation manual, you should have all the necessary packages already. The starting point of this tutorial is an installation of CRAM including projection with PR2. If you followed the installation manual, you should have all the necessary packages already.
Line 9: Line 9:
  
 The first thing that we actually need is the robot, more precisely, its URDF description. The first thing that we actually need is the robot, more precisely, its URDF description.
-For Boxy it is located in a repo on Github, so let's clone it into our ROS workspace:+For the quadrotor it is located in a repo on Github and if you're lucky it's also release as a Debian, so let's install it / clone it into our ROS workspace:
  
 <code bash> <code bash>
 +sudo apt-get install ros-YOUR-ROS-DISTRO-hector-quadrotor-description
 +rospack profile
 +# or
 cd ROS_WORKSPACE_FOR_LISP_CODE cd ROS_WORKSPACE_FOR_LISP_CODE
 cd src cd src
-git clone https://github.com/code-iai/iai_robots.git+git clone https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor.git
 cd .. cd ..
 catkin_make catkin_make
 </code> </code>
  
-CRAM takes the URDF descriptions of the robots from the ROS parameter server, i.e., you will need to upload the URDFs of your robots as a ROS parameter. For Boxy there is a launch file doing that, you will find it here:+CRAM takes the URDF descriptions of the robots from the ROS parameter server, i.e., you will need to upload the URDFs of your robots as a ROS parameter. For our robot there is a launch file doing that, you will find it here:
  
 <code bash> <code bash>
-roscd iai_boxy_description/launch/ && ls -l+roscd hector_quadrotor_description/launch/ && ls -l
 </code> </code>
  
-It's called ''upload_boxy.launch'' and has the following contents:+It's called ''xacrodisplay_quadrotor_base.launch'' and has the following contents:
  
 <code xml> <code xml>
 <launch> <launch>
-  <arg name="urdf-name" default="boxy_description.urdf.xacro"/> +    <param name="robot_description" command="$(find xacro)/xacro.py $(find hector_quadrotor_description)/urdf/quadrotor.urdf.xacro" /> 
-  <arg name="urdf-path" default="$(find iai_boxy_description)/robots/$(arg urdf-name)"/> +    <param name="use_gui" value="True"/> 
- +     
-  <param +    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node> 
-    name="robot_description" +    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /> 
-    command="$(find xacro)/xacro.py '$(arg urdf-path)'" />+    <node name="rviz" pkg="rviz" type="rviz" />
 </launch> </launch>
 </code> </code>
  
-As we will need to know the names of the TF frames later, we will launch a general file that includes uploading the URDF as well as a robot state publisher:+It includes a robot state publisher to publish the TF, we will need it to know the names of the TF frames of the robot later. 
 +It also starts Rviz:
  
 <code bash> <code bash>
-roslaunch iai_boxy_description upload_boxy.launch +roslaunch hector_quadrotor_description xacrodisplay_quadrotor_base.launch 
 </code> </code>
  
 (It also starts a GUI to play with the joint angles but let's ignore that.) (It also starts a GUI to play with the joint angles but let's ignore that.)
  
-Let's check if the URDF's on the parameter server using RViz:+Let's check if the URDF's on the parameter server using RViz. For that, in RViz click:
  
 <code bash> <code bash>
-rosrun rviz rviz +Add -> RobotModel -> Robot Description: robot_description
-Add -> Robot Model -> Robot description: robot description+
 Add -> TF Add -> TF
 </code> </code>
  
-To be able to see the TF frames choose ''base_footprint'' as the global fixed frame.+To be able to see the TF frames choose ''base_link'' as the global fixed frame.
  
 {{ :tutorials:advanced:boxy_rviz.png?direct&800 |}} {{ :tutorials:advanced:boxy_rviz.png?direct&800 |}}
- 
- 
-===== Loading the world ===== 
- 
-Now let's try to load the same robot also in our Bullet world. 
-We will spawn the floor, the robot, and a couple of household objects. 
- 
-For that, we  
-  - load the bullet reasoning package in the REPL 
-  - start a ROS node 
-  - spawn the robot and the floor 
-  - spawn a big box for a table and a couple of household objects to go on top 
- 
-<code lisp> 
-;; step 1 
-(asdf:load-system :cram-bullet-reasoning) 
-(in-package :btr) 
- 
-;; step 2 
-(roslisp:start-ros-node "cram_bullet_boxy") 
- 
-;; step 3 
-(let ((robot-urdf (cl-urdf:parse-urdf (roslisp:get-param "robot_description")))) 
-  (prolog 
-     `(and 
-       (clear-bullet-world) 
-       (bullet-world ?w) 
-       (debug-window ?w) 
-       (robot ?robot) 
-       (assert (object ?w :static-plane floor ((0 0 0) (0 0 0 1)) 
-                       :normal (0 0 1) :constant 0 
-                       :disable-collisions-with (?robot))) 
-       (assert (object ?w :urdf ?robot ((0 0 0) (0 0 0 1)) :urdf ,robot-urdf))))) 
- 
-;; step 4 
-(prolog 
- `(and 
-   (bullet-world ?w) 
-   (assert (object ?w :box table-0 ((2 0 0.5) (0 0 0 1)) 
-                   :size (1 1.5 1) :mass 10.0)) 
-   (assert (object ?w :mesh pot-0 ((2 0 1.1) (0 0 0 1)) :mass 0.5 :color (0.1 0.2 0.3) 
-                   :mesh :pot)) 
-   (assert (object ?w :mesh mug-0 ((2.2 -0.2 1.07) (0 0 0 1)) :mass 0.2 :color (0.8 0.3 0) 
-                   :mesh :mug)) 
-   (assert (object ?w :mesh mug-1 ((2.3 0 1.07) (0 0 0 1)) :mass 0.2 :color (0.5 0.8 0) 
-                   :mesh :mug)))) 
-</code> 
  
  
 ===== Directory / file setup ===== ===== Directory / file setup =====
  
-First of all, let's create a directory for our code and the metapackage: in the ''src'' of a catkin workspace create a new directory, we will call it ''cram_boxy''. Therein create a catkin metapackage with the same name:+Now let's setup a directory structure for our own packages: a directory (repo) and the metapackage inside: in the ''src'' of a catkin workspace create a new directory, we will call it ''cram_quadrotor''. Therein create a catkin metapackage with the same name:
  
 <code bash> <code bash>
-mkdir cram_boxy && cd cram_boxy +mkdir cram_quadrotor && cd cram_quadrotor 
-catkin_create_pkg cram_boxy && cd cram_boxy+catkin_create_pkg cram_quadrotor && cd cram_quadrotor
 </code> </code>
  
Line 119: Line 75:
 <code cmake> <code cmake>
 cmake_minimum_required(VERSION 2.8.3) cmake_minimum_required(VERSION 2.8.3)
-project(cram_boxy)+project(cram_quadrotor)
 find_package(catkin REQUIRED) find_package(catkin REQUIRED)
 catkin_metapackage() catkin_metapackage()
 </code> </code>
  
-The first ROS package we will create will be the Prolog description of our robot. We will call it ''cram_boxy_description''. +The first ROS package we will create will be the Prolog description of our robot. We will call it ''cram_quadrotor_description''. 
-Go back to the root of your ''cram_boxy'' directory and create the package. The dependencies we will definitely need for now will be ''cram_prolog'' (as we're defining Prolog rules) and ''cram_robot_interfaces'' (which defines the names of predicates for describing robots in CRAM):+Go back to the root of your ''cram_quadrotor'' directory and create the package. The dependencies we will definitely need for now will be ''cram_prolog'' (as we're defining Prolog rules) and ''cram_robot_interfaces'' (which defines the names of predicates for describing robots in CRAM):
  
 <code bash> <code bash>
 cd .. cd ..
-catkin_create_pkg cram_boxy_knowledge cram_prolog cram_robot_interfaces+catkin_create_pkg cram_quadrotor_knowledge cram_prolog cram_robot_interfaces
 </code> </code>
  
-Now let's create the corresponding ASD file called ''cram_boxy_knowledge/cram-boxy-knowledge.asd'':+Now let's create the corresponding ASDF file called ''cram_quadrotor_knowledge/cram-quadrotor-knowledge.asd'':
  
 <code lisp> <code lisp>
 ;;; You might want to add a license header first ;;; You might want to add a license header first
  
-(defsystem cram-boxy-knowledge+(defsystem cram-quadrotor-knowledge
   :author "Your Name"   :author "Your Name"
   :license "BSD"   :license "BSD"
Line 146: Line 102:
     :components     :components
     ((:file "package")     ((:file "package")
-     (:file "boxy-knowledge" :depends-on ("package"))))))+     (:file "quadrotor-knowledge" :depends-on ("package"))))))
 </code> </code>
  
-Now create the corresponding ''src'' directory and a ''cram_boxy_knowledge/src/package.lisp'' file in it:+Now create the corresponding ''src'' directory and a ''cram_quadrotor_knowledge/src/package.lisp'' file in it:
  
 <code lisp> <code lisp>
Line 156: Line 112:
 (in-package :cl-user) (in-package :cl-user)
  
-(defpackage cram-boxy-knowledge+(defpackage cram-quadrotor-knowledge
   (:use #:common-lisp   (:use #:common-lisp
         #:cram-prolog         #:cram-prolog
Line 162: Line 118:
 </code> </code>
  
 +Also, an empty ''cram_quadrotor/cram_quadrotor_knowledge/src/quadrotor-knowledge.lisp''.
  
 +Now, let's compile our new packages (''catkin_make'') and load them through the REPL (you need to restart your REPL after you create a new ROS package so that it finds it):
 +
 +<code lisp>
 +CL-USER> ,
 +ros-load-system
 +cram_quadrotor_knowledge
 +cram-quadrotor-knowledge
 +</code>
  
  
 ===== Boxy Prolog description ===== ===== Boxy Prolog description =====
  
-Now that we have a URDF on the ROS parameter server, let's describe our robot also in Prolog.+Now, let's describe our robot in Prolog, such that we could do some reasoning with it.
  
-We create a file ''cram_boxy/cram_boxy_knowledge/src/boxy-knowledge.lisp'' and fill it in with the simplest descriptions, which are the ones relevant for navigation and vision. We will leave out manipulation for now as it is too complex for the scope of this tutorial.+We fill in the file ''cram_quadrotor/cram_quadrotor_knowledge/src/quadrotor-knowledge.lisp'' with some simple descriptions of our robot, which are the ones relevant for navigation and vision. For robots with arms one could also implement manipulation-specific functionality but this is too complex for the scope of this tutorial.
  
 As can be seen from the TF tree, our robot has 3 camera frames, 1 depth and 1 RGB frame from a Kinect camera, and an RGB frame from a Kinect2 camera. As can be seen from the TF tree, our robot has 3 camera frames, 1 depth and 1 RGB frame from a Kinect camera, and an RGB frame from a Kinect2 camera.
Line 196: Line 161:
 </code> </code>
  
 +Finally, compile the file (''Ctrl-c Ctrl-k'').
 +
 +
 +
 +===== Loading the world =====
 +
 +Now let's try to load our robot also into the Bullet world.
 +We will spawn the floor, the robot, and a couple of household objects.
 +
 +For that, we 
 +  - load the bullet reasoning package in the REPL
 +  - start a ROS node
 +  - spawn the robot and the floor
 +  - spawn a big box for a table and a couple of household objects to go on top
 +
 +<code lisp>
 +;; step 1
 +(asdf:load-system :cram-bullet-reasoning)
 +(in-package :btr)
 +
 +;; step 2
 +(roslisp:start-ros-node "cram_bullet_boxy")
 +
 +;; step 3
 +(let ((robot-urdf (cl-urdf:parse-urdf (roslisp:get-param "robot_description"))))
 +  (prolog
 +     `(and
 +       (clear-bullet-world)
 +       (bullet-world ?w)
 +       (debug-window ?w)
 +       (robot ?robot)
 +       (assert (object ?w :static-plane floor ((0 0 0) (0 0 0 1))
 +                       :normal (0 0 1) :constant 0
 +                       :disable-collisions-with (?robot)))
 +       (assert (object ?w :urdf ?robot ((0 0 0) (0 0 0 1)) :urdf ,robot-urdf)))))
 +
 +;; step 4
 +(prolog
 + `(and
 +   (bullet-world ?w)
 +   (assert (object ?w :box table-0 ((2.1 0 0.5) (0 0 0 1))
 +                   :size (0.7 1.5 1) :mass 10.0))
 +   (assert (object ?w :mesh pot-0 ((2 0 1.1) (0 0 0 1)) :mass 0.5 :color (0.1 0.2 0.3)
 +                   :mesh :pot))
 +   (assert (object ?w :mesh mug-0 ((2.2 -0.2 1.07) (0 0 0 1)) :mass 0.2 :color (0.8 0.3 0)
 +                   :mesh :mug))
 +   (assert (object ?w :mesh mug-1 ((2.3 0 1.07) (0 0 0 1)) :mass 0.2 :color (0.5 0.8 0)
 +                   :mesh :mug))))
 +</code>
 +
 +{{ :tutorials:advanced:boxy_bullet.png?direct&800 |}}
 +
 +
 +
 +===== Reasoning with Boxy =====
 +
 +Let's try some reasoning:
 +
 +<code lisp>
 +BTR> (prolog '(visible ?world ?robot pot-0))
 +NIL
 +</code>
 +
 +The query asks if the object ''pot-0'' is visible to the robot.
 +The answer is "No", as our robot is very tall and look straight ahead.
 +Now let's put the object higher and try again:
 +
 +<code lisp>
 +BTR> (prolog '(and 
 +               (bullet-world ?w)
 +               (assert (object-pose ?w pot-0 ((2 0 2) (0 0 0 1))))))
 +BTR> (prolog '(visible ?world ?robot pot-0))
 +(((?WORLD . #<BT-REASONING-WORLD {100D78C5F3}>)
 +  (?ROBOT . CRAM-BOXY-KNOWLEDGE::BOXY))
 + . #S(CRAM-UTILITIES::LAZY-CONS-ELEM :GENERATOR #<CLOSURE # {101275A8FB}>))
 +</code>
 +
 +Now it is visible.
 +
 +Let's put the pot down:
 +
 +<code lisp>
 +BTR> (simulate *current-bullet-world* 50)
 +</code>
 +
 +Now let's do something more complex: let's generate a visibility costmap, that will generate poses from where Boxy will be able to see the pot.
 +As we're going to be using costmaps, we will need to define costmap metadata first:
 +
 +<code lisp>
 +BTR> (def-fact-group costmap-metadata ()
 +       (<- (location-costmap:costmap-size 12 12))
 +       (<- (location-costmap:costmap-origin -6 -6))
 +       (<- (location-costmap:costmap-resolution 0.05))
 +
 +       (<- (location-costmap:costmap-padding 0.35))
 +       (<- (location-costmap:costmap-manipulation-padding 0.35))
 +       (<- (location-costmap:costmap-in-reach-distance 1.0))
 +       (<- (location-costmap:costmap-reach-minimal-distance 0.1)))
 +</code>
 +
 +We should also load the ''cram_bullet_reasoning_designators'' package, as visibility costmap is implemented in it:
 +<code lisp>
 +BTR> (asdf:load-system :cram-bullet-reasoning-designators)
 +</code>
 +
 +Now let's create a location designator for a pose to see the ''mug-0'' object, which is the orange mug in the corner, and try to resolve it:
 +
 +<code lisp>
 +BTR> (let ((location-to-see (desig:make-designator :location '((:to :see)
 +                                                               (:object mug-0)))))
 +       (desig:reference location-to-see))
 +</code>
  
 +{{ :tutorials:advanced:boxy_visibility_cm.png?direct&800 |}}