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