Improving Actions using Probabilities

In this tutorial we will look at probabilistic specifications of actions and especially at an advance plan to pick up objects. After this tutorial you will know: - Why are probabilities useful for robotics - How to use probabilistic models to specify actions - How to use probabilistic machine learning to improve actions

Let’s start by importing all the necessary modules.

import json

import numpy as np
import os
import random
import time

import pandas as pd
import sqlalchemy
import sqlalchemy.orm

import plotly
import plotly.graph_objects as go
import tqdm

from probabilistic_model.learning.jpt.jpt import JPT
from probabilistic_model.learning.jpt.variables import infer_variables_from_dataframe
from probabilistic_model.probabilistic_circuit.probabilistic_circuit import ProbabilisticCircuit
from random_events.events import Event

import pycram.orm.base
from pycram.designators.actions.actions import MoveTorsoActionPerformable
from pycram.plan_failures import PlanFailure
from pycram.designators.object_designator import ObjectDesignatorDescription
from pycram.bullet_world import BulletWorld, Object
from pycram.robot_descriptions import robot_description
from pycram.enums import ObjectType, Arms, Grasp
from pycram.pose import Pose
from pycram.ros.viz_marker_publisher import VizMarkerPublisher
from pycram.process_module import ProcessModule, simulated_robot
from pycram.resolver.probabilistic.probabilistic_action import MoveAndPickUp
from pycram.task import task_tree, reset_tree

ProcessModule.execution_delay = False
np.random.seed(69)
random.seed(69)

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']
[WARN] [1711028821.218958]: Could not import RoboKudo messages, RoboKudo interface could not be initialized

Next, we connect to a database where we can store and load robot experiences.

pycrorm_uri = os.getenv('PYCRORM_URI')
pycrorm_uri = "mysql+pymysql://" + pycrorm_uri
engine = sqlalchemy.create_engine('sqlite:///:memory:')
session = sqlalchemy.orm.sessionmaker(bind=engine)()
pycram.orm.base.Base.metadata.create_all(engine)

Now we construct an empty world with just a floating milk, where we can learn about PickUp actions.

world = BulletWorld("DIRECT")
robot = Object(robot_description.name, ObjectType.ROBOT, robot_description.name + ".urdf")
milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9]))
viz_marker_publisher = VizMarkerPublisher()
milk_description = ObjectDesignatorDescription(types=[ObjectType.MILK]).ground()

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 attribute "type" in /robot[@name='pr2']/link[@name='base_laser_link']
Unknown tag "contact" in /robot[@name='plane']/link[@name='planeLink']
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']

Next, we create a default, probabilistic model that describes how to pick up objects. We visualize the default policy. The default policy tries to pick up the object by standing close to it, but not too close.

fpa = MoveAndPickUp(milk_description, arms=[Arms.LEFT.value, Arms.RIGHT.value, Arms.BOTH.value], grasps=[Grasp.FRONT.value, Grasp.LEFT.value, Grasp.RIGHT.value, Grasp.TOP.value])
p_xy = fpa.policy.marginal([fpa.variables.relative_x, fpa.variables.relative_y])
fig = go.Figure(p_xy.root.plot(), p_xy.root.plotly_layout())
fig.update_layout(title="Marginal View of relative x and y position of the robot with respect to the object.")
fig.show()

ProbabilisticCircuit with 15 nodes and 14 edges

Next, we will perform pick up tasks using the default policy and observe the success rate. The robot will now experiment with the behaviour specified by the default policy and observe his success rate in doing so. After finishing the experiments, we insert the results into the database.

pycram.orm.base.ProcessMetaData().description = "Experimenting with Pick Up Actions"
fpa.sample_amount = 500
batches = 1
for batch in tqdm.tqdm(range(batches), total=batches, desc="Processing Batches", disable=True):
    with simulated_robot:
        fpa.batch_rollout()
    task_tree.insert(session)
    reset_tree()
session.commit()

100%|██████████| 500/500 [00:30<00:00, 16.24it/s, Success Probability=0.228]
Inserting TaskTree into database: 100%|██████████| 5049/5049 [01:15<00:00, 66.72it/s]

Let’s query the data that is needed to learn a pick up action and have a look at it.

samples = pd.read_sql(fpa.query_for_database(), engine)
samples

	arm	grasp	relative_x	relative_y
0	left	left	-0.546259	-0.356499
1	left	right	-0.203552	-0.605282
2	left	left	-0.230694	-0.350203
3	left	front	-0.366892	0.442297
4	left	right	-0.296431	0.054815
...	...	...	...	...
109	right	front	-0.014655	0.674613
110	right	right	0.039657	0.299975
111	left	front	-0.173449	0.398118
112	left	left	-0.155267	0.715950
113	left	right	0.148996	0.282944

114 rows × 4 columns

We can now learn a probabilistic model from the data. We will use the JPT algorithm to learn a model from the data.

variables = infer_variables_from_dataframe(samples, scale_continuous_types=False)
model = JPT(variables, min_samples_leaf=25)
model.fit(samples)
model = model.probabilistic_circuit

arm, grasp, relative_x, relative_y = model.variables

Let’s have a look at how the model looks like. We will visualize the model density when we condition on grasping the object from the front with the left arm.

event = Event({arm:"left", grasp:"front"})
conditional_model, conditional_probability = model.conditional(event)
p_xy = conditional_model.marginal([relative_x, relative_y])
fig = go.Figure(p_xy.root.plot_2d(), p_xy.root.plotly_layout())
fig.show()

Let’s make a monte carlo estimate on the success probability of the new model.

fpa.policy = model
fpa.sample_amount = 100
with simulated_robot:
    fpa.batch_rollout()

100%|██████████| 100/100 [00:06<00:00, 14.76it/s, Success Probability=0.31]

We can see, that our new and improved model has a success probability of 60% as opposed to the 30% from the standard policy.

Next, we put the learned model to the test in a complex environment, where the milk is placed in a difficult to access area.

kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "apartment.urdf")

milk.set_pose(Pose([0.5, 3.15, 1.04]))
milk_description = ObjectDesignatorDescription(types=[ObjectType.MILK]).ground()
fpa = MoveAndPickUp(milk_description, arms=[Arms.LEFT.value, Arms.RIGHT.value, Arms.BOTH.value], grasps=[Grasp.FRONT.value, Grasp.LEFT.value, Grasp.RIGHT.value, Grasp.TOP.value], policy=model)
fpa.sample_amount = 200

Unknown tag "material" in /robot[@name='apartment']/link[@name='coffe_machine']/collision[1]
Unknown tag "material" in /robot[@name='apartment']/link[@name='coffe_machine']/collision[1]

p_xy = model.marginal([relative_x, relative_y])
fig = go.Figure(p_xy.root.plot_2d(), p_xy.root.plotly_layout())
fig.show()

Let’s look at the density of the relative x and y position of the robot with respect to the milk. We can see that he would like to access the object from the right front area.

grounded_model = fpa.ground_model()
p_xy = grounded_model.marginal([relative_x, relative_y]).simplify()
fig = go.Figure(p_xy.root.plot_2d(), p_xy.root.plotly_layout())
fig.update_layout(title="Marginal View of relative x and y position with respect to the milk",
                  xaxis_range=[-1, 1], yaxis_range=[-1, 1])
fig.show()

Finally, we observe our improved plan in action.

from pycram.designators.actions.actions import ParkArmsActionPerformable

world.reset_bullet_world()
milk.set_pose(Pose([0.5, 3.15, 1.04]))
with simulated_robot:

    MoveTorsoActionPerformable(0.3).perform()
    for sample in fpa:
        try:
            ParkArmsActionPerformable(Arms.BOTH).perform()
            sample.perform()
            break
        except PlanFailure as e:
            continue

# world.exit()
# viz_marker_publisher._stop_publishing()