human_model.py 文件源码

def __init__(self, description='human_description', prefix='human', control=False):
        self.description = description
        self.robot_commander = RobotCommander(description)
        if control:
            self.joint_publisher = rospy.Publisher('/human/set_joint_values', JointState, queue_size=1)
        self.groups = {}
        self.groups['head'] = MoveGroupCommander('Head', description)
        self.groups['right_arm'] = MoveGroupCommander('RightArm', description)
        self.groups['left_arm'] = MoveGroupCommander('LeftArm', description)
        self.groups['right_leg'] = MoveGroupCommander('RightLeg', description)
        self.groups['left_leg'] = MoveGroupCommander('LeftLeg', description)
        self.groups['upper_body'] = MoveGroupCommander('UpperBody', description)
        self.groups['lower_body'] = MoveGroupCommander('LowerBody', description)
        self.groups['whole_body'] = MoveGroupCommander('WholeBody', description)
        # initialize end-effectors dict
        self.end_effectors = {}
        # fill both dict
        for key, value in self.groups.iteritems():
            self.end_effectors[key] = value.get_end_effector_link()
        # add the list of end-effectors for bodies
        self.end_effectors['upper_body'] = [self.end_effectors['head'],
                                            self.end_effectors['right_arm'],
                                            self.end_effectors['left_arm']]
        self.end_effectors['lower_body'] = [self.end_effectors['right_leg'],
                                            self.end_effectors['left_leg']]
        self.end_effectors['whole_body'] = self.end_effectors['upper_body'] + self.end_effectors['lower_body']
        self.prefix = prefix
        self.urdf_reader = URDFReader()

        rospy.wait_for_service('compute_fk')
        self.compute_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
        self.current_state = self.get_initial_state()