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 TestSuctionCups(TestCore):
def setUp(self):
self.pyrep = PyRep()
self.pyrep.launch(path.join(ASSET_DIR, 'test_scene_robots.ttt'),
headless=True)
self.pyrep.step()
self.pyrep.start()
def test_get_suction_cup(self):
for cup_name, cup_type in SUCTION_CUPS:
with self.subTest(suction_cup=cup_name):
cup = cup_type()
self.assertIsInstance(cup, cup_type)
def test_grasp_and_release_with_suction(self):
for cup_name, cup_type in SUCTION_CUPS:
with self.subTest(suction_cup=cup_name):
suction_cube = Shape('%s_cube' % cup_name)
cube = Shape('cube')
cup = cup_type()
self.pyrep.step()
grasped = cup.grasp(cube)
self.assertFalse(grasped)
self.assertEqual(len(cup.get_grasped_objects()), 0)
grasped = cup.grasp(suction_cube)
self.assertTrue(grasped)
self.assertListEqual(cup.get_grasped_objects(), [suction_cube])
cup.release()
self.assertEqual(len(cup.get_grasped_objects()), 0)
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 = 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 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 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 TestArmsAndConfigurationPaths(TestCore):
def setUp(self):
self.pyrep = PyRep()
self.pyrep.launch(path.join(
ASSET_DIR, 'test_scene_robots.ttt'), headless=True)
self.pyrep.step()
self.pyrep.start()
def test_get_gripper(self):
for gripper_name, gripper_type, vel in GRIPPERS:
with self.subTest(gripper=gripper_name):
gripper = gripper_type()
self.assertIsInstance(gripper, gripper_type)
def test_close_open_gripper(self):
for gripper_name, gripper_type, vel in GRIPPERS:
with self.subTest(gripper=gripper_name):
gripper = gripper_type()
self.pyrep.step()
done = False
i = 0
while not done:
done = gripper.actuate(0.0, velocity=vel)
self.pyrep.step()
i += 1
if i > 1000:
self.fail('Took too many steps to close')
done = False
i = 0
open_amount = 1.0 if gripper_name == 'Robotiq85Gripper' else 0.8
while not done:
done = gripper.actuate(open_amount, velocity=vel)
self.pyrep.step()
i += 1
if i > 1000:
self.fail('Took too many steps to open')
self.assertAlmostEqual(
gripper.get_open_amount()[0], open_amount, delta=0.05)
def test_get_duplicate_gripper(self):
g = BaxterGripper(1)
self.assertIsInstance(g, BaxterGripper)
def test_copy_gripper(self):
g = JacoGripper()
new_g = g.copy()
self.assertNotEqual(g, new_g)
class Plane3D:
def __init__(self, headless=False):
self._pr = PyRep()
self._pr.launch(scene_file=scene_file, headless=headless)
self._pr.start()
self.workspace_base = Shape("workspace")
self.workspace = self._get_worksapce()
self.camera = VisionSensor("camera")
self.obstacles = []
self.velocity_scale = 0
self.repiration_cycle = 0
def _get_worksapce(self):
base_pos = self.workspace_base.get_position()
bbox = self.workspace_base.get_bounding_box()
min_pos = [bbox[2 * i] + base_pos[i] for i in range(3)]
max_pos = [bbox[2 * i + 1] + base_pos[i] for i in range(3)]
return [min_pos, max_pos]
def reset(self, obstacle_num, velocity_scale, respiration_cycle=0):
for obstacle in self.obstacles:
obstacle.remove()
self._pr.step()
self.velocity_scale = velocity_scale
self.repiration_cycle = respiration_cycle
self.obstacles = []
for i in range(obstacle_num):
obs = Obstacle2D.create_random_obstacle(
workspace=self.workspace,
velocity_scale=velocity_scale,
respiration_cycle=respiration_cycle)
self._pr.step()
self.obstacles.append(obs)
def get_image(self):
rgb = self.camera.capture_rgb()
return rgb
def step(self):
# update config
for obs in self.obstacles:
if self.repiration_cycle > 0:
obs.respire()
if self.velocity_scale > 0:
obs.keep_velocity()
self._pr.step()
class TestArmsAndConfigurationPaths(TestCore):
def setUp(self):
self.pyrep = PyRep()
self.pyrep.launch(path.join(
ASSET_DIR, 'test_scene_robots.ttt'), headless=True)
self.pyrep.step()
self.pyrep.start()
def test_get_gripper(self):
for gripper_name, gripper_type, vel in GRIPPERS:
with self.subTest(gripper=gripper_name):
gripper = gripper_type()
self.assertIsInstance(gripper, gripper_type)
def test_close_open_gripper(self):
for gripper_name, gripper_type, vel in GRIPPERS:
with self.subTest(gripper=gripper_name):
gripper = gripper_type()
self.pyrep.step()
done = False
i = 0
while not done:
done = gripper.actuate(0.0, velocity=vel)
self.pyrep.step()
i += 1
if i > 1000:
self.fail('Took too many steps to close')
done = False
i = 0
while not done:
done = gripper.actuate(0.8, velocity=vel)
self.pyrep.step()
i += 1
if i > 1000:
self.fail('Took too many steps to open')
self.assertAlmostEqual(
gripper.get_open_amount()[0], 0.8, delta=0.05)
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 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,
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 IK_via_vrep(robot: CtRobot,
pos: list,
ori: list,
pr: PyRep,
dt: float = 0.01):
'''
Input :
robot = Class of robot arm based on PyRep
pos = target position [x, y, z]
ori = target orientation [alpha, beta, gamma]
pr = PyRep handle
dt = desired simulation time (default = 0.01s)
The function is to call Pseudoinverse solver of VREP IK configuration
to perform inverse kinematics. And use 'step' function of PyRep to update
position of VREP model.
'''
# Set joint to be in IK mode and disable all dynamics of part
for i in range(robot._num_joints - 1):
robot.joints[i].set_joint_mode(JointMode.IK)
robot.arms[i].set_dynamic(False)
# Disable last joint to make sure needle is not inserted during robot setup
robot.joints[-1].set_joint_mode(JointMode.PASSIVE)
robot.arms[-1].set_dynamic(False)
# Set IK target to target pose
robot._ik_target.set_position(pos)
robot._ik_target.set_orientation(ori)
pr.step()
# Retrive joint position anc check whether it reach target pose
joint_pos = []
t = 0
er = reach_target(robot)
tmp = np.zeros(robot._num_joints)
while er[6] > 1.7e-4 and t < 4 * dt and er[7] > 3e-3:
# Precision is set to 0.1 mm and 0.1 deg in terms of pos and ori respectively
for i in range(robot._num_joints):
tmp[i] = robot.joints[i].get_joint_position()
joint_pos.append(tmp)
t += dt
er = reach_target(robot)
if er[6] < 1.7e-4 and er[7] < 3e-3:
print('Reached Target')
else:
if er[6] > 1.7e-4:
print(
'Unable to reach target with respect to position, Error is %.6f'
% er[6])
print('error on x-axis: %.6f' % er[0])
print('error on y-axis: %.6f' % er[1])
print('error on z-axis: %.6f' % er[2])
print('Check your movement of robot')
if er[7] > 3e-3:
print(
'Unable to reach target with respect to orientation, Error is %.6f'
% er[7])
print('error on x-axis: %.6f' % er[3])
print('error on y-axis: %.6f' % er[4])
print('error on z-axis: %.6f' % er[5])
print('Check your movement of robot')
print('(+) run the simulator')
print('(n) for new task.')
print('(s) to save the .ttm')
print('(r) to rename the task')
inp = input()
if inp == 'q':
break
if pr.running:
if inp == '+':
pr.stop()
pr.step_ui()
elif inp == 'p':
[pr.step() for _ in range(100)]
elif inp == 'd':
loaded_task.new_demo()
elif inp == 'v':
loaded_task.new_variation()
elif inp == 'e':
loaded_task.new_episode()
else:
if inp == '+':
loaded_task.reload_python()
loaded_task.reset_variation()
pr.start()
pr.step_ui()
elif inp == 'n':
inp = input('Do you want to save the current task first?\n')
if inp == 'y':
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]])
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 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 SpecificWorker(GenericWorker):
def __init__(self, proxy_map):
super(SpecificWorker, self).__init__(proxy_map)
def __del__(self):
print('SpecificWorker destructor')
def setParams(self, params):
SCENE_FILE = '../../etc/basic_scene.ttt'
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.robot = TurtleBot()
self.drone = Shape('Quadricopter')
cam = VisionSensor("frontCamera")
self.camera = {
"handle":
cam,
"id":
0,
"angle":
np.radians(cam.get_perspective_angle()),
"width":
cam.get_resolution()[0],
"height":
cam.get_resolution()[1],
"focal": (cam.get_resolution()[0] / 2) /
np.tan(np.radians(cam.get_perspective_angle() / 2)),
"rgb":
np.array(0),
"depth":
np.ndarray(0)
}
self.joystick_newdata = []
self.speed_robot = []
self.speed_robot_ant = []
self.last_received_data_time = 0
self.once = False
#@QtCore.Slot()
def compute(self):
cont = 0
start = time.time()
while True:
self.pr.step()
self.read_camera(self.camera)
self.read_joystick()
elapsed = time.time() - start
if elapsed < 0.05:
time.sleep(0.05 - elapsed)
cont += 1
if elapsed > 1:
print("Freq -> ", cont)
cont = 0
start = time.time()
###########################################
def read_camera(self, cam):
image_float = cam["handle"].capture_rgb()
depth = cam["handle"].capture_depth(True)
image = cv2.normalize(src=image_float,
dst=None,
alpha=0,
beta=255,
norm_type=cv2.NORM_MINMAX,
dtype=cv2.CV_8U)
cam["rgb"] = RoboCompCameraRGBDSimple.TImage(cameraID=cam["id"],
width=cam["width"],
height=cam["height"],
depth=3,
focalx=cam["focal"],
focaly=cam["focal"],
alivetime=time.time(),
image=image.tobytes())
cam["depth"] = RoboCompCameraRGBDSimple.TDepth(
cameraID=cam["id"],
width=cam["handle"].get_resolution()[0],
height=cam["handle"].get_resolution()[1],
focalx=cam["focal"],
focaly=cam["focal"],
alivetime=time.time(),
depthFactor=1.0,
depth=depth.tobytes())
# try:
# self.camerargbdsimplepub_proxy.pushRGBD(cam["rgb"], cam["depth"])
# except Ice.Exception as e:
# print(e)
###########################################
### JOYSITCK read and move the robot
###########################################
def read_joystick(self):
if self.joystick_newdata: #and (time.time() - self.joystick_newdata[1]) > 0.1:
datos = self.joystick_newdata[0]
adv = 0.0
rot = 0.0
side = 0.0
height = 0.0
for x in datos.axes:
if x.name == "advance":
adv = x.value / 20 if np.abs(x.value) > 0.001 else 0
if x.name == "rotate":
side = x.value / 20 if np.abs(x.value) > 0.001 else 0
if x.name == "tilt":
height = x.value / 20 if np.abs(x.value) > 0.001 else 0
if x.name == "side":
rot = x.value / 15 if np.abs(x.value) > 0.001 else 0
print("Joystick ", adv, side, height, rot)
self.move_quad_target([adv, side, height, rot])
self.joystick_newdata = None
self.last_received_data_time = time.time()
#################################################################################
def move_quad_target(self, vels):
target = Shape('Quadricopter_target')
adv, side, height, rot = vels
current_pos = target.get_position(self.drone)
current_ori = target.get_orientation(self.drone)
target.set_position([
current_pos[0] - adv, current_pos[1] - side,
current_pos[2] - height
], self.drone)
target.set_orientation([
current_ori[0] - adv, current_ori[1] - side, current_ori[2] - rot
], self.drone)
##################################################################################
# SUBSCRIPTION to sendData method from JoystickAdapter interface
###################################################################################
def JoystickAdapter_sendData(self, data):
self.joystick_newdata = [data, time.time()]
##################################################################################
# Methods for CameraRGBDSimple
# ===============================================================================
#
# getAll
#
def CameraRGBDSimple_getAll(self, camera):
return RoboCompCameraRGBDSimple.TRGBD(self.cameras[camera]["rgb"],
self.cameras[camera]["depth"])
#
# getDepth
#
def CameraRGBDSimple_getDepth(self, camera):
return self.cameras[camera]["depth"]
#
# getImage
#
def CameraRGBDSimple_getImage(self, camera):
return self.cameras[camera]["rgb"]
class TestArmsAndConfigurationPaths(TestCore):
def setUp(self):
self.pyrep = PyRep()
self.pyrep.launch(path.join(
ASSET_DIR, 'test_scene_robots.ttt'), headless=True)
self.pyrep.step()
self.pyrep.start()
# It is enough to only test the constructor of each arm (in there we make
# assumptions about the structure of the arm model). All other tests
# can be run on one arm.
def test_get_arm(self):
for arm_name, arm_type in ARMS:
with self.subTest(arm=arm_name):
arm = arm_type()
self.assertIsInstance(arm, arm_type)
def test_set_ik_element_properties(self):
arm = Panda()
arm.set_ik_element_properties(constraint_gamma=False)
arm.set_ik_element_properties()
def test_get_configs_for_tip_pose(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
configs = arm.get_configs_for_tip_pose(
waypoint.get_position(), waypoint.get_orientation())
self.assertIsNotNone(configs)
current_config = arm.get_joint_positions()
# Checks correct config (last)
arm.set_joint_positions(configs[-1])
self.assertTrue(np.allclose(
arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001))
# Checks correct config (first)
arm.set_joint_positions(configs[0])
self.assertTrue(np.allclose(
arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001))
# Checks order
prev_config_dist = 0
for config in configs:
config_dist = sum(
[(c - f)**2 for c, f in zip(current_config, config)])
# This test requires that the metric scale for each joint remains at
# 1.0 in _getConfigDistance lua function
self.assertLessEqual(prev_config_dist, config_dist)
prev_config_dist = config_dist
def test_get_path_from_cartesian_path(self):
arm = Panda()
cartesian_path = CartesianPath('Panda_cartesian_path')
path = arm.get_path_from_cartesian_path(cartesian_path)
self.assertIsNotNone(path)
def test_get_linear_path(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_linear_path(
waypoint.get_position(), waypoint.get_orientation())
self.assertIsNotNone(path)
def test_get_nonlinear_path(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_nonlinear_path(
waypoint.get_position(), waypoint.get_orientation())
self.assertIsNotNone(path)
def test_get_nonlinear_path_out_of_reach(self):
arm = Panda()
with self.assertRaises(ConfigurationPathError):
arm.get_nonlinear_path([99, 99, 99], [0.] * 3)
def test_get_linear_path_out_of_reach(self):
arm = Panda()
with self.assertRaises(ConfigurationPathError):
arm.get_linear_path([99, 99, 99], [0.] * 3)
def test_get_linear_path_and_step(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_linear_path(
waypoint.get_position(), waypoint.get_orientation())
self.assertIsNotNone(path)
done = False
while not done:
done = path.step()
self.pyrep.step()
self.assertTrue(np.allclose(
arm.get_tip().get_position(), waypoint.get_position(), atol=0.01))
def test_get_linear_path_and_get_end(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_linear_path(
waypoint.get_position(), waypoint.get_orientation())
path.set_to_end()
self.assertTrue(np.allclose(
arm.get_tip().get_position(), waypoint.get_position(), atol=0.001))
def test_get_linear_path_visualize(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_linear_path(
waypoint.get_position(), waypoint.get_orientation())
# Check that it does not error
path.visualize()
def test_get_duplicate_arm(self):
arm = UR3(1)
self.assertIsInstance(arm, UR3)
def test_copy_arm(self):
arm = UR10()
new_arm = arm.copy()
self.assertNotEqual(arm, new_arm)
def test_get_jacobian(self):
arm = Panda()
jacobian = arm.get_jacobian()
self.assertEqual(jacobian.shape, (7, 6))
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 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 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()
class SpecificWorker(GenericWorker):
def __init__(self, proxy_map):
super(SpecificWorker, self).__init__(proxy_map)
def __del__(self):
print('SpecificWorker destructor')
self.pr.stop()
self.pr.shutdown()
def setParams(self, params):
SCENE_FILE = params["scene_dir"]
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.cameras = {}
cam = VisionSensor("Camera_Shoulder")
self.cameras["Camera_Shoulder"] = {
"handle":
cam,
"id":
1,
"angle":
np.radians(cam.get_perspective_angle()),
"width":
cam.get_resolution()[0],
"height":
cam.get_resolution()[1],
"depth":
3,
"focal":
cam.get_resolution()[0] /
np.tan(np.radians(cam.get_perspective_angle())),
"position":
cam.get_position(),
"rotation":
cam.get_quaternion(),
"image_rgb":
np.array(0),
"image_rgbd":
np.array(0),
"depth":
np.ndarray(0)
}
self.grasping_objects = {}
can = Shape("can")
self.grasping_objects["002_master_chef_can"] = {
"handle": can,
"sim_pose": None,
"pred_pose_rgb": None,
"pred_pose_rgbd": None
}
with (open("objects_pcl.pickle", "rb")) as file:
self.object_pcl = pickle.load(file)
self.intrinsics = np.array(
[[
self.cameras["Camera_Shoulder"]["focal"], 0.00000000e+00,
self.cameras["Camera_Shoulder"]["width"] / 2.0
],
[
0.00000000e+00, self.cameras["Camera_Shoulder"]["focal"],
self.cameras["Camera_Shoulder"]["height"] / 2.0
], [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
self.arm_ops = {
"MoveToHome": 1,
"MoveToObj": 2,
"CloseGripper": 3,
"OpenGripper": 4
}
self.grasping_iter = 10
self.arm_base = Shape("gen3")
self.arm_target = Dummy("target")
self.gripper = Joint("RG2_openCloseJoint")
def compute(self):
print('SpecificWorker.compute...')
try:
self.pr.step()
# open the arm gripper
self.move_gripper(self.arm_ops["OpenGripper"])
# read arm camera RGB signal
cam = self.cameras["Camera_Shoulder"]
image_float = cam["handle"].capture_rgb()
depth = cam["handle"].capture_depth(in_meters=False)
image = cv2.normalize(src=image_float,
dst=None,
alpha=0,
beta=255,
norm_type=cv2.NORM_MINMAX,
dtype=cv2.CV_8U)
cam["image_rgb"] = RoboCompCameraRGBDSimple.TImage(
width=cam["width"],
height=cam["height"],
depth=3,
focalx=cam["focal"],
focaly=cam["focal"],
image=image.tobytes())
cam["image_rgbd"] = RoboCompCameraRGBDSimple.TImage(
width=cam["width"],
height=cam["height"],
depth=3,
focalx=cam["focal"],
focaly=cam["focal"],
image=image.tobytes())
cam["depth"] = RoboCompCameraRGBDSimple.TDepth(
width=cam["width"],
height=cam["height"],
depthFactor=1.0,
depth=depth.tobytes())
# get objects's poses from simulator
for obj_name in self.grasping_objects.keys():
self.grasping_objects[obj_name][
"sim_pose"] = self.grasping_objects[obj_name][
"handle"].get_pose()
# get objects' poses from RGB
pred_poses = self.objectposeestimationrgb_proxy.getObjectPose(
cam["image_rgb"])
self.visualize_poses(image_float, pred_poses, "rgb_pose.png")
for pose in pred_poses:
if pose.objectname in self.grasping_objects.keys():
obj_trans = [pose.x, pose.y, pose.z]
obj_quat = [pose.qx, pose.qy, pose.qz, pose.qw]
obj_pose = self.process_pose(obj_trans, obj_quat)
self.grasping_objects[
pose.objectname]["pred_pose_rgb"] = obj_pose
# get objects' poses from RGBD
pred_poses = self.objectposeestimationrgbd_proxy.getObjectPose(
cam["image_rgbd"], cam["depth"])
self.visualize_poses(image_float, pred_poses, "rgbd_pose.png")
for pose in pred_poses:
if pose.objectname in self.grasping_objects.keys():
obj_trans = [pose.x, pose.y, pose.z]
obj_quat = [pose.qx, pose.qy, pose.qz, pose.qw]
obj_pose = self.process_pose(obj_trans, obj_quat)
self.grasping_objects[
pose.objectname]["pred_pose_rgbd"] = obj_pose
# create a dummy for arm path planning
approach_dummy = Dummy.create()
approach_dummy.set_name("approach_dummy")
# initialize approach dummy in embedded lua scripts
call_ret = self.pr.script_call(
"initDummy@gen3", vrepConst.sim_scripttype_childscript)
# set object pose for the arm to follow
# NOTE : choose simulator or predicted pose
dest_pose = self.grasping_objects["002_master_chef_can"][
"pred_pose_rgbd"]
dest_pose[
2] += 0.04 # add a small offset along z-axis to grasp the object top
# set dummy pose with the pose of object to be grasped
approach_dummy.set_pose(dest_pose)
# move gen3 arm to the object
self.move_arm(approach_dummy, self.arm_ops["MoveToObj"])
# close the arm gripper
self.move_gripper(self.arm_ops["CloseGripper"])
# change approach dummy pose to the final destination pose
dest_pose[2] += 0.4
approach_dummy.set_pose(dest_pose)
# move gen3 arm to the final destination
self.move_arm(approach_dummy, self.arm_ops["MoveToObj"])
# remove the created approach dummy
approach_dummy.remove()
except Exception as e:
print(e)
return True
def process_pose(self, obj_trans, obj_rot):
# convert an object pose from camera frame to world frame
# define camera pose and z-axis flip matrix
cam_trans = self.cameras["Camera_Shoulder"]["position"]
cam_rot_mat = R.from_quat(self.cameras["Camera_Shoulder"]["rotation"])
z_flip = R.from_matrix(np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]))
# get object position in world coordinates
obj_trans = np.dot(
cam_rot_mat.as_matrix(),
np.dot(z_flip.as_matrix(),
np.array(obj_trans).reshape(-1, )))
final_trans = obj_trans + cam_trans
# get object orientation in world coordinates
obj_rot_mat = R.from_quat(obj_rot)
final_rot_mat = obj_rot_mat * z_flip * cam_rot_mat
final_rot = final_rot_mat.as_quat()
# return final object pose in world coordinates
final_pose = list(final_trans)
final_pose.extend(list(final_rot))
return final_pose
def visualize_poses(self, image, poses, img_name):
# visualize the predicted poses on RGB image
image = np.uint8(image * 255.0)
for pose in poses:
# visualize only defined objects
if pose.objectname not in self.grasping_objects.keys():
continue
obj_pcl = self.object_pcl[pose.objectname]
obj_trans = np.array([pose.x, pose.y, pose.z])
if img_name == "rgb_pose.png":
obj_trans[2] -= 0.2
obj_rot = R.from_quat([pose.qx, pose.qy, pose.qz,
pose.qw]).as_matrix()
proj_pcl = self.vertices_reprojection(obj_pcl, obj_rot, obj_trans,
self.intrinsics)
image = self.draw_pcl(image,
proj_pcl,
r=1,
color=(randint(0, 255), randint(0, 255),
randint(0, 255)))
cv2.imwrite(os.path.join("output", img_name), image)
def vertices_reprojection(self, vertices, r, t, k):
# re-project vertices in pixel space
p = np.matmul(k, np.matmul(r, vertices.T) + t.reshape(-1, 1))
p[0] = p[0] / (p[2] + 1e-5)
p[1] = p[1] / (p[2] + 1e-5)
return p[:2].T
def draw_pcl(self, img, p2ds, r=1, color=(255, 0, 0)):
# draw object point cloud on RGB image
h, w = img.shape[0], img.shape[1]
for pt_2d in p2ds:
pt_2d[0] = np.clip(pt_2d[0], 0, w)
pt_2d[1] = np.clip(pt_2d[1], 0, h)
img = cv2.circle(img, (int(pt_2d[0]), int(pt_2d[1])), r, color, -1)
return img
def move_arm(self, dummy_dest, func_number):
# move arm to destination
# NOTE : this function is using remote lua scripts embedded in the arm
# for better path planning, so make sure to use the correct arm model
call_function = True
init_pose = np.array(
self.arm_target.get_pose(relative_to=self.arm_base))
# loop until the arm reached the object
while True:
# step the simulation
self.pr.step()
# set function index to the desired operation
if call_function:
try:
# call thearded child lua scripts via PyRep
call_ret = self.pr.script_call(
"setFunction@gen3",
vrepConst.sim_scripttype_childscript,
ints=[func_number])
except Exception as e:
print(e)
# get current poses to compare
actual_pose = self.arm_target.get_pose(relative_to=self.arm_base)
object_pose = dummy_dest.get_pose(relative_to=self.arm_base)
# compare poses to check for operation end
pose_diff = np.abs(np.array(actual_pose) - np.array(init_pose))
if call_function and pose_diff[0] > 0.01 or pose_diff[
1] > 0.01 or pose_diff[2] > 0.01:
call_function = False
# check whether the arm reached the target
dest_pose_diff = np.abs(
np.array(actual_pose) - np.array(object_pose))
if dest_pose_diff[0] < 0.015 and dest_pose_diff[
1] < 0.015 and dest_pose_diff[2] < 0.015:
break
def move_gripper(self, func_number):
# open or close the arm gripper
# NOTE : this function is using remote lua scripts embedded in the arm
# for better path planning, so make sure to use the correct arm model
call_function = True
open_percentage = init_position = self.gripper.get_joint_position()
# loop until the gripper is completely open (or closed)
for iter in range(self.grasping_iter):
# step the simulation
self.pr.step()
# set function index to the desired operation
if call_function:
try:
# call thearded child lua scripts via PyRep
call_ret = self.pr.script_call(
"setFunction@gen3",
vrepConst.sim_scripttype_childscript,
ints=[func_number])
except Exception as e:
print(e)
# compare the gripper position to determine whether the gripper moved
if abs(self.gripper.get_joint_position() -
init_position) > 0.005:
call_function = False
# compare the gripper position to determine whether the gripper closed or opened
if not call_function and abs(open_percentage - self.gripper.
get_joint_position()) < 0.003:
break
#actualizamos el porcentaje de apertura
open_percentage = self.gripper.get_joint_position()
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])
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 BlockSim:
def __init__(self, move_tolerance=1e-3):
self.pr = PyRep()
self.pr.launch(headless=False)
self.pr.start()
self.move_tolerance = move_tolerance
self.blocks = ['T', 'p', 'V', 'L', 'Z', 'b', 'a']
def reset(self):
self.pr.step()
self.pr.stop()
def setup(self, actions):
self.pr.start()
# Make cube for
self.obj_list = []
self.block_order = []
for i, a in enumerate(actions):
col_idx = self.blocks.index(a[-1])
self.block_order.append(a[-1])
color = (cols.colors[col_idx][0:3]).tolist()
pose = np.array(' '.join(a[0:-1].split(',')).split()).reshape(
-1, 3).astype(int) * 0.05 + 0.05
block_list = []
for p in pose:
obj = Shape.create(type=PrimitiveShape.CUBOID,
color=color,
size=[0.05, 0.05, 0.05],
position=p.tolist())
obj.set_color(color)
block_list.append(obj)
self.obj_list.append(block_list)
for j in range(20):
self.pr.step()
return
def remove(self, piece):
try:
idx = self.block_order.index(piece)
parts = self.obj_list[idx]
for p in parts:
p.remove()
except:
print('No such piece. Not Removed.')
for j in range(2):
self.pr.step()
return self.check_moving()
def check_moving(self):
vels = []
for p in self.obj_list:
for block in p:
try:
t, w = block.get_velocity()
vels.append(np.sum(t**2) + np.sum(w**2))
except:
vels.append(0)
if np.sum(np.array(vels) > self.move_tolerance) > 1:
self.collapsed = True
return True
else:
return False
class CoppeliaSimEnvWrapper(EnvironmentSpec):
def __init__(self, visualize=True, mode="train", **params):
super().__init__(visualize=visualize, mode=mode)
# Scene selection
scene_file_path = os.path.join(os.getcwd(), 'simulation/UR5.ttt')
# Simulator launch
self.env = PyRep()
self.env.launch(scene_file_path, headless=False)
self.env.start()
self.env.step()
# Task related initialisations in Simulator
self.vision_sensor = objects.vision_sensor.VisionSensor(
"Vision_sensor")
self.gripper = objects.dummy.Dummy("UR5_target")
self.gripper_zero_pose = self.gripper.get_pose()
self.goal = objects.dummy.Dummy("goal_target")
self.goal_STL = objects.shape.Shape("goal")
self.goal_STL_zero_pose = self.goal_STL.get_pose()
self.grasped_STL = objects.shape.Shape("Peg")
self.stacking_area = objects.shape.Shape("Plane")
self.vision_sensor = objects.vision_sensor.VisionSensor(
"Vision_sensor")
self.step_counter = 0
self.max_step_count = 100
self.target_pose = None
self.initial_distance = None
self.image_width, self.image_height = 320, 240
self.vision_sensor.set_resolution(
(self.image_width, self.image_height))
self._history_len = 1
self._observation_space = Dict({
"cam_image":
Box(0,
255, [self.image_height, self.image_width, 1],
dtype=np.uint8)
})
self._action_space = Box(-1, 1, (3, ))
self._state_space = extend_space(self._observation_space,
self._history_len)
@property
def history_len(self):
return self._history_len
@property
def observation_space(self) -> Space:
return self._observation_space
@property
def state_space(self) -> Space:
return self._state_space
@property
def action_space(self) -> Space:
return self._action_space
def step(self, action):
done = False
info = {}
prev_distance_to_goal = self.distance_to_goal()
# Make a step in simulation
self.apply_controls(action)
self.env.step()
self.step_counter += 1
# Reward calculations
success_reward = self.success_check()
distance_reward = (prev_distance_to_goal -
self.distance_to_goal()) / self.initial_distance
reward = distance_reward + success_reward
# Check reset conditions
if self.step_counter > self.max_step_count:
done = True
logging.info('--------Reset: Timeout--------')
elif self.distance_to_goal() > 0.8:
done = True
logging.info('--------Reset: Too far from target--------')
elif self.collision_check():
done = True
logging.info('--------Reset: Collision--------')
return self.get_observation(), reward, done, info
def reset(self):
logging.info("Episode reset...")
self.step_counter = 0
self.env.stop()
self.env.start()
self.env.step()
self.setup_scene()
observation = self.get_observation()
return observation
# -------------- all methods above are required for any Gym environment, everything below is env-specific --------------
def distance_to_goal(self):
goal_pos = self.goal.get_position()
tip_pos = self.gripper.get_position()
return np.linalg.norm(np.array(tip_pos) - np.array(goal_pos))
def setup_goal(self):
goal_position = self.goal_STL_zero_pose[:3]
# 2D goal randomization
self.target_pose = [
goal_position[0] + (2 * np.random.rand() - 1.) * 0.1,
goal_position[1] + (2 * np.random.rand() - 1.) * 0.1,
goal_position[2]
]
self.target_pose = np.append(self.target_pose,
self.goal_STL_zero_pose[3:]).tolist()
self.goal_STL.set_pose(self.target_pose)
# Randomizing the RGB of the goal and the plane
rgb_values_goal = list(np.random.rand(3, ))
rgb_values_plane = list(np.random.rand(3, ))
self.goal_STL.set_color(rgb_values_goal)
self.stacking_area.set_color(rgb_values_plane)
self.initial_distance = self.distance_to_goal()
def setup_scene(self):
self.setup_goal()
self.gripper.set_pose(self.gripper_zero_pose)
def get_observation(self):
cam_image = self.vision_sensor.capture_rgb()
gray_image = np.uint8(
cv2.cvtColor(cam_image, cv2.COLOR_BGR2GRAY) * 255)
obs_image = np.expand_dims(gray_image, axis=2)
return {"cam_image": obs_image}
def collision_check(self):
return self.grasped_STL.check_collision(
self.stacking_area) or self.grasped_STL.check_collision(
self.goal_STL)
def success_check(self):
success_reward = 0.
if self.distance_to_goal() < 0.01:
success_reward = 1
logging.info('--------Success state--------')
return success_reward
def apply_controls(self, action):
gripper_position = self.gripper.get_position()
# predicted action is in range (-1, 1) so we are normalizing it to physical units
new_position = [
gripper_position[i] + (action[i] / 200.) for i in range(3)
]
self.gripper.set_position(new_position)
class SpecificWorker(GenericWorker):
def __init__(self, proxy_map):
super(SpecificWorker, self).__init__(proxy_map)
self.pinza = False
def __del__(self):
print('SpecificWorker destructor')
def setParams(self, params):
SCENE_FILE = '../etc/gen3-robolab.ttt'
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.mode = 0
self.bloqueo = True
#self.robot = Viriato()
#self.robot = YouBot()
self.robot_object = Shape("gen3")
self.cameras = {}
self.camera_arm_name = "camera_arm"
cam = VisionSensor(self.camera_arm_name)
self.cameras[self.camera_arm_name] = {
"handle":
cam,
"id":
0,
"angle":
np.radians(cam.get_perspective_angle()),
"width":
cam.get_resolution()[0],
"height":
cam.get_resolution()[1],
"focal": (cam.get_resolution()[0] / 2) /
np.tan(np.radians(cam.get_perspective_angle() / 2)),
"rgb":
np.array(0),
"depth":
np.ndarray(0)
}
self.joystick_newdata = []
self.last_received_data_time = 0
def compute(self):
tc = TimeControl(0.05)
while True:
self.pr.step()
self.read_joystick()
self.read_camera(self.cameras[self.camera_arm_name])
if self.pinza:
self.pr.script_call("close@RG2", 1)
else:
self.pr.script_call("open@RG2", 1)
tc.wait()
###########################################
def read_camera(self, cam):
image_float = cam["handle"].capture_rgb()
depth = cam["handle"].capture_depth(True)
image = cv2.normalize(src=image_float,
dst=None,
alpha=0,
beta=255,
norm_type=cv2.NORM_MINMAX,
dtype=cv2.CV_8U)
cam["rgb"] = RoboCompCameraRGBDSimple.TImage(cameraID=cam["id"],
width=cam["width"],
height=cam["height"],
depth=3,
focalx=cam["focal"],
focaly=cam["focal"],
alivetime=time.time(),
image=image.tobytes())
cam["depth"] = RoboCompCameraRGBDSimple.TDepth(
cameraID=cam["id"],
width=cam["handle"].get_resolution()[0],
height=cam["handle"].get_resolution()[1],
focalx=cam["focal"],
focaly=cam["focal"],
alivetime=time.time(),
depthFactor=1.0,
depth=depth.tobytes())
###########################################
### JOYSITCK read and move the robot
###########################################
def read_joystick(self):
if self.joystick_newdata:
print(self.joystick_newdata)
self.update_joystick(self.joystick_newdata[0])
self.joystick_newdata = None
self.last_received_data_time = time.time()
else:
elapsed = time.time() - self.last_received_data_time
if elapsed > 2 and elapsed < 3:
pass
def update_joystick(self, datos):
adv = advR = 0.0
rot = rotR = 0.0
side = sideR = 0.0
print(datos.buttons)
for x in datos.buttons:
if x.name == "mode":
self.mode += x.step
if self.mode % 2 == 1:
self.bloqueo = True
else:
self.bloqueo = False
for x in datos.axes:
print(x.name + "" + str(x.value))
if x.name == "X_axis":
adv = x.value if np.abs(x.value) > 1 else 0
advR = x.value if np.abs(x.value) > 1 else 0
if x.name == "Y_axis":
rot = x.value if np.abs(x.value) > 0.01 else 0
rotR = x.value if np.abs(x.value) > 0.01 else 0
if x.name == "Z_axis":
side = x.value if np.abs(x.value) > 1 else 0
sideR = x.value if np.abs(x.value) > 1 else 0
if x.name == "gripper":
if x.value > 1:
self.pr.script_call("open@RG2", 1)
print("abriendo")
if x.value < -1:
print("cerrando")
self.pr.script_call("close@RG2", 1)
dummy = Dummy("target")
parent_frame_object = None
position = dummy.get_position()
orientation = dummy.get_orientation()
if self.bloqueo == False:
print("modo0\n\n")
dummy.set_position([
position[0] + adv / 1000, position[1] + rot / 1000,
position[2] + side / 1000
], parent_frame_object)
else:
print("modo1\n\n")
dummy.set_orientation([
orientation[0] + advR / 1000, orientation[1] + rotR / 1000,
orientation[2] + sideR / 1000
], parent_frame_object)
##################################################################################
# SUBSCRIPTION to sendData method from JoystickAdapter interface
###################################################################################
def JoystickAdapter_sendData(self, data):
self.joystick_newdata = [data, time.time()]
##################################################################################
# Methods for CameraRGBDSimple
# ===============================================================================
#
# getAll
#
def CameraRGBDSimple_getAll(self, camera):
return RoboCompCameraRGBDSimple.TRGBD(self.cameras[camera]["rgb"],
self.cameras[camera]["depth"])
#
# getDepth
#
def CameraRGBDSimple_getDepth(self, camera):
return self.cameras[camera]["depth"]
#
# getImage
#
def CameraRGBDSimple_getImage(self, camera):
return self.cameras[camera]["rgb"]
# ===================================================================
# CoppeliaUtils
# ===================================================================
def CoppeliaUtils_addOrModifyDummy(self, type, name, pose):
if not Dummy.exists(name):
dummy = Dummy.create(0.1)
dummy.set_name(name)
else:
dummy = Dummy("target")
#parent_frame_object = None
parent_frame_object = Shape("gen3")
#print("Coppelia ", name, pose.x/1000, pose.y/1000, pose.z/1000)
#dummy.set_position([pose.x / 1000., pose.y / 1000., pose.z / 1000.], parent_frame_object)
dummy.set_position([pose.x, pose.y, pose.z], parent_frame_object)
print(pose.x, pose.y, pose.z)
print(dummy.get_position())
dummy.set_orientation([pose.rx, pose.ry, pose.rz],
parent_frame_object)
def KinovaArm_getCenterOfTool(self, referencedTo):
ret = RoboCompKinovaArm.TPose()
parent_frame_object = Shape('gen3')
tip = Dummy("tip")
pos = tip.get_position(parent_frame_object)
rot = tip.get_orientation(parent_frame_object)
ret.x = pos[0]
ret.y = pos[1]
ret.z = pos[2]
ret.rx = rot[0]
ret.ry = rot[1]
ret.rz = rot[2]
return ret
def KinovaArm_openGripper(self):
#self.pr.script_call("open@RG2", 1)
print("Opening gripper")
self.pinza = False
def KinovaArm_closeGripper(self):
#self.pr.script_call("close@RG2", 1)
print("Closing gripper")
self.pinza = True
#
# IMPLEMENTATION of setCenterOfTool method from KinovaArm interface
#
def KinovaArm_setCenterOfTool(self, pose, referencedTo):
target = Dummy("target")
parent_frame_object = Shape('gen3')
position = target.get_position(parent_frame_object)
#target.set_position([position[0] + pose.x / 1000, position[1] + pose.y / 1000, position[2] + pose.z / 1000], parent_frame_object)
target.set_position([
position[0] + pose.x / 1000, position[1] + pose.y / 1000,
position[2] + pose.z / 1000
], parent_frame_object)
def KinovaArm_setPosition(self, pose, referencedTo):
target = Dummy("target")
parent_frame_object = Shape('gen3')
position = target.get_position(parent_frame_object)
target.set_position([pose.x, pose.y, pose.z], parent_frame_object)
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 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
