def execute(self, oracle): EXECUTE_TRAJECTORIES(SMOOTH_TRAJECTORIES(self.base_trajs)) EXECUTE_TRAJECTORIES(self.push.start_trajs) grab(oracle, self.object_name) EXECUTE_TRAJECTORIES([self.push.push_traj]) release(oracle, self.object_name) EXECUTE_TRAJECTORIES(self.push.end_trajs)
def step(self, oracle): oracle.set_robot_config(self.pap.approach_config) yield oracle.set_robot_config(self.pap.grasp_config) open_gripper(oracle) release(oracle, self.object_name) yield oracle.set_robot_config(self.pap.approach_config) yield
def step(self, oracle): oracle.set_robot_config(self.push.approach_config) yield oracle.set_robot_config(self.push.start_contact_config) grab(oracle, self.object_name) yield oracle.set_robot_config(self.push.end_contact_config) release(oracle, self.object_name) yield oracle.set_robot_config(self.push.approach_config) yield
def step(self, oracle): oracle.set_robot_config(self.pap.approach_config) yield if STEP_VECTOR: oracle.set_robot_config(self.pap.vector_config) yield oracle.set_robot_config(self.pap.grasp_config) open_gripper(oracle) release(oracle, self.object_name) yield if STEP_VECTOR: oracle.set_robot_config(self.pap.vector_config) yield oracle.set_robot_config(self.pap.approach_config) yield
def plan_base_traj(oracle, start_config, end_config, holding=None, obstacle_poses={}): with oracle.state_saver(): oracle.set_all_object_poses(obstacle_poses) oracle.set_robot_config(end_config) base_trans = get_trans(oracle.robot) oracle.set_robot_config(start_config) open_gripper(oracle) if holding is not None: oracle.set_pose(holding.object_name, object_pose_from_robot_config(oracle, start_config, holding.grasp)) grab(oracle, holding.object_name) traj = cspace_traj(oracle, CSpace.robot_base(oracle.robot), base_values_from_trans(base_trans)) if holding is not None: release(oracle, holding.object_name) return traj
def execute(self, oracle): EXECUTE_TRAJECTORIES(SMOOTH_TRAJECTORIES(self.base_trajs)) EXECUTE_TRAJECTORIES(reverse_trajectories(self.pap.trajs)) release(oracle, self.object_name) self.pap.grasp.gripper_traj.reverse().execute() EXECUTE_TRAJECTORIES(self.pap.trajs)