controller.py 文件源码

def execute_iteration(self, task, method, iteration, trial, num_iterations):
        rospy.logwarn("Controller starts iteration {} {}/{} trial {}".format(method, iteration, num_iterations, trial))
        rospy.wait_for_service('ergo/reset')  # Ensures Ergo keeps working or wait till it reboots

        # After resuming, we keep the same iteration
        if self.perception.has_been_pressed('buttons/help'):
            rospy.sleep(1.5)  # Wait for the robot to fully stop
            self.recorder.record(task, method, trial, iteration)
            self.perception.switch_led('button_leds/pause', True)
            recording = self.perception.record(human_demo=True, nb_points=self.params['nb_points'])
            self.torso.set_torque_max(self.torso_params['torques']['reset'])
            self.torso.reset(slow=True)
            return True
        else:
            trajectory = self.learning.produce(skill_to_demonstrate=self.demonstrate).torso_trajectory
            self.torso.set_torque_max(self.torso_params['torques']['motion'])
            self.recorder.record(task, method, trial, iteration) 
            self.perception.switch_led('button_leds/pause', True)
            self.torso.execute_trajectory(trajectory)  # TODO: blocking, non-blocking, action server?
            recording = self.perception.record(human_demo=False, nb_points=self.params['nb_points'])
            self.perception.switch_led('button_leds/pause', False)
            recording.demo.torso_demonstration = JointTrajectory()
            self.torso.set_torque_max(80)
            self.torso.reset(slow=False)
            return self.learning.perceive(recording.demo)