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
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 np.concatenate([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()
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)
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'
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')
# 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)
position_min, position_max = [0.8, 1.8, 0.0], [1.0, 2.2, 0.4]
position_min1, position_max1 = [-0.5, 1.4, 0.1], [1.0, 0.5, 0.1]
starting_joint_positions = arm.get_joint_positions()
starting_pose = mobile.get_2d_pose()
for i in range(LOOPS):
# Reset the arm at the start of each 'episode'
#agent.set_joint_positions(starting_joint_positions)
#agent1.set_2d_pose(starting_pose)
# Get a random position within a cuboid and set the target position
pos = list(np.random.uniform(position_min, position_max))
pos1 = list(np.random.uniform(position_min1, position_max1))
target.set_position(pos)
target1.set_position(pos1)
class Env:
def __init__(self, cfg):
self.enabled_joints = cfg.agent.enabled_joints
self._launch(cfg.env.env_path, cfg.env.headless)
self._setup_robot()
self._setup_vision("Vision_sensor")
self._setup_actions(cfg.agent.n_discrete_actions, cfg.agent.vel_min,
cfg.agent.vel_max)
self._setup_debug_cameras("debug_vis1", "debug_vis2")
self._setup_target()
self._setup_distractor()
self._setup_table()
def _setup_distractor(self):
self.distractor = Shape('distractor')
self.hide_distractor()
def _setup_table(self):
self.table = Shape("diningTable")
self.table_near = False
self.table_initial_pos = self.table.get_position()
def _setup_target(self):
self.target = Shape('target')
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 _setup_vision(self, vis_name):
self.vision = VisionSensor(vis_name)
def _launch(self, path, headless):
self.pr = PyRep()
self.pr.launch(path, headless=headless)
self.pr.start()
def _setup_actions(self, n_discrete_actions, vel_min, vel_max):
self.poss_actions = np.linspace(vel_min, vel_max, n_discrete_actions)
def _convert_action(self, action):
return self.poss_actions[action]
def _setup_debug_cameras(self, name0, name1):
self.vis_debug0 = VisionSensor(name0)
self.vis_debug1 = VisionSensor(name1)
def step(self, action):
converted_action = self._convert_action(action)
action = np.zeros(7)
action[self.enabled_joints] = converted_action
self.robot.set_joint_target_velocities(action)
self.pr.step()
reward = self._calculate_reward()
rgb = self.vision.capture_rgb()
reward = 0
done = False
# todo: change to include more meaningful info
return rgb, reward, done, {}
def show_distractor(self):
self.distractor.set_position([0.15, 0., 1.])
def hide_distractor(self):
self.distractor.set_position([-1.5, 0., 1.])
def toggle_table(self):
if self.table_near:
self._move_table_far()
else:
self._move_table_near()
def _move_table_near(self):
print("moving table near")
pos = self.table_initial_pos
pos[0] -= 1
self.table.set_position(pos)
def _move_table_far(self):
self.table.set_position(self.table_initial_pos)
def _calculate_reward(self):
ax, ay, az = self.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
def reset(self):
self.robot.set_joint_positions(self.joint_init)
rgb = self.vision.capture_rgb()
return rgb
def get_debug_images(self):
debug0 = self.vis_debug0.capture_rgb()
debug1 = self.vis_debug1.capture_rgb()
return debug0, debug1
def get_joint_positions(self):
pos = np.array(self.robot.get_joint_positions())
pos = pos[self.enabled_joints]
return pos
class Environment(object):
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 get_image(self, camera):
view = (camera.capture_rgb()*255).round().astype(np.uint8)
view = np.asarray(I.fromarray(view))
return cv.cvtColor(view, cv.COLOR_BGR2GRAY)
def get_state(self):
positions = self.actuator.get_joint_positions()
velocities = self.actuator.get_joint_velocities()
target_position = self.target.get_position()
images = (self.get_image(self.top_camera), self.get_image(self.side_camera), self.get_image(self.front_camera))
return (positions, velocities, target_position, images)
def done(self):
done = [self.inside_range( self.target.get_position()[i] - RANGE_DISCOUNT,
self.target.get_position()[i] + RANGE_DISCOUNT,
self.actuator.get_tip().get_position()[i]) for i in range(3)]
return done[0] == True and (done[0]==done[1] and done[0]==done[2])
def inside_range(self, min, max, x):
return True if (min <= x and x <= max) else False
def reset_scene(self):
#new_target_pos = list(np.random.uniform(self.POS_MIN, self.POS_MAX))
self.target.set_position(self.target_initial_pos)
self.actuator.set_joint_positions(self.actuator_initial_position)
return self.get_state()
def do_step(self, action, model_string):
self.actuator.set_joint_target_velocities(action)
self.controller.step()
if model_string == "base":
return self.base_article_reward(), self.get_state()
else:
return self.get_reward(), self.get_state()
def base_article_reward(self):
if(self.done()):
rw = BASE_REWARD
else:
ax, ay, az = self.actuator_tip.get_position()
tx, ty, tz = self.target.get_position()
rw = math.e * (-0.25 * np.sqrt((ax-tx)**2+(ay-ty)**2+(az-tz)**2))
return rw
def get_reward(self):
ax, ay, az = self.actuator_tip.get_position()
tx, ty, tz = self.target.get_position()
rw = -np.sqrt((ax-tx)**2+(ay-ty)**2+(az-tz)**2)
return rw
def shutdown(self):
return (self.controller.stop()) and (self.controller.shutdown())
def start_sim(self):
return self.controller.start()
def stop_sim(self):
return self.controller.stop()
