position_min, position_max = [0.8, -0.2, 1.0], [1.0, 0.2, 1.4] starting_joint_positions = agent.get_joint_positions() for i in range(LOOPS): # Reset the arm at the start of each 'episode' agent.set_joint_positions(starting_joint_positions) # Get a random position within a cuboid and set the target position pos = list(np.random.uniform(position_min, position_max)) target.set_position(pos) # Get a path to the target (rotate so z points down) try: path = agent.get_path(position=pos, euler=[0, math.radians(180), 0]) except ConfigurationPathError as e: print('Could not find path') continue # Step the simulation and advance the agent along the path done = False while not done: done = path.step() pr.step() print('Reached target %d!' % i) pr.stop() pr.shutdown()
for i in range(0, 6): cube = Shape.create(type=PrimitiveShape.CUBOID, size=[0.1, 0.1, 0.1], color=[1.0, 0.1, 0.1]) cubes.append(cube) cube.set_position(pos[i]) dummies[i].set_position(pos[i]) dummies[i].set_parent(cubes[i]) pr.step() prev_pos, quat = panda.get_tip().get_position(), panda.get_tip( ).get_quaternion() prev_pos1, quat1 = panda1.get_tip().get_position(), panda1.get_tip( ).get_quaternion() for i in range(0, 3): try: path = panda.get_path(position=prev_pos, quaternion=quat) except ConfigurationPathError as e: print('Could not find path') done = False while not done: done = path.step() pr.step() try: path = panda1.get_path(position=prev_pos1, quaternion=quat1) except ConfigurationPathError as e: print('Could not find path') done = False while not done: done = path.step() pr.step()