Notebook

PyCRAM Presentation¶

In [1]:

import pycram

Bullet World¶

The BulletWorld is the internal simulation of PyCRAM. You can simulate different actions and reason about the outcome of different actions.

It is possible to spawn objects and robots into the BulletWorld, these objects can come from URDF, OBJ or STL files.

A BulletWorld can be created by simply creating an object of the BulletWorld class.

In [2]:

from pycram.bullet_world import BulletWorld, Object
from pycram.pose import Pose

world = BulletWorld()

Unknown tag "material" in /robot[@name='plane']/link[@name='planeLink']/collision[1]
Unknown tag "contact" in /robot[@name='plane']/link[@name='planeLink']
Unknown tag "material" in /robot[@name='plane']/link[@name='planeLink']/collision[1]
Unknown tag "contact" in /robot[@name='plane']/link[@name='planeLink']

The world can also be closed with the 'exit' method

In [37]:

world.exit()

The BulletWorld allows to render images from arbitrary positoins. In the following example we render images with the camera at the position [0.3, 0, 1] and pointing towards [1, 0, 1], so we are looking upwards along the x-axis.

The renderer returns 3 different kinds of images which are also shown at the left side of the BulletWorld window. These images are:

A RGB image which shows everything like it is rendered in the BulletWorld window, just from another perspective.
A depth image which consists of distance values from the camera towards the objects in the field of view.
A segmentation mask image which segments the image into the different objects displayed. The segmentation is done by assigning every pixel the unique id of the object that is displayed there.

In [3]:

from pycram.bullet_world_reasoning import _get_images_for_target

_get_images_for_target(Pose([1, 0, 1], [0, 0, 0, 1]), Pose([0.3, 0, 1], [0, 0, 0, 1]))

Out[3]:

[array([[[255, 255, 255, 255],
         [255, 255, 255, 255],
         [255, 255, 255, 255],
         ...,
         [255, 255, 255, 255],
         [255, 255, 255, 255],
         [255, 255, 255, 255]],
 
        [[255, 255, 255, 255],
         [255, 255, 255, 255],
         [255, 255, 255, 255],
         ...,
         [255, 255, 255, 255],
         [255, 255, 255, 255],
         [255, 255, 255, 255]],
 
        [[255, 255, 255, 255],
         [255, 255, 255, 255],
         [255, 255, 255, 255],
         ...,
         [255, 255, 255, 255],
         [255, 255, 255, 255],
         [255, 255, 255, 255]],
 
        ...,
 
        [[239, 239, 239, 255],
         [239, 239, 239, 255],
         [239, 239, 239, 255],
         ...,
         [239, 239, 239, 255],
         [239, 239, 239, 255],
         [239, 239, 239, 255]],
 
        [[239, 239, 239, 255],
         [239, 239, 239, 255],
         [239, 239, 239, 255],
         ...,
         [239, 239, 239, 255],
         [239, 239, 239, 255],
         [239, 239, 239, 255]],
 
        [[239, 239, 239, 255],
         [239, 239, 239, 255],
         [239, 239, 239, 255],
         ...,
         [239, 239, 239, 255],
         [239, 239, 239, 255],
         [239, 239, 239, 255]]], dtype=uint8),
 array([[0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,
         0.99999994],
        [0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,
         0.99999994],
        [0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,
         0.99999994],
        ...,
        [0.80473447, 0.80473447, 0.80473447, ..., 0.80473447, 0.80473447,
         0.80473447],
        [0.8031688 , 0.8031688 , 0.8031688 , ..., 0.8031688 , 0.8031688 ,
         0.8031688 ],
        [0.80160314, 0.80160314, 0.80160314, ..., 0.80160314, 0.80160314,
         0.80160314]], dtype=float32),
 array([[-1, -1, -1, ..., -1, -1, -1],
        [-1, -1, -1, ..., -1, -1, -1],
        [-1, -1, -1, ..., -1, -1, -1],
        ...,
        [ 1,  1,  1, ...,  1,  1,  1],
        [ 1,  1,  1, ...,  1,  1,  1],
        [ 1,  1,  1, ...,  1,  1,  1]], dtype=int32)]

Objects¶

Everything that is located inside the BulletWorld is an Object. Objects can be created from URDF, OBJ or STL files. Since everything is of type Object a robot might share the same methods as a milk (with some limitations).

Signature: Object:

Name
Type
Filename or Filepath

Optional:

Position
Orientation
World
Color
Ignore Cached Files

If there is only a filename and no path PyCRAM will check in the resource directory if there is a matching file.

In [4]:

milk = Object("Milk", "milk", "milk.stl")

Objects provide methods to change the position and rotation, change the color, attach other objects, set the state of joints if the objects has any or get the position and orientation of a link.

These methods are the same for every Object, however since some Objects may not have joints or more than one link methods related to these will not work.

In [5]:

milk.set_position(Pose([1, 0, 0]))

To remove an Object from the BulletWorld just call the 'remove' method on the Object.

In [6]:

milk.remove()

Since everything inside the BulletWorld is an Object, even a complex environment Object like the kitchen can be spawned in the same way as the milk.

In [7]:

kitchen = Object("kitchen", "environment", "kitchen.urdf")

Scalar element defined multiple times: limit
Scalar element defined multiple times: limit

Costmaps¶

Costmaps are a way to get positions with respect to certain criterias. The currently available costmaps are:

Occupancy Costmap
Visibility Costmap
Semantic Costmap
Gaussian Costmap

It is also possible to merge multiple costmaps to combine different criteria.

Visibility Costmaps¶

Visibility costmaps determine every position, around a target position, from which the target is visible. Visibility Costmaps are able to work with cameras that are moveable in height, for example if the robot has a movable torso.

In [8]:

import pycram.costmaps as cm
v = cm.VisibilityCostmap(1.27, 1.60, size=300, resolution=0.02, origin=Pose([0, 0, 0.1], [0, 0, 0, 1]))

In [9]:

v.visualize()

In [10]:

v.close_visualization()

Occupancy Costmap¶

Is valid for every position where the robot can be placed without colliding with an object.

In [11]:

o = cm.OccupancyCostmap(0.2, from_ros=False, size=300, resolution=0.02, origin=Pose([0, 0, 0.1], [0, 0, 0, 1]))

In [12]:

s = cm.SemanticCostmap(kitchen, "kitchen_island_surface", size=100, resolution=0.02)

g = cm.GaussianCostmap(200, 15, resolution=0.02)

You can visualize the costmap in the BulletWorld to get an impression what information is actually contained in the costmap. With this you could also check if the costmap was created correctly. Visualization can be done via the 'visualize' method of each costmap.

In [13]:

o.visualize()

In [14]:

o.close_visualization()

It is also possible to combine two costmap, this will result in a new costmap with the same size which contains the information of both previous costmaps. Combination is done by checking for each position in the two costmaps if they are zero, in this case to same position in the new costmap will also be zero in any other case the new position will be the normalized product of the two combined costmaps.

In [15]:

ov = o + v

In [16]:

ov.visualize()

In [17]:

ov.close_visualization()

Bullet World Reasoning¶

Allows for geometric reasoning in the BulletWorld. At the moment the following types of reasoning are supported:

Stable
Contact
Visible
Occluding
Reachable
Blocking
Supporting

To show the geometric reasoning we first a robot as well as the milk Object again.

In [18]:

import pycram.bullet_world_reasoning as btr
milk = Object("Milk", "milk", "milk.stl", pose=Pose([1, 0, 1]))
pr2 = Object("pr2", "robot", "pr2.urdf")

Unknown attribute "type" in /robot[@name='pr2']/link[@name='base_laser_link']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='laser_tilt_link']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='base_laser_link']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='laser_tilt_link']

We start with testing for visibility

In [20]:

milk.set_position(Pose([1,0,1]))
visible = btr.visible(milk, pr2.get_link_pose("wide_stereo_optical_frame"))
print(f"Milk visible: {visible}")

Milk visible: True

In [23]:

milk.set_position(Pose([1, 0, 0.05]))

plane = BulletWorld.current_bullet_world.objects[0]
contact = btr.contact(milk, plane)
print(f"Milk is in contact with the floor: {contact}")

Milk is in contact with the floor: False

In [22]:

milk.set_position(Pose([0.6, -0.5, 0.7]))

reachable = btr.reachable(milk, pr2, "r_gripper_tool_frame")
print(f"Milk is reachable for the PR2: {reachable}")

Milk is reachable for the PR2: True

Designator¶

Designator are symbolic descriptions of Actions, Motions, Objects or Locations. In PyCRAM the different types of designators are representet by a class which takes a description, the description then tells the designator what to do.

For example, let's look at a Motion Designator to move the robot to a specific location.

Motion Designator¶

When using a Motion Designator you need to specify which Process Module needs to be used, either the Process Module for the real or the simulated robot. This can be done either with a decorator which can be added to a function and then executes every designator in this function on the specified robot. The other possibility is a "with" scope which wraps a code piece.

These two ways can also be combined, you could write a function which should be executed on the real robot and in the function is a "with" scope which executes something on the simulated robot for reasoning purposes.

In [24]:

from pycram.designators.motion_designator import *
from pycram.process_module import simulated_robot, with_simulated_robot

description = MoveMotion(target=Pose([1, 0, 0], [0, 0, 0, 1]))

with simulated_robot:
    description.resolve().perform()
    

In [25]:

from pycram.process_module import with_simulated_robot

@with_simulated_robot
def move():
    MoveMotion(target=Pose([0, 0, 0], [0, 0, 0, 1])).resolve().perform()

move()

Other implemented Motion Designator descriptions are:

Pick up
Place
Accessing
Move TCP
Looking
Move Gripper
Detecting
Move Arm Joint
World State Detecting

Object Designator¶

Object Designator represent objects, these objects could either be from the BulletWorld or the real world. Object Designator are used, for example, by the PickUpAction to know which object should be picked up.

In [26]:

from pycram.designators.object_designator import *

milk_desig = BelieveObject(names=["Milk"])
milk_desig.resolve()

Out[26]:

BelieveObject.Object(name='Milk', type='milk', bullet_world_object=Object(world=<pycram.bullet_world.BulletWorld object at 0x7f249449bb80>, 
name=Milk, 
type=milk, 
color=[1, 1, 1, 1], 
id=4, 
path=/home/jonas/workspace/ros/src/pycram-1/src/pycram/../../resources/cached/milk.urdf, 
joints: ..., 
links: ..., 
attachments: ..., 
cids: ..., 
original_pose=header: 
  seq: 0
  stamp: 
    secs: 1690205352
    nsecs: 248950242
  frame_id: "map"
pose: 
  position: 
    x: 1
    y: 0
    z: 1
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0, 
base_origin_shift=[ 4.15300950e-04 -6.29518181e-05  8.96554102e-02], 
local_transformer=<pycram.local_transformer.LocalTransformer object at 0x7f2494430520>, 
tf_frame=Milk_4, 
urdf_object: ...), _pose=<bound method Object.get_pose of Object(world=<pycram.bullet_world.BulletWorld object at 0x7f249449bb80>, 
name=Milk, 
type=milk, 
color=[1, 1, 1, 1], 
id=4, 
path=/home/jonas/workspace/ros/src/pycram-1/src/pycram/../../resources/cached/milk.urdf, 
joints: ..., 
links: ..., 
attachments: ..., 
cids: ..., 
original_pose=header: 
  seq: 0
  stamp: 
    secs: 1690205352
    nsecs: 248950242
  frame_id: "map"
pose: 
  position: 
    x: 1
    y: 0
    z: 1
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0, 
base_origin_shift=[ 4.15300950e-04 -6.29518181e-05  8.96554102e-02], 
local_transformer=<pycram.local_transformer.LocalTransformer object at 0x7f2494430520>, 
tf_frame=Milk_4, 
urdf_object: ...)>)

Location Designator¶

Location Designator can create a position in cartisian space from a symbolic desctiption

In [27]:

from pycram.designators.location_designator import *
from pycram.designators.object_designator import *

robot_desig = BelieveObject(types=["robot"]).resolve()
milk_desig = BelieveObject(names=["Milk"]).resolve()
location_desig = CostmapLocation(target=milk_desig, visible_for=robot_desig)

print(f"Resolved: {location_desig.resolve()}")
print()

for pose in location_desig:
    print(pose)
                              

Resolved: CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205407
    nsecs:  53105115
  frame_id: "map"
pose: 
  position: 
    x: 0.78
    y: -0.14
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.27958765375807687
    w: 0.9601201715754407, reachable_arms=None)

CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205408
    nsecs: 363292217
  frame_id: "map"
pose: 
  position: 
    x: 0.78
    y: -0.14
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.27958765375807687
    w: 0.9601201715754407, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205408
    nsecs: 383865356
  frame_id: "map"
pose: 
  position: 
    x: 0.78
    y: -0.12
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.24708746132251994
    w: 0.968993181842469, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205408
    nsecs: 409863471
  frame_id: "map"
pose: 
  position: 
    x: 0.78
    y: -0.1
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.2116996959579716
    w: 0.9773347628787704, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205408
    nsecs: 439810991
  frame_id: "map"
pose: 
  position: 
    x: 0.78
    y: -0.08
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.17350299206578973
    w: 0.9848333421164306, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205408
    nsecs: 469390630
  frame_id: "map"
pose: 
  position: 
    x: 0.78
    y: -0.06
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.13273315102540856
    w: 0.9911518100769761, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205408
    nsecs: 498826742
  frame_id: "map"
pose: 
  position: 
    x: 0.78
    y: -0.04
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.08980559531591699
    w: 0.9959593139531121, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205408
    nsecs: 536117553
  frame_id: "map"
pose: 
  position: 
    x: 0.78
    y: -0.02
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.04531442117194118
    w: 0.9989727740203193, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205408
    nsecs: 615728139
  frame_id: "map"
pose: 
  position: 
    x: 0.78
    y: 0.0
    z: 0.0
  orientation: 
    x: -0.0
    y: 0.0
    z: 1.2246467991473532e-16
    w: -1.0, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205408
    nsecs: 685858964
  frame_id: "map"
pose: 
  position: 
    x: 0.76
    y: 0.04
    z: 0.0
  orientation: 
    x: -0.0
    y: 0.0
    z: 0.08248053154489343
    w: -0.9965926760297167, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205408
    nsecs: 819170713
  frame_id: "map"
pose: 
  position: 
    x: 0.76
    y: 0.0
    z: 0.0
  orientation: 
    x: -0.0
    y: 0.0
    z: 1.2246467991473532e-16
    w: -1.0, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205408
    nsecs: 913299083
  frame_id: "map"
pose: 
  position: 
    x: 0.76
    y: -0.04
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0824805315448933
    w: 0.9965926760297167, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205408
    nsecs: 991844177
  frame_id: "map"
pose: 
  position: 
    x: 0.76
    y: -0.08
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.1601822430069672
    w: 0.9870874576374967, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205409
    nsecs:  26787519
  frame_id: "map"
pose: 
  position: 
    x: 0.78
    y: 0.02
    z: 0.0
  orientation: 
    x: -0.0
    y: 0.0
    z: 0.045314421171941524
    w: -0.9989727740203193, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205409
    nsecs:  69372415
  frame_id: "map"
pose: 
  position: 
    x: 0.78
    y: 0.04
    z: 0.0
  orientation: 
    x: -0.0
    y: 0.0
    z: 0.08980559531591711
    w: -0.9959593139531121, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205409
    nsecs: 115944147
  frame_id: "map"
pose: 
  position: 
    x: 0.78
    y: 0.06
    z: 0.0
  orientation: 
    x: -0.0
    y: 0.0
    z: 0.13273315102540847
    w: -0.9911518100769761, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205409
    nsecs: 185698986
  frame_id: "map"
pose: 
  position: 
    x: 0.78
    y: 0.1
    z: 0.0
  orientation: 
    x: -0.0
    y: 0.0
    z: 0.21169969595797175
    w: -0.9773347628787704, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205409
    nsecs: 223813772
  frame_id: "map"
pose: 
  position: 
    x: 0.8
    y: -0.16
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.3310069414355006
    w: 0.9436283191604177, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205409
    nsecs: 257478475
  frame_id: "map"
pose: 
  position: 
    x: 0.8
    y: -0.14
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.30063938487909353
    w: 0.9537378886567945, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205409
    nsecs: 403844594
  frame_id: "map"
pose: 
  position: 
    x: 0.74
    y: 0.02
    z: 0.0
  orientation: 
    x: -0.0
    y: 0.0
    z: 0.03837651950358735
    w: -0.99926335004882, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205409
    nsecs: 461148500
  frame_id: "map"
pose: 
  position: 
    x: 0.8
    y: -0.1
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.2297529205473612
    w: 0.9732489894677302, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205409
    nsecs: 536622285
  frame_id: "map"
pose: 
  position: 
    x: 0.8
    y: -0.08
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.18910752115495127
    w: 0.9819563867314218, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205409
    nsecs: 615195989
  frame_id: "map"
pose: 
  position: 
    x: 0.8
    y: -0.06
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.14521314468540483
    w: 0.9894003954974829, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205409
    nsecs: 693008422
  frame_id: "map"
pose: 
  position: 
    x: 0.74
    y: -0.06
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.11315653826752592
    w: 0.9935771725675414, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205409
    nsecs: 730560541
  frame_id: "map"
pose: 
  position: 
    x: 0.8
    y: -0.04
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.09853761796664222
    w: 0.9951333266680702, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205409
    nsecs: 778785943
  frame_id: "map"
pose: 
  position: 
    x: 0.8
    y: -0.02
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.049813701880159676
    w: 0.998758526924799, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205409
    nsecs: 814137697
  frame_id: "map"
pose: 
  position: 
    x: 0.8
    y: 0.0
    z: 0.0
  orientation: 
    x: -0.0
    y: 0.0
    z: 1.2246467991473532e-16
    w: -1.0, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205409
    nsecs: 854085445
  frame_id: "map"
pose: 
  position: 
    x: 0.8
    y: 0.02
    z: 0.0
  orientation: 
    x: -0.0
    y: 0.0
    z: 0.0498137018801598
    w: -0.998758526924799, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205409
    nsecs: 891801834
  frame_id: "map"
pose: 
  position: 
    x: 0.8
    y: 0.04
    z: 0.0
  orientation: 
    x: -0.0
    y: 0.0
    z: 0.09853761796664212
    w: -0.9951333266680702, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205410
    nsecs:   1260519
  frame_id: "map"
pose: 
  position: 
    x: 0.8
    y: 0.08
    z: 0.0
  orientation: 
    x: -0.0
    y: 0.0
    z: 0.18910752115495139
    w: -0.9819563867314218, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205410
    nsecs:  41796445
  frame_id: "map"
pose: 
  position: 
    x: 0.8
    y: 0.12
    z: 0.0
  orientation: 
    x: -0.0
    y: 0.0
    z: 0.26693358189581157
    w: -0.9637149282107609, reachable_arms=None)
CostmapLocation.Location(pose=header: 
  seq: 0
  stamp: 
    secs: 1690205410
    nsecs: 106158733
  frame_id: "map"
pose: 
  position: 
    x: 0.8
    y: 0.1
    z: 0.0
  orientation: 
    x: -0.0
    y: 0.0
    z: 0.22975292054736132
    w: -0.9732489894677301, reachable_arms=None)

Action Designator¶

Action Designator are used to describe high-level actions. Action Designator are usually composed of other Designators to describe the high-level action in detail.

In [28]:

from pycram.designators.action_designator import *
from pycram.enums import Arms

with simulated_robot:
    ParkArmsAction([Arms.BOTH]).resolve().perform()

Making a simple plan¶

To get familiar with the PyCRAM Framework we will write a simple pick and place plan. This plan will let the robot grab a cereal box from the kitchen counter and place it on the kitchen island. This is a simple pick and place plan.

In [29]:

from pycram.designators.object_designator import *
cereal = Object("cereal", "cereal", "breakfast_cereal.stl", pose=Pose([1.4, 1, 0.95]))

In [30]:

cereal_desig = ObjectDesignatorDescription(names=["cereal"])
kitchen_desig = ObjectDesignatorDescription(names=["kitchen"])
robot_desig = ObjectDesignatorDescription(names=["pr2"]).resolve()
with simulated_robot:
    ParkArmsAction([Arms.BOTH]).resolve().perform()

    MoveTorsoAction([0.3]).resolve().perform()

    pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve()
    pickup_arm = pickup_pose.reachable_arms[0]

    NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform()

    PickUpAction(object_designator_description=cereal_desig, arms=[pickup_arm], grasps=["front"]).resolve().perform()

    ParkArmsAction([Arms.BOTH]).resolve().perform()

    place_island = SemanticCostmapLocation("kitchen_island_surface", kitchen_desig.resolve(), cereal_desig.resolve()).resolve()

    place_stand = CostmapLocation(place_island.pose, reachable_for=robot_desig, reachable_arm=pickup_arm).resolve()

    NavigateAction(target_locations=[place_stand.pose]).resolve().perform()

    PlaceAction(cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform()

    ParkArmsAction([Arms.BOTH]).resolve().perform()

Task Trees¶

Task trees are a hirachical representation of all Actions involved in a plan. The Task tree can later be used to inspect and restructre the execution order of Actions in the plan.

In [31]:

import pycram.task
import anytree
tt = pycram.task.task_tree
print(anytree.RenderTree(tt))

no_operation()
├── perform(ParkArmsAction, )
├── perform(ParkArmsAction, )
├── perform(MoveTorsoAction, )
├── perform(NavigateAction, )
├── perform(PickUpAction, )
├── perform(ParkArmsAction, )
├── perform(NavigateAction, )
├── perform(PlaceAction, )
└── perform(ParkArmsAction, )

In [30]:

from anytree.dotexport import RenderTreeGraph, DotExporter
RenderTreeGraph(tt).to_picture("tree.png")

ORM¶

In [34]:

import sqlalchemy
import sqlalchemy.orm
import pycram.orm.base
import pycram.orm.task
import pycram.orm.object_designator
import pycram.orm.motion_designator
import pycram.orm.action_designator

# set description of what we are doing
pycram.orm.base.MetaData().description = "Tutorial for getting familiar with the ORM."

engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:", echo=False)
session = sqlalchemy.orm.Session(bind=engine)
pycram.orm.base.Base.metadata.create_all(engine)
session.commit()


tt.insert(session)

Inserting TaskTree into database: 100%|██████████| 10/10 [00:00<00:00, 22.52it/s]

Out[34]:

pycram.orm.task.TaskTreeNode(1, 2023-07-24 14:51:45.453499, None, TaskStatus.RUNNING, None, None, 1, 1)

In [35]:

navigations = session.query(pycram.orm.action_designator.NavigateAction).all()
print(*navigations, sep="\n")

pycram.orm.action_designator.NavigateAction(Navigate, 4, 4, 1, 4, 4)
pycram.orm.action_designator.NavigateAction(Navigate, 7, 7, 1, 9, 9)

In [36]:

navigations = session.query(pycram.orm.action_designator.NavigateAction, 
                            pycram.orm.base.Position, 
                            pycram.orm.base.Quaternion).\
                                join(pycram.orm.base.Position).\
                                join(pycram.orm.base.Quaternion).all()
print(*navigations, sep="\n")

(pycram.orm.action_designator.NavigateAction(Navigate, 4, 4, 1, 4, 4), pycram.orm.base.Position(0.5799999999999998, 0.7, 0.0, 4, None), pycram.orm.base.Quaternion(0.0, 0.0, 0.17446654103248407, 0.9846631028225646, 4, None))
(pycram.orm.action_designator.NavigateAction(Navigate, 7, 7, 1, 9, 9), pycram.orm.base.Position(-1.8874999952316287, 0.959200015068054, 0.0, 9, None), pycram.orm.base.Quaternion(0.0, 0.0, 0.043989487236265, 0.9990319939885262, 9, None))