[Documentation] [TitleIndex] [WordIndex

Only released in EOL distros:  

hrl-kdl: hrl_geom | pykdl_utils

Package Summary

Contains pose_converter, a set of Python scripts which automatically detect the type of the input position/orientation pose and allows you to convert to any of the other types in a single call. Also contains a copy of transformations.py

hrl_kdl: hrl_geom | pykdl_utils

Package Summary

Contains pose_converter, a set of Python scripts which automatically detect the type of the input position/orientation pose and allows you to convert to any of the other types in a single call. Also contains a copy of transformations.py

PoseConv Usage

Each of the methods in PoseConv converts a pose-like Python tuple, numpy matrix or tuple, or ROS message into another. Each of the methods is called using PoseConv.to_%pose_type% where %pose_type% is in PoseConv.POSE_TYPES. Notes on various types:

The input parsing is fairly forgiving on the exact type and shape of the Python/numpy arguments.

The first argument can optionally be a string, representing the designed header frame_id if the requested type is a stamped message. The stamp is always set as the current time and seq is 0. After the optional header string, you can pass the pose. If the input type is a tuple, you can pass either the complete tuple as one argument or split it into two arguments.

   1 import roslib; roslib.load_manifest("hrl_geom")
   2 from hrl_geom.pose_converter import PoseConv
   3 pos_euler = [[0.1, 0.2, 0.3], [0.3, 0., 0.]]
   4 twist_msg = PoseConv.to_twist_msg(pos_euler)
   5 print twist_msg
   6 #linear: 
   7 #  x: 0.1
   8 #  y: 0.2
   9 #  z: 0.3
  10 #angular: 
  11 #  x: 0.3
  12 #  y: 0.0
  13 #  z: 0.0
  14 homo_mat = PoseConv.to_homo_mat(twist_msg)
  15 print homo_mat
  16 #[[ 1.          0.          0.          0.1       ]
  17 # [ 0.          0.95533649 -0.29552021  0.2       ]
  18 # [-0.          0.29552021  0.95533649  0.3       ]
  19 # [ 0.          0.          0.          1.        ]]
  20 pos, rot = homo_mat[:3,3], homo_mat[:3,:3]
  21 tf_stamped_msg = PoseConv.to_tf_stamped_msg("base_link", pos, rot)
  22 print tf_stamped_msg
  23 #header: 
  24 #  seq: 0
  25 #  stamp: 
  26 #    secs: 1341611677
  27 #    nsecs: 579762935
  28 #  frame_id: base_link
  29 #child_frame_id: ''
  30 #transform: 
  31 #  translation: 
  32 #    x: 0.1
  33 #    y: 0.2
  34 #    z: 0.3
  35 #  rotation: 
  36 #    x: 0.149438132474
  37 #    y: 0.0
  38 #    z: 0.0
  39 #    w: 0.988771077936
  40 new_pos_euler = PoseConv.to_pos_euler(tf_stamped_msg)
  41 print new_pos_euler
  42 #([0.10000000000000001, 0.20000000000000001, 0.29999999999999999], (0.30000000000000004, -0.0, 0.0))

Unit Testing

You can also run the main file to see some unit tests converting everything to everything else:

roscore # if not running already
rosrun hrl_geom pose_converter.py


2024-03-16 12:37