human_model.py 文件源码

def inverse_kinematic(self, desired_poses, fixed_joints={}, tolerance=0.001, group_names='whole_body', seed=None):
        def compute_ik_client():
            rospy.wait_for_service('compute_human_ik')
            try:
                compute_ik = rospy.ServiceProxy('compute_human_ik', GetHumanIK)
                res = compute_ik(poses, fixed_joint_state, tolerance, group_names, seed)
                return res.joint_state
            except rospy.ServiceException, e:
                print "Service call failed: %s" % e
                # in case of failure return T pose
                return self.get_initial_state()
        if seed is None:
            seed = self.get_current_state()
        if group_names is not list:
            group_names = [group_names]
        # convert the desired poses to PoseStamped
        poses = []
        for key, value in desired_poses.iteritems():
            pose = transformations.list_to_pose(value)
            pose.header.frame_id = key
            poses.append(pose)
        # convert the fixed joints to joint state
        fixed_joint_state = JointState()
        for key, value in fixed_joints.iteritems():
            fixed_joint_state.name += [key]
            fixed_joint_state.position += [value]
        return compute_ik_client()