def forward_kinematic(self, joint_state, base='base', links=None):
def compute_fk_client():
try:
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = self.prefix + '/base'
rs = RobotState()
rs.joint_state = joint_state
res = self.compute_fk(header, links, rs)
return res.pose_stamped
except rospy.ServiceException, e:
print "Service call failed: %s" % e
# in case of troubles return 0 pose stamped
return []
if links is None:
links = self.end_effectors['whole_body']
if type(links) is not list:
if links == "all":
links = self.get_link_names('whole_body')
else:
links = [links]
# check that the base is in links
if base != 'base' and base not in links:
links.append(base)
pose_stamped_list = compute_fk_client()
if not pose_stamped_list:
return {}
# transform it in a dict of poses
pose_dict = {}
if base != 'base':
tr_base = transformations.pose_to_list(pose_stamped_list[links.index(base)].pose)
inv_base = transformations.inverse_transform(tr_base)
for i in range(len(links)):
if links[i] != base:
tr = transformations.pose_to_list(pose_stamped_list[i].pose)
pose_dict[links[i]] = transformations.multiply_transform(inv_base, tr)
else:
for i in range(len(links)):
pose_dict[links[i]] = transformations.pose_to_list(pose_stamped_list[i].pose)
return pose_dict
评论列表
文章目录