def __init__(self): super().__init__() self.path_point_nums = 50 self.pose = None self.home_joints = [0, -np.pi/4, 0, -3 * np.pi/4, 0, np.pi/2, 0] self.position = self.get_position() self.kine = FrankaKinematics() self.position_min = [0.8, -0.3, 0.83] self.position_max = [1.0, 0.3, 1.2] self.gripper = PandaGripper() self.clear_path = False
def test_run_task_validator(self): for task_file in TASKS: test_name = task_file.split('.py')[0] with self.subTest(task=test_name): if test_name in FLAKY_TASKS: self.skipTest('Flaky task.') sim = PyRep() ttt_file = os.path.join( DIR_PATH, '..', '..', 'rlbench', TTT_FILE) sim.launch(ttt_file, headless=True) sim.step_ui() sim.set_simulation_timestep(50.0) sim.step_ui() sim.start() robot = Robot(Panda(), PandaGripper()) obs = ObservationConfig() obs.set_all(False) scene = Scene(sim, robot, obs) sim.start() task_class = task_file_to_task_class(task_file) active_task = task_class(sim, robot) try: task_smoke(active_task, scene, variation=-1, max_variations=2, success=0.25) except Exception as e: sim.stop() sim.shutdown() raise e sim.stop() sim.shutdown()
def launch(self): if self._pyrep is not None: raise RuntimeError('Already called launch!') self._pyrep = PyRep() self._pyrep.launch(join(DIR_PATH, TTT_FILE), headless=self._headless) self._pyrep.set_simulation_timestep(0.005) self._robot = Robot(Panda(), PandaGripper()) self._scene = Scene(self._pyrep, self._robot, self._obs_config) self._set_arm_control_action()
def __init__(self, headless_mode: bool, variation="1container"): # Inicialización de la tarea, lanzando PyRep, cargando la escena en el simulador, cargando el robot y los # elementos de la escena y inicializando las listas. self.pr = PyRep() self.variation = variation self.ttt_file = 'pick_and_place_' + self.variation + '.ttt' self.pr.launch(join(DIR_PATH, self.ttt_file), headless=headless_mode) self.robot = Robot(Panda(), PandaGripper(), Dummy('Panda_tip')) self.task = InitTask(self.variation) self.lists = Lists()
def __init__(self, headless_mode: bool, variation: str): # Al inicializar la clase se carga PyRep, se carga la escena en el simulador, se carga el Robot y los objetos # de la escena y se inicializan las listas. self.pyrep = PyRep() self.variation = variation self.ttt_file = 'slide_block_' + self.variation + ".ttt" self.pyrep.launch(join(DIR_PATH, self.ttt_file), headless=headless_mode) self.robot = Robot(Panda(), PandaGripper(), Dummy('Panda_tip')) self.task = InitTask(variation) self.lists = Lists()
def __init__(self, headless_mode: bool, variation='1button'): self.pyrep = PyRep() self.variation = variation self.ttt_file = 'push_button_' + self.variation + '.ttt' self.pyrep.launch(join(DIR_PATH, self.ttt_file), headless=headless_mode) self.robot = Robot(Panda(), PandaGripper(), Dummy('Panda_tip')) self.task = InitTask(self.variation) self.param = Parameters(self.variation) self.param.original_pos0 = self.task.joint0.get_joint_position() self.param.original_or = self.task.wp0.get_orientation() if self.variation == '2button': self.param.original_pos1 = self.task.joint1.get_joint_position() self.lists = Lists()
def launch(self): if self._pyrep is not None: raise RuntimeError('Already called launch!') self._pyrep = PyRep() self._pyrep.launch(join(DIR_PATH, TTT_FILE), headless=self._headless) self._pyrep.set_simulation_timestep(0.005) self._robot = Robot(Panda(), PandaGripper()) self._scene = DomainRandomizationScene(self._pyrep, self._robot, self._obs_config, self._randomize_every, self._frequency, self._visual_rand_config, self._dynamics_rand_config) self._set_arm_control_action() # Raise the domain randomized floor. Shape('Floor').set_position(Dummy('FloorAnchor').get_position())
def init(self, display=False): if self._pyrep is not None: self.finalize() with suppress_std_out_and_err(): self._pyrep = PyRep() # TODO: TTT_FILE should be defined by robot, but now robot depends on launched pyrep self._pyrep.launch(join(DIR_PATH, TTT_FILE), headless=not display) self._pyrep.set_simulation_timestep(0.005) # TODO: Load arm and gripper from name self._robot = Robot(Panda(), PandaGripper()) self._scene = Scene(self._pyrep, self._robot, self._obs_config) self._set_arm_control_action() # Str comparison because sometimes class comparison doesn't work. if self._task is not None: self._task.unload() self._task = self._get_class_by_name(self._task_name, tasks)(self._pyrep, self._robot) self._scene.load(self._task) self._pyrep.start()
args = parser.parse_args() python_file = os.path.join(TASKS_PATH, args.task) if not os.path.isfile(python_file): raise RuntimeError('Could not find the task file: %s' % python_file) task_class = task_file_to_task_class(args.task) DIR_PATH = os.path.dirname(os.path.abspath(__file__)) sim = PyRep() ttt_file = os.path.join(DIR_PATH, '..', 'rlbench', TTT_FILE) sim.launch(ttt_file, headless=True) sim.step_ui() sim.set_simulation_timestep(0.005) sim.step_ui() sim.start() robot = Robot(Panda(), PandaGripper()) active_task = task_class(sim, robot) obs = ObservationConfig() obs.set_all(False) scene = Scene(sim, robot, obs) try: task_smoke(active_task, scene, variation=2) except TaskValidationError as e: sim.shutdown() raise e sim.shutdown() print('Validation successful!')
def __init__(self, headless_mode: bool): self.pyrep = PyRep() self.pyrep.launch(join(DIR_PATH, TTT_FILE), headless=headless_mode) self.robot = Robot(Panda(), PandaGripper(), Dummy('Panda_tip')) self.task = InitTask() self.lists = Lists()
def setUp(self): self.pyrep = PyRep() self.pyrep.launch(join(environment.DIR_PATH, TTT_FILE), headless=True) self.pyrep.set_simulation_timestep(0.005) self.robot = Robot(Panda(), PandaGripper())
class Franka(Panda): def __init__(self): super().__init__() self.path_point_nums = 50 self.pose = None self.home_joints = [0, -np.pi/4, 0, -3 * np.pi/4, 0, np.pi/2, 0] self.position = self.get_position() self.kine = FrankaKinematics() self.position_min = [0.8, -0.3, 0.83] self.position_max = [1.0, 0.3, 1.2] self.gripper = PandaGripper() self.clear_path = False def grasp(self,env,obj:None,force_mode=False): ''' gripper grasp ''' while not self.gripper.actuate(0.0,0.1): env.step() self.grasped_obj = obj if force_mode: self.gripper._grasped_objects.append(self.grasped_obj) self.gripper._old_parents.append(self.grasped_obj.get_parent()) # type: ignore self.obj.set_parent(self.gripper._attach_point, keep_in_place=True) else: self.gripper.grasp(self.grasped_obj) def release(self,env): ''' gripper open ''' while not self.gripper.actuate(1.0,0.1): env.step() if self.grasped_obj is not None: self.gripper.release() self.grasped_obj = None def _rot_value(self,euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None): if euler is not None: return R.from_euler('xyz',euler) elif quaternion is not None: return R.from_quat(quaternion) else: raise ValueError('input eluer or quternion') def _get_linear_path(self, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None ) -> ArmConfigurationPath: # start joints = self.get_joint_positions() H_start = self.kine.fk(joints) # rot ~ rots = [get_rotation_part(H_start),self._rot_value(euler,quaternion)] slerp = Slerp([0,1], rots) times = [x/self.path_point_nums for x in range(self.path_point_nums+1)] interp_rots = slerp(times) # trans ~ d_position = (position - self.pose)/self.path_point_nums # ik ret_floats = [] q_guess = self.home_joints start_position = get_transition_part(H_start) for i in range(self.path_point_nums+1): H_target = set_rotation_part(np.eye(4),interp_rots[i]) H_target = set_position_part(H_target,start_position) q = self.kine.ik(H_target, q_guess) # solve_ik ret_floats.append(q) q_guess = q return ArmConfigurationPath(self, ret_floats) def _get_nonlinear_path(self, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None) -> ArmConfigurationPath: r = self._rot_value(euler,quaternion) H_target = set_position_part(set_rotation_part(np.eye(4),r),position) q_target = self.kine.ik(H_target,self.home_joints) #self.move_j(q_target) def move_j(self,q_target,env): _q_target = copy.copy(q_target) _q_target[6] += np.pi/4 q_start = np.array(self.get_joint_positions()) dq = (_q_target - q_start)/self.path_point_nums res = [] for i in range(self.path_point_nums): res.append(q_start + dq * i) res = np.array(res) res = res.reshape((1,-1)) path = ArmConfigurationPath(self, res.tolist()[0]) done = False while not done: done = path.step() env.step() def home(self,env): self.move_j(self.home_joints,env) def move(self,env, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None): path = self.get_path( position=position, euler=euler, quaternion = quaternion) if path is None: raise RuntimeError('no path found') path.visualize() env.step() # Step the simulation and advance the agent along the path done = False while not done: done = path.step() env.step() if self.clear_path: path.clear_visualization() def go_to_position(self,position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None) -> ArmConfigurationPath: r = self._rot_value(euler,quaternion) H_target = set_position_part(set_rotation_part(np.eye(4),r),np.array(position)) q = self.kine.ik(H_target,self.home_joints) self.set_joint_positions(q) def get_path(self, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None, ignore_collisions=False, trials=100, max_configs=60, trials_per_goal=6, algorithm=Algos.SBL ) -> ArmConfigurationPath: ''' para --- position(franka frame) euler or quaternion ''' #position = np.array(position) + np.array(self.position) position = np.array(position) try: p = self.get_linear_path(position, euler, quaternion, ignore_collisions=ignore_collisions) return p except ConfigurationPathError: print('get linear path fail\n') pass # Allowed. Try again, but with non-linear. try: # TODO: _get_linear_path #p = self._get_linear_path(position,euler,quaternion) #return p pass except ConfigurationError: pass try: p = self.get_nonlinear_path( position, euler, quaternion, ignore_collisions, trials, max_configs, trials_per_goal, algorithm) return p except ConfigurationPathError: print('get nonlinear path fail\n') #p = self._get_nonlinear_path(position,euler,quaternion) #return p pass
Q_object_target.eval() G_li = [] loss_li = [] all_times_per_steps = [] all_times_per_updates = [] max_episode_steps = 200 max_episode_steps_eval = 200 for episode in range(params['max_episode']): epsilon = 1.0 / numpy.power(episode + 1, 1.0 / params['policy_parameter']) print("episode {}".format(episode), "epsilon {}".format(epsilon)) gripper = PandaGripper() arm = Panda() tip = arm.get_tip() target = Shape('target') s, done, t = env.reset(), False, 0 # new s is [arm.get_joint_positions(), tip.get_pose(), target.get_position()] s = [*s[8:15], *s[22:29], *s[-3:]] #override s to make observation space smaller # new s is [arm.get_joint_positions(), tip.get_pose(), target.get_position()] #you can use the following function to get extra information of the scene temp_buffer = [] #indicate whether the arm tip has reached the target
SCENE_FILE = join(dirname(abspath(__file__)), 'petanque.ttt') pr = PyRep() pr.launch(SCENE_FILE, headless=False) # Launch the application with a scene file that contains a robot class MyRobot(object): # Definir la estructura del robot def __init__(self, my_robot_arm, my_robot_gripper, my_robot_tip): self.arm = my_robot_arm self.gripper = my_robot_gripper self.tip = my_robot_tip self.pos = self.arm.get_position() arm = Panda() # Get the panda from the scene gripper = PandaGripper() # Get the gripper tip = Dummy('Panda_tip') my_panda = MyRobot(arm, gripper, tip) # Create the robot structure # Declaración y definición de los elementos de la tarea class InitTask(object): def __init__(self): self.sphere = Shape('Sphere') self.wp0 = Dummy('waypoint0') self.wp1 = Dummy('waypoint1') self.target = Shape('target') self.success = ProximitySensor('success') task = InitTask()
sim_result = [] inputs = [] for i in range(max_sims): pr.launch("scenes/emptyVortex.ttt", headless=False, responsive_ui=True) scene_view = Camera('DefaultCamera') scene_view.set_position([3.45, 0.18, 2.0]) scene_view.set_orientation(np.array([180, -70, 90]) * np.pi / 180.0) ex_world, used_its = scenario.generate() inputs.append(ex_world) # Import Robots c_objs = cop_from_prs(pr, ex_world) panda_1, gripper_1 = Panda(0), PandaGripper(0) panda_2, gripper_2 = Panda(1), PandaGripper(1) pr.start() pr.step() ## Neatness setup d_cam: VisionSensor = c_objs["Camera"][0] cube = c_objs["CUBOID"][0] tr1, tr2 = c_objs["Tray"] d_cam.set_entity_to_render(cube.get_handle()) pr.step() d_im = np.array(d_cam.capture_depth()) ## Robot movement code
from pyrep.robots.arms.ur3 import UR3 from pyrep.robots.arms.panda import Panda from pyrep.robots.end_effectors.baxter_gripper import BaxterGripper from pyrep.robots.end_effectors.panda_gripper import PandaGripper # SCENE_FILE = join(dirname(abspath(__file__)), 'scene_ur3_reinforcement_learning_env.ttt') SCENE_FILE = join(dirname(abspath(__file__)), 'scene_reinforcement_learning_env.ttt') pr = PyRep() # Launch the application with a scene file that contains a robot pr.launch(SCENE_FILE, headless=False) pr.start() # Start the simulation arm = Panda() gripper = PandaGripper() arm.set_control_loop_enabled(False) # arm.set_motor_locked_at_zero_velocity(True) for i in range(300): arm.set_joint_target_velocities( list(np.random.uniform(-0.5, 0.5, size=(7, )))) pr.step() for i in range(100): gripper_state = random.random() done = False # Open the gripper to gripper_state (1 is open 0 is closed) at a velocity of 0.04. while not done: done = gripper.actuate(gripper_state, velocity=0.04)