def cal_joint_angle_kdl(self, x_w, y_w, z_w): pos_euler = [[x_w, y_w, z_w], [0.0, 0.0, 0.0]] twist_msg = PoseConv.to_twist_msg(pos_euler) homo_mat = PoseConv.to_homo_mat(twist_msg) print(homo_mat) q = self.kdl_kin.random_joint_angles() q_r = self.kdl_kin.inverse(homo_mat, q) return q_r
#import roslib; roslib.load_manifest("hrl_geom") from hrl_geom.pose_converter import PoseConv pos_euler = [[0.1, 0.2, 0.3], [0.3, 0., 0.]] twist_msg = PoseConv.to_twist_msg(pos_euler) print twist_msg #linear: # x: 0.1 # y: 0.2 # z: 0.3 #angular: # x: 0.3 # y: 0.0 # z: 0.0 homo_mat = PoseConv.to_homo_mat(twist_msg) print homo_mat #[[ 1. 0. 0. 0.1 ] # [ 0. 0.95533649 -0.29552021 0.2 ] # [-0. 0.29552021 0.95533649 0.3 ] # [ 0. 0. 0. 1. ]] [pos, rot] = homo_mat[:3, 3], homo_mat[:3, :3] tf_stamped_msg = PoseConv.to_tf_stamped_msg("base_link", pos, rot) print tf_stamped_msg #header: # seq: 0 # stamp: # secs: 1341611677 # nsecs: 579762935 # frame_id: base_link #child_frame_id: '' #transform: