Pose

Poses in PyCRAM are represented by the Pose class which inherits from the PoseStamped message of ROS. This makes PyCRAMs poses compatible with everything in ROS like services, topics or TF.

This notebook will provide an overview about poses, how to use them and what they can do. We will start by simply creating a pose.

Before we start a few words about naming convention of Poses in PyCRAM. Naming convention is similar to the PoseStamped message so if you are familiar with that this should be easy.

• Position: A position means the position in cartesian space, so the x, y, and z coordinates.

• Orientation: An orientation is the rotation in all three axes represented as a quaternion with x, y, z, w.

• Pose: A pose is the combination of a position and an orientation. Poses in PyCRAM also contain a frame of reference to which the position and orientation are relative.

from pycram.pose import Pose

example_pose = Pose([1, 2, 3], [0, 0, 0, 1], "map")
print(example_pose)

header:
  seq: 0
  stamp:
    secs: 1699448767
    nsecs: 355911970
  frame_id: "map"
pose:
  position:
    x: 1
    y: 2
    z: 3
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0

As you can see we created the example_pose with a position of [1, 2, 3] and an orientation of [0, 0, 0, 1] in the frame map. But we don’t need to provide all these parameters for a Pose, in case there is no parameter the Pose will use default parameter.

from pycram.pose import Pose

default_pose = Pose()
print(default_pose)

header:
  seq: 0
  stamp:
    secs: 1699448769
    nsecs: 128770112
  frame_id: "map"
pose:
  position:
    x: 0.0
    y: 0.0
    z: 0.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0

In case no parameter is provided the defualt parameter are:

• position: [0, 0, 0]

• orientation: [o, 0, 0, 1]

• frame: map

The following example will show how to access the data stored in a pose.

from pycram.pose import Pose

example_pose = Pose([1, 2, 3], [0, 0, 0, 1], "map")

print(f"Access to a component of the position: {example_pose.position.y}")

print(f"Access to a component of the rotation: {example_pose.orientation.x}")

print(f"Get the whole position as geometry_msgs/Pose:\n{example_pose.position}")

print(f"You can also get position or orientation as a list: {example_pose.position_as_list()}")

print(f"Same with the whole pose: {example_pose.to_list()}")

print(f"Access the reference frame: {example_pose.frame}")

Access to a component of the position: 2
Access to a component of the rotation: 0.0
Get the whole position as geometry_msgs/Pose:
x: 1
y: 2
z: 3
You can also get position or orientation as a list: [1, 2, 3]
Same with the whole pose: [[1, 2, 3], [0.0, 0.0, 0.0, 1.0]]
Access the reference frame: map

Editing a pose

You can also edit the data saved in a Pose, similar to how you access it.

from pycram.pose import Pose

example_pose = Pose([1, 2, 3], [0, 0, 0, 1], "map")

# Edit a single component of the position
example_pose.position.x = 3
print(f"Edit only one component:\n{example_pose.position}", "\n")

# Edit the whole position
example_pose.position = [0, 0, 1]
print(f"Edit the whole position:\n{example_pose.position}", "\n")

example_pose.frame = "new_frame"
print(f"Set a new frame:\n{example_pose.frame}", "\n")

example_pose.set_position([3, 2, 1])
print(f"Set the position via method:\n{example_pose.position}", "\n")

Edit only one component:
x: 3
y: 2
z: 3

Edit the whole position:
x: 0
y: 0
z: 1

Set a new frame:
new_frame

Set the position via method:
x: 3
y: 2
z: 1

Copy Poses

You can also copy Poses to create a new Pose with the same data. This can be useful if you have a method which would need to alter the Pose, since poses are passed by reference to a method every change done to the Pose in the method would affect the instanced passed to the method.

from pycram.pose import Pose

example_pose = Pose([1, 2, 3], [0, 0, 0, 1], "map")

copy_pose = example_pose.copy()

print(example_pose, "\n")
print(copy_pose)

header:
  seq: 0
  stamp:
    secs: 1699448775
    nsecs: 284231901
  frame_id: "map"
pose:
  position:
    x: 1
    y: 2
    z: 3
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0

header:
  seq: 0
  stamp:
    secs: 1699448775
    nsecs: 284231901
  frame_id: "map"
pose:
  position:
    x: 1
    y: 2
    z: 3
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0

Convert to Transform

PyCRAM also has its own transform at which we will take a look in the next section. However, here we will take a look at how to convert a Pose into a Transform.

For this example we will take a Pose which represents the current pose of a milk object and convert it into a Transform which represents the transformation from the map frame to the milk frame.

from pycram.pose import Pose

milk_pose = Pose([3, 4, 1], [1, 0, 0, 1], "map")

milk_transform = milk_pose.to_transform("milk")

print(milk_transform)

header:
  seq: 0
  stamp:
    secs: 1699448777
    nsecs: 693948984
  frame_id: "map"
child_frame_id: "milk"
transform:
  translation:
    x: 3
    y: 4
    z: 1
  rotation:
    x: 0.7071067811865476
    y: 0.0
    z: 0.0
    w: 0.7071067811865476

Transforms

Transforms are similar to Poses but instead of representing a Pose in a frame of reference they represent a transformation from one frame of reference to another. For this purpose Transforms have an additional parameter called child_frame_id which is the frame of reference to which the Transform is pointing.

Transforms in PyCRAM inherit from the TransformStamped message of ROS which makes them, like Poses, compatible to ROS services and topics that expect a TransformStamped message. Therefore, the naming conventions of Transforms are the same as of TransformStamped which.

• Translation: The vector describing the transformation in cartesian space.

• Rotation: The quaternion describing the transformation of rotation.

• Transform: The combination of translation and rotation

from pycram.pose import Transform

example_transform = Transform([1, 2, 2], [0, 0, 0, 1], "map", "object")

print(example_transform)

header:
  seq: 0
  stamp:
    secs: 1699448779
    nsecs: 614031314
  frame_id: "map"
child_frame_id: "object"
transform:
  translation:
    x: 1
    y: 2
    z: 2
  rotation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0

Transforms have the same methods to get and set values as Poses have, therefore only a short showcase will be given. For more details please look at the Pose example or the API documentation.

from pycram.pose import Transform

example_transform = Transform([2, 5, 1], [0, 0, 1, 1], "map", "object")

print(f"Access the rotation:\n{example_transform.rotation}", "\n")

print(f"Access the child_frane: {example_transform.child_frame_id}", "\n")

# changing translation and rotation is exactly like with Poses.

example_transform.translation = [1, 1, 1]
print(f"New translation:\n{example_transform.translation}")

Access the rotation:
x: 0.0
y: 0.0
z: 0.7071067811865475
w: 0.7071067811865475

Access the child_frane: object

New translation:
x: 1
y: 1
z: 1

Convert to Pose and Copy

Analog to Poses Transforms have a method that converts a Transform to a Pose, in this process the child_frame_id will be lost.

Also like in Poses Transforms have a copy method which creates an exact copy of this Transform.

from pycram.pose import Transform

milk_transform = Transform([1, 1, 1], [0, 0, 0, 1], "map", "milk")

milk_pose = milk_transform.to_pose()

print(f"The converted pose:\n{milk_pose}", "\n")

example_transform = Transform([1, 1, 1], [0, 0, 0, 1], "map", "milk")

copy_transform = example_transform.copy()

print(f"The copied transform:\n{copy_transform}")

The converted pose:
header:
  seq: 0
  stamp:
    secs: 1699448783
    nsecs: 267217397
  frame_id: "map"
pose:
  position:
    x: 1
    y: 1
    z: 1
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0

The copied transform:
header:
  seq: 0
  stamp:
    secs: 1699448783
    nsecs: 268019437
  frame_id: "map"
child_frame_id: "milk"
transform:
  translation:
    x: 1
    y: 1
    z: 1
  rotation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0

Operations on Transforms

Transforms have, unlike Poses, operations that can be done. These operations are:

• Multiplication

• Invert

• InverseTimes

Multiplication

We will first take a look at the multiplication of Transforms. We will use an example were we have two Transforms, the first from map to a hand frame and the second from the hand to a milk frame. By multiplying these two we get the Transform from map to milk frame.

from pycram.pose import Transform

map_to_hand = Transform([1, 1, 1], [0, 0, 0, 1], "map", "hand")

hand_to_milk = Transform([0.1, 0.05, 0], [0, 0, 0, 1], "hand", "milk")

map_to_milk = map_to_hand * hand_to_milk

print(map_to_milk)

header:
  seq: 0
  stamp:
    secs: 1699448785
    nsecs: 359950304
  frame_id: "map"
child_frame_id: "milk"
transform:
  translation:
    x: 1.1
    y: 1.05
    z: 1.0
  rotation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0

Invert

This inverts a Transform, so in we have a transform from map to milk then inverting it results in a Transform from milk to map .

from pycram.pose import Transform

map_to_milk = Transform([1, 1, 0.5], [0, 0, 0, 1], "map", "milk")

milk_to_map = map_to_milk.invert()

print(milk_to_map)

header:
  seq: 0
  stamp:
    secs: 1699448787
    nsecs:   1776695
  frame_id: "milk"
child_frame_id: "map"
transform:
  translation:
    x: -1.0
    y: -1.0
    z: -0.5
  rotation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0

Inverse Times

Inverse times combines the inverting and multiplication of Transforms, this results in a ‘minus’ for Transforms. We will again use the example of a hand holding a milk, but this time we have the Transforms from map to milk and hand to milk.

from pycram.pose import Transform

map_to_milk = Transform([1.1, 1.05, 1], [0, 0, 0, 1], "map", "milk")

hand_to_milk = Transform([0.1, 0.05, 0], [0, 0, 0, 1], "hand", "milk")

map_to_milk = map_to_milk.inverse_times(hand_to_milk)

print(map_to_milk)

header:
  seq: 0
  stamp:
    secs: 1699448788
    nsecs: 592707633
  frame_id: "map"
child_frame_id: "hand"
transform:
  translation:
    x: 1.0
    y: 1.0
    z: 1.0
  rotation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0