def launch(self, responsive_ui=False):
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, responsive_ui=responsive_ui)
arm_class, gripper_class, _ = SUPPORTED_ROBOTS[
self._robot_configuration]
# We assume the panda is already loaded in the scene.
if self._robot_configuration != '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_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_configs_for_tip_pose(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
configs = arm.get_configs_for_tip_pose(waypoint.get_position(),
waypoint.get_orientation())
self.assertIsNotNone(configs)
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 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 __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 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 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 __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_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 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()
"""
A Franka Panda moves using delta end effector pose control.
This script contains examples of:
- IK calculations.
- Joint movement by setting joint target positions.
"""
from os.path import dirname, join, abspath
from pyrep import PyRep
from pyrep.robots.arms.panda import Panda
SCENE_FILE = join(dirname(abspath(__file__)), 'scene_panda_reach_target.ttt')
DELTA = 0.01
pr = PyRep()
pr.launch(SCENE_FILE, headless=False)
pr.start()
agent = Panda()
starting_joint_positions = agent.get_joint_positions()
pos, quat = agent.get_tip().get_position(), agent.get_tip().get_quaternion()
def move(index, delta):
pos[index] += delta
new_joint_angles = agent.solve_ik_via_jacobian(pos, quaternion=quat)
agent.set_joint_target_positions(new_joint_angles)
pr.step()
[move(2, -DELTA) for _ in range(20)]
[move(1, -DELTA) for _ in range(20)]
[move(2, DELTA) for _ in range(10)]
def test_get_linear_path_out_of_reach(self):
arm = Panda()
with self.assertRaises(ConfigurationPathError):
arm.get_linear_path([99, 99, 99], [0.] * 3)
class TestJointGroups(TestCore):
def setUp(self):
super().setUp()
self.robot = Panda()
self.num_joints = len(self.robot.joints)
def test_get_joint_types(self):
self.assertEqual(self.robot.get_joint_types(),
[JointType.REVOLUTE] * self.num_joints)
def test_get_set_joint_positions(self):
self.robot.set_joint_positions([0.1] * self.num_joints)
self.assertEqual(len(self.robot.get_joint_positions()),
self.num_joints)
def test_get_set_joint_target_positions(self):
self.robot.set_joint_target_positions([0.1] * self.num_joints)
self.assertEqual(len(self.robot.get_joint_target_positions()),
self.num_joints)
def test_get_set_joint_target_velocities(self):
self.robot.set_joint_target_velocities([0.1] * self.num_joints)
self.assertEqual(len(self.robot.get_joint_target_velocities()),
self.num_joints)
def test_get_set_joint_forces(self):
self.robot.set_joint_target_velocities([-999] * self.num_joints)
self.robot.set_joint_forces([0.6] * self.num_joints)
self.pyrep.step()
self.assertEqual(len(self.robot.get_joint_forces()), self.num_joints)
def test_get_velocities(self):
self.assertEqual(len(self.robot.get_joint_velocities()),
self.num_joints)
def test_get_set_joint_intervals(self):
self.robot.set_joint_intervals([False] * self.num_joints,
[[-2.0, 2.0]] * self.num_joints)
cyclics, intervals = self.robot.get_joint_intervals()
self.assertEqual(len(cyclics), self.num_joints)
self.assertEqual(len(intervals), self.num_joints)
def test_get_joint_upper_velocity_limits(self):
self.assertEqual(len(self.robot.get_joint_upper_velocity_limits()),
self.num_joints)
def test_get_visuals(self):
self.assertEqual(len(self.robot.get_visuals()), 10)
def setUp(self):
super().setUp()
self.robot = Panda()
self.num_joints = len(self.robot.joints)
"""
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'
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)
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_set_ik_group_properties(self):
arm = Panda()
arm.set_ik_group_properties('damped_least_squares')
arm.set_ik_group_properties('pseudo_inverse')
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_jacobian(self):
arm = Panda()
jacobian = arm.get_jacobian()
self.assertEqual(jacobian.shape, (7, 6))
class ReacherEnv(object):
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 _get_state(self):
# Return state containing arm joint angles/velocities & target position
return (self.agent.get_joint_positions() +
self.agent.get_joint_velocities() + self.target.get_position())
def reset(self):
# Get a random position within a cuboid and set the target position
pos = list(np.random.uniform(POS_MIN, POS_MAX))
self.target.set_position(pos)
self.agent.set_joint_positions(self.initial_joint_positions)
return self._get_state()
def step(self, action):
self.agent.set_joint_target_velocities(action) # Execute action on arm
self.pr.step() # Step the physics simulation
ax, ay, az = self.agent_ee_tip.get_position()
tx, ty, tz = self.target.get_position()
# Reward is negative distance to target
reward = -np.sqrt((ax - tx)**2 + (ay - ty)**2 + (az - tz)**2)
return reward, self._get_state()
def shutdown(self):
self.pr.stop()
self.pr.shutdown()
def test_set_ik_element_properties(self):
arm = Panda()
arm.set_ik_element_properties(constraint_gamma=False)
arm.set_ik_element_properties()
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(
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_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)
