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()
class BaseEnv:
def __init__(self, scene_file="", headless=False):
self.pr = PyRep()
# Launch the application with a scene file in headless mode
self.pr.launch(scene_file, headless=headless)
self.pr.start() # Start the simulation
def step(self):
self.pr.step()
def stop(self):
self.pr.stop()
self.pr.shutdown()
def load_scene_object_from_file(self, file_path):
respondable = self.pr.import_model(file_path)
visible = respondable.get_objects_in_tree(exclude_base=True)[0]
return SceneObject(respondable_part=respondable, visible_part=visible)
def load_mesh_from_file(self, file_path, scaling_factor=1):
shape = Shape.import_shape(filename=file_path,
scaling_factor=scaling_factor)
self.pr.step()
return shape
class SimulationConsumerAbstract(mp.Process):
_id = 0
"""This class sole purpose is to better 'hide' all interprocess related code
from the user."""
def __init__(self, process_io, scene="", gui=False):
super().__init__(name="simulation_consumer_{}".format(
SimulationConsumerAbstract._id))
self._id = SimulationConsumerAbstract._id
SimulationConsumerAbstract._id += 1
self._scene = scene
self._gui = gui
self._process_io = process_io
np.random.seed()
def run(self):
self._pyrep = PyRep()
self._pyrep.launch(self._scene,
headless=not self._gui,
write_coppeliasim_stdout_to_file=True)
self._process_io["simulaton_ready"].set()
self._main_loop()
def _close_pipes(self):
self._process_io["command_pipe_out"].close()
self._process_io["return_value_pipe_in"].close()
# self._process_io["exception_pipe_in"].close() # let this one open
def _main_loop(self):
success = True
while success and not self._process_io["must_quit"].is_set():
success = self._consume_command()
self._pyrep.shutdown()
self._close_pipes()
def _consume_command(self):
try: # to execute the command and send result
success = True
command = self._process_io["command_pipe_out"].recv()
self._process_io["slot_in_command_queue"].release()
ret = command[0](self, *command[1], **command[2])
if command[0]._communicate_return_value:
self._communicate_return_value(ret)
except Exception as e: # print traceback, dont raise
traceback = format_exc()
success = False # return False: quit the main loop
self._process_io["exception_pipe_in"].send((e, traceback))
finally:
return success
def _communicate_return_value(self, value):
self._process_io["return_value_pipe_in"].send(value)
def signal_command_pipe_empty(self):
self._process_io["command_pipe_empty"].set()
while self._process_io["command_pipe_empty"].is_set():
time.sleep(0.1)
def good_bye(self):
pass
class TestCore(unittest.TestCase):
def setUp(self):
self.pyrep = PyRep()
self.pyrep.launch(path.join(ASSET_DIR, 'test_scene.ttt'),
headless=True)
self.pyrep.step()
self.pyrep.start()
def tearDown(self):
self.pyrep.stop()
self.pyrep.step_ui()
self.pyrep.shutdown()
class ReacherEnv(object):
def __init__(self):
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
# self.agent = Panda()
self.agent = Sawyer()
self.gripper = BaxterGripper()
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)
self.pr.step()
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
distance = (ax - tx)**2 + (ay - ty)**2 + (az - tz)**2
done = False
if distance < 5:
done = True
self.gripper.actuate(
0, velocity=0.04
) # if done, close the hand, 0 for close and 1 for open.
self.pr.step() # Step the physics simulation
reward = -np.sqrt(distance)
return self._get_state(), reward, done
def shutdown(self):
self.pr.stop()
self.pr.shutdown()
class ReacherEnv(object):
def __init__(self):
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.agent = TurtleBot()
self.agent.set_control_loop_enabled(False)
self.agent.set_motor_locked_at_zero_velocity(True)
self.target = Shape.create(type=PrimitiveShape.SPHERE,
size=[0.05, 0.05, 0.05],
color=[7.0, 0.5, 0.5],
static=True,
respondable=False)
# 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_base_actuation(),
self.agent.get_base_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))
pos_agent = list(np.random.uniform([0, 0, 0], [0, 0, 0]))
self.target.set_position(pos)
self.agent.set_position(pos_agent)
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.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)
print(int(reward * 100), int(ax * 100), int(ay * 100), int(az * 100),
int(tx * 100), int(ty * 100), int(tz * 100))
return reward, self._get_state()
def shutdown(self):
self.pr.stop()
self.pr.shutdown()
class TestScene(unittest.TestCase):
"""Tests the following:
- Getting observations from the scene
- Applying noise
- Applying domain randomization
"""
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 tearDown(self):
self.pyrep.shutdown()
def test_sensor_noise_images(self):
cam_config = CameraConfig(rgb_noise=GaussianNoise(0.05, (.0, 1.)))
obs_config = ObservationConfig(left_shoulder_camera=cam_config,
joint_forces=False,
task_low_dim_state=False)
scene = Scene(self.pyrep, self.robot, obs_config)
scene.load(ReachTarget(self.pyrep, self.robot))
obs1 = scene.get_observation()
obs2 = scene.get_observation()
self.assertTrue(
np.array_equal(obs1.right_shoulder_rgb, obs2.right_shoulder_rgb))
self.assertFalse(
np.array_equal(obs1.left_shoulder_rgb, obs2.left_shoulder_rgb))
self.assertTrue(obs1.left_shoulder_rgb.max() <= 1.0)
self.assertTrue(obs1.left_shoulder_rgb.min() >= 0.0)
def test_sensor_noise_robot(self):
obs_config = ObservationConfig(
joint_velocities_noise=GaussianNoise(0.01),
joint_forces=False,
task_low_dim_state=False)
scene = Scene(self.pyrep, self.robot, obs_config)
scene.load(ReachTarget(self.pyrep, self.robot))
obs1 = scene.get_observation()
obs2 = scene.get_observation()
self.assertTrue(
np.array_equal(obs1.joint_positions, obs2.joint_positions))
self.assertFalse(
np.array_equal(obs1.joint_velocities, obs2.joint_velocities))
class Environment:
def __init__(
self,
texture="/home/aecgroup/aecdata/Textures/mcgillManMade_600x600_png_selection/",
scene="/home/aecgroup/aecdata/Software/vrep_scenes/stereo_vision_robot_collection.ttt",
headless=True):
self.pyrep = PyRep()
self.pyrep.launch(scene, headless=headless)
min_distance = 0.5
max_distance = 5
max_speed = 0.5
path = texture
textures_names = listdir(path)
textures_list = [
self.pyrep.create_texture(path + name)[1]
for name in textures_names
]
self.screen = RandomScreen(min_distance, max_distance, max_speed,
textures_list)
self.robot = StereoVisionRobot(min_distance, max_distance)
self.pyrep.start()
def step(self):
# step simulation
self.pyrep.step()
# move screen
self.screen.increment_iteration()
def episode_reset(self, preinit=False):
# reset screen
self.screen.episode_reset(preinit=preinit)
# reset robot
self.robot.episode_reset()
# self.robot.set_vergence_position(to_angle(self.screen.distance))
self.pyrep.step()
def close(self):
self.pyrep.stop()
self.pyrep.shutdown()
class VrepSim(object):
"""Class that interfaces with Habitat sim"""
def __init__(
self, configs, scene_path=None, seed=0
): # TODO: extend the arguments of constructor
self.sim_config = copy.deepcopy(configs.COMMON.SIMULATOR)
if scene_path is None:
raise RuntimeError("Please specify the .ttt v-rep scene file path!")
self.sim = PyRep()
self.sim.launch(scene_path, headless=False)
self.sim.start()
[self.sim.step() for _ in range(50)]
def __del__(self):
self.sim.stop()
self.sim.shutdown()
def reset(self):
"""Reset the Habitat simultor"""
raise NotImplemented("Reset method for v-rep has not been implemented")
class ReacherEnv(object):
def __init__(self):
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.agent = Pioneer()
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 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 _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 shutdown(self):
self.pr.stop()
self.pr.shutdown()
class EnvPollos(Env):
def __init__(self, ep_length=100):
"""
Pollos environment for testing purposes
:param dim: (int) the size of the dimensions you want to learn
:param ep_length: (int) the length of each episodes in timesteps
"""
self.vrep = vrep
logging.basicConfig(level=logging.DEBUG)
# they have to be simmetric. We interpret actions as angular increments
#self.action_space = Box(low=np.array([-0.1, -1.7, -2.5, -1.5, 0.0, 0.0]),
# high=np.array([0.1, 1.7, 2.5, 1.5, 0.0, 0.0]))
inc = 0.1
self.action_space = Box(low=np.array([-inc, -inc, -inc, -inc, 0,
-inc]),
high=np.array([inc, inc, inc, inc, 0, inc]))
self.observation_space = Box(low=np.array([-0.5, 0.0, -0.2]),
high=np.array([0.5, 1.0, 1.0]))
#
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.agent = UR10()
self.agent.max_velocity = 1
self.agent.set_control_loop_enabled(True)
self.agent.set_motor_locked_at_zero_velocity(True)
self.joints = [
Joint('UR10_joint1'),
Joint('UR10_joint2'),
Joint('UR10_joint3'),
Joint('UR10_joint4'),
Joint('UR10_joint5'),
Joint('UR10_joint6')
]
#self.joints_limits = [[j.get_joint_interval()[1][0],j.get_joint_interval()[1][0]+j.get_joint_interval()[1][1]]
# for j in self.joints]
self.high_joints_limits = [0.1, 1.7, 2.7, 0.0, 0.02, 0.3]
self.low_joints_limits = [-0.1, -0.2, 0.0, -1.5, -0.02, -0.5]
#self.agent_hand = Shape('hand')
self.initial_agent_tip_position = self.agent.get_tip().get_position()
self.initial_agent_tip_quaternion = self.agent.get_tip(
).get_quaternion()
self.initial_agent_tip_euler = self.agent.get_tip().get_orientation()
self.target = Dummy('UR10_target')
self.initial_target_orientation = self.target.get_orientation()
self.initial_joint_positions = self.agent.get_joint_positions()
self.pollo_target = Dummy('pollo_target')
self.pollo = Shape('pollo')
#self.table_target = Dummy('table_target')
self.initial_pollo_position = self.pollo.get_position()
self.initial_pollo_orientation = self.pollo.get_quaternion()
#self.table_target = Dummy('table_target')
#self.succion = Shape('suctionPad')
self.waypoints = [Dummy('dummy_gancho%d' % i) for i in range(4)]
# objects
#self.scene_objects = [Shape('table0'), Shape('Plane'), Shape('Plane0'), Shape('ConcretBlock')]
#self.agent_tip = self.agent.get_tip()
self.initial_distance = np.linalg.norm(
np.array(self.initial_pollo_position) -
np.array(self.initial_agent_tip_position))
# camera
self.camera = VisionSensor('kinect_depth')
self.camera_matrix_extrinsics = vrep.simGetObjectMatrix(
self.camera.get_handle(), -1)
self.np_camera_matrix_extrinsics = np.delete(
np.linalg.inv(self.vrepToNP(self.camera_matrix_extrinsics)), 3, 0)
width = 640.0
height = 480.0
angle = math.radians(57.0)
focalx_px = (width / 2.0) / math.tan(angle / 2.0)
focaly_px = (height / 2.0) / math.tan(angle / 2.0)
self.np_camera_matrix_intrinsics = np.array([[-focalx_px, 0.0, 320.0],
[0.0, -focalx_px, 240.0],
[0.0, 0.0, 1.0]])
self.reset()
def reset(self):
pos = list(
np.random.uniform([-0.1, -0.1, 0.0], [0.1, 0.1, 0.1]) +
self.initial_pollo_position)
self.pollo.set_position(pos)
self.pollo.set_quaternion(self.initial_pollo_orientation)
self.agent.set_joint_positions(self.initial_joint_positions)
self.initial_epoch_time = time.time()
while True: # wait for arm to stop
self.pr.step() # Step the physics simulation
a = self.agent.get_joint_velocities()
if not np.any(np.where(np.fabs(a) < 0.1, False, True)):
break
print("ENV RESET DONE")
return self._get_state()
def step(self, action):
if action is None:
self.pr.step()
return self._get_state(), 0.0, False, {}
# check for nan
if np.any(np.isnan(action)):
print(action)
return self._get_state(), -1.0, False, {}
# action[1] = np.interp(action[1], [-1,7,1.7], [-0.2,1.7])
# action[2] = np.interp(action[2], [-2.5,2.5], [0,2.5])
# action[3] = np.interp(action[3], [-1.5,1.5], [-1.5,0])
# add actions as increments to current joints value
new_joints = np.array(
self.agent.get_joint_positions()) + np.array(action)
# check limits
if np.any(np.greater(new_joints, self.high_joints_limits)) or np.any(
np.less(new_joints, self.low_joints_limits)):
logging.debug("New joints value out of limits r=-10")
return self._get_state(), -10.0, True, {}
# move the robot and wait to stop
self.agent.set_joint_target_positions(new_joints)
reloj = time.time()
while True: # wait for arm to stop
self.pr.step() # Step the physics simulation
a = self.agent.get_joint_velocities()
if not np.any(np.where(np.fabs(a) < 0.1, False,
True)) or (time.time() - reloj) > 3:
break
# compute relevant magnitudes
np_pollo_target = np.array(self.pollo_target.get_position())
np_robot_tip_position = np.array(self.agent.get_tip().get_position())
dist = np.linalg.norm(np_pollo_target - np_robot_tip_position)
altura = np.clip((np_pollo_target[2] - self.initial_pollo_position[2]),
0, 0.5)
for obj in self.scene_objects: # colisión con la mesa
if self.agent_hand.check_collision(obj):
logging.debug("Collision with objects r=-10")
return self._get_state(), -10.0, True, {}
if altura > 0.3: # pollo en mano
logging.debug("Height reached !!! r=0")
return self._get_state(), 30.0, True, {}
elif dist > self.initial_distance: # mano se aleja
logging.debug("Hand moving away from chicken r=-10")
return self._get_state(), -10.0, True, {}
if time.time() - self.initial_epoch_time > 5: # too much time
logging.debug("Time out. End of epoch r=-10")
return self._get_state(), -10.0, True, {}
# Reward
#pollo_height = np.exp(-altura*20) # para 0.3m pollo_height = 0.002, para 0m pollo_height = 1
reward = altura - dist
logging.debug("New joints value out of limits r=-10")
return self._get_state(), reward, False, {}
def close(self):
self.pr.stop()
self.pr.shutdown()
def render(self):
print("RENDER")
np_pollo_en_camara = self.getPolloEnCamara()
# capture depth image
depth = self.camera.capture_rgb()
circ = plt.Circle(
(int(np_pollo_en_camara[0]), int(np_pollo_en_camara[1])), 10)
plt.clf()
fig = plt.gcf()
ax = fig.gca()
ax.add_artist(circ)
ax.imshow(depth, cmap='hot')
plt.pause(0.000001)
# Aux
# transform env.pollo_target.get_position() to camera coordinates and project pollo_en_camera a image coordinates
def getPolloEnCamara(self):
np_pollo_target = np.array(self.pollo_target.get_position())
np_pollo_target_cam = self.np_camera_matrix_extrinsics.dot(
np.append([np_pollo_target], 1.0))
np_pollo_en_camara = self.np_camera_matrix_intrinsics.dot(
np_pollo_target_cam)
np_pollo_en_camara = np_pollo_en_camara / np_pollo_en_camara[2]
np_pollo_en_camara = np.delete(np_pollo_en_camara, 2)
return np_pollo_en_camara
def getPolloEnCamaraEx(self):
np_pollo_target = np.array(self.pollo_target.get_position())
np_pollo_en_camara = self.np_camera_matrix_extrinsics.dot(
np.append([np_pollo_target], 1.0))
return np_pollo_en_camara
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.pollo_target.get_position())
p = self.getPolloEnCamaraEx()
j = self.agent.get_joint_positions()
#r = np.array([p[0],p[1],p[2],j[0],j[1],j[2],j[3],j[4],j[5]])
r = np.array([p[0], p[1], p[2]])
return r
def vrepToNP(self, c):
return np.array([[c[0], c[4], c[8], c[3]], [c[1], c[5], c[9], c[7]],
[c[2], c[6], c[10], c[11]], [0, 0, 0, 1]])
from pyrep import PyRep
pr = PyRep()
# Launch the application with a scene file in headless mode
pr.launch('/v-rep_model/Infinite_basket2.ttt', headless=True)
pr.start() # Start the simulation
# Do some stuff
pr.stop() # Stop the simulation
pr.shutdown() # Close the application
class BaseSimulator:
def __init__(self, interactive, try_use_sim):
self._interactive = interactive
self._try_use_sim = try_use_sim
if PyRep is not None and try_use_sim:
self.with_pyrep = True
self._pyrep_init()
else:
self.with_pyrep = False
def _user_prompt(self, str_in):
if self._interactive:
input(str_in)
else:
print(str_in)
def _pyrep_init(self):
# pyrep
self._pyrep = PyRep()
scene_filepath = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'scene.ttt')
self._pyrep.launch(scene_filepath, headless=False, responsive_ui=True)
# target
self._target = Dummy('target')
# default camera
self._default_camera = Camera('DefaultCamera')
# default vision sensor
self._default_vision_sensor = VisionSensor('DefaultVisionSensor')
# vision sensors
self._vision_sensor_0 = VisionSensor('vision_sensor_0')
self._vision_sensor_1 = VisionSensor('vision_sensor_1')
self._vision_sensor_2 = VisionSensor('vision_sensor_2')
self._vision_sensor_3 = VisionSensor('vision_sensor_3')
self._vision_sensor_4 = VisionSensor('vision_sensor_4')
self._vision_sensor_5 = VisionSensor('vision_sensor_5')
self._vision_sensors = [self._vision_sensor_0, self._vision_sensor_1, self._vision_sensor_2,
self._vision_sensor_3, self._vision_sensor_4, self._vision_sensor_5]
# vision sensor bodies
self._vision_sensor_body_0 = Shape('vision_sensor_body_0')
self._vision_sensor_body_1 = Shape('vision_sensor_body_1')
self._vision_sensor_body_2 = Shape('vision_sensor_body_2')
self._vision_sensor_body_3 = Shape('vision_sensor_body_3')
self._vision_sensor_body_4 = Shape('vision_sensor_body_4')
self._vision_sensor_body_5 = Shape('vision_sensor_body_5')
self._vision_sensor_bodies =\
[self._vision_sensor_body_0, self._vision_sensor_body_1, self._vision_sensor_body_2,
self._vision_sensor_body_3, self._vision_sensor_body_4, self._vision_sensor_body_5]
# vision sensor rays
self._vision_sensor_rays_0 = [Shape('ray{}vs0'.format(i)) for i in range(4)]
self._vision_sensor_rays_1 = [Shape('ray{}vs1'.format(i)) for i in range(4)]
self._vision_sensor_rays_2 = [Shape('ray{}vs2'.format(i)) for i in range(4)]
self._vision_sensor_rays_3 = [Shape('ray{}vs3'.format(i)) for i in range(4)]
self._vision_sensor_rays_4 = [Shape('ray{}vs4'.format(i)) for i in range(4)]
self._vision_sensor_rays_5 = [Shape('ray{}vs5'.format(i)) for i in range(4)]
self._vision_sensor_rays = [self._vision_sensor_rays_0, self._vision_sensor_rays_1, self._vision_sensor_rays_2,
self._vision_sensor_rays_3, self._vision_sensor_rays_4, self._vision_sensor_rays_5]
# objects
self._dining_chair_0 = Shape('diningChair0')
self._dining_chair_1 = Shape('diningChair1')
self._dining_table = Shape('diningTable_visible')
self._high_table_0 = Shape('highTable0')
self._high_table_1 = Shape('highTable1')
self._plant = Shape('indoorPlant_visible')
self._sofa = Shape('sofa')
self._swivel_chair = Shape('swivelChair')
self._rack = Shape('_rack')
self._cupboard = Shape('cupboard')
self._box = Shape('Cuboid')
self._objects = [self._dining_chair_0, self._dining_chair_1, self._dining_table, self._high_table_0,
self._high_table_1, self._plant, self._sofa, self._swivel_chair, self._rack, self._cupboard,
self._box]
# spherical vision sensor
self._spherical_vision_sensor = SphericalVisionSensor('sphericalVisionRGBAndDepth')
# drone
self._drone = Shape('Quadricopter')
# robot
self._robot = Arm(0, 'Mico', 6)
self._robot_base = Dummy('Mico_dh_base')
self._robot_target = Arm(0, 'MicoTarget', 6)
# spline paths
self._spline_paths = list()
# primitive scene
self._with_primitive_scene_vis = False
def _update_path_visualization_pyrep(self, multi_spline_points, multi_spline_sdf_vals):
if len(self._spline_paths) > 0:
for spline_path_segs in self._spline_paths:
for spline_path_seg in spline_path_segs:
spline_path_seg.remove()
self._spline_paths.clear()
for spline_points, sdf_vals in zip(multi_spline_points, multi_spline_sdf_vals):
sdf_flags_0 = sdf_vals[1:, 0] > 0
sdf_flags_1 = sdf_vals[:-1, 0] > 0
sdf_borders = sdf_flags_1 != sdf_flags_0
borders_indices = ivy.indices_where(sdf_borders)
if borders_indices.shape[0] != 0:
to_concat = (ivy.array([0], 'int32'), ivy.cast(borders_indices, 'int32')[:, 0],
ivy.array([-1], 'int32'))
else:
to_concat = (ivy.array([0], 'int32'), ivy.array([-1], 'int32'))
border_indices = ivy.concatenate(to_concat, 0)
num_groups = border_indices.shape[0] - 1
spline_path = list()
for i in range(num_groups):
border_idx_i = int(ivy.to_numpy(border_indices[i]).item())
border_idx_ip1 = int(ivy.to_numpy(border_indices[i + 1]).item())
if i < num_groups - 1:
control_group = spline_points[border_idx_i:border_idx_ip1]
sdf_group = sdf_vals[border_idx_i:border_idx_ip1]
else:
control_group = spline_points[border_idx_i:]
sdf_group = sdf_vals[border_idx_i:]
num_points = control_group.shape[0]
orientation_zeros = np.zeros((num_points, 3))
color = (0.2, 0.8, 0.2) if sdf_group[-1] > 0 else (0.8, 0.2, 0.2)
control_poses = np.concatenate((ivy.to_numpy(control_group), orientation_zeros), -1)
spline_path_seg = CartesianPath.create(show_orientation=False, show_position=False, line_size=8,
path_color=color)
spline_path_seg.insert_control_points(control_poses.tolist())
spline_path.append(spline_path_seg)
self._spline_paths.append(spline_path)
@staticmethod
def depth_to_xyz(depth, inv_ext_mat, inv_calib_mat, img_dims):
uniform_pixel_coords = ivy_vision.create_uniform_pixel_coords_image(img_dims)
pixel_coords = uniform_pixel_coords * depth
cam_coords = ivy_vision.ds_pixel_to_cam_coords(pixel_coords, inv_calib_mat, [], img_dims)
return ivy_vision.cam_to_world_coords(cam_coords, inv_ext_mat)[..., 0:3]
@staticmethod
def get_pix_coords():
return ivy_vision.create_uniform_pixel_coords_image([360, 720])[..., 0:2]
def setup_primitive_scene_no_sim(self, box_pos=None):
# lists
shape_matrices_list = list()
shape_dims_list = list()
this_dir = os.path.dirname(os.path.realpath(__file__))
for i in range(11):
shape_mat = np.load(os.path.join(this_dir, 'no_sim/obj_inv_ext_mat_{}.npy'.format(i)))
if i == 10 and box_pos is not None:
shape_mat[..., -1:] = box_pos.reshape((1, 3, 1))
shape_matrices_list.append(ivy.array(shape_mat, 'float32'))
shape_dims_list.append(
ivy.array(np.load(os.path.join(this_dir, 'no_sim/obj_bbx_{}.npy'.format(i))), 'float32')
)
# matices
shape_matrices = ivy.concatenate(shape_matrices_list, 0)
shape_dims = ivy.concatenate(shape_dims_list, 0)
# sdf
primitive_scene = PrimitiveScene(cuboid_ext_mats=ivy.inv(ivy_mech.make_transformation_homogeneous(
shape_matrices))[..., 0:3, :], cuboid_dims=shape_dims)
self.sdf = primitive_scene.sdf
def setup_primitive_scene(self):
# shape matrices
shape_matrices = ivy.concatenate([ivy.reshape(ivy.array(obj.get_matrix(), 'float32'), (1, 3, 4))
for obj in self._objects], 0)
# shape dims
x_dims = ivy.concatenate([ivy.reshape(ivy.array(
obj.get_bounding_box()[1] - obj.get_bounding_box()[0], 'float32'), (1, 1)) for obj in self._objects], 0)
y_dims = ivy.concatenate([ivy.reshape(ivy.array(
obj.get_bounding_box()[3] - obj.get_bounding_box()[2], 'float32'), (1, 1)) for obj in self._objects], 0)
z_dims = ivy.concatenate([ivy.reshape(ivy.array(
obj.get_bounding_box()[5] - obj.get_bounding_box()[4], 'float32'), (1, 1)) for obj in self._objects], 0)
shape_dims = ivy.concatenate((x_dims, y_dims, z_dims), -1)
# primitve scene visualization
if self._with_primitive_scene_vis:
scene_vis = [Shape.create(PrimitiveShape.CUBOID, ivy.to_numpy(shape_dim).tolist())
for shape_dim in shape_dims]
[obj.set_matrix(ivy.to_numpy(shape_mat).reshape(-1).tolist())
for shape_mat, obj in zip(shape_matrices, scene_vis)]
[obj.set_transparency(0.5) for obj in scene_vis]
# sdf
primitive_scene = PrimitiveScene(cuboid_ext_mats=ivy.inv(ivy_mech.make_transformation_homogeneous(
shape_matrices))[..., 0:3, :], cuboid_dims=shape_dims)
self.sdf = primitive_scene.sdf
def update_path_visualization(self, multi_spline_points, multi_spline_sdf_vals, img_path):
if not self.with_pyrep:
if not self._interactive:
return
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
plt.ion()
plt.imshow(mpimg.imread(img_path))
plt.show()
plt.pause(0.1)
plt.ioff()
return
with utils.step_lock:
self._update_path_visualization_pyrep(multi_spline_points, multi_spline_sdf_vals)
def close(self):
if self._interactive:
input('\nPress enter to end demo.\n')
print('\nClosing simulator...\n')
# noinspection PyUnresolvedReferences
def __del__(self):
if self.with_pyrep:
self._pyrep.stop()
self._pyrep.shutdown()
print('\nDemo finished.\n')
class SlideBlock(object
): # Clase de la función de la tarea de empujar un objeto
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 slide_block(self, slide_params: np.array):
# Definición del punto de empuje
wp1_pos_rel = np.array(
[slide_params[0], slide_params[1], slide_params[2]])
wp1_pos_abs = wp1_pos_rel + self.task.wp0.get_position()
wp1_or_rel = np.array([0.0, 0.0, slide_params[4]])
wp1_or_abs = wp1_or_rel + self.task.wp0.get_orientation()
self.task.wp1.set_position(wp1_pos_abs)
self.task.wp1.set_orientation(wp1_or_abs)
# Definición del objetivo final
distance = slide_params[3]
orientation = slide_params[4]
final_pos_rel = np.array([
distance * math.sin(orientation), distance * math.cos(orientation),
0
])
final_pos_abs = final_pos_rel + self.task.wp1.get_position()
final_or_abs = wp1_or_abs
self.task.wp2.set_position(final_pos_abs)
self.task.wp2.set_orientation(final_or_abs)
# Definicion de la trayectoria del robot
tray = [self.task.wp0, self.task.wp1, self.task.wp2]
# Iniciar la simulacion
self.pyrep.start()
# Se declaran los parametros de la recompensa
distance_slide = 0.0
distance_target0 = 0.0
distance_target1 = 0.0
or_target0 = 0.0
or_target1 = 0.0
# Para la primera variación, se cierra la pinza para poder empujar el objeto.
if self.variation == '1block':
done = False
while not done:
done = self.robot.gripper.actuate(0, velocity=0.05)
self.pyrep.step()
# Ejecución de la trayectoria
for pos in tray:
try:
path = self.robot.arm.get_path(position=pos.get_position(),
euler=pos.get_orientation())
# Step the simulation and advance the agent along the path
done = False
while not done:
done = path.step()
self.pyrep.step()
if pos == self.task.wp1: # En WP1 se calcula el parametro d_slide
distance_slide = self.robot.gripper.check_distance(
self.task.block)
elif pos == self.task.wp2: # En WP2 se calcula el parametro d_slide y el error de orientación
distance_target0 = calc_distance(
self.task.block.get_position(),
self.task.target_wp0.get_position())
or_target0 = self.task.block.get_orientation(
)[2] - self.task.target_wp0.get_orientation()[2]
if self.variation == '2block': # En la 2da variacion se calculan los parametros para 2 soluciones
distance_target1 = calc_distance(
self.task.block.get_position(),
self.task.target_wp1.get_position())
or_target1 = self.task.block.get_orientation(
)[2] - self.task.target_wp1.get_orientation()[2]
except ConfigurationPathError:
# Si no se encuentra una configuracion para la trayectoria con los waypoints correspondientes
# se asigna una recompensa de -85
print('Could not find path')
reward = -85
self.pyrep.stop() # Stop the simulation
self.lists.list_of_rewards.append(reward)
self.lists.list_of_parameters.append(list(slide_params))
return -reward
# Calculo de la recompensa
reward = -(10 * distance**2 + 200 * distance_slide**2 +
200 * distance_target0**2 + 400 * distance_target1**2 +
3500 * distance_target0 * distance_target1 +
200 * np.abs(or_target0) * distance_target1 +
500 * np.abs(or_target1) * distance_target0)
self.pyrep.stop() # Parar la simulación
self.lists.list_of_rewards.append(
reward) # Se guarda la recompensa del episodio
self.lists.list_of_parameters.append(
list(slide_params)) # Se guardan los parametros del episodio
return -reward
def clean_lists(self):
self.lists = Lists() # Se limpian las listas
def return_lists(self):
return self.lists # Devolver las listas
def shutdown(self):
self.pyrep.shutdown() # Close the application
class PushButton(object): # Clase de la tarea
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 push_button(self, push_params: np.array):
# Definición del punto de empuje
push_pos_rel = np.array([push_params[0], push_params[1], push_params[2]])
push_pos = self.task.wp0.get_position() + push_pos_rel
self.task.wp1.set_position(push_pos)
# Definicion de la orientacion
or_rel = np.array([push_params[3], push_params[4], push_params[5]])
or_abs = self.param.original_or + or_rel
self.task.wp0.set_orientation(or_abs)
self.task.wp1.set_orientation(or_abs)
# Definicion de la trayectoria
tray = [self.task.wp0, self.task.wp1, self.task.wp0]
# Ejecución de la trayectoria
self.pyrep.start()
# Declaración de los parametros de la recompensa
distance_objective0 = 0.0
distance_objective1 = 0.0
error_alpha = 0.0
error_beta = 0.0
error_gamma = 0.0
done = False
# Cerrar la pinza para poder apretar el boton.
while not done:
done = self.robot.gripper.actuate(0, velocity=0.05)
self.pyrep.step()
# Ejecucion de la trayectoria
for pos in tray:
try:
path = self.robot.arm.get_path(position=pos.get_position(),
euler=pos.get_orientation())
# Step the simulation and advance the agent along the path
done = False
while not done:
done = path.step()
self.pyrep.step()
if pos == self.task.wp1: # Cuando se llega a WP1 (al boton) se calculan los parametros de la recompensa
distance_objective0 = self.robot.tip.check_distance(self.task.button_wp0)
error_alpha = self.task.button_wp0.get_orientation()[0] - self.task.wp1.get_orientation()[0]
error_beta = self.task.button_wp0.get_orientation()[1] - self.task.wp1.get_orientation()[1]
error_gamma = self.task.button_wp0.get_orientation()[2] - self.task.wp1.get_orientation()[2]
if self.variation == '2button':
distance_objective1 = self.robot.tip.check_distance(self.task.button_wp1)
except ConfigurationPathError:
# Si no se encuentra una configuracion para la trayectoria se asigna una recompensa de -300
print('Could not find path')
reward = -300
self.pyrep.stop() # Stop the simulation
self.lists.list_of_rewards.append(reward)
self.lists.list_of_parameters.append(list(push_params))
return -reward
# Calculo de la recompensa.
reward = (-400 * distance_objective0 ** 2 - 5 * error_alpha ** 2 - 5 * error_beta ** 2
- 1 * error_gamma ** 2 - 800 * distance_objective1 ** 2 -
3000 * distance_objective0 * distance_objective1)
self.pyrep.stop() # Stop the simulation
self.lists.list_of_rewards.append(reward)
self.lists.list_of_parameters.append(list(push_params))
return -reward
def clean_lists(self):
self.lists = Lists()
def return_lists(self):
return self.lists
def shutdown(self):
self.pyrep.shutdown() # Close the application
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()
class Environment(object):
"""Each environment has a scene."""
def __init__(self, action_mode: ActionMode, dataset_root: str= '',
obs_config=ObservationConfig(), headless=False,
static_positions: bool = False):
self._dataset_root = dataset_root
self._action_mode = action_mode
self._obs_config = obs_config
self._headless = headless
self._static_positions = static_positions
self._check_dataset_structure()
self._pyrep = None
self._robot = None
self._scene = None
self._prev_task = None
def _set_arm_control_action(self):
self._robot.arm.set_control_loop_enabled(True)
if (self._action_mode.arm == ArmActionMode.ABS_JOINT_VELOCITY or
self._action_mode.arm == ArmActionMode.DELTA_JOINT_VELOCITY):
self._robot.arm.set_control_loop_enabled(False)
self._robot.arm.set_motor_locked_at_zero_velocity(True)
elif (self._action_mode.arm == ArmActionMode.ABS_JOINT_POSITION or
self._action_mode.arm == ArmActionMode.DELTA_JOINT_POSITION or
self._action_mode.arm == ArmActionMode.ABS_EE_POSE or
self._action_mode.arm == ArmActionMode.DELTA_EE_POSE or
self._action_mode.arm == ArmActionMode.ABS_EE_VELOCITY or
self._action_mode.arm == ArmActionMode.DELTA_EE_VELOCITY):
self._robot.arm.set_control_loop_enabled(True)
elif (self._action_mode.arm == ArmActionMode.ABS_JOINT_TORQUE or
self._action_mode.arm == ArmActionMode.DELTA_JOINT_TORQUE):
self._robot.arm.set_control_loop_enabled(False)
else:
raise RuntimeError('Unrecognised action mode.')
def _check_dataset_structure(self):
if len(self._dataset_root) > 0 and not exists(self._dataset_root):
raise RuntimeError(
'Data set root does not exists: %s' % self._dataset_root)
def _string_to_task(self, task_name: str):
task_name = task_name.replace('.py', '')
try:
class_name = ''.join(
[w[0].upper() + w[1:] for w in task_name.split('_')])
mod = importlib.import_module("rlbench.tasks.%s" % task_name)
except Exception as e:
raise RuntimeError(
'Tried to interpret %s as a task, but failed. Only valid tasks '
'should belong in the tasks/ folder' % task_name) from e
return getattr(mod, class_name)
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 shutdown(self):
self._pyrep.shutdown()
self._pyrep = None
def get_task(self, task_class: Type[Task]) -> TaskEnvironment:
# Str comparison because sometimes class comparison doesn't work.
if self._prev_task is not None:
self._prev_task.unload()
task = task_class(self._pyrep, self._robot)
self._prev_task = task
return TaskEnvironment(
self._pyrep, self._robot, self._scene, task,
self._action_mode, self._dataset_root, self._obs_config,
self._static_positions)
class EnvPollos(Env):
def __init__(self, ep_length=100):
"""
Pollos environment for testing purposes
:param dim: (int) the size of the dimensions you want to learn
:param ep_length: (int) the length of each episodes in timesteps
"""
logging.basicConfig(level=logging.DEBUG)
#
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.agent = UR10()
self.agent.max_velocity = 0.8
self.agent.set_control_loop_enabled(True)
#self.agent.set_motor_locked_at_zero_velocity(True)
self.MAX_INC = 0.2
self.joints = [
Joint('UR10_joint1'),
Joint('UR10_joint2'),
Joint('UR10_joint3'),
Joint('UR10_joint4'),
Joint('UR10_joint5'),
Joint('UR10_joint6')
]
#self.joints_limits = [[j.get_joint_interval()[1][0],j.get_joint_interval()[1][0]+j.get_joint_interval()[1][1]]
# for j in self.joints]
self.high_joints_limits = [0.1, 1.7, 2.7, 0.0, 0.02, 0.3]
self.low_joints_limits = [-0.1, -0.2, 0.0, -1.5, -0.02, -0.5]
self.initial_joint_positions = self.agent.get_joint_positions()
self.agent_hand = Shape('hand')
self.initial_agent_tip_position = self.agent.get_tip().get_position()
self.initial_agent_tip_quaternion = self.agent.get_tip(
).get_quaternion()
self.target = Dummy('UR10_target')
self.pollo_target = Dummy('pollo_target')
self.pollo = Shape('pollo')
self.initial_pollo_position = self.pollo.get_position()
self.initial_pollo_orientation = self.pollo.get_quaternion()
self.table_target = Dummy('table_target')
self.table_target = Dummy('table_target')
# objects to check collisions
self.scene_objects = [
Shape('table0'),
Shape('Plane'),
Shape('Plane0'),
Shape('ConcretBlock')
]
self.initial_distance = np.linalg.norm(
np.array(self.initial_pollo_position) -
np.array(self.initial_agent_tip_position))
# camera
self.camera = VisionSensor('kinect_depth')
self.camera_matrix_extrinsics = vrep.simGetObjectMatrix(
self.camera.get_handle(), -1)
self.np_camera_matrix_extrinsics = np.delete(
np.linalg.inv(self.vrepToNP(self.camera_matrix_extrinsics)), 3, 0)
width = 640.0
height = 480.0
angle = math.radians(57.0)
focalx_px = (width / 2.0) / math.tan(angle / 2.0)
focaly_px = (height / 2.0) / math.tan(angle / 2.0)
self.np_camera_matrix_intrinsics = np.array([[-focalx_px, 0.0, 320.0],
[0.0, -focalx_px, 240.0],
[0.0, 0.0, 1.0]])
self.reset()
def reset(self):
pos = list(
np.random.uniform([-0.1, -0.1, 0.0], [0.1, 0.1, 0.1]) +
self.initial_pollo_position)
self.pollo.set_position(pos)
self.pollo.set_quaternion(self.initial_pollo_orientation)
self.agent.set_joint_positions(self.initial_joint_positions)
self.initial_epoch_time = time.time()
while True: # wait for arm to stop
self.pr.step() # Step the physics simulation
a = self.agent.get_joint_velocities()
if not np.any(np.where(np.fabs(a) < 0.1, False, True)):
break
return self._get_state()
def step(self, action):
if action is None:
print(self.total_reward)
return self._get_state(), -10.0, True, {}
# check for nan
if np.any(np.isnan(action)):
print("NAN values ", action)
self.NANS_COMING = True
return self._get_state(), -10.0, True, {}
# check for strange values
# if np.any(np.greater(action, self.MAX_INC)) or np.any(np.less(action, -self.MAX_INC)):
# print("Strange values ", action)
# self.NANS_COMING = True
# return self._get_state(), -10.0, True, {}
# check for NAN in VREP get_position()
if np.any(np.isnan(self.agent.get_tip().get_position())):
print("NAN values in get_position()", action,
self.agent.get_tip().get_position())
return self._get_state(), -10.0, True, {}
self.pr.step()
return self._get_state(), 0, True, {}
def close(self):
self.pr.stop()
self.pr.shutdown()
def render(self):
print("RENDER")
np_pollo_en_camara = self.getPolloEnCamara()
# capture depth image
depth = self.camera.capture_rgb()
circ = plt.Circle(
(int(np_pollo_en_camara[0]), int(np_pollo_en_camara[1])), 10)
plt.clf()
fig = plt.gcf()
ax = fig.gca()
ax.add_artist(circ)
ax.imshow(depth, cmap='hot')
plt.pause(0.000001)
# Aux
# transform env.pollo_target.get_position() to camera coordinates and project pollo_en_camera a image coordinates
def getPolloEnCamara(self):
np_pollo_target = np.array(self.pollo_target.get_position())
np_pollo_target_cam = self.np_camera_matrix_extrinsics.dot(
np.append([np_pollo_target], 1.0))
np_pollo_en_camara = self.np_camera_matrix_intrinsics.dot(
np_pollo_target_cam)
np_pollo_en_camara = np_pollo_en_camara / np_pollo_en_camara[2]
np_pollo_en_camara = np.delete(np_pollo_en_camara, 2)
return np_pollo_en_camara
def getPolloEnCamaraEx(self):
np_pollo_target = np.array(self.pollo_target.get_position())
np_pollo_en_camara = self.np_camera_matrix_extrinsics.dot(
np.append([np_pollo_target], 1.0))
return np_pollo_en_camara
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.pollo_target.get_position())
p = self.getPolloEnCamaraEx()
j = self.agent.get_joint_positions()
#r = np.array([p[0],p[1],p[2],j[0],j[1],j[2],j[3],j[4],j[5]])
r = np.array([p[0], p[1], p[2]])
return r
def vrepToNP(self, c):
return np.array([[c[0], c[4], c[8], c[3]], [c[1], c[5], c[9], c[7]],
[c[2], c[6], c[10], c[11]], [0, 0, 0, 1]])
class PyRepIO(AbstractIO):
""" This class is used to get/set values from/to a CoppeliaSim scene.
It is based on PyRep (http://www.coppeliarobotics.com/helpFiles/en/PyRep.htm).
"""
def __init__(self,
scene="",
start=False,
headless=False,
responsive_ui=False,
blocking=False):
""" Starts the connection with the CoppeliaSim remote API server.
:param str scene: path to a CoppeliaSim scene file
:param bool start: whether to start the scene after loading it
"""
self._closed = False
self._collision_handles = {}
self._objects = {}
self.pyrep = PyRep()
self.pyrep.launch(scene, headless, responsive_ui, blocking)
if start:
self.start_simulation()
def close(self):
if not self._closed:
self.pyrep.shutdown()
self._closed = True
def load_scene(self, scene_path, start=False):
""" Loads a scene on the CoppeliaSim server.
:param str scene_path: path to a CoppeliaSim scene file
:param bool start: whether to directly start the simulation after
loading the scene
.. note:: It is assumed that the scene file is always available on the
server side.
"""
self.stop_simulation()
if not os.path.exists(scene_path):
raise IOError("No such file or directory: '{}'".format(scene_path))
sim.simLoadScene(scene_path)
if start:
self.start_simulation()
def start_simulation(self):
""" Starts the simulation. """
self.pyrep.start()
def restart_simulation(self):
""" Re-starts the simulation. """
self.stop_simulation()
self.start_simulation()
def stop_simulation(self):
""" Stops the simulation. """
self.pyrep.stop()
def pause_simulation(self):
""" Pauses the simulation. """
sim.simPauseSimulation()
def resume_simulation(self):
""" Resumes the simulation. """
self.start_simulation()
def set_simulation_timestep(self, dt):
"""Sets the simulation time step.
:param dt: The time step value.
"""
self.pyrep.set_simulation_timestep(dt)
def get_simulation_state(self):
"""Retrieves current simulation state"""
return lib.simGetSimulationState()
def simulation_step(self):
"""Execute the next simulation step.
If the physics simulation is not running, then this will only update
the UI.
"""
self.pyrep.step()
def ui_step(self):
"""Update the UI.
This will not execute the next simulation step, even if the physics
simulation is running.
This is only applicable when PyRep was launched without a responsive UI.
"""
self.pyrep.step_ui()
def get_motor_position(self, motor_name):
""" Gets the motor current position. """
joint = self.get_object(motor_name)
return joint.get_joint_position()
def set_motor_position(self, motor_name, position):
""" Sets the motor target position. """
joint = self.get_object(motor_name)
joint.set_joint_target_position(position)
def get_motor_force(self, motor_name):
""" Retrieves the force or torque applied to a joint along/about its
active axis. """
joint = self.get_object(motor_name)
return joint.get_joint_force()
def set_motor_force(self, motor_name, force):
""" Sets the maximum force or torque that a joint can exert. """
joint = self.get_object(motor_name)
joint.set_joint_force(force)
def get_motor_limits(self, motor_name):
""" Retrieves joint limits """
joint = self.get_object(motor_name)
_, interval = joint.get_joint_interval()
return interval[0], sum(interval)
def get_object_position(self, object_name, relative_to_object=None):
""" Gets the object position. """
scene_object = self.get_object(object_name)
if relative_to_object is not None:
relative_to_object = self.get_object(relative_to_object)
return scene_object.get_position(relative_to_object)
def set_object_position(self, object_name, position=[0, 0, 0]):
""" Sets the object position. """
scene_object = self.get_object(object_name)
scene_object.set_position(position)
def get_object_orientation(self, object_name, relative_to_object=None):
""" Gets the object orientation. """
scene_object = self.get_object(object_name)
if relative_to_object is not None:
relative_to_object = self.get_object(relative_to_object)
return scene_object.get_orientation(relative_to_object)
def get_object_handle(self, name):
""" Gets the coppelia sim object handle. """
scene_object = self.get_object(name)
return scene_object.get_handle()
def get_collision_state(self, collision_name):
""" Gets the collision state. """
return sim.simReadCollision(self.get_collision_handle(collision_name))
def get_collision_handle(self, collision):
""" Gets a coppelia sim collisions handle. """
if collision not in self._collision_handles:
h = sim.simGetCollisionHandle(collision)
self._collision_handles[collision] = h
return self._collision_handles[collision]
def get_simulation_current_time(self):
""" Gets the simulation current time. """
try:
return lib.simGetSimulationTime()
except CoppeliaIOErrors:
return 0.0
def add_cube(
self,
name,
size,
mass=1.0,
backface_culling=False,
visible_edges=False,
smooth=False,
respondable=True,
static=False,
renderable=True,
position=None,
orientation=None,
color=None,
):
"""
Add Cube
:param name: Name of the created object.
:param size: A list of the x, y, z dimensions.
:param mass: A float representing the mass of the object.
:param backface_culling: If backface culling is enabled.
:param visible_edges: If the object will have visible edges.
:param smooth: If the shape appears smooth.
:param respondable: Shape is responsible.
:param static: If the shape is static.
:param renderable: If the shape is renderable.
:param position: The x, y, z position.
:param orientation: The z, y, z orientation (in radians).
:param color: The r, g, b values of the shape.
"""
self._create_pure_shape(
name,
PrimitiveShape.CUBOID,
size,
mass,
backface_culling,
visible_edges,
smooth,
respondable,
static,
renderable,
position,
orientation,
color,
)
def add_sphere(
self,
name,
size,
mass=1.0,
backface_culling=False,
visible_edges=False,
smooth=False,
respondable=True,
static=False,
renderable=True,
position=None,
orientation=None,
color=None,
):
"""
Add Sphere
:param name: Name of the created object.
:param size: A list of the x, y, z dimensions.
:param mass: A float representing the mass of the object.
:param backface_culling: If backface culling is enabled.
:param visible_edges: If the object will have visible edges.
:param smooth: If the shape appears smooth.
:param respondable: Shape is responsible.
:param static: If the shape is static.
:param renderable: If the shape is renderable.
:param position: The x, y, z position.
:param orientation: The z, y, z orientation (in radians).
:param color: The r, g, b values of the shape.
"""
self._create_pure_shape(
name,
PrimitiveShape.SPHERE,
size,
mass,
backface_culling,
visible_edges,
smooth,
respondable,
static,
renderable,
position,
orientation,
color,
)
def add_cylinder(
self,
name,
size,
mass=1.0,
backface_culling=False,
visible_edges=False,
smooth=False,
respondable=True,
static=False,
renderable=True,
position=None,
orientation=None,
color=None,
):
"""
Add Cylinder
:param name: Name of the created object.
:param size: A list of the x, y, z dimensions.
:param mass: A float representing the mass of the object.
:param backface_culling: If backface culling is enabled.
:param visible_edges: If the object will have visible edges.
:param smooth: If the shape appears smooth.
:param respondable: Shape is responsible.
:param static: If the shape is static.
:param renderable: If the shape is renderable.
:param position: The x, y, z position.
:param orientation: The z, y, z orientation (in radians).
:param color: The r, g, b values of the shape.
"""
self._create_pure_shape(
name,
PrimitiveShape.CYLINDER,
size,
mass,
backface_culling,
visible_edges,
smooth,
respondable,
static,
renderable,
position,
orientation,
color,
)
def add_cone(
self,
name,
size,
mass=1.0,
backface_culling=False,
visible_edges=False,
smooth=False,
respondable=True,
static=False,
renderable=True,
position=None,
orientation=None,
color=None,
):
"""
Add Cone
:param name: Name of the created object.
:param size: A list of the x, y, z dimensions.
:param mass: A float representing the mass of the object.
:param backface_culling: If backface culling is enabled.
:param visible_edges: If the object will have visible edges.
:param smooth: If the shape appears smooth.
:param respondable: Shape is responsible.
:param static: If the shape is static.
:param renderable: If the shape is renderable.
:param position: The x, y, z position.
:param orientation: The z, y, z orientation (in radians).
:param color: The r, g, b values of the shape.
"""
self._create_pure_shape(
name,
PrimitiveShape.CONE,
size,
mass,
backface_culling,
visible_edges,
smooth,
respondable,
static,
renderable,
position,
orientation,
color,
)
def change_object_name(self, obj, new_name):
""" Change object name """
old_name = obj.get_name()
if old_name in self._objects:
self._objects.pop(old_name)
obj.set_name(new_name)
def _create_pure_shape(
self,
name,
primitive_type,
size,
mass=1.0,
backface_culling=False,
visible_edges=False,
smooth=False,
respondable=True,
static=False,
renderable=True,
position=None,
orientation=None,
color=None,
):
"""
Create Pure Shape
:param name: Name of the created object.
:param primitive_type: The type of primitive to shape. One of:
PrimitiveShape.CUBOID
PrimitiveShape.SPHERE
PrimitiveShape.CYLINDER
PrimitiveShape.CONE
:param size: A list of the x, y, z dimensions.
:param mass: A float representing the mass of the object.
:param backface_culling: If backface culling is enabled.
:param visible_edges: If the object will have visible edges.
:param smooth: If the shape appears smooth.
:param respondable: Shape is responsible.
:param static: If the shape is static.
:param renderable: If the shape is renderable.
:param position: The x, y, z position.
:param orientation: The z, y, z orientation (in radians).
:param color: The r, g, b values of the shape.
"""
obj = Shape.create(
primitive_type,
size,
mass,
backface_culling,
visible_edges,
smooth,
respondable,
static,
renderable,
position,
orientation,
color,
)
self.change_object_name(obj, name)
def _create_object(self, name):
"""Creates pyrep object for the correct type"""
# TODO implement other types
handle = sim.simGetObjectHandle(name)
o_type = sim.simGetObjectType(handle)
if ObjectType(o_type) == ObjectType.JOINT:
return Joint(handle)
elif ObjectType(o_type) == ObjectType.SHAPE:
return Shape(handle)
else:
return None
def get_object(self, name):
""" Gets CoppeliaSim scene object"""
if name not in self._objects:
self._objects[name] = self._create_object(name)
return self._objects[name]
class ReacherEnv(object):
def __init__(self,
headless,
control_mode='end_position',
visual_control=False):
'''
:visual_control: bool, controlled by visual state or not (vector state).
'''
self.reward_offset = 10.0
self.reward_range = self.reward_offset # reward range for register gym env when using vectorized env wrapper
self.fall_down_offset = 0.1 # for judging the target object fall off the table
self.metadata = [] # gym env format
self.visual_control = visual_control
self.control_mode = control_mode
self.pr = PyRep()
if control_mode == 'end_position': # need to use different scene, the one with all joints in inverse kinematics mode
SCENE_FILE = join(dirname(abspath(__file__)),
'./scenes/sawyer_reacher_rl_new_ik.ttt')
elif control_mode == 'joint_velocity': # the scene with all joints in force/torche mode for forward kinematics
SCENE_FILE = join(dirname(abspath(__file__)),
'./scenes/sawyer_reacher_rl_new.ttt')
self.pr.launch(SCENE_FILE, headless=headless)
self.pr.start()
self.agent = Sawyer()
self.gripper = BaxterGripper()
self.gripper_left_pad = Shape(
'BaxterGripper_leftPad') # the pad on the gripper finger
self.proximity_sensor = ProximitySensor(
'BaxterGripper_attachProxSensor'
) # need the name of the sensor here
self.vision_sensor = VisionSensor(
'Vision_sensor') # need the name of the sensor here
if control_mode == 'end_position':
self.agent.set_control_loop_enabled(True) # if false, won't work
self.action_space = np.zeros(
4) # 3 DOF end position control + 1 rotation of gripper
elif control_mode == 'joint_velocity':
self.agent.set_control_loop_enabled(False)
self.action_space = np.zeros(
8) # 7 DOF velocity control + 1 rotation of gripper
else:
raise NotImplementedError
if self.visual_control == False:
self.observation_space = np.zeros(
17
) # position and velocity of 7 joints + position of the target
else:
self.observation_space = np.zeros(100) # dim of img!
self.agent.set_motor_locked_at_zero_velocity(True)
self.target = Shape('target') # object
self.tip_target = Dummy(
'Sawyer_target') # the target point of the tip to move towards
# self.table = Shape('diningTable')
self.agent_ee_tip = self.agent.get_tip()
self.tip_pos = self.agent_ee_tip.get_position()
self.tip_quat = self.agent_ee_tip.get_quaternion(
) # tip rotation as quaternion, if not control the rotation
# set a proper initial gesture/tip position
if control_mode == 'end_position':
# agent_position=self.agent.get_position()
# initial_pos_offset = [0.4, 0.3, 0.2] # initial relative position of gripper to the whole arm
# initial_pos = [(a + o) for (a, o) in zip(agent_position, initial_pos_offset)]
initial_pos = [0.3, 0.1, 0.9]
self.tip_target.set_position(initial_pos)
# one big step for rotation setting is enough, with reset_dynamics=True, set the rotation instantaneously
self.tip_target.set_orientation(
[0, 3.1415, 1.5707],
reset_dynamics=True) # make gripper face downwards
self.pr.step()
elif control_mode == 'joint_velocity':
self.initial_joint_positions = [
0.001815199851989746, -1.4224984645843506, 0.704303503036499,
2.54307222366333, 2.972468852996826, -0.4989511966705322,
4.105560302734375
]
self.agent.set_joint_positions(self.initial_joint_positions)
self.initial_joint_positions = self.agent.get_joint_positions()
self.initial_tip_positions = self.agent_ee_tip.get_position()
self.initial_target_positions = self.target.get_position()
def _get_state(self):
# Return state containing arm joint angles/velocities & target position
return np.array(self.agent.get_joint_positions() +
self.agent.get_joint_velocities() +
self.target.get_position())
def _is_holding(self):
'''
Return is holding the target or not, return bool.
'''
# Nothe that the collision check is not always accurate all the time,
# for continuous conllision, maybe only the first 4-5 frames of collision can be detected
pad_collide_object = self.gripper_left_pad.check_collision(self.target)
if pad_collide_object and self.proximity_sensor.is_detected(
self.target) == True:
return True
else:
return False
def _get_visual_state(self):
# Return a numpy array of size (width, height, 3)
return self.vision_sensor.capture_rgb(
) # A numpy array of size (width, height, 3)
def _is_holding(self):
# Return is holding the target or not, return bool
# Nothe that the collision check is not always accurate all the time,
# for continuous conllision, maybe only the first 4-5 frames of collision can be detected
pad_collide_object = self.gripper_left_pad.check_collision(self.target)
if pad_collide_object and self.proximity_sensor.is_detected(
self.target) == True:
return True
else:
return False
# def _move(self, action):
# '''
# Move the tip according to the action with inverse kinematics for 'end_position' control;
# with control of tip target in inverse kinematics mode instead of using .solve_ik() in forward kinematics mode.
# '''
# robot_moving_unit=0.01 # the amount of single step move of robot, not accurate
# moving_loop_itr=int(np.sum(np.abs(action[:3]))/robot_moving_unit)+1 # adaptive number of moving steps, with minimal of 1 step.
# # print(moving_loop_itr)
# small_step = list(1./moving_loop_itr*np.array(action)) # break the action into small steps, as the robot cannot move to the target position within one frame
# pos=self.agent_ee_tip.get_position()
# '''
# there is a mismatch between the object set_orientation() and get_orientation():
# the (x,y,z) in set_orientation() will be (y,x,-z) in get_orientation().
# '''
# ori_z=-self.agent_ee_tip.get_orientation()[2] # the minus is because the mismatch between the set and get
# assert len(small_step) == len(pos)+1 # 3 values for position, 1 value for rotation
# for _ in range(moving_loop_itr):
# for idx in range(len(pos)):
# pos[idx] += small_step[idx]
# self.tip_target.set_position(pos)
# self.pr.step()
# ''' deprecated! no need to use small steps for the rotation with reset_dynamics=True'''
# # ori_z+=small_step[3] # change the orientation along z-axis with a small step
# # self.tip_target.set_orientation([0,3.1415,ori_z], reset_dynamics=True) # make gripper face downwards
# # self.pr.step()
# ''' one big step for z-rotation is enough, with reset_dynamics=True, set the rotation instantaneously '''
# ori_z+=action[3]
# self.tip_target.set_orientation([0,3.1415,ori_z], reset_dynamics=True) # make gripper face downwards
# self.pr.step()
def _move(self, action):
'''
Move the tip according to the action with inverse kinematics for 'end_position' control;
with control of tip target in inverse kinematics mode instead of using .solve_ik() in forward kinematics mode.
Mode 2: a close-loop control, using ik.
'''
pos = self.gripper.get_position()
bounding_offset = 0.1
step_factor = 0.2 # small step factor mulitplied on the gradient step calculated by inverse kinematics
max_itr = 20 # maximum moving iterations
max_error = 0.1 # upper bound of distance error for movement at each call
rotation_norm = 5. # factor for normalization of rotation values
# check if state+action will be within of the bounding box, if so, move normally; else no action.
# x_min < x < x_max and y_min < y < y_max and z > z_min
if pos[0]+action[0]>POS_MIN[0]-bounding_offset and pos[0]+action[0]<POS_MAX[0]+bounding_offset \
and pos[1]+action[1] > POS_MIN[1]-bounding_offset and pos[1]+action[1] < POS_MAX[1]+bounding_offset \
and pos[2]+action[2] > POS_MIN[2]-bounding_offset:
'''
there is a mismatch between the object set_orientation() and get_orientation():
the (x,y,z) in set_orientation() will be (y,x,-z) in get_orientation().
'''
ori_z = -self.agent_ee_tip.get_orientation()[
2] # the minus is because the mismatch between the set and get
target_pos = np.array(self.agent_ee_tip.get_position()) + np.array(
action[:3])
diff = 1
itr = 0
# print('before: ', ori_z)
while np.sum(np.abs(diff)) > max_error and itr < max_itr:
itr += 1
# set pos in small step
cur_pos = self.agent_ee_tip.get_position()
diff = target_pos - cur_pos
pos = cur_pos + step_factor * diff
self.tip_target.set_position(pos.tolist())
self.pr.step()
''' one big step for z-rotation is enough '''
ori_z += rotation_norm * action[3]
self.tip_target.set_orientation([0, np.pi, ori_z
]) # make gripper face downwards
self.pr.step()
else:
# print("Potential Movement Out of the Bounding Box!")
pass # no action if potentially out of the bounding box
def reset(self):
# Get a random position within a cuboid and set the target position
max_itr = 10
pos = list(np.random.uniform(POS_MIN, POS_MAX))
self.target.set_position(pos)
self.target.set_orientation([0, 0, 0])
self.pr.step()
# changing the color or texture for domain randomization
self.target.set_color(
np.random.uniform(low=0, high=1, size=3).tolist()
) # set [r,g,b] 3 channel values of object color
# set end position to be initialized
if self.control_mode == 'end_position': # JointMode.IK
self.agent.set_control_loop_enabled(True)
self.tip_target.set_position(
self.initial_tip_positions
) # cannot set joint positions directly due to in ik mode nor force/torch mode
self.pr.step()
# prevent stuck case
itr = 0
while np.sum(
np.abs(
np.array(self.agent_ee_tip.get_position() -
np.array(self.initial_tip_positions)))
) > 0.1 and itr < max_itr:
itr += 1
self.step(np.random.uniform(
-0.2, 0.2,
4)) # take random actions for preventing the stuck cases
self.pr.step()
elif self.control_mode == 'joint_velocity': # JointMode.FORCE
self.agent.set_joint_positions(
self.initial_joint_positions
) # sometimes the gripper is stuck, cannot get back to initial
self.pr.step()
# self.table.set_collidable(True)
self.gripper_left_pad.set_collidable(
True
) # set the pad on the gripper to be collidable, so as to check collision
self.target.set_collidable(True)
if np.sum(self.gripper.get_open_amount()) < 1.5:
self.gripper.actuate(1, velocity=0.5) # open the gripper
self.pr.step()
if self.visual_control:
return self._get_visual_state()
else:
return self._get_state()
def step(self, action):
'''
Move the robot arm according to the action.
If control_mode=='joint_velocity', action is 7 dim of joint velocity values + 1 dim of gripper rotation.
if control_mode=='end_position', action is 3 dim of tip (end of robot arm) position values + 1 dim of gripper rotation.
'''
if self.control_mode == 'end_position':
if action is None or action.shape[0] != 4:
action = list(np.random.uniform(-0.1, 0.1, 4)) # random
self._move(action)
elif self.control_mode == 'joint_velocity':
if action is None or action.shape[0] != 7:
print('No actions or wrong action dimensions!')
action = list(np.random.uniform(-0.1, 0.1, 7)) # random
self.agent.set_joint_target_velocities(
action) # Execute action on arm
self.pr.step()
else:
raise NotImplementedError
ax, ay, az = self.gripper.get_position()
tx, ty, tz = self.target.get_position()
# Reward is negative distance to target
distance = (ax - tx)**2 + (ay - ty)**2 + (az - tz)**2
done = False
# print(self.proximity_sensor.is_detected(self.target))
current_vision = self.vision_sensor.capture_rgb(
) # capture a screenshot of the view with vision sensor
plt.imshow(current_vision)
plt.savefig('./img/vision.png')
reward = 0
# close the gripper if close enough to the object and the object is detected with the proximity sensor
if distance < 0.1 and self.proximity_sensor.is_detected(
self.target) == True:
# make sure the gripper is open before grasping
self.gripper.actuate(1, velocity=0.5)
self.pr.step()
self.gripper.actuate(
0, velocity=0.5
) # if done, close the hand, 0 for close and 1 for open.
self.pr.step() # Step the physics simulation
if self._is_holding():
# reward for hold here!
reward += 10
done = True
else:
self.gripper.actuate(1, velocity=0.5)
self.pr.step()
elif np.sum(
self.gripper.get_open_amount()
) < 1.5: # if gripper is closed due to collision or esle, open it; .get_open_amount() return list of gripper joint values
self.gripper.actuate(1, velocity=0.5)
self.pr.step()
else:
pass
reward -= np.sqrt(distance)
if tz < self.initial_target_positions[
2] - self.fall_down_offset: # the object fall off the table
done = True
reward = -self.reward_offset
if self.visual_control:
return self._get_visual_state(), reward, done, {}
else:
return self._get_state(), reward, done, {}
def shutdown(self):
self.pr.stop()
self.pr.shutdown()
def simulate(scene_dir, cls_indices):
# read 3d point cloud vertices npy (for visualization)
vertex_npy = np.load("mesh_data/custom_vertex.npy")
# loop over all scene files in scenes directory
for scene_path in os.listdir(scene_dir):
# check whether it's a scene file or not
if not scene_path[-3:] == 'ttt':
continue
# create an output directory for each scene
scene_out_dir = os.path.join('./sim_data/', scene_path[:-4])
if not os.path.isdir(scene_out_dir):
os.mkdir(scene_out_dir)
# launch scene file
SCENE_FILE = os.path.join(os.path.dirname(os.path.abspath(__file__)), os.path.join(scene_dir, scene_path))
pr = PyRep()
pr.launch(SCENE_FILE, headless=True)
pr.start()
pr.step()
# define vision sensor
camera = VisionSensor('cam')
# set camera absolute pose to world coordinates
camera.set_pose([0,0,0,0,0,0,1])
pr.step()
# define scene shapes
shapes = []
shapes.append(Shape('Shape1'))
shapes.append(Shape('Shape2'))
shapes.append(Shape('Shape3'))
shapes.append(Shape('Shape4'))
pr.step()
print("Getting vision sensor intrinsics ...")
# get vision sensor parameters
cam_res = camera.get_resolution()
cam_per_angle = camera.get_perspective_angle()
ratio = cam_res[0]/cam_res[1]
cam_angle_x = 0.0
cam_angle_y = 0.0
if (ratio > 1):
cam_angle_x = cam_per_angle
cam_angle_y = 2 * degrees(atan(tan(radians(cam_per_angle / 2)) / ratio))
else:
cam_angle_x = 2 * degrees(atan(tan(radians(cam_per_angle / 2)) / ratio))
cam_angle_y = cam_per_angle
# get vision sensor intrinsic matrix
cam_focal_x = float(cam_res[0] / 2.0) / tan(radians(cam_angle_x / 2.0))
cam_focal_y = float(cam_res[1] / 2.0) / tan(radians(cam_angle_y / 2.0))
intrinsics = np.array([[cam_focal_x, 0.00000000e+00, float(cam_res[0]/2.0)],
[0.00000000e+00, cam_focal_y, float(cam_res[1]/2.0)],
[0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
# loop to get 5000 samples per scene
for i in range(5000):
print("Randomizing objects' poses ...")
# set random pose to objects in the scene
obj_colors = []
for shape in shapes:
shape.set_pose([
random.uniform(-0.25,0.25), random.uniform(-0.25,0.25), random.uniform(0.25,2),
random.uniform(-1,1), random.uniform(-1,1), random.uniform(-1,1),
random.uniform(-1,1)
])
obj_colors.append([random.uniform(0,1), random.uniform(0,1), random.uniform(0,1)])
pr.step()
print("Reading vision sensor RGB signal ...")
# read vision sensor RGB image
img = camera.capture_rgb()
img = np.uint8(img * 255.0)
print("Reading vision sensor depth signal ...")
# read vision sensor depth image
depth = camera.capture_depth()
depth = np.uint8(depth * 255.0)
print("Generating front mask for RGB signal ...")
# generate RGB front mask
front_mask = np.sum(img, axis=2)
front_mask[front_mask != 0] = 255
front_mask = Image.fromarray(np.uint8(front_mask))
alpha_img = Image.fromarray(img)
alpha_img.putalpha(front_mask)
print("Saving sensor output ...")
# save sensor output
alpha_img.save(scene_out_dir+f'/{str(i).zfill(6)}-color.png')
imsave(scene_out_dir+f'/{str(i).zfill(6)}-depth.png', depth)
print("Getting objects' poses ...")
# get poses for all objects in scene
poses = []
for shape in shapes:
poses.append(get_object_pose(shape, camera))
pose_mat = np.stack(poses, axis=2)
# save pose visualization on RGB image
img_viz = visualize_predictions(poses, cls_indices[scene_path], img, vertex_npy, intrinsics)
imsave(scene_out_dir+f'/{str(i).zfill(6)}-viz.png', img_viz)
print("Saving meta-data ...")
# save meta-data .mat
meta_dict = {
'cls_indexes' : np.array(cls_indices[scene_path]),
'intrinsic_matrix' : intrinsics,
'poses' : pose_mat
}
savemat(scene_out_dir+f'/{str(i).zfill(6)}-meta.mat', meta_dict)
print("Performing semantic segmentation of RGB signal ...")
# perform semantic segmentation of RGB image
seg_img = camera.capture_rgb()
seg_img = perform_segmentation(seg_img, cls_indices[scene_path], poses, vertex_npy, intrinsics)
imsave(scene_out_dir+f'/{str(i).zfill(6)}-label.png', seg_img)
# stop simulation
pr.stop()
pr.shutdown()
class PyRepNavGoalEnv(Env):
def __init__(self,
n_obs=7,
n_act=3,
render=False,
seed=1337,
scene_file="my_scene_turtlebot_navigation.ttt",
dist_reach_thresh=0.3):
self.n_obs = n_obs
self.n_act = n_act
# PPO
self.action_space = spaces.Discrete(n_act)
self.observation_space = spaces.Box(-np.inf,
np.inf,
shape=[
n_obs,
],
dtype='float32')
# self.observation_space = spaces.Box(
# low=np.array([-0.8, -0.8, -0.8, -0.8, -3.1415]),
# high=np.array([0.8, 0.8, 0.8, 0.8, 3.1415]),
# dtype=np.float32)
# self.action_space = spaces.Box(low=-5, high=5, shape=(4,), dtype=np.float32)
self.scene_file = join(dirname(abspath(__file__)), scene_file)
self.pr = PyRep()
self.pr.launch(self.scene_file, headless=not render)
self.pr.start()
self.agent = TurtleBot()
# We could have made this target in the scene, but lets create one dynamically
self.target = Shape.create(type=PrimitiveShape.SPHERE,
size=[0.05, 0.05, 0.05],
color=[1.0, 0.1, 0.1],
static=True,
respondable=False)
self.position_min, self.position_max = [-1.5, -1.5,
0.1], [1.5, 1.5, 0.1]
self.target_pos = [] # initialized in reset
self.starting_pose = [0.0, 0.0, 0.061]
self.agent.set_motor_locked_at_zero_velocity(True)
self.trajectory_step_counter = 0
self.done_dist_thresh = dist_reach_thresh
def step(self, action):
self._set_action(action)
self.pr.step()
self.trajectory_step_counter += 1
o = self._get_obs()
r = self._get_reward()
d = self._is_done()
i = {}
return o, r, d, i # obs, reward, done, info..
def reset(self):
# Get a random position within a cuboid for the goal and set the target position
self.target_pos = list(
np.random.uniform(self.position_min, self.position_max))
# make sure it doesn*t spawn too close to robot...
if self.target_pos[0] > 0 and self.target_pos[0] < 0.7:
self.target_pos[0] = 0.7
elif self.target_pos[0] < 0 and self.target_pos[0] > -0.7:
self.target_pos[0] = 0.7
if self.target_pos[1] > 0 and self.target_pos[1] < 0.7:
self.target_pos[1] = 0.7
elif self.target_pos[1] < 0 and self.target_pos[1] > -0.7:
self.target_pos[1] = 0.7
# hard goal goal for now:
self.target_pos[0] = 1.1
self.target_pos[1] = 0
self.target.set_position(self.target_pos)
# Reset robot position to starting position
self.agent.set_position(self.starting_pose)
agent_rot = self.agent.get_orientation()
# agent_rot[2] = 0
agent_rot[2] = np.random.uniform(-3.1415, 3.1415)
self.agent.set_orientation(agent_rot)
self.agent.set_joint_target_velocities([0, 0]) # reset velocities
self.trajectory_step_counter = 0
self.pr.step(
) # think we need this for first obs to already return the values we set here above...
return self._get_obs()
def render(self, mode='human'):
pass
def close(self):
self.pr.stop()
self.pr.shutdown()
def seed(self, seed=None):
pass
def _get_obs(self):
agent_pos = self.agent.get_2d_pose()
agent_rot_rads = self.agent.get_orientation()
# obs = [agent_pos[0], agent_pos[1], agent_rot_rads[2]] # x_pos, y_pos, abs z_orientation
# achieved_goal = [agent_pos[0], agent_pos[1]]
# desired_goal = [self.target_pos[0], self.target_pos[1]]
# obs = {'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(),
# 'desired_goal': np.array(desired_goal.copy()),
# 'non_noisy_obs': agent_pos.copy()}
agent_joint_vels = self.agent.get_joint_velocities()
obs = [
self.target_pos[0], self.target_pos[1], agent_pos[0], agent_pos[1],
agent_rot_rads[2]
]
for v in agent_joint_vels:
obs.append(v)
obs = [round(o, 3) for o in obs]
return obs
def _set_action(self, action):
# set motor velocities
target_vels = []
frac = 0.5
if action == 0:
target_vels = [3.1415 * frac, 3.1415 * frac]
elif action == 1:
target_vels = [3.1415 * frac, -3.1415 * frac]
elif action == 2:
target_vels = [-3.1415 * frac, 3.1415 * frac]
self.agent.set_joint_target_velocities(target_vels)
def _get_reward(self):
agent_pos = self.agent.get_2d_pose()
dist = np.sqrt((self.target_pos[0] - agent_pos[0])**2 +
(self.target_pos[1] - agent_pos[1])**2)
if dist <= self.done_dist_thresh:
return 0
else:
return -1
def _is_done(self):
agent_pos = self.agent.get_2d_pose()
dist = np.sqrt((self.target_pos[0] - agent_pos[0])**2 +
(self.target_pos[1] - agent_pos[1])**2)
# print(dist)
if dist <= self.done_dist_thresh:
print("done because close")
return True
else:
return 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!')
class ThreeObstacles(object):
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 avoidance_with_waypoints(self, wp_params: np.array):
waypoint1, waypoint2 = self.get_waypoints_esf(wp_params)
# Definición de la trayectoria
tray = [
self.task.initial_pos, waypoint1, waypoint2, self.task.final_pos
]
d_tray_1 = self.task.initial_pos.check_distance(waypoint1)
d_tray_2 = waypoint1.check_distance(waypoint2)
d_tray_3 = waypoint2.check_distance(self.task.final_pos)
d_tray = d_tray_1 + d_tray_2 + d_tray_3
# Ejecución de la trayectoria
self.pyrep.start()
reward_long = -4 * d_tray**2
reward_dist = 0.0
for pos in tray:
try:
path = self.robot.arm.get_linear_path(
position=pos.get_position(),
euler=[0.0, np.radians(180), 0.0])
# Step the simulation and advance the agent along the path
done = False
while not done:
done = path.step()
self.pyrep.step()
distance_obstacle0 = self.robot.gripper.check_distance(
self.task.obstacle0)
distance_obstacle1 = self.robot.gripper.check_distance(
self.task.obstacle1)
distance_obstacle2 = self.robot.gripper.check_distance(
self.task.obstacle2)
reward_dist -= (20 * np.exp(-300 * distance_obstacle0) +
20 * np.exp(-300 * distance_obstacle1) +
20 * np.exp(-300 * distance_obstacle2))
except ConfigurationPathError:
reward = -400.0
self.pyrep.stop()
self.lists.list_of_parameters.append(list(wp_params))
self.lists.list_of_rewards.append(reward)
return -reward
reward = reward_long + reward_dist
self.pyrep.stop()
self.lists.list_of_parameters.append(list(wp_params))
self.lists.list_of_rewards.append(reward)
return -reward
def shutdown(self):
self.pyrep.shutdown() # Close the application
def clean_lists(self):
self.lists = Lists()
def return_lists(self):
return self.lists
def get_waypoints_esf(self, wp_params: np.array):
radio1 = wp_params[0]
tita1 = wp_params[1]
pos1_rel = np.array(
[radio1 * np.sin(tita1), radio1 * np.cos(tita1), 0])
pos1_abs = pos1_rel + self.task.initial_pos.get_position()
waypoint1 = Dummy.create()
waypoint1.set_position(pos1_abs)
radio2 = wp_params[2]
tita2 = wp_params[3]
pos2_rel = np.array(
[radio2 * np.sin(tita2), radio2 * np.cos(tita2), 0])
pos2_abs = pos2_rel + pos1_abs
waypoint2 = Dummy.create()
waypoint2.set_position(pos2_abs)
return waypoint1, waypoint2
class PyRepModule(object):
def __init__(self, logger, headless=False):
self.logger = logger
self.callback = {
PyRepRequestType.initialize_part_to_scene:
self.initialize_part_to_scene,
PyRepRequestType.update_group_to_scene:
self.update_group_to_scene,
PyRepRequestType.get_assembly_point:
self.get_assembly_point,
PyRepRequestType.update_part_status:
self.update_part_status,
PyRepRequestType.get_cost_of_available_pair:
self.get_cost_of_available_pair
}
self.pr = PyRep()
self.pr.launch(headless=headless)
self.pr.start()
self.scene_path = "./test_scene"
# used to visualize and assembly
self.part_info = None
self.part_status = None
self.primitive_parts = {}
self.group_info = None
self.group_obj = {}
def initialize_server(self):
self.logger.info("Initialize PyRep Server")
host = SocketType.pyrep.value["host"]
port = SocketType.pyrep.value["port"]
self.server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.server.bind((host, port))
self.server.listen(True)
self.host = host
self.port = port
try:
self.logger.info("Waiting for PyRep client {}:{}".format(
self.host, self.port))
self.connected_client, addr = self.server.accept()
self.logger.info("Connected to {}".format(addr))
except:
self.logger.info("PyRep Server Error")
finally:
self.server.close()
def get_callback(self, request):
return self.callback[request]
def save_scene(self, path):
self.pr.export_scene(path)
def close(self):
self.connected_client.close()
self.server.close()
self.pr.stop()
self.pr.shutdown()
#region socket function
def initialize_part_to_scene(self):
global ASSEMBLY_PAIR
self.logger.info("ready to initialize part to scene")
sendall_pickle(self.connected_client, True)
request = recvall_pickle(self.connected_client)
self.part_info = request["part_info"]
ASSEMBLY_PAIR = request["pair_info"]
self.logger.info("...initializing pyrep scene")
for part_name in self.part_info.keys():
self._import_part_info(part_name)
self.logger.info("End to initialize pyrep scene")
sendall_pickle(self.connected_client, True)
def _import_part_info(self, part_name):
assert self.part_info
self.logger.info("import part info of {}".format(part_name))
obj_path = self.part_info[part_name]["obj_file"]
# part_obj = Shape.import_mesh(obj_path, scaling_factor=0.001)
part_obj = ObjObject.create_object(obj_path=obj_path)
part_obj.set_name(part_name)
# import each assembly points to scene
assembly_points = self.part_info[part_name]["assembly_points"]
points = {}
for idx in assembly_points.keys():
ap = assembly_points[idx]
radius = ap["radius"] / 1000
depth = ap["depth"]
position = ap["pose"]["position"]
quaternion = ap["pose"]["quaternion"]
direction = ap["direction"]
assembly_point = Shape.create(PrimitiveShape.SPHERE,
size=[radius] * 3,
respondable=False,
static=True,
color=[1, 0, 0])
pose = position + quaternion
assembly_point.set_pose(pose)
assembly_point.set_name("{}_AP_{}".format(part_name, idx))
assembly_point.set_parent(part_obj.shape)
points[idx] = assembly_point
part_region_info = self.part_info[part_name]["region_info"]
region_info = {}
for region_id in part_region_info.keys():
position = part_region_info[region_id]["position"]
point_list = part_region_info[region_id]["points"]
region_shape = Shape.create(PrimitiveShape.SPHERE,
size=[0.05] * 3,
respondable=False,
static=True,
color=[0, 1, 0])
region_shape.set_name("{}_region_{}".format(part_name, region_id))
region_shape.set_parent(part_obj.shape)
region_shape.set_position(position)
for point_idx in point_list:
points[point_idx].set_parent(region_shape)
region_info[region_id] = {
"shape": region_shape,
"points": point_list
}
self.primitive_parts[part_name] = PartObject(part_object=part_obj,
assembly_points=points,
region_info=region_info)
def update_group_to_scene(self):
self.logger.info("ready to update group to scene")
sendall_pickle(self.connected_client, True)
request = recvall_pickle(self.connected_client)
new_group_info = request["group_info"]
self.logger.info("...updating group to scene")
self._update_group_obj(new_group_info)
self.logger.info("End to update group to scene")
sendall_pickle(self.connected_client, True)
def _update_group_obj(self, new_group_info):
for group_id in self.group_obj.keys():
self.group_obj[group_id].remove()
self.group_obj = {}
for group_id in new_group_info.keys():
if not new_group_info[group_id]["is_exist"]:
continue
self.logger.info("update group_{} to scene".format(group_id))
group_info = new_group_info[group_id]
obj_root = group_info["obj_root"]
file_list = get_file_list(obj_root)
base_obj = None
composed_parts = []
composed_objects = []
group_pose = load_yaml_to_dic(join(obj_root, "group_pose.yaml"))
composed_part_pose = {}
for file_path in file_list:
obj_name, ext = get_file_name(file_path)
if not ext == ".obj":
continue
if obj_name == "base": # import base object
base_obj = ObjObject.create_object(file_path)
base_obj.set_name("G{}".format(group_id))
else: # ikea_stefan_bottom_0
instance_id = obj_name.split("_")[-1]
part_name = obj_name.replace("_{}".format(instance_id), "")
obj = ObjObject.create_object(file_path)
obj.set_name("G{}_{}_{}".format(group_id, part_name,
instance_id))
part_instance = {
"part_name": part_name,
"instance_id": int(instance_id)
}
composed_parts.append(part_instance)
composed_part_idx = composed_parts.index(part_instance)
composed_part_pose[composed_part_idx] = group_pose[
obj_name]
object_dict = {
"group_object": obj,
"primitive": self.primitive_parts[part_name]
}
composed_objects.append(object_dict)
assert base_obj, "No base object"
composed_parts = tuple(composed_parts)
composed_objects = tuple(composed_objects)
for object_dict in composed_objects:
obj = object_dict["group_object"]
obj.set_parent(base_obj.shape)
self.group_obj[group_id] = GroupObject(base_obj, composed_parts,
composed_objects,
composed_part_pose)
def update_part_status(self):
self.logger.info("ready to update part_status")
sendall_pickle(self.connected_client, True)
request = recvall_pickle(self.connected_client)
self.part_status = request["part_status"]
self.logger.info("Update part_status")
sendall_pickle(self.connected_client, True)
#TODO: delete assembly point
def get_assembly_point(self):
self.logger.info("ready to get assembly point from scene")
sendall_pickle(self.connected_client, True)
request = recvall_pickle(self.connected_client)
group_id = int(request["group_id"])
connection_locs = request["connection_locs"]
connector_name = request["connector_name"]
#TODO:
self.logger.info(
"...get assembly points of group {} from scene".format(group_id))
target_group = self.group_obj[group_id]
assembly_points = None
try:
assembly_points = target_group.get_assembly_points(
locations=connection_locs,
connector_name=connector_name,
part_status=self.part_status)
except Exception as e:
self.logger.info("Error occur {}".format(e))
self.save_scene(
join(self.scene_path,
"error_scene_{}.ttt".format(get_time_stamp())))
self.logger.info("End to get assembly point from pyrep scene")
sendall_pickle(self.connected_client, assembly_points)
self.save_scene(
join(self.scene_path,
"test_scene_{}.ttt".format(get_time_stamp())))
def get_cost_of_available_pair(self):
self.logger.info("ready to get cost from scene")
sendall_pickle(self.connected_client, True)
request = recvall_pickle(self.connected_client)
group_id = int(request["group_id"])
pair_list = request["check_pair"]
self.logger.info("...get cost of group {} from scene".format(group_id))
target_group = self.group_obj[group_id]
pair_cost = None
try:
pair_cost = target_group.get_cost_between_pair(pair_list)
except Exception as e:
self.logger.info("Error occur {}".format(e))
self.save_scene("test_error_scene/error_scene_{}.ttt".format(
get_time_stamp()))
self.logger.info("End to get assembly point from pyrep scene")
sendall_pickle(self.connected_client, pair_cost)
self.save_scene("test_scene/test_scene_{}.ttt".format(
get_time_stamp()))
class DroneEnv(object):
def __init__(self,
random,
headless=True,
use_vision_sensor=False,
seed=42,
state="Normal",
SCENE_FILE=None,
reward_function_name='Normal',
buffer_size=None,
neta=0.9,
restart=False):
if reward_function_name not in list(rew_functions2.import_dict.keys()):
print(
"Wrong parameter passed on 'reward_function_name. Must be one of these: ",
list(rew_functions.import_dict.keys()))
if SCENE_FILE == None:
if (use_vision_sensor):
SCENE_FILE = join(
dirname(abspath(__file__))
) + '/../../scenes/ardrone_modeled_headless_vision_sensor.ttt' ## FIX
else:
SCENE_FILE = join(
dirname(abspath(__file__))
) + '/../../scenes/ardrone_modeled_headless.ttt' ## FIX
else:
SCENE_FILE = join(dirname(abspath(__file__))) + SCENE_FILE
assert state in ['Normal', 'New_Double', 'New_action']
assert random in [False, 'Gaussian', 'Uniform','Discretized_Uniform'], \
"random should be one of these values [False, 'Gaussian', 'Uniform', 'Discretized_Uniform]"
# assert random in [False, 'Gaussian', 'Uniform','Weighted','Discretized_Uniform'], \
# "random should be one of these values [False, 'Gaussian', 'Uniform', 'Weighted','Discretized_Uniform]"
## Setting Pyrep
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=headless)
self.pr.start()
self.agent = Drone(count=0,
name="Quadricopter",
num_joints=4,
use_vision_sensor=use_vision_sensor)
self.agent.set_control_loop_enabled(
False) ## Disables the built-in PID control on V-REP motors
#self.agent.set_motor_locked_at_zero_velocity(True) ## When the force is set to zero, it locks the motor to prevent drifting
self.agent.set_motor_locked_at_zero_velocity(
False
) ## When the force is set to zero, it locks the motor to prevent drifting
self.target = Shape('Quadricopter_target')
##Attributes
self.action_space = self.agent.action_space
if state == 'New_action':
self.observation_space = 22
elif state == 'New_Double':
self.observation_space = 36
else:
self.observation_space = self.agent.observation_space
self.restart = restart
self.random = random
self.dt = np.round(sim.simGetSimulationTimeStep(), 4) ## timestep
## Integrative buffer
self.buffer_size = buffer_size
# if self.buffer_size:
# assert (isinstance(buffer_size,int)) and (buffer_size < 100)
# self.integrative_buffer_size = self.buffer_size
# self.integrative_buffer = np.empty((self.buffer_size, 3)) # 3 because its [x,y,z] or [roll,pitch,yaw]
# self.neta = neta
self.state = state
## initial observation
self._initial_state()
self.first_obs = True ## hack so it doesn't calculate it at the first time
self._make_observation()
self.last_state = self.observation[:18]
self.weighted = False
# Creating lists
if self.random == 'Discretized_Uniform':
self._create_discretized_uniform_list()
self.ptr = 0
# self._creating_initialization_list()
## Setting seed
self.seed = seed
self._set_seed(self.seed, self.seed)
## Reward Functions
self.reward_function = partial(
rew_functions2.reward,
rew_functions2.import_dict[reward_function_name]['weight_list'])
keys = ["r_alive","radius","pitch","yaw","roll","pitch_vel","yaw_vel","roll_vel","lin_x_vel","lin_y_vel","lin_z_vel","norm_a", \
"std_a","death","integrative_error_x","integrative_error_y","integrative_error_z"]
self.weight_dict = dict(
zip(
keys, rew_functions2.import_dict[reward_function_name]
['weight_list']))
def shutdown(self):
self.pr.stop()
self.pr.shutdown()
def _make_observation(self):
lst_o = []
# Pose of the drone
drone_pos = self.agent.get_position(relative_to=None)
rel_orient = self.agent.get_orientation(relative_to=self.target)
global_orient = self.agent.get_orientation(relative_to=None)
lin_vel, ang_vel = self.agent.get_velocity()
# Pose of the target
target_pos = self.target.get_position(relative_to=None)
goal_lin_vel, goal_ang_vel = self.agent.get_velocity(
self.target.get_handle())
# Relative pos:
rel_pos = self.agent.get_position(relative_to=self.target)
rel_ang_vel = ang_vel
# Rotation matrix calculation (drone -> world)
g11, g12, g13, g21, g22, g23, g31, g32, g33 = self.agent._rotatation_drone_world(
global_orient)
# State of the environment
lst_o += list(rel_pos)
lst_o += [g11, g12, g13, g21, g22, g23, g31, g32, g33]
lst_o += rel_ang_vel
lst_o += lin_vel
# ## fifo
# if self.buffer_size:
# self.integrative_buffer[:-1] = self.integrative_buffer[1:]; self.integrative_buffer[-1] = rel_ang_vel ## FIFO
# self.integrative_error = self._calc_accum_error(self.integrative_buffer, neta=self.neta)
## Actual State
if self.state == 'New_action':
if not self.first_obs:
lst_o += list(self.current_action)
else:
lst_o += [0, 0, 0, 0]
self.observation = np.array(lst_o).astype('float32')
if not self.first_obs:
if self.state == 'New_Double':
self.observation = np.append(self.observation[:18],
self.last_state)
# Relative angular acceleration
rel_ang_acc = ((self.observation[12:15] - self.last_state[12:15]) /
self.dt)
# Relative linear acceleration
lin_acc = (self.observation[15:18] -
self.last_state[15:18]) / self.dt
else:
if self.state == 'New_Double':
self.observation = np.append(self.observation,
self.observation)
self.first_obs = False
def _set_seed(self, random_seed, numpy_seed):
random.seed(a=random_seed)
np.random.seed(seed=numpy_seed)
def _make_action(self, a):
self.agent.set_thrust_and_torque(a, force_zero=False)
self.current_action = a
def step(self, action):
if isinstance(action, dict):
ac = np.zeros(4)
for i in range(4):
ac[i] = action['action{}'.format(i)]
action = ac
# Clipping the action taken
action = np.clip(action, 0, self.agent.joints_max_velocity)
# Actuate
self._make_action(action)
# Step
self.pr.step() # Step the physics simulation
self.last_state = self.observation[:18]
# Observe
self._make_observation()
# Reward
drone_pos, drone_orient, yaw,pitch, roll,yaw_vel,pitch_vel, roll_vel,lin_x_vel, lin_y_vel, lin_z_vel, \
norm_a, std_a = self._get_reward_data()
reward, reward_dict = self.reward_function(self, self.radius, yaw,
pitch, roll, yaw_vel,
pitch_vel, roll_vel,
lin_x_vel, lin_y_vel,
lin_z_vel, norm_a, std_a,
self.integrative_error)
info = reward_dict
# Check if state is terminal
if self.weighted:
stand_threshold = 11
elif self.random == 'Discretized_Uniform':
stand_threshold = 6.5
else:
stand_threshold = 3.2
done = (self.radius > stand_threshold)
if done:
reward += self.weight_dict['death']
return self.observation, reward, done, info
def _get_reward_data(self):
drone_pos = self.agent.get_position(relative_to=self.target)
drone_orient = self.agent.get_orientation(relative_to=self.target)
self.drone_orientation = drone_orient
roll = drone_orient[0]
pitch = drone_orient[1]
yaw = drone_orient[2]
roll_vel, pitch_vel, yaw_vel = self.observation[12:15]
lin_x_vel, lin_y_vel, lin_z_vel = self.observation[15:18]
self.radius = math.sqrt(drone_pos[0]**2 + drone_pos[1]**2 +
drone_pos[2]**2)
lin_x_vel, lin_y_vel, lin_z_vel = self.observation[15:18]
norm_a = np.linalg.norm(self.current_action, ord=2)
std_a = self.current_action.std()
if not self.buffer_size:
self.integrative_error = None
return drone_pos, drone_orient, yaw, pitch, roll, yaw_vel, pitch_vel, roll_vel, lin_x_vel, lin_y_vel, lin_z_vel, norm_a, std_a
def reset(self):
## Zeroing the integrative buffer:
if self.buffer_size:
self.integrative_buffer[:] = np.nan
self.current_action = np.array([0, 0, 0, 0])
if self.restart == True:
self.pr.stop()
self._reset_position(how=self.random)
self.pr.start()
else:
self._reset_position(how=self.random)
self.first_obs = True
self._make_observation()
self.last_state = self.observation[:18]
return self.observation
def _reset_position(self, how=False):
# self.pr.step()
# self.agent.set_orientation([-0,0,-0])
if how == "Gaussian":
# self._set_gaussian_position()
position, orientation = utils._set_gaussian_position()
self._set_pose(position, orientation)
elif how == 'Uniform':
position, orientation = utils._set_uniform_position()
self._set_pose(position, orientation)
# self._set_uniform_position()
elif how == 'Discretized_Uniform':
position, orientation = utils._set_discretized_uniform_position(
self)
self._set_pose(position, orientation)
elif how == 'Weighted':
raise NotImplementedError
# if self.weighted == False:
# self.weighted = True
# else:
# pass
# chosen_position, chosen_orientation = self._sampling_weighted_multinomial()
# self.agent.set_position(chosen_position)
# self.agent.set_orientation(chosen_orientation.tolist())
else:
self._set_initial_position()
self.agent.set_thrust_and_torque(np.asarray([0.] * 4), force_zero=True)
self.agent.set_joint_positions(self.initial_joint_positions)
self.agent.set_joint_target_velocities(self.initial_joint_velocities)
self.agent.set_joint_target_positions(
self.initial_joint_target_positions)
def _set_pose(self, position, orientation):
self.agent.set_orientation(np.round(orientation, 2).tolist())
self.agent.set_position(np.round(position, 2).tolist())
def _set_initial_position(self):
self.agent.set_position(self.initial_position)
self.target.set_position(self.target_initial_position)
self.agent.set_orientation(self.initial_orientation)
self.target.set_orientation(self.target_initial_orientation)
# self.agent.set_joint_positions(self.initial_joint_positions)
def _create_discretized_uniform_list(self):
num_discretization = 7
bound_of_distribuition = 1.5
size = 2
self.x_y_ticks = np.round(
np.linspace(-bound_of_distribuition,
bound_of_distribuition,
num=num_discretization), 2)
num_discretization = 11
bound_of_distribuition = 0.5
size = 1
z_ticks = np.round(
np.linspace(-bound_of_distribuition,
bound_of_distribuition,
num=num_discretization), 2)
self.z_ticks = (z_ticks + 1.7)
num_discretization = 11
bound_of_distribuition = 1.57 / 2
size = 3
self.ang_ticks = np.round(
np.linspace(-bound_of_distribuition,
bound_of_distribuition,
num=num_discretization), 2)
def _initial_state(self):
self.initial_joint_positions = self.agent.get_joint_positions()
self.initial_position = self.agent.get_position()
self.initial_orientation = self.agent.get_orientation()
self.drone_orientation = self.initial_orientation
self.target_initial_position = self.target.get_position()
self.target_initial_orientation = self.target.get_orientation()
self.initial_joint_velocities = self.agent.get_joint_velocities()
self.initial_joint_target_velocities = self.agent.get_joint_target_velocities(
)
self.initial_joint_target_positions = self.agent.get_joint_target_positions(
)
self.current_action = np.array([0, 0, 0, 0])
class Environment(object):
"""Each environment has a scene."""
def __init__(
self,
action_mode: ActionMode,
dataset_root: str = '',
obs_config=ObservationConfig(),
headless=False,
static_positions: bool = False,
robot_configuration='panda',
randomize_every: RandomizeEvery = None,
frequency: int = 1,
visual_randomization_config: VisualRandomizationConfig = None,
dynamics_randomization_config: DynamicsRandomizationConfig = None,
attach_grasped_objects: bool = True):
self._dataset_root = dataset_root
self._action_mode = action_mode
self._obs_config = obs_config
self._headless = headless
self._static_positions = static_positions
self._robot_configuration = robot_configuration
self._randomize_every = randomize_every
self._frequency = frequency
self._visual_randomization_config = visual_randomization_config
self._dynamics_randomization_config = dynamics_randomization_config
self._attach_grasped_objects = attach_grasped_objects
if robot_configuration not in SUPPORTED_ROBOTS.keys():
raise ValueError('robot_configuration must be one of %s' %
str(SUPPORTED_ROBOTS.keys()))
if (randomize_every is not None and visual_randomization_config is None
and dynamics_randomization_config is None):
raise ValueError(
'If domain randomization is enabled, must supply either '
'visual_randomization_config or dynamics_randomization_config')
self._check_dataset_structure()
self._pyrep = None
self._robot = None
self._scene = None
self._prev_task = None
def _set_arm_control_action(self):
self._robot.arm.set_control_loop_enabled(True)
if (self._action_mode.arm == ArmActionMode.ABS_JOINT_VELOCITY or
self._action_mode.arm == ArmActionMode.DELTA_JOINT_VELOCITY):
self._robot.arm.set_control_loop_enabled(False)
self._robot.arm.set_motor_locked_at_zero_velocity(True)
elif (self._action_mode.arm == ArmActionMode.ABS_JOINT_POSITION
or self._action_mode.arm == ArmActionMode.DELTA_JOINT_POSITION
or self._action_mode.arm == ArmActionMode.ABS_EE_POSE_WORLD_FRAME
or self._action_mode.arm
== ArmActionMode.DELTA_EE_POSE_WORLD_FRAME
or self._action_mode.arm
== ArmActionMode.ABS_EE_POSE_PLAN_WORLD_FRAME
or self._action_mode.arm
== ArmActionMode.DELTA_EE_POSE_PLAN_WORLD_FRAME
or self._action_mode.arm == ArmActionMode.EE_POSE_PLAN_EE_FRAME
or self._action_mode.arm == ArmActionMode.EE_POSE_EE_FRAME):
self._robot.arm.set_control_loop_enabled(True)
elif (self._action_mode.arm == ArmActionMode.ABS_JOINT_TORQUE
or self._action_mode.arm == ArmActionMode.DELTA_JOINT_TORQUE):
self._robot.arm.set_control_loop_enabled(False)
else:
raise RuntimeError('Unrecognised action mode.')
def _check_dataset_structure(self):
if len(self._dataset_root) > 0 and not exists(self._dataset_root):
raise RuntimeError('Data set root does not exists: %s' %
self._dataset_root)
def _string_to_task(self, task_name: str):
task_name = task_name.replace('.py', '')
try:
class_name = ''.join(
[w[0].upper() + w[1:] for w in task_name.split('_')])
mod = importlib.import_module("rlbench.tasks.%s" % task_name)
except Exception as e:
raise RuntimeError(
'Tried to interpret %s as a task, but failed. Only valid tasks '
'should belong in the tasks/ folder' % task_name) from e
return getattr(mod, class_name)
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 shutdown(self):
if self._pyrep is not None:
self._pyrep.shutdown()
self._pyrep = None
def get_task(self, task_class: Type[Task]) -> TaskEnvironment:
# If user hasn't called launch, implicitly call it.
if self._pyrep is None:
self.launch()
self._scene.unload()
task = task_class(self._pyrep, self._robot)
self._prev_task = task
return TaskEnvironment(self._pyrep, self._robot, self._scene, task,
self._action_mode, self._dataset_root,
self._obs_config, self._static_positions,
self._attach_grasped_objects)
@property
def action_size(self):
arm_action_size = 0
gripper_action_size = 1 # Only one gripper style atm
if (self._action_mode.arm == ArmActionMode.ABS_JOINT_VELOCITY
or self._action_mode.arm == ArmActionMode.DELTA_JOINT_VELOCITY
or self._action_mode.arm == ArmActionMode.ABS_JOINT_POSITION
or self._action_mode.arm == ArmActionMode.DELTA_JOINT_POSITION
or self._action_mode.arm == ArmActionMode.ABS_JOINT_TORQUE
or self._action_mode.arm == ArmActionMode.DELTA_JOINT_TORQUE):
arm_action_size = SUPPORTED_ROBOTS[self._robot_configuration][2]
elif (self._action_mode.arm == ArmActionMode.ABS_EE_POSE_WORLD_FRAME or
self._action_mode.arm == ArmActionMode.DELTA_EE_POSE_WORLD_FRAME
or self._action_mode.arm
== ArmActionMode.ABS_EE_POSE_PLAN_WORLD_FRAME
or self._action_mode.arm
== ArmActionMode.DELTA_EE_POSE_PLAN_WORLD_FRAME
or self._action_mode.arm == ArmActionMode.EE_POSE_PLAN_EE_FRAME
or self._action_mode.arm == ArmActionMode.EE_POSE_EE_FRAME):
arm_action_size = 7 # pose is always 7
return arm_action_size + gripper_action_size
def get_demos(self,
task_name: str,
amount: int,
variation_number=0,
image_paths=False) -> List[Demo]:
if self._dataset_root is None or len(self._dataset_root) == 0:
raise RuntimeError(
"Can't ask for a stored demo when no dataset root provided.")
demos = utils.get_stored_demos(amount, image_paths, self._dataset_root,
variation_number, task_name,
self._obs_config)
return demos
class CoppeliaSimEnv(gym.Env):
"""
Superclass for CoppeliaSim gym environments.
"""
metadata = {"render.modes": ["human"]}
def __init__(self,
scene: str,
dt: float,
model: str = None,
headless_mode: bool = False):
"""
Class constructor
Args:
scene: String, name of the scene to be loaded
dt: Float, a time step of the simulation
model: Optional[String], name of the model that to be imported
headless_mode: Bool, define mode of the simulation
"""
self.seed()
self._assets_path = os.path.join(
os.path.dirname(os.path.realpath(__file__)), "assets")
self._scenes_path = os.path.join(self._assets_path, "scenes")
self._models_path = os.path.join(self._assets_path, "models")
self._pr = PyRep()
self._launch_scene(scene, headless_mode)
self._import_model(model)
if not headless_mode:
self._clear_gui()
self._pr.set_simulation_timestep(dt)
self._pr.start()
def _launch_scene(self, scene: str, headless_mode: bool):
assert os.path.splitext(scene)[1] == ".ttt"
assert scene in os.listdir(self._scenes_path)
scene_path = os.path.join(self._scenes_path, scene)
self._pr.launch(scene_path, headless=headless_mode)
def _import_model(self, model: str):
if model is not None:
assert os.path.splitext(model)[1] == ".ttm"
assert model in os.listdir(self._models_path)
model_path = os.path.join(self._models_path, model)
self._pr.import_model(model_path)
@staticmethod
def _clear_gui():
sim.simSetBoolParameter(simConst.sim_boolparam_browser_visible, False)
sim.simSetBoolParameter(simConst.sim_boolparam_hierarchy_visible,
False)
sim.simSetBoolParameter(simConst.sim_boolparam_console_visible, False)
def step(self, action):
return NotImplementedError
def reset(self):
return NotImplementedError
def close(self):
self._pr.stop()
self._pr.shutdown()
def seed(self, seed: int = None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def render(self, mode: str = "human") -> NoReturn:
print("Not implemented yet")
class GraspEnv(object):
def __init__(self, headless, control_mode='joint_velocity'):
self.headless = headless
self.reward_offset = 10.0
self.reward_range = self.reward_offset
self.penalty_offset = 1
#self.penalty_offset = 1.
self.fall_down_offset = 0.1
self.metadata = [] #gym env argument
self.control_mode = control_mode
#launch and setup the scene, and set the proxy variables in present of the counterparts in the scene
self.pr = PyRep()
if control_mode == 'end_position':
SCENE_FILE = join(dirname(abspath(__file__)),
'./scenes/UnicarRobot_ik.ttt')
elif control_mode == 'joint_velocity':
SCENE_FILE = join(dirname(abspath(__file__)),
'./scenes/UnicarRobot.ttt')
self.pr.launch(SCENE_FILE, headless=headless)
self.pr.start()
self.agent = UnicarRobotArm() #drehkranz + UR10
#self.gripper = UnicarRobotGreifer()#suction
#self.suction = UnicarRobotGreifer()
self.suction = Shape("UnicarRobotGreifer_body_sub0")
self.proximity_sensor = ProximitySensor('UnicarRobotGreifer_sensor')
self.table = Shape('UnicarRobotTable')
self.carbody = Shape('UnicarRobotCarbody')
if control_mode == 'end_position':
self.agent.set_control_loop_enabled(True)
self.action_space = np.zeros(4)
elif control_mode == 'joint_velocity':
self.agent.set_control_loop_enabled(False)
self.action_space = np.zeros(7)
else:
raise NotImplementedError
#self.observation_space = np.zeros(17)
self.observation_space = np.zeros(20)
self.agent.set_motor_locked_at_zero_velocity(True)
self.target = Shape("UnicarRobotTarget") #Box
self.agent_ee_tip = self.agent.get_tip()
self.tip_target = Dummy('UnicarRobotArm_target')
self.tip_pos = self.agent_ee_tip.get_position()
# set a proper initial robot gesture or tip position
if control_mode == 'end_position':
initial_pos = [0, 1.5, 1.6]
self.tip_target.set_position(initial_pos)
#one big step for rotatin is enough, with reset_dynamics = True, set the rotation instantaneously
self.tip_target.set_orientation([0, 0, 0], reset_dynamics=True)
elif control_mode == 'joint_velocity':
self.initial_joint_positions = [0, 0, 0, 0, 0, 0, 0]
self.agent.set_joint_positions(self.initial_joint_positions)
self.pr.step()
self.initial_tip_positions = self.agent_ee_tip.get_position()
self.initial_target_positions = self.target.get_position()
def _get_state(self):
'''
Return state containging arm joint positions/velocities & target position.
'''
return np.array(self.agent.get_joint_positions() +
self.agent.get_joint_velocities() +
self.agent_ee_tip.get_position() +
self.agent_ee_tip.get_orientation()) #all 20
def _is_holding(self):
'''
Return the state of holding the UnicarRobotTarget or not, return bool value
'''
if self.proximity_sensor.is_detected(self.target) == True:
return True
else:
return False
def _move(self,
action,
bounding_offset=0.15,
step_factor=0.2,
max_itr=20,
max_error=0.05,
rotation_norm=5):
pos = self.suction.get_position()
if pos[0]+action[0] > POS_MIN[0]-bounding_offset and pos[0]+action[0] < POS_MAX[0]-bounding_offset \
and pos[1]+action[1] > POS_MIN[1]-bounding_offset and pos[1]+action[1] < POS_MAX[1]-bounding_offset \
and pos[2]+action[2] > POS_MIN[2]-2*bounding_offset:
ori_z = -self.agent_ee_tip.get_orientation(
)[2] # the minus is because the mismatch between the set_orientation() and get_orientation()
#ori_z = self.agent_ee_tip.get_orientation()[2]
target_pos = np.array(self.agent_ee_tip.get_position() +
np.array(action[:3]))
diff = 1
itr = 0
while np.sum(np.abs(diff)) > max_error and itr < max_itr:
itr += 1
# set pos in small step
cur_pos = self.agent_ee_tip.get_position()
diff = target_pos - cur_pos
pos = cur_pos + step_factor * diff
self.tip_target.set_position(pos.tolist())
self.pr.step()
ori_z += rotation_norm * action[3]
self.tip_target.set_orientation([0, np.pi, ori_z])
self.pr.step()
else:
print("Potantial Movement Out of the Bounding Box!")
self.pr.step()
def reinit(self):
self.shutdown()
self.__init__(self.headless)
def reset(self, random_target=False):
'''
Get a random position within a cuboid and set the target position
'''
# set target object
if random_target:
pos = list(np.random.uniform(POS_MIN, POS_MAX))
self.target.set_position(pos)
else:
self.target.set_position(self.initial_target_positions)
self.target.set_orientation([0, 0, 0])
self.pr.step()
#set end position to be initialized
if self.control_mode == 'end_position':
self.agent.set_control_loop_enabled(True) # IK mode
self.tip_target.set_position(self.initial_tip_positions)
self.pr.step()
itr = 0
max_itr = 10
while np.sum(
np.abs(
np.array(self.agent_ee_tip.get_position() -
np.array(self.initial_tip_positions)))
) > 0.1 and itr < max_itr:
itr += 1
self.step(np.random.uniform(-0.2, 0.2, 4))
self.pr.step()
elif self.control_mode == 'joint_velocity': #JointMode Force
self.agent.set_joint_positions(self.initial_joint_positions)
self.pr.step()
#set collidable, for collision detection
#self.gripper.set_collidable(True)
self.suction.set_collidable(True)
self.target.set_collidable(True)
return self._get_state() #return the current state of the environment
def step(self, action):
'''
move the robot arm according to the control mode
if control_mode == 'end_position' then action is 3 dim of tip (end of robot arm) position values + 1 dim rotation of suction
if control_mode == 'joint_velocity' then action is 7 dim of joint velocity + 1 dim of rotation of suction
'''
#initialization
done = False #episode finishes
reward = 0
hold_flag = False #hold the object or not
if self.control_mode == 'end_position':
if action is None or action.shape[0] != 4: #check action is valid
print('No actions or wrong action dimensions!')
action = list(np.random.uniform(-0.1, 0.1, 4))
self._move(action)
elif self.control_mode == 'joint_velocity':
if action is None or action.shape[0] != 7: #???
print('No actions or wrong action dimensions!')
action = list(np.random.uniform(-1, 1, 7))
self.agent.set_joint_target_velocities(action)
self.pr.step()
else:
raise NotImplementedError
#ax,ay,az = self.gripper.get_position()#gripper position
ax, ay, az = self.suction.get_position() #suction position
if math.isnan(
ax): #capture the broken suction cases during exploration
print("Suction position is nan.")
self.reinit()
done = True
desired_position_tip = [0.0, 1.5513, 1.74]
desired_orientation_tip = [-np.pi, 0, 0.001567]
tip_x, tip_y, tip_z = self.agent_ee_tip.get_position(
) #end_effector position
tip_row, tip_pitch, tip_yaw = self.agent_ee_tip.get_orientation(
) #end_effector orientation
tx, ty, tz = self.target.get_position() #box position
offset = 0.312 #augmented reward: offset of target position above the target object
#square distance between the gripper and the target object
sqr_distance = np.sqrt((tip_x - desired_position_tip[0])**2 +
(tip_y - desired_position_tip[1])**2 +
(tip_z - desired_position_tip[2])**2)
sqr_orientation = np.sqrt((tip_row - desired_orientation_tip[0])**2 +
(tip_pitch - desired_orientation_tip[1])**2 +
(tip_yaw - desired_orientation_tip[2])**2)
''' for visual-based control only, large time consumption! '''
# current_vision = self.vision_sensor.capture_rgb() # capture a screenshot of the view with vision sensor
# plt.imshow(current_vision)
# plt.savefig('./img/vision.png')
desired_orientation = [0, 0, -np.pi / 2]
desired_orientation_tip = [-np.pi, 0, 0.001567]
#Enable the suction if close enough to the object and the object is detected with the proximity sensor
if sqr_distance < 0.001 and self.proximity_sensor.is_detected(
self.target) == True and sqr_orientation < 0.001:
#make sure the suction is not worked
self.suction.release()
self.pr.step()
self.suction.grasp(self.target)
self.pr.step()
if self._is_holding():
reward += self.reward_offset
done = True
hold_flag = True
else:
self.suction.release()
self.pr.step()
else:
pass
#the base reward is negative distance from suction to target
#reward -= (np.sqrt(sqr_distance))
#case when the object is fall off the table
if tz < self.initial_target_positions[
2] - self.fall_down_offset: #tz is target(box) position in z direction
done = True
reward -= self.reward_offset
# Augmented reward for orientation: better grasping gesture if the suction has vertical orientation to the target object
desired_position_tip = [0.0, 1.5513, 1.74]
tip_x, tip_y, tip_z = self.agent_ee_tip.get_position()
tip_row, tip_pitch, tip_yaw = self.agent_ee_tip.get_orientation()
reward -= (np.sqrt((tip_x - desired_position_tip[0])**2 +
(tip_y - desired_position_tip[1])**2 +
(tip_z - desired_position_tip[2])**2) +
np.sqrt((tip_row - desired_orientation_tip[0])**2 +
(tip_pitch - desired_orientation_tip[1])**2 +
(tip_yaw - desired_orientation_tip[2])**2))
#Penalty for collision with the table
if self.suction.check_collision(
self.table) or self.suction.check_collision(
self.carbody) or self.agent.check_collision(
self.table) or self.suction.check_collision(
self.target) or self.agent.check_collision(
self.target):
reward -= self.penalty_offset
if math.isnan(reward):
reward = 0.
return self._get_state(), reward, done, {'finished': hold_flag}
def shutdown(self):
'''close the simulator'''
self.pr.stop()
self.pr.shutdown()
class ControllerTester:
"""
A class to test performance of torque controller based on VREP simulation
"""
def __init__(self):
rospy.init_node("controller_tester", anonymous=True)
rospy.loginfo("controller tester node is initialized...")
self.target_pub = rospy.Publisher("target_joint_angles", JointState, queue_size=1)
# Launch VREP using pyrep
self.pr = PyRep()
self.pr.launch(
f"/home/{os.environ['USER']}/Documents/igr/src/software_interface/vrep_robot_control/in-bore.ttt",
headless=False)
self.inbore = CtRobot(name='inbore_arm', num_joints=4, joint_type=['r','r','r','p'])
# Set up simulation parameter
self.dt = 0.002
self.pr.start()
self.pr.set_simulation_timestep(self.dt)
# Set up dynamics properties of arm
for j in range(1, self.inbore._num_joints + 1):
self.inbore.arms[j].set_dynamic(False)
self.inbore.arms[0].set_dynamic(False)
# Set up joint properties
for i in range(self.inbore._num_joints):
self.inbore.joints[i].set_joint_mode(JointMode.PASSIVE)
self.inbore.joints[i].set_control_loop_enabled(False)
self.inbore.joints[i].set_motor_locked_at_zero_velocity(True)
self.inbore.joints[i].set_joint_position(0)
self.inbore.joints[i].set_joint_target_velocity(0)
self.inbore.joints[1].set_joint_mode(JointMode.FORCE)
self.inbore.joints[1].set_control_loop_enabled(False)
self.inbore.joints[1].set_motor_locked_at_zero_velocity(True)
self.inbore.arms[2].set_dynamic(True)
self.inbore.joints[0].set_joint_mode(JointMode.FORCE)
self.inbore.joints[0].set_control_loop_enabled(False)
self.inbore.joints[0].set_motor_locked_at_zero_velocity(True)
self.inbore.arms[1].set_dynamic(True)
self.pr.step()
# Generate trajectory
t = sp.Symbol('t')
# Step response
traj = [
(-40/180*np.pi)*sp.ones(1),
(30/180*np.pi)*sp.ones(1),
(0/180*np.pi)*sp.ones(1),
0.000*sp.ones(1)
]
# traj = [
# (-30/180*np.pi)*sp.sin(t*4),
# (30/180*np.pi)*sp.cos(t*4),
# (30/180*np.pi)*sp.cos(t*4),
# 0.006*sp.sin(t/2)+0.006
# ]
self.pos = [sp.lambdify(t, i) for i in traj]
self.vel = [sp.lambdify(t, i.diff(t)) for i in traj]
self.acc = [sp.lambdify(t, i.diff(t).diff(t)) for i in traj]
def signal_handler(self, sig, frame):
"""
safely shutdown vrep when control C is pressed
"""
rospy.loginfo("Calling exit for pyrep")
self.shutdown_vrep()
rospy.signal_shutdown("from signal_handler")
def shutdown_vrep(self):
"""
shutdown vrep safely
"""
rospy.loginfo("Stopping pyrep.")
self.pr.stop()
rospy.loginfo("V-REP shutting down.")
self.pr.shutdown()
def publish(self):
t = 0
while not rospy.is_shutdown():
posd = [float(j(t)) for j in self.pos]
veld = [float(j(t)) for j in self.vel]
# accd = [float(j(t)) for j in self.acc]
target = JointState()
target.position = posd
target.velocity = veld
t = t + self.dt
signal.signal(signal.SIGINT, self.signal_handler)
self.target_pub.publish(target)
self.pr.step()
