def inverse_transform_list(point, pose):
'''
Inverse transforms a list using pose as the transform.
Takes a point defined in the frame defined in which pose is defined and converts it into the frame defined
by pose (i.e. the frame in which pose is the origin). This is useful, for example, if we know the position
of the object in the world is (x, y, z) and the pose of a table is table_pose. We can find the height of
the object above the table by transforming it into the pose of the table using
transform_point(point, table_pose) and looking at the z coordinate of the result.
**Args:**
**point ((double, double, double)):** Point to transform as (x, y, z)
**pose (geometry_msgs.msg.Pose):** The transform
**Returns:**
(x', y', z')
'''
translation = point_to_list(pose.position)
rotMatrix = rotation_matrix(pose.orientation)
newpoint = []
for row in range(0,3):
newpoint.append(rotMatrix[0][row]*(point[0]-translation[0]) +
rotMatrix[1][row]*(point[1]-translation[1]) +
rotMatrix[2][row]*(point[2]-translation[2]))
return newpoint
def transform_list(point, pose):
'''
Transforms a list using pose as the transform
Takes a point defined in the frame defined by pose (i.e. the frame in which pose is the origin) and returns
it in the frame in which pose is defined. Calling this with the point (0, 0, 0) will return pose. This
is useful, for example, in finding the corner of a box arbitrarily positioned in space. In the box's frame
the corner is (xdim, ydim, zdim). In the world, the corner is at transform_point(corner, box_pose).
**Args:**
**point ((double, double, double)):** Point to transform as (x, y, z)
**pose (geometry_msgs.msg.Pose):** The transform
Returns:
(x', y', z')
'''
translation = point_to_list(pose.position)
rotMatrix = rotation_matrix(pose.orientation)
newpoint = []
for row in range(0,3):
newpoint.append(rotMatrix[row][0]*point[0] +
rotMatrix[row][1]*point[1] +
rotMatrix[row][2]*point[2] +
translation[row])
return newpoint
def transform_point(point, pose):
'''
Transforms a point using pose as the transformation.
Takes a point defined in the frame defined by pose (i.e. the frame in which pose is the origin) and returns
it in the frame in which pose is defined. Calling this with the point (0, 0, 0) will return pose. This
is useful, for example, in finding the corner of a box arbitrarily positioned in space. In the box's frame
the corner is (xdim, ydim, zdim). In the world, the corner is at transform_point(corner, box_pose).
**Args:**
**point (geometry_msgs.msg.Point):** Point to transform
**pose (geometry_msgs.msg.Pose):** The transform
**Returns:**
A geometry_msgs.msg.Point
'''
return list_to_point(transform_list(point_to_list(point), pose))
def inverse_transform_point(point, pose):
'''
Inverse transforms a point using pose as the transform
Takes a point defined in the frame defined in which pose is defined and converts it into the frame defined
by pose (i.e. the frame in which pose is the origin). This is useful, for example, if we know the position
of the object in the world is (x, y, z) and the pose of a table is table_pose. We can find the height of
the object above the table by transforming it into the pose of the table using
transform_point(point, table_pose) and looking at the z coordinate of the result.
**Args:**
**point (geometry_msgs.msg.Point):** Point to transform
**pose (geometry_msgs.msg.Pose):** The transform
**Returns:**
A geometry_msgs.msg.Point
'''
return list_to_point(inverse_transform_list(point_to_list(point), pose))