def test_solve_ik_via_sampling(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
configs = arm.solve_ik_via_sampling(
waypoint.get_position(), waypoint.get_orientation(), max_configs=5)
self.assertIsNotNone(configs)
self.assertEqual(len(configs), 5)
current_config = arm.get_joint_positions()
# Checks correct config (last)
arm.set_joint_positions(configs[-1])
self.assertTrue(np.allclose(
arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001))
# Checks correct config (first)
arm.set_joint_positions(configs[0])
self.assertTrue(np.allclose(
arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001))
# Checks order
prev_config_dist = 0
for config in configs:
config_dist = sum(
[(c - f)**2 for c, f in zip(current_config, config)])
# This test requires that the metric scale for each joint remains at
# 1.0 in _getConfigDistance lua function
self.assertLessEqual(prev_config_dist, config_dist)
prev_config_dist = config_dist
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)
arm_class, gripper_class, _ = SUPPORTED_ROBOTS[
self._robot_configuration]
# We assume the panda is already loaded in the scene.
if self._robot_configuration is not 'panda':
# Remove the panda from the scene
panda_arm = Panda()
panda_pos = panda_arm.get_position()
panda_arm.remove()
arm_path = join(DIR_PATH, 'robot_ttms',
self._robot_configuration + '.ttm')
self._pyrep.import_model(arm_path)
arm, gripper = arm_class(), gripper_class()
arm.set_position(panda_pos)
else:
arm, gripper = arm_class(), gripper_class()
self._robot = Robot(arm, gripper)
if self._randomize_every is None:
self._scene = Scene(self._pyrep, self._robot, self._obs_config)
else:
self._scene = DomainRandomizationScene(
self._pyrep, self._robot, self._obs_config,
self._randomize_every, self._frequency,
self._visual_randomization_config,
self._dynamics_randomization_config)
self._set_arm_control_action()
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 test_get_linear_path_visualize(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_linear_path(
waypoint.get_position(), waypoint.get_orientation())
# Check that it does not error
path.visualize()
def test_get_linear_path_and_get_end(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_linear_path(
waypoint.get_position(), waypoint.get_orientation())
path.set_to_end()
self.assertTrue(np.allclose(
arm.get_tip().get_position(), waypoint.get_position(), atol=0.001))
def test_solve_ik_via_jacobian(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
new_config = arm.solve_ik_via_jacobian(
waypoint.get_position(), waypoint.get_orientation())
arm.set_joint_positions(new_config)
self.assertTrue(np.allclose(
arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001))
def __init__(self):
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.agent = Panda()
self.agent.set_control_loop_enabled(False)
self.agent.set_motor_locked_at_zero_velocity(True)
self.target = Shape('target')
self.agent_ee_tip = self.agent.get_tip()
self.initial_joint_positions = self.agent.get_joint_positions()
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 test_get_linear_path_and_step(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_linear_path(
waypoint.get_position(), waypoint.get_orientation())
self.assertIsNotNone(path)
done = False
while not done:
done = path.step()
self.pyrep.step()
self.assertTrue(np.allclose(
arm.get_tip().get_position(), waypoint.get_position(), atol=0.01))
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()
def __init__(self, not_render):
super(Environment, self).__init__()
self.controller = PyRep()
scene_file = join(dirname(abspath(__file__)),'scene/scene_reinforcement_learning_env.ttt')
self.controller.launch(scene_file, headless=not_render)
self.actuator = Panda()
self.actuator.set_control_loop_enabled(False)
self.actuator.set_motor_locked_at_zero_velocity(True)
self.top_camera = VisionSensor('Vision_TOP')
self.side_camera = VisionSensor('Vision_SIDE')
self.front_camera = VisionSensor('Vision_FRONT')
self.target = Shape('target')
self.target_initial_pos = self.target.get_position()
self.start_sim()
self.actuator_tip = self.actuator.get_tip()
self.actuator_initial_position = self.actuator.get_joint_positions()
self.POS_MIN = [0.8, -0.2, 1.0]
self.POS_MAX = [1.0, 0.2, 1.4]
def test_get_path_from_cartesian_path(self):
arm = Panda()
cartesian_path = CartesianPath('Panda_cartesian_path')
path = arm.get_path_from_cartesian_path(cartesian_path)
self.assertIsNotNone(path)
def test_get_linear_path(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_linear_path(
waypoint.get_position(), waypoint.get_orientation())
self.assertIsNotNone(path)
def test_get_jacobian(self):
arm = Panda()
jacobian = arm.get_jacobian()
self.assertEqual(jacobian.shape, (7, 6))
def test_set_ik_element_properties(self):
arm = Panda()
arm.set_ik_element_properties(constraint_gamma=False)
arm.set_ik_element_properties()
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())
def _setup_robot(self):
self.robot = Panda()
self.joint_init = self.robot.get_joint_positions()
self.robot.set_control_loop_enabled(False)
self.robot.set_motor_locked_at_zero_velocity(True)
self.tip = self.robot.get_tip()
def test_get_linear_path_out_of_reach(self):
arm = Panda()
with self.assertRaises(ConfigurationPathError):
arm.get_linear_path([99, 99, 99], [0.] * 3)
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()
"""
from os.path import dirname, join, abspath
from pyrep import PyRep
from pyrep.robots.arms.panda import Panda
from pyrep.objects.shape import Shape
from pyrep.const import PrimitiveShape
from pyrep.errors import ConfigurationPathError
import numpy as np
import math
LOOPS = 10
SCENE_FILE = join(dirname(abspath(__file__)), 'panda_reach_target.ttt')
pr = PyRep()
pr.launch(SCENE_FILE, headless=False)
pr.start()
agent = Panda()
# We could have made this target in the scene, but lets create one dynamically
target = Shape.create(type=PrimitiveShape.SPHERE,
size=[0.05, 0.05, 0.05],
color=[1.0, 0.1, 0.1],
static=True,
respondable=False)
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'
def setUp(self):
super().setUp()
self.robot = Panda()
self.num_joints = len(self.robot.joints)
from pyrep import PyRep
from pyrep.robots.arms.panda import Panda
from pyrep.robots.mobiles.youbot import YouBot
from pyrep.objects.shape import Shape
from pyrep.const import PrimitiveShape
from pyrep.errors import ConfigurationPathError
import numpy as np
import math
from pyrep.robots.end_effectors.panda_gripper import PandaGripper
LOOPS = 10
SCENE_FILE = join(dirname(abspath(__file__)), 'colab.ttt')
pr = PyRep()
pr.launch(SCENE_FILE, headless=False)
pr.start()
arm = Panda()
mobile = YouBot()
#gripper = PandaGripper()
# We could have made this target in the scene, but lets create one dynamically
target = Shape.create(type=PrimitiveShape.SPHERE,
size=[0.05, 0.05, 0.05],
color=[1.0, 0.1, 0.1],
static=True,
respondable=False)
target1 = Shape.create(type=PrimitiveShape.SPHERE,
size=[0.05, 0.05, 0.05],
color=[1.0, 0.4, 0.2],
static=True,
respondable=False)
arm_path = arm.get_path(position,
quaternion=quaternion,
ignore_collisions=ignore_collisions)
arm_path.visualize()
done = False
while not done:
done = arm_path.step()
pr.step()
arm_path.clear_visualization()
SCENE_FILE = join(dirname(abspath(__file__)), 'double_panda.ttt')
pr = PyRep()
pr.launch(SCENE_FILE, headless=False)
pr.start()
panda = Panda()
panda1 = Panda(1)
cubes = []
pos = [[0.675, -0.45, 0.82], [0.65, -0.225, 0.82], [0.45, -0.175, 0.82],
[0.425, 0.2, 0.82], [0.625, 0.275, 0.82], [0.625, 0.525, 0.82]]
dummies = [Dummy.create() for i in range(0, 6)]
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(
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 test_get_linear_path_relative(self):
arm = Panda()
path = arm.get_linear_path([0, 0, 0.01], [0, 0, 0],
relative_to=arm.get_tip())
self.assertIsNotNone(path)
def test_set_ik_group_properties(self):
arm = Panda()
arm.set_ik_group_properties('damped_least_squares')
arm.set_ik_group_properties('pseudo_inverse')