This step involves importing the required modules and initializing key components for our tasks.
from pycram.bullet_world import BulletWorld, Object
from pycram.pose import Transform
from pycram.local_transformer import LocalTransformer
Every robot simulation requires a world where it can interact. This world serves as the playground where the robot performs tasks. Let's start by creating this world.
# Create an instance of the BulletWorld
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 be closed by calling the exit method of the world, but don't call this method yet since it would close the world.
world.exit()
For our robot to perform meaningful tasks, we need to populate its world with objects. In this section, we'll add a variety of objects, from a simple floor plane to kitchen setups and items like milk and bowls. These objects will be used in subsequent tasks.
from pycram.bullet_world import Object
from pycram.enums import ObjectType
plane = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=world)
kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf")
milk = Object("milk", ObjectType.MILK, "milk.stl", Pose([0.9, 1, 0.95]))
bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", Pose([1.6, 1, 0.90]))
Unknown tag "material" in /robot[@name='plane']/link[@name='planeLink']/collision[1] Unknown tag "contact" in /robot[@name='plane']/link[@name='planeLink'] Scalar element defined multiple times: limit Unknown tag "material" in /robot[@name='plane']/link[@name='planeLink']/collision[1] Unknown tag "contact" in /robot[@name='plane']/link[@name='planeLink'] Scalar element defined multiple times: limit
The local transformer is implemented as a singelton, meaing regardless of how much and where an instance is created it will always be the same instance. This is done since the local transfomer collects all transformations between frames and would there always be a new instance, all transformations woulb need to be re-collected.
from pycram.local_transformer import LocalTransformer
local_transformer = LocalTransformer()
print(local_transformer)
new_local_transformer = LocalTransformer()
print(new_local_transformer)
<pycram.local_transformer.LocalTransformer object at 0x7f75a0b75ee0> <pycram.local_transformer.LocalTransformer object at 0x7f75a0b75ee0>
Now that we have our world set up, let's perform some transformations. We'll use the LocalTransformer to transform poses relative to our objects.
from pycram.local_transformer import LocalTransformer
from pycram.pose import Pose
l = LocalTransformer()
test_pose = Pose([1, 1, 1], [0, 0, 0, 1], "map")
transformed_pose = l.transform_to_object_frame(test_pose, milk)
print(transformed_pose)
new_pose = l.transform_pose(transformed_pose, "map")
print(new_pose)
header:
seq: 0
stamp:
secs: 1699448290
nsecs: 552922010
frame_id: "milk_4"
pose:
position:
x: 0.09999999999999998
y: 0.0
z: 0.050000000000000044
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
header:
seq: 0
stamp:
secs: 1699448290
nsecs: 553559541
frame_id: "map"
pose:
position:
x: 1.0
y: 1.0
z: 1.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
In the above code, we first transformed a pose to the object frame of the milk object, and then we transformed it back to the map frame. This demonstrates how we can easily manipulate poses relative to objects in our environment. You can also transform poses relative to other poses. by using the transform_pose method. Further u can set a Transform.
from pycram.pose import Transform
l.setTransform(Transform([1, 1, 1], [0, 0, 0, 1], "map", "test_frame"))
p = Pose()
transformed_pose = l.transform_pose(p, "test_frame")
You can also set a Pose to an object and update the transforms for that object. However, this is usually done in the background when necessary so you should only use this method if there is something wrong with the Transformation.
milk.set_pose(Pose([1, 2, 1]))
l.update_transforms_for_object(milk)
As you can see in the example above the frame_id of the object is not 'milk' but 'milk_4'. This is done since frame_ids need to be unique, however, the name of an Object does not. To solve this problem the name of an Object is concatenated with a unique id therefore making it unique.
Furthermore, links of an Object are represented by the Object frame_id + '/' + link name. Since link names need to be unique for an URDF this is no problem.
These frames need to be used in whenever you are transforming something with the local transformer. To get the base frame of an Object, meaning the frame name without any link there is the attribute tf_frame and for the frame of a link there is the method get_link_tf_frame.
print(milk.tf_frame)
print(kitchen.get_link_tf_frame("kitchen_island_surface"))
milk_4 kitchen_3/kitchen_island_surface