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.enums import ObjectType
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 [4]:

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[4]:

[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 [5]:

milk = Object("Milk", ObjectType.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 [6]:

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

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

In [7]:

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 [8]:

kitchen = Object("kitchen", ObjectType.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 [9]:

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 [10]:

v.visualize()

In [11]:

v.close_visualization()

Occupancy Costmap¶

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

In [12]:

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

In [13]:

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 [14]:

o.visualize()

In [15]:

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 [16]:

ov = o + v

In [17]:

ov.visualize()

In [18]:

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 [19]:

import pycram.bullet_world_reasoning as btr
milk = Object("Milk", ObjectType.MILK, "milk.stl", pose=Pose([1, 0, 1]))
pr2 = Object("pr2", ObjectType.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']

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}")

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']

Milk visible: True

In [21]:

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: True

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 [23]:

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 [24]:

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 [25]:

from pycram.designators.object_designator import *

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

Out[25]:

BelieveObject.Object(name='Milk', type=<ObjectType.MILK: 1>, bullet_world_object=Object(world=<pycram.bullet_world.BulletWorld object at 0x7fa4da174760>, 
local_transformer=<pycram.local_transformer.LocalTransformer object at 0x7fa4da174b50>, 
name=Milk, 
type=ObjectType.MILK, 
color=[1, 1, 1, 1], 
id=4, 
path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/../../resources/cached/milk.urdf, 
joints: ..., 
links: ..., 
attachments: ..., 
cids: ..., 
original_pose=header: 
  seq: 0
  stamp: 
    secs: 1699448180
    nsecs: 830921888
  frame_id: "map"
pose: 
  position: 
    x: 1.0
    y: 0.0
    z: 1.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0, 
tf_frame=Milk_4, 
urdf_object: ..., 
_current_pose=header: 
  seq: 0
  stamp: 
    secs: 1699448184
    nsecs: 735546827
  frame_id: "map"
pose: 
  position: 
    x: 0.6
    y: -0.5
    z: 0.7
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0, 
_current_link_poses={'milk_main': header: 
  seq: 0
  stamp: 
    secs: 1699448184
    nsecs: 735546827
  frame_id: "map"
pose: 
  position: 
    x: 0.6
    y: -0.5
    z: 0.7
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0}, 
_current_link_transforms={'milk_main': header: 
  seq: 0
  stamp: 
    secs: 1699448185
    nsecs: 354960680
  frame_id: "map"
child_frame_id: "Milk_4"
transform: 
  translation: 
    x: 0.6
    y: -0.5
    z: 0.7
  rotation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0}, 
_current_joint_states={}, 
base_origin_shift=[ 4.15300950e-04 -6.29518181e-05  8.96554102e-02], 
link_to_geometry={'milk_main': <urdf_parser_py.urdf.Mesh object at 0x7fa4d95f5310>}), _pose=<bound method Object.get_pose of Object(world=<pycram.bullet_world.BulletWorld object at 0x7fa4da174760>, 
local_transformer=<pycram.local_transformer.LocalTransformer object at 0x7fa4da174b50>, 
name=Milk, 
type=ObjectType.MILK, 
color=[1, 1, 1, 1], 
id=4, 
path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/../../resources/cached/milk.urdf, 
joints: ..., 
links: ..., 
attachments: ..., 
cids: ..., 
original_pose=header: 
  seq: 0
  stamp: 
    secs: 1699448180
    nsecs: 830921888
  frame_id: "map"
pose: 
  position: 
    x: 1.0
    y: 0.0
    z: 1.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0, 
tf_frame=Milk_4, 
urdf_object: ..., 
_current_pose=header: 
  seq: 0
  stamp: 
    secs: 1699448184
    nsecs: 735546827
  frame_id: "map"
pose: 
  position: 
    x: 0.6
    y: -0.5
    z: 0.7
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0, 
_current_link_poses={'milk_main': header: 
  seq: 0
  stamp: 
    secs: 1699448184
    nsecs: 735546827
  frame_id: "map"
pose: 
  position: 
    x: 0.6
    y: -0.5
    z: 0.7
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0}, 
_current_link_transforms={'milk_main': header: 
  seq: 0
  stamp: 
    secs: 1699448185
    nsecs: 354960680
  frame_id: "map"
child_frame_id: "Milk_4"
transform: 
  translation: 
    x: 0.6
    y: -0.5
    z: 0.7
  rotation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0}, 
_current_joint_states={}, 
base_origin_shift=[ 4.15300950e-04 -6.29518181e-05  8.96554102e-02], 
link_to_geometry={'milk_main': <urdf_parser_py.urdf.Mesh object at 0x7fa4d95f5310>})>)

Location Designator¶

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

In [ ]:

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

robot_desig = BelieveObject(types=[ObjectType.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)
                              

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 [29]:

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 [30]:

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

In [31]:

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 [32]:

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 [33]:

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, 143.50it/s]

Out[34]:

pycram.orm.task.TaskTreeNode(1, 2023-11-08 13:55:09.816577, 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, 1.28, 0.0, 4, None), pycram.orm.base.Quaternion(0.0, 0.0, 0.16378361324016077, -0.9864962889104031, 4, None))
(pycram.orm.action_designator.NavigateAction(Navigate, 7, 7, 1, 9, 9), pycram.orm.base.Position(-1.9074999952316287, 0.779200015068054, 0.0, 9, None), pycram.orm.base.Quaternion(0.0, 0.0, 0.16439898730535735, 0.9863939238321437, 9, None))