interactive.py 文件源码

def set_goal(self, x_des, joint_des=None, refining=True):
        """
        Set a new task-space goal, and determine which primitive will be used
        Flushes the previous goal log and update id with this goal in the same time
        :param x_des: desired task-space goal
        :param joint_des desired joint-space goal **ONLY used for plots**
        :param refining: True to refine the trajectory by optimization after conditioning
        :return: True if the goal has been taken into account, False if a new demo is needed to reach it
        """
        def distance_from_goal(trajectory, goal):
            reached_goal = self.fk.get(trajectory[-1])
            return norm(reached_goal[0] - goal[0])

        self.promp_write_index = -1
        self.goal_id += 1
        self.goal_log = []
        if self.num_primitives > 0:
            for promp_index, promp in enumerate(self.promps):
                if self._is_a_target(promp, x_des):
                    self.generated_trajectory = promp.generate_trajectory(x_des, refining, joint_des, 'set_goal_{}'.format(self.goal_id))
                    distance = distance_from_goal(self.generated_trajectory, x_des)
                    if distance < self.epsilon_ok:
                        self.goal = x_des
                        self.promp_read_index = promp_index
                        self.goal_log.append({"mp_id": promp_index, "is_a_target": True, "is_reached": True, "precision": distance})
                        return True
                    else:
                        self.promp_write_index = promp_index
                        self.promp_read_index = -1  # A new demo is requested
                        self.goal_log.append({"mp_id": promp_index, "is_a_target": True, "is_reached": False, "precision": distance})
                        return False
                else:
                    _ = promp.generate_trajectory(x_des, refining, joint_des, 'set_goal_{}_not_a_target'.format(self.goal_id))  # Only for plotting
                    self.promp_read_index = -1  # A new promp is requested
                    self.goal_log.append({"mp_id": promp_index, "is_a_target": False, "is_reached": False})
            return False