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:
