def test_solve_ik_via_sampling(self): arm = Panda() waypoint = Dummy('Panda_waypoint') configs = arm.solve_ik_via_sampling( waypoint.get_position(), waypoint.get_orientation(), max_configs=5) self.assertIsNotNone(configs) self.assertEqual(len(configs), 5) current_config = arm.get_joint_positions() # Checks correct config (last) arm.set_joint_positions(configs[-1]) self.assertTrue(np.allclose( arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001)) # Checks correct config (first) arm.set_joint_positions(configs[0]) self.assertTrue(np.allclose( arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001)) # Checks order prev_config_dist = 0 for config in configs: config_dist = sum( [(c - f)**2 for c, f in zip(current_config, config)]) # This test requires that the metric scale for each joint remains at # 1.0 in _getConfigDistance lua function self.assertLessEqual(prev_config_dist, config_dist) prev_config_dist = config_dist
def render(self, mode='human'): if self._gym_cam is None: # Add the camera to the scene cam_placeholder = Dummy('cam_cinematic_placeholder') self._gym_cam = VisionSensor.create([640, 360]) self._gym_cam.set_pose(cam_placeholder.get_pose()) self._gym_cam.set_render_mode(RenderMode.OPENGL3_WINDOWED)
def test_solve_ik_via_jacobian(self): arm = Panda() waypoint = Dummy('Panda_waypoint') new_config = arm.solve_ik_via_jacobian( waypoint.get_position(), waypoint.get_orientation()) arm.set_joint_positions(new_config) self.assertTrue(np.allclose( arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001))
def __init__(self, task_class, observation_mode='state', render_mode: Union[None, str] = None): self._observation_mode = observation_mode self._render_mode = render_mode obs_config = ObservationConfig() if observation_mode == 'state': obs_config.set_all_high_dim(False) obs_config.set_all_low_dim(True) elif observation_mode == 'vision': obs_config.set_all(True) else: raise ValueError('Unrecognised observation_mode: %s.' % observation_mode) action_mode = ActionMode(ArmActionMode.ABS_JOINT_VELOCITY) self.env = Environment(action_mode, obs_config=obs_config, headless=True) self.env.launch() self.task = self.env.get_task(task_class) _, obs = self.task.reset() self.action_space = spaces.Box(low=-1.0, high=1.0, shape=(self.env.action_size, )) if observation_mode == 'state': self.observation_space = spaces.Box( low=-np.inf, high=np.inf, shape=obs.get_low_dim_data().shape) elif observation_mode == 'vision': self.observation_space = spaces.Dict({ "state": spaces.Box(low=-np.inf, high=np.inf, shape=obs.get_low_dim_data().shape), "left_shoulder_rgb": spaces.Box(low=0, high=1, shape=obs.left_shoulder_rgb.shape), "right_shoulder_rgb": spaces.Box(low=0, high=1, shape=obs.right_shoulder_rgb.shape), "wrist_rgb": spaces.Box(low=0, high=1, shape=obs.wrist_rgb.shape), "front_rgb": spaces.Box(low=0, high=1, shape=obs.front_rgb.shape), }) if render_mode is not None: # Add the camera to the scene cam_placeholder = Dummy('cam_cinematic_placeholder') self._gym_cam = VisionSensor.create([640, 360]) self._gym_cam.set_pose(cam_placeholder.get_pose()) if render_mode == 'human': self._gym_cam.set_render_mode(RenderMode.OPENGL3_WINDOWED) else: self._gym_cam.set_render_mode(RenderMode.OPENGL3)
def main(argv): obs_config = ObservationConfig(record_gripper_closing=True) obs_config.set_all(False) vrc = rand_every = None frequency = 0 if FLAGS.domain_randomization: vrc = VisualRandomizationConfig(FLAGS.textures_path) rand_every = RandomizeEvery.TRANSITION frequency = 10 env = Environment(ActionMode(), obs_config=obs_config, randomize_every=rand_every, frequency=frequency, visual_randomization_config=vrc, headless=FLAGS.headless) env.launch() # Add the camera to the scene cam_placeholder = Dummy('cam_cinematic_placeholder') cam = VisionSensor.create(FLAGS.camera_resolution) cam.set_pose(cam_placeholder.get_pose()) cam.set_parent(cam_placeholder) cam_motion = CircleCameraMotion(cam, Dummy('cam_cinematic_base'), 0.005) tr = TaskRecorder(env, cam_motion, fps=30) if len(FLAGS.tasks) > 0: task_names = FLAGS.tasks else: task_names = [t.split('.py')[0] for t in os.listdir(TASKS_PATH) if t != '__init__.py' and t.endswith('.py')] task_classes = [task_file_to_task_class( task_file) for task_file in task_names] for i, (name, cls) in enumerate(zip(task_names, task_classes)): good = tr.record_task(cls) if FLAGS.individual and good: tr.save(os.path.join(FLAGS.save_dir, '%s.avi' % name)) if not FLAGS.individual: tr.save(os.path.join(FLAGS.save_dir, 'recorded_tasks.avi')) env.shutdown()
def __init__(self, machine, camera, state="state"): super(CustomEnv, self).__init__() # Define action and observation space # They must be gym.spaces objects self.machine = machine self.camera = camera self.action_space = spaces.Discrete(N_ACTIONS) if state == "state": self.observation_space = spaces.Box(low=-10, high=10, shape=(1, 15)) elif state == "vision": self.observation_space = spaces.Box(low=0, high=1, shape=(128, 128, 3)) self.min_force = 1 cam_placeholder = Dummy('cam_cinematic_placeholder') self._gym_cam = VisionSensor.create([640, 360]) self._gym_cam.set_pose(cam_placeholder.get_pose()) self._gym_cam.set_render_mode(RenderMode.OPENGL3_WINDOWED) # self._gym_cam.set_render_mode(RenderMode.OPENGL3) self.state_rep = state
def render(self, mode='human'): # todo render available at any time if self._render_mode is None: self._render_mode = mode # Add the camera to the scene cam_placeholder = Dummy('cam_cinematic_placeholder') self._gym_cam = VisionSensor.create([640, 360]) self._gym_cam.set_pose(cam_placeholder.get_pose()) if mode == 'human': self._gym_cam.set_render_mode(RenderMode.OPENGL3_WINDOWED) else: self._gym_cam.set_render_mode(RenderMode.OPENGL3) if mode != self._render_mode: raise ValueError( 'The render mode must match the render mode selected in the ' 'constructor. \nI.e. if you want "human" render mode, then ' 'create the env by calling: ' 'gym.make("reach_target-state-v0", render_mode="human").\n' 'You passed in mode %s, but expected %s.' % (mode, self._render_mode)) if mode == 'rgb_array': return self._gym_cam.capture_rgb()
class Arm(RobotComponent): """Base class representing a robot arm with path planning support. """ def __init__(self, count: int, name: str, num_joints: int, base_name: str = None, max_velocity=1.0, max_acceleration=4.0, max_jerk=1000): """Count is used for when we have multiple copies of arms""" joint_names = ['%s_joint%d' % (name, i + 1) for i in range(num_joints)] super().__init__(count, name, joint_names, base_name) # Used for motion planning self.max_velocity = max_velocity self.max_acceleration = max_acceleration self.max_jerk = max_jerk # Motion planning handles suffix = '' if count == 0 else '#%d' % (count - 1) self._ik_target = Dummy('%s_target%s' % (name, suffix)) self._ik_tip = Dummy('%s_tip%s' % (name, suffix)) self._ik_group = vrep.simGetIkGroupHandle('%s_ik%s' % (name, suffix)) self._collision_collection = vrep.simGetCollectionHandle( '%s_arm%s' % (name, suffix)) def get_configs_for_tip_pose(self, position: List[float], euler: List[float] = None, quaternion: List[float] = None, ignore_collisions=False, trials=300, max_configs=60) -> List[List[float]]: """Gets a valid joint configuration for a desired end effector pose. Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param ignore_collisions: If collision checking should be disabled. :param trials: The maximum number of attempts to reach max_configs :param max_configs: The maximum number of configurations we want to generate before ranking them. :raises: ConfigurationError if no joint configuration could be found. :return: A list of valid joint configurations for the desired end effector pose. """ if not ((euler is None) ^ (quaternion is None)): raise ConfigurationPathError( 'Specify either euler or quaternion values, but not both.') prev_pose = self._ik_target.get_pose() self._ik_target.set_position(position) if euler is not None: self._ik_target.set_orientation(euler) elif quaternion is not None: self._ik_target.set_quaternion(quaternion) handles = [j.get_handle() for j in self.joints] # Despite verbosity being set to 0, OMPL spits out a lot of text with utils.suppress_std_out_and_err(): _, ret_floats, _, _ = utils.script_call( 'findSeveralCollisionFreeConfigsAndCheckApproach@PyRep', PYREP_SCRIPT_TYPE, ints=[ self._ik_group, self._collision_collection, int(ignore_collisions), trials, max_configs ] + handles) self._ik_target.set_pose(prev_pose) if len(ret_floats) == 0: raise ConfigurationError( 'Could not find a valid joint configuration for desired end effector pose.' ) num_configs = int(len(ret_floats) / len(handles)) return [[ ret_floats[len(handles) * i + j] for j in range(len(handles)) ] for i in range(num_configs)] def solve_ik(self, position: List[float], euler: List[float] = None, quaternion: List[float] = None) -> List[float]: """Solves an IK group and returns the calculated joint values. Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :return: A list containing the calculated joint values. """ self._ik_target.set_position(position) if euler is not None: self._ik_target.set_orientation(euler) elif quaternion is not None: self._ik_target.set_quaternion(quaternion) ik_result, joint_values = vrep.simCheckIkGroup( self._ik_group, [j.get_handle() for j in self.joints]) if ik_result == vrep.sim_ikresult_fail: raise IKError( 'IK failed. Perhaps the distance was between the tip ' ' and target was too large.') elif ik_result == vrep.sim_ikresult_not_performed: raise IKError('IK not performed.') return joint_values def get_path_from_cartesian_path( self, path: CartesianPath) -> ArmConfigurationPath: """Translate a path from cartesian space, to arm configuration space. Note: It must be possible to reach the start of the path via a linear path, otherwise an error will be raised. :param path: A :py:class:`CartesianPath` instance to be translated to a configuration-space path. :raises: ConfigurationPathError if no path could be created. :return: A path in the arm configuration space. """ handles = [j.get_handle() for j in self.joints] _, ret_floats, _, _ = utils.script_call( 'getPathFromCartesianPath@PyRep', PYREP_SCRIPT_TYPE, ints=[ path.get_handle(), self._ik_group, self._ik_target.get_handle() ] + handles) if len(ret_floats) == 0: raise ConfigurationPathError( 'Could not create a path from cartesian path.') return ArmConfigurationPath(self, ret_floats) def get_linear_path(self, position: List[float], euler: List[float] = None, quaternion: List[float] = None, steps=50, ignore_collisions=False) -> ArmConfigurationPath: """Gets a linear configuration path given a target pose. Generates a path that drives a robot from its current configuration to its target dummy in a straight line (i.e. shortest path in Cartesian space). Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param steps: The desired number of path points. Each path point contains a robot configuration. A minimum of two path points is required. If the target pose distance is large, a larger number of steps leads to better results for this function. :param ignore_collisions: If collision checking should be disabled. :raises: ConfigurationPathError if no path could be created. :return: A linear path in the arm configuration space. """ if not ((euler is None) ^ (quaternion is None)): raise ConfigurationPathError( 'Specify either euler or quaternion values, but not both.') prev_pose = self._ik_target.get_pose() self._ik_target.set_position(position) if euler is not None: self._ik_target.set_orientation(euler) elif quaternion is not None: self._ik_target.set_quaternion(quaternion) handles = [j.get_handle() for j in self.joints] # Despite verbosity being set to 0, OMPL spits out a lot of text with utils.suppress_std_out_and_err(): _, ret_floats, _, _ = utils.script_call( 'getLinearPath@PyRep', PYREP_SCRIPT_TYPE, ints=[ steps, self._ik_group, self._collision_collection, int(ignore_collisions) ] + handles) self._ik_target.set_pose(prev_pose) if len(ret_floats) == 0: raise ConfigurationPathError('Could not create path.') return ArmConfigurationPath(self, ret_floats) def get_nonlinear_path(self, position: List[float], euler: List[float] = None, quaternion: List[float] = None, ignore_collisions=False, trials=100, max_configs=60, trials_per_goal=6, algorithm=Algos.SBL) -> ArmConfigurationPath: """Gets a non-linear (planned) configuration path given a target pose. A path is generated by finding several configs for a pose, and ranking them according to the distance in configuration space (smaller is better). Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param ignore_collisions: If collision checking should be disabled. :param trials: The maximum number of attempts to reach max_configs :param max_configs: The maximum number of configurations we want to generate before ranking them. :param trials_per_goal: The number of paths per config we want to trial. :param algorithm: The algorithm for path planning to use. :raises: ConfigurationPathError if no path could be created. :return: A non-linear path in the arm configuration space. """ if not ((euler is None) ^ (quaternion is None)): raise ConfigurationPathError( 'Specify either euler or quaternion values, but not both.') prev_pose = self._ik_target.get_pose() self._ik_target.set_position(position) if euler is not None: self._ik_target.set_orientation(euler) elif quaternion is not None: self._ik_target.set_quaternion(quaternion) handles = [j.get_handle() for j in self.joints] # Despite verbosity being set to 0, OMPL spits out a lot of text with utils.suppress_std_out_and_err(): _, ret_floats, _, _ = utils.script_call( 'getNonlinearPath@PyRep', PYREP_SCRIPT_TYPE, ints=[ self._ik_group, self._collision_collection, int(ignore_collisions), trials, max_configs, trials_per_goal ] + handles, strings=[algorithm.value]) self._ik_target.set_pose(prev_pose) if len(ret_floats) == 0: raise ConfigurationPathError('Could not create path.') return ArmConfigurationPath(self, ret_floats) def get_path(self, position: List[float], euler: List[float] = None, quaternion: List[float] = None, ignore_collisions=False, trials=100, max_configs=60, trials_per_goal=6, algorithm=Algos.SBL) -> ArmConfigurationPath: """Tries to get a linear path, failing that tries a non-linear path. Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param ignore_collisions: If collision checking should be disabled. :param trials: The maximum number of attempts to reach max_configs. (Only applicable if a non-linear path is needed) :param max_configs: The maximum number of configurations we want to generate before ranking them. (Only applicable if a non-linear path is needed) :param trials_per_goal: The number of paths per config we want to trial. (Only applicable if a non-linear path is needed) :param algorithm: The algorithm for path planning to use. (Only applicable if a non-linear path is needed) :raises: ConfigurationPathError if neither a linear or non-linear path can be created. :return: A linear or non-linear path in the arm configuration space. """ try: p = self.get_linear_path(position, euler, quaternion, ignore_collisions=ignore_collisions) return p except ConfigurationPathError: pass # Allowed. Try again, but with non-linear. # This time if an exception is thrown, we dont want to catch it. p = self.get_nonlinear_path(position, euler, quaternion, ignore_collisions, trials, max_configs, trials_per_goal, algorithm) return p def get_tip(self) -> Dummy: """Gets the tip of the arm. Each arm is required to have a tip for path planning. :return: The tip of the arm. """ return self._ik_tip def get_jacobian(self): """Calculates the Jacobian. :return: the row-major Jacobian matix. """ self._ik_target.set_matrix(self._ik_tip.get_matrix()) vrep.simCheckIkGroup(self._ik_group, [j.get_handle() for j in self.joints]) jacobian, (rows, cols) = vrep.simGetIkGroupMatrix(self._ik_group, 0) jacobian = np.array(jacobian).reshape((rows, cols), order='F') return jacobian
def __init__(self, task_class, observation_mode='state', randomization_mode="none", rand_config=None, img_size=256, special_start=[], fixed_grip=-1, force_randomly_place=False, force_change_position=False, sparse=False, not_special_p = 0, ground_p = 0, special_is_grip=False, altview=False, procedural_ind=0, procedural_mode='same', procedural_set = [], is_mesh_obs=False, blank=False, COLOR_TUPLE=[1,0,0]): # blank is 0s agent. self.blank = blank #altview is whether to have second camera angle or not. True/False, "both" to concatentae the observations. self.altview=altview self.img_size=img_size self.sparse = sparse self.task_class = task_class self._observation_mode = observation_mode self._randomization_mode = randomization_mode #special start is a list of actions to take at the beginning. self.special_start = special_start #fixed_grip temp hack for keeping the gripper a certain way. Change later. 0 for fixed closed, 0.1 for fixed open, -1 for not fixed self.fixed_grip = fixed_grip #to force the task to be randomly placed self.force_randomly_place = force_randomly_place #force the task to change position in addition to rotation self.force_change_position = force_change_position obs_config = ObservationConfig() if observation_mode == 'state': obs_config.set_all_high_dim(False) obs_config.set_all_low_dim(True) elif observation_mode == 'vision' or observation_mode=="visiondepth" or observation_mode=="visiondepthmask": # obs_config.set_all(True) obs_config.set_all_high_dim(False) obs_config.set_all_low_dim(True) else: raise ValueError( 'Unrecognised observation_mode: %s.' % observation_mode) action_mode = ActionMode(ArmActionMode.DELTA_EE_POSE_PLAN_NOQ) print("using delta pose pan") if randomization_mode == "random": objs = ['target', 'boundary', 'Floor', 'Roof', 'Wall1', 'Wall2', 'Wall3', 'Wall4', 'diningTable_visible'] if rand_config is None: assert False self.env = DomainRandomizationEnvironment( action_mode, obs_config=obs_config, headless=True, randomize_every=RandomizeEvery.EPISODE, frequency=1, visual_randomization_config=rand_config ) else: self.env = Environment( action_mode, obs_config=obs_config, headless=True ) self.env.launch() self.task = self.env.get_task(task_class) # Probability. Currently used for probability that pick and lift task will start off gripper at a certain location (should probs be called non_special p) self.task._task.not_special_p = not_special_p # Probability that ground goal. self.task._task.ground_p = ground_p # For the "special" case, whether to grip the object or just hover above it. self.task._task.special_is_grip = special_is_grip # for procedural env self.task._task.procedural_ind = procedural_ind # procedural mode: same, increase, or random. self.task._task.procedural_mode = procedural_mode # ideally a list-like object, dictates the indices to sample from each episode. self.task._task.procedural_set = procedural_set # if state obs is mesh obs self.task._task.is_mesh_obs = is_mesh_obs self.task._task.sparse = sparse self.task._task.COLOR_TUPLE = COLOR_TUPLE _, obs = self.task.reset() cam_placeholder = Dummy('cam_cinematic_placeholder') cam_pose = cam_placeholder.get_pose().copy() #custom pose cam_pose = [ 1.59999931, 0. , 2.27999949 , 0.65328157, -0.65328145, -0.27059814, 0.27059814] cam_pose[0] = 1 cam_pose[2] = 1.5 self.frontcam = VisionSensor.create([img_size, img_size]) self.frontcam.set_pose(cam_pose) self.frontcam.set_render_mode(RenderMode.OPENGL) self.frontcam.set_perspective_angle(60) self.frontcam.set_explicit_handling(1) self.frontcam_mask = VisionSensor.create([img_size, img_size]) self.frontcam_mask.set_pose(cam_pose) self.frontcam_mask.set_render_mode(RenderMode.OPENGL_COLOR_CODED) self.frontcam_mask.set_perspective_angle(60) self.frontcam_mask.set_explicit_handling(1) if altview: alt_pose = [0.25 , -0.65 , 1.5, 0, 0.93879825 ,0.34169483 , 0] self.altcam = VisionSensor.create([img_size, img_size]) self.altcam.set_pose(alt_pose) self.altcam.set_render_mode(RenderMode.OPENGL) self.altcam.set_perspective_angle(60) self.altcam.set_explicit_handling(1) self.altcam_mask = VisionSensor.create([img_size, img_size]) self.altcam_mask.set_pose(alt_pose) self.altcam_mask.set_render_mode(RenderMode.OPENGL_COLOR_CODED) self.altcam_mask.set_perspective_angle(60) self.altcam_mask.set_explicit_handling(1) self.action_space = spaces.Box( low=-1.0, high=1.0, shape=(action_mode.action_size,)) if observation_mode == 'state': self.observation_space = spaces.Dict({ "observation": spaces.Box( low=-np.inf, high=np.inf, shape=self.task._task.get_state_obs().shape), "achieved_goal": spaces.Box( low=-np.inf, high=np.inf, shape=self.task._task.get_achieved_goal().shape), 'desired_goal': spaces.Box( low=-np.inf, high=np.inf, shape=self.task._task.get_desired_goal().shape) }) # Use the frontvision cam elif observation_mode == 'vision': self.frontcam.handle_explicitly() self.observation_space = spaces.Dict({ "state": spaces.Box( low=-np.inf, high=np.inf, shape=obs.get_low_dim_data().shape), "observation": spaces.Box( low=0, high=1, shape=self.frontcam.capture_rgb().transpose(2,0,1).flatten().shape, ) }) if altview == "both": example = self.frontcam.capture_rgb().transpose(2,0,1).flatten() self.observation_space = spaces.Dict({ "state": spaces.Box( low=-np.inf, high=np.inf, shape=obs.get_low_dim_data().shape), "observation": spaces.Box( low=0, high=1, shape=np.array([example,example]).shape, ) }) elif observation_mode == 'visiondepth': self.frontcam.handle_explicitly() self.observation_space = spaces.Dict({ "state": spaces.Box( low=-np.inf, high=np.inf, shape=obs.get_low_dim_data().shape), "observation": spaces.Box( #thinking about not flattening the shape, because primarily used for dataset and not for training low=0, high=1, shape=np.array([self.frontcam.capture_rgb().transpose(2,0,1), self.frontcam.capture_depth()[None,...]]).shape, ) }) elif observation_mode == 'visiondepthmask': self.frontcam.handle_explicitly() self.frontcam_mask.handle_explicitly() self.observation_space = spaces.Dict({ "state": spaces.Box( low=-np.inf, high=np.inf, shape=obs.get_low_dim_data().shape), "observation": spaces.Box( #thinking about not flattening the shape, because primarily used for dataset and not for training low=0, high=1, shape=np.array([self.frontcam.capture_rgb().transpose(2,0,1), self.frontcam.capture_depth()[None,...], rgb_handles_to_mask(self.frontcam_mask.capture_rgb())]).shape, ) }) if altview == "both": self.observation_space = spaces.Dict({ "state": spaces.Box( low=-np.inf, high=np.inf, shape=obs.get_low_dim_data().shape), "observation": spaces.Box( #thinking about not flattening the shape, because primarily used for dataset and not for training low=0, high=1, shape=np.array([self.frontcam.capture_rgb().transpose(2,0,1), self.frontcam.capture_rgb().transpose(2,0,1), self.frontcam.capture_depth()[None,...], rgb_handles_to_mask(self.frontcam_mask.capture_rgb())]).shape, ) }) self._gym_cam = None
class ArmPSM(PyRep): def __init__(self, pr, armNumber=1): """self.pr = PyRep() self.pr.launch(scenePath) self.pr.start() self.pr.step()""" self.pr = pr self.psm = armNumber self.ik_mode = 1 self.base_handle = Shape('RCM_PSM{}'.format(self.psm)) self.j1_handle = Joint('J1_PSM{}'.format(self.psm)) self.j2_handle = Joint('J2_PSM{}'.format(self.psm)) self.j3_handle = Joint('J3_PSM{}'.format(self.psm)) self.j4_handle = Joint('J1_TOOL{}'.format(self.psm)) self.j5_handle = Joint('J2_TOOL{}'.format(self.psm)) self.j6d_handle = Joint('J3_dx_TOOL{}'.format(self.psm)) self.j6s_handle = Joint('J3_sx_TOOL{}'.format(self.psm)) self.j5_dummy_handle = Dummy('J2_virtual_TOOL{}'.format(self.psm)) self.j6d_tip_dummy_handle = Dummy('J3_dx_tip_TOOL{}'.format(self.psm)) self.j6s_tip_dummy_handle = Dummy('J3_sx_tip_TOOL{}'.format(self.psm)) self.ik_target_dx_dummy_handle = Dummy('IK_target_dx_PSM{}'.format( self.psm)) self.ik_target_sx_dummy_handle = Dummy('IK_target_sx_PSM{}'.format( self.psm)) self.marker = Shape('yaw_{}'.format(self.psm)) self.EE_virtual_handle = Dummy('EE_virtual_TOOL{}'.format(self.psm)) self.ik_signal = IntegerSignal("run_IK_PSM{}".format(self.psm)) self.dyn_signal = IntegerSignal("run_dyn_PSM{}".format(self.psm)) #Set IK mode off to save on computation for VREP: self.setIkMode(0) #Set dynamics mode off to save on compuation time for VREP: self.setDynamicsMode(0) #dyn_mode = 1 turns on dynamics #dyn_mode = 0 turns off dynamics def setDynamicsMode(self, dyn_mode): self.dyn_mode = dyn_mode self.dyn_signal.set(self.dyn_mode) #ik_mode = 1 turns on ik_mode #ik_mode = 0 turns off ik_mode def setIkMode(self, ik_mode): self.ik_mode = ik_mode self.ik_signal.set(self.ik_mode) def posquat2Matrix(self, pos, quat): T = np.eye(4) T[0:3, 0:3] = quaternions.quat2mat([quat[-1], quat[0], quat[1], quat[2]]) T[0:3, 3] = pos return np.array(T) def matrix2posquat(self, T): pos = T[0:3, 3] quat = quaternions.mat2quat(T[0:3, 0:3]) quat = [quat[1], quat[2], quat[3], quat[0]] return np.array(pos), np.array(quat) def getJawAngle(self): pos6d = self.j6d_handle.get_joint_position() pos6s = self.j6s_handle.get_joint_position() jawAngle = 0.5 * (pos6d + pos6s) / 0.4106 return jawAngle def getJointAngles(self): pos1 = self.j1_handle.get_joint_position() pos2 = self.j2_handle.get_joint_position() pos3 = self.j3_handle.get_joint_position() pos4 = self.j4_handle.get_joint_position() pos5 = self.j5_handle.get_joint_position() pos6s = self.j6s_handle.get_joint_position() pos6d = self.j6d_handle.get_joint_position() pos6 = 0.5 * (pos6d - pos6s) jawAngle = 0.5 * (pos6d + pos6s) / 0.4106 jointAngles = np.array([pos1, pos2, pos3, pos4, pos5, pos6]) return jointAngles, jawAngle def getJointVelocities(self): vel1 = self.j1_handle.get_joint_velocity() vel2 = self.j2_handle.get_joint_velocity() vel3 = self.j3_handle.get_joint_velocity() vel4 = self.j4_handle.get_joint_velocity() vel5 = self.j5_handle.get_joint_velocity() vel6s = self.j6s_handle.get_joint_velocity() vel6d = self.j6d_handle.get_joint_velocity() vel6 = 0.5 * (vel6s - vel6d) jawVel = 0.5 * (vel6s + vel6d) / 0.4106 jointVelocities = np.array([vel1, vel2, vel3, vel4, vel5, vel6]) return jointVelocities, jawVel def setJointAngles(self, jointAngles, jawAngle): self.j1_handle.set_joint_position(jointAngles[0]) self.j2_handle.set_joint_position(jointAngles[1]) self.j3_handle.set_joint_position(jointAngles[2]) self.j4_handle.set_joint_position(jointAngles[3]) self.j5_handle.set_joint_position(jointAngles[4]) pos6s = 0.4106 * jawAngle - jointAngles[5] pos6d = 0.4106 * jawAngle + jointAngles[5] self.j6s_handle.set_joint_position(pos6s) self.j6d_handle.set_joint_position(pos6d) def getPoseAtJoint(self, j): if j == 0: pose = self.base_handle.get_pose() pos, quat = pose[0:3], pose[3:] elif j == 1: pose = self.j2_handle.get_pose(relative_to=self.base_handle) pos, quat = pose[0:3], pose[3:] T = self.posquat2Matrix(pos, quat) rot90x = [[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 0], [0, 0, 0, 1]] pos, quat = self.matrix2posquat(np.dot(T, rot90x)) elif j == 2: pose = self.j3_handle.get_pose(relative_to=self.base_handle) pos, quat = pose[0:3], pose[3:] T = self.posquat2Matrix(pos, quat) rot = [[0, 0, 1, 0], [-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 0, 1]] pos, quat = self.matrix2posquat(np.dot(T, rot)) elif j == 3: pose = self.j4_handle.get_pose(relative_to=self.base_handle) pos, quat = pose[0:3], pose[3:] T = self.posquat2Matrix(pos, quat) rot = [[ -1, 0, 0, 0, ], [0, -1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]] pos, quat = self.matrix2posquat(np.dot(T, rot)) elif j == 4: pose = self.j5_handle.get_pose(relative_to=self.base_handle) pos, quat = pose[0:3], pose[3:] T = self.posquat2Matrix(pos, quat) rot = [[0, 0, -1, 0], [1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 0, 1]] pos, quat = self.matrix2posquat(np.dot(T, rot)) elif j == 5: pose = self.j5_dummy_handle.get_pose(relative_to=self.base_handle) pos, quat = pose[0:3], pose[3:] else: pose = self.EE_virtual_handle.get_pose( relative_to=self.base_handle) pos, quat = pose[0:3], pose[3:] if j != 6: T = self.posquat2Matrix(pos, quat) ct = np.cos(0) st = np.sin(0) ca = np.cos(-np.pi / 2.0) sa = np.sin(-np.pi / 2.0) T_x = np.array([[1, 0, 0, 0], [0, ca, -sa, 0], [0, sa, ca, 0], [0, 0, 0, 1]]) T_z = np.array([[ct, -st, 0, 0], [st, ct, 0, 0], [0, 0, 1, 0.0102], [0, 0, 0, 1]]) T = np.dot(np.dot(T, T_x), T_z) pos, quat = self.matrix2posquat(T) return np.array(pos), np.array(quat) def getPoseAtEE(self): return self.getPoseAtJoint(6) #def getVelocityAtEE(self): # return self.EE_virtual_handle.get_velocity() def setPoseAtEE(self, pos, quat, jawAngle): theta = 0.4106 * jawAngle b_T_ee = self.posquat2Matrix(pos, quat) ee_T_sx = np.array([ [9.99191168e-01, 4.02120491e-02, -5.31786338e-06, 4.17232513e-07], [-4.01793160e-02, 9.98383134e-01, 4.02087139e-02, -1.16467476e-04], [1.62218404e-03, -4.01759782e-02, 9.99191303e-01, -3.61323357e-04], [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00] ]) ee_T_dx = np.array([[ -9.99191251e-01, -4.02099858e-02, -1.98098369e-06, 4.17232513e-07 ], [-4.01773877e-02, 9.98383193e-01, -4.02091818e-02, -1.16467476e-04], [ 1.61878841e-03, -4.01765831e-02, -9.99191284e-01, -3.61323357e-04 ], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00 ]]) b_T_sx = np.dot(b_T_ee, ee_T_sx) b_T_dx = np.dot(b_T_ee, ee_T_dx) ct = np.cos(theta) st = np.sin(theta) x_T_ts = np.array([[ ct, -st, 0, -st * 0.009, ], [st, ct, 0, ct * 0.009], [ 0, 0, 1, 0, ], [0, 0, 0, 1]]) pos_sx, quat_sx = self.matrix2posquat(np.dot(b_T_sx, x_T_ts)) pos_dx, quat_dx = self.matrix2posquat(np.dot(b_T_dx, x_T_ts)) self.ik_target_dx_dummy_handle.set_pose(np.r_[pos_dx, quat_dx], relative_to=self.base_handle) self.ik_target_sx_dummy_handle.set_pose(np.r_[pos_sx, quat_sx], relative_to=self.base_handle) def get_marker_position(self, relative_to=None): return self.marker.get_position(relative_to) """def stopSim(self):
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 Arm(RobotComponent): """Base class representing a robot arm with path planning support. """ def __init__(self, count: int, name: str, num_joints: int, base_name: str = None, max_velocity=1.0, max_acceleration=4.0, max_jerk=1000): """Count is used for when we have multiple copies of arms""" joint_names = ['%s_joint%d' % (name, i+1) for i in range(num_joints)] super().__init__(count, name, joint_names, base_name) # Used for motion planning self.max_velocity = max_velocity self.max_acceleration = max_acceleration self.max_jerk = max_jerk # Motion planning handles suffix = '' if count == 0 else '#%d' % (count - 1) self._ik_target = Dummy('%s_target%s' % (name, suffix)) self._ik_tip = Dummy('%s_tip%s' % (name, suffix)) self._ik_group = sim.simGetIkGroupHandle('%s_ik%s' % (name, suffix)) self._collision_collection = sim.simGetCollectionHandle( '%s_arm%s' % (name, suffix)) def set_ik_element_properties(self, constraint_x=True, constraint_y=True, constraint_z=True, constraint_alpha_beta=True, constraint_gamma=True) -> None: constraints = 0 if constraint_x: constraints |= sim.sim_ik_x_constraint if constraint_y: constraints |= sim.sim_ik_y_constraint if constraint_z: constraints |= sim.sim_ik_z_constraint if constraint_alpha_beta: constraints |= sim.sim_ik_alpha_beta_constraint if constraint_gamma: constraints |= sim.sim_ik_gamma_constraint sim.simSetIkElementProperties( ikGroupHandle=self._ik_group, tipDummyHandle=self._ik_tip.get_handle(), constraints=constraints, precision=None, weight=None, ) def set_ik_group_properties(self, resolution_method='pseudo_inverse', max_iterations=6, dls_damping=0.1) -> None: try: res_method = {'pseudo_inverse': sim.sim_ik_pseudo_inverse_method, 'damped_least_squares': sim.sim_ik_damped_least_squares_method, 'jacobian_transpose': sim.sim_ik_jacobian_transpose_method}[resolution_method] except KeyError: raise Exception('Invalid resolution method,' 'Must be one of ["pseudo_inverse" | "damped_least_squares" | "jacobian_transpose"]') sim.simSetIkGroupProperties( ikGroupHandle=self._ik_group, resolutionMethod=res_method, maxIterations=max_iterations, damping=dls_damping ) def solve_ik_via_sampling(self, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None, ignore_collisions: bool = False, trials: int = 300, max_configs: int = 1, distance_threshold: float = 0.65, max_time_ms: int = 10, relative_to: Object = None ) -> np.ndarray: """Solves an IK group and returns the calculated joint values. This IK method performs a random searches for manipulator configurations that matches the given end-effector pose in space. When the tip pose is close enough then IK is computed in order to try to bring the tip onto the target. This is the method that should be used when the start pose is far from the end pose. We generate 'max_configs' number of samples within X number of 'trials', before ranking them according to angular distance. Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param ignore_collisions: If collision checking should be disabled. :param trials: The maximum number of attempts to reach max_configs. :param max_configs: The maximum number of configurations we want to generate before sorting them. :param distance_threshold: Distance indicating when IK should be computed in order to try to bring the tip onto the target. :param max_time_ms: Maximum time in ms spend searching for each configuation. :param relative_to: Indicates relative to which reference frame we want the target pose. Specify None to retrieve the absolute pose, or an Object relative to whose reference frame we want the pose. :raises: ConfigurationError if no joint configuration could be found. :return: 'max_configs' number of joint configurations, ranked according to angular distance. """ if not ((euler is None) ^ (quaternion is None)): raise ConfigurationError( 'Specify either euler or quaternion values, but not both.') prev_pose = self._ik_target.get_pose() self._ik_target.set_position(position, relative_to) if euler is not None: self._ik_target.set_orientation(euler, relative_to) elif quaternion is not None: self._ik_target.set_quaternion(quaternion, relative_to) handles = [j.get_handle() for j in self.joints] cyclics, intervals = self.get_joint_intervals() low_limits, max_limits = list(zip(*intervals)) # If there are huge intervals, then limit them low_limits = np.maximum(low_limits, -np.pi*2).tolist() max_limits = np.minimum(max_limits, np.pi*2).tolist() collision_pairs = [] if not ignore_collisions: collision_pairs = [self._collision_collection, sim.sim_handle_all] metric = joint_options = None valid_joint_positions = [] for i in range(trials): config = sim.simGetConfigForTipPose( self._ik_group, handles, distance_threshold, int(max_time_ms), metric, collision_pairs, joint_options, low_limits, max_limits) if len(config) > 0: valid_joint_positions.append(config) if len(valid_joint_positions) >= max_configs: break self._ik_target.set_pose(prev_pose) if len(valid_joint_positions) == 0: raise ConfigurationError( 'Could not find a valid joint configuration for desired ' 'end effector pose.') if len(valid_joint_positions) > 1: current_config = np.array(self.get_joint_positions()) # Sort based on angular distance valid_joint_positions.sort( key=lambda x: np.linalg.norm(current_config - x)) return np.array(valid_joint_positions) def get_configs_for_tip_pose(self, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None, ignore_collisions=False, trials=300, max_configs=60, relative_to: Object = None ) -> List[List[float]]: """Gets a valid joint configuration for a desired end effector pose. Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param ignore_collisions: If collision checking should be disabled. :param trials: The maximum number of attempts to reach max_configs :param max_configs: The maximum number of configurations we want to generate before ranking them. :param relative_to: Indicates relative to which reference frame we want the target pose. Specify None to retrieve the absolute pose, or an Object relative to whose reference frame we want the pose. :raises: ConfigurationError if no joint configuration could be found. :return: A list of valid joint configurations for the desired end effector pose. """ warnings.warn("Please use 'solve_ik_via_sampling' instead.", DeprecationWarning) return list(self.solve_ik_via_sampling( position, euler, quaternion, ignore_collisions, trials, max_configs, relative_to=relative_to)) def solve_ik_via_jacobian( self, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None, relative_to: Object = None) -> List[float]: """Solves an IK group and returns the calculated joint values. This IK method performs a linearisation around the current robot configuration via the Jacobian. The linearisation is valid when the start and goal pose are not too far away, but after a certain point, linearisation will no longer be valid. In that case, the user is better off using 'solve_ik_via_sampling'. Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param relative_to: Indicates relative to which reference frame we want the target pose. Specify None to retrieve the absolute pose, or an Object relative to whose reference frame we want the pose. :return: A list containing the calculated joint values. """ self._ik_target.set_position(position, relative_to) if euler is not None: self._ik_target.set_orientation(euler, relative_to) elif quaternion is not None: self._ik_target.set_quaternion(quaternion, relative_to) ik_result, joint_values = sim.simCheckIkGroup( self._ik_group, [j.get_handle() for j in self.joints]) if ik_result == sim.sim_ikresult_fail: raise IKError('IK failed. Perhaps the distance was between the tip ' ' and target was too large.') elif ik_result == sim.sim_ikresult_not_performed: raise IKError('IK not performed.') return joint_values def solve_ik(self, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None, relative_to: Object = None) -> List[float]: """Solves an IK group and returns the calculated joint values. Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param relative_to: Indicates relative to which reference frame we want the target pose. Specify None to retrieve the absolute pose, or an Object relative to whose reference frame we want the pose. :return: A list containing the calculated joint values. """ warnings.warn("Please use 'solve_ik_via_jacobian' instead.", DeprecationWarning) return self.solve_ik_via_jacobian( position, euler, quaternion, relative_to) def get_path_from_cartesian_path(self, path: CartesianPath ) -> ArmConfigurationPath: """Translate a path from cartesian space, to arm configuration space. Note: It must be possible to reach the start of the path via a linear path, otherwise an error will be raised. :param path: A :py:class:`CartesianPath` instance to be translated to a configuration-space path. :raises: ConfigurationPathError if no path could be created. :return: A path in the arm configuration space. """ handles = [j.get_handle() for j in self.joints] _, ret_floats, _, _ = utils.script_call( 'getPathFromCartesianPath@PyRep', PYREP_SCRIPT_TYPE, ints=[path.get_handle(), self._ik_group, self._ik_target.get_handle()] + handles) if len(ret_floats) == 0: raise ConfigurationPathError( 'Could not create a path from cartesian path.') return ArmConfigurationPath(self, ret_floats) def get_linear_path(self, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None, steps=50, ignore_collisions=False, relative_to: Object = None) -> ArmConfigurationPath: """Gets a linear configuration path given a target pose. Generates a path that drives a robot from its current configuration to its target dummy in a straight line (i.e. shortest path in Cartesian space). Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param steps: The desired number of path points. Each path point contains a robot configuration. A minimum of two path points is required. If the target pose distance is large, a larger number of steps leads to better results for this function. :param ignore_collisions: If collision checking should be disabled. :param relative_to: Indicates relative to which reference frame we want the target pose. Specify None to retrieve the absolute pose, or an Object relative to whose reference frame we want the pose. :raises: ConfigurationPathError if no path could be created. :return: A linear path in the arm configuration space. """ if not ((euler is None) ^ (quaternion is None)): raise ConfigurationPathError( 'Specify either euler or quaternion values, but not both.') prev_pose = self._ik_target.get_pose() self._ik_target.set_position(position, relative_to) if euler is not None: self._ik_target.set_orientation(euler, relative_to) elif quaternion is not None: self._ik_target.set_quaternion(quaternion, relative_to) handles = [j.get_handle() for j in self.joints] collision_pairs = [] if not ignore_collisions: collision_pairs = [self._collision_collection, sim.sim_handle_all] joint_options = None ret_floats = sim.generateIkPath( self._ik_group, handles, steps, collision_pairs, joint_options) self._ik_target.set_pose(prev_pose) if len(ret_floats) == 0: raise ConfigurationPathError('Could not create path.') return ArmConfigurationPath(self, ret_floats) def get_nonlinear_path(self, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None, ignore_collisions=False, trials=300, max_configs=1, distance_threshold: float = 0.65, max_time_ms: int = 10, trials_per_goal=1, algorithm=Algos.RRTConnect, relative_to: Object = None ) -> ArmConfigurationPath: """Gets a non-linear (planned) configuration path given a target pose. A path is generated by finding several configs for a pose, and ranking them according to the distance in configuration space (smaller is better). Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param ignore_collisions: If collision checking should be disabled. :param trials: The maximum number of attempts to reach max_configs. See 'solve_ik_via_sampling'. :param max_configs: The maximum number of configurations we want to generate before sorting them. See 'solve_ik_via_sampling'. :param distance_threshold: Distance indicating when IK should be computed in order to try to bring the tip onto the target. See 'solve_ik_via_sampling'. :param max_time_ms: Maximum time in ms spend searching for each configuation. See 'solve_ik_via_sampling'. :param trials_per_goal: The number of paths per config we want to trial. :param algorithm: The algorithm for path planning to use. :param relative_to: Indicates relative to which reference frame we want the target pose. Specify None to retrieve the absolute pose, or an Object relative to whose reference frame we want the pose. :raises: ConfigurationPathError if no path could be created. :return: A non-linear path in the arm configuration space. """ handles = [j.get_handle() for j in self.joints] try: configs = self.solve_ik_via_sampling( position, euler, quaternion, ignore_collisions, trials, max_configs, distance_threshold, max_time_ms, relative_to) except ConfigurationError as e: raise ConfigurationPathError('Could not create path.') from e _, ret_floats, _, _ = utils.script_call( 'getNonlinearPath@PyRep', PYREP_SCRIPT_TYPE, ints=[self._collision_collection, int(ignore_collisions), trials_per_goal] + handles, floats=configs.flatten().tolist(), strings=[algorithm.value]) if len(ret_floats) == 0: raise ConfigurationPathError('Could not create path.') return ArmConfigurationPath(self, ret_floats) def get_path(self, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None, ignore_collisions=False, trials=300, max_configs=1, distance_threshold: float = 0.65, max_time_ms: int = 10, trials_per_goal=1, algorithm=Algos.RRTConnect, relative_to: Object = None ) -> ArmConfigurationPath: """Tries to get a linear path, failing that tries a non-linear path. Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param ignore_collisions: If collision checking should be disabled. :param trials: The maximum number of attempts to reach max_configs. See 'solve_ik_via_sampling'. :param max_configs: The maximum number of configurations we want to generate before sorting them. See 'solve_ik_via_sampling'. :param distance_threshold: Distance indicating when IK should be computed in order to try to bring the tip onto the target. See 'solve_ik_via_sampling'. :param max_time_ms: Maximum time in ms spend searching for each configuation. See 'solve_ik_via_sampling'. :param trials_per_goal: The number of paths per config we want to trial. :param algorithm: The algorithm for path planning to use. :param relative_to: Indicates relative to which reference frame we want the target pose. Specify None to retrieve the absolute pose, or an Object relative to whose reference frame we want the pose. :raises: ConfigurationPathError if neither a linear or non-linear path can be created. :return: A linear or non-linear path in the arm configuration space. """ try: p = self.get_linear_path(position, euler, quaternion, ignore_collisions=ignore_collisions, relative_to=relative_to) return p except ConfigurationPathError: pass # Allowed. Try again, but with non-linear. # This time if an exception is thrown, we dont want to catch it. p = self.get_nonlinear_path( position, euler, quaternion, ignore_collisions, trials, max_configs, distance_threshold, max_time_ms, trials_per_goal, algorithm, relative_to) return p def get_tip(self) -> Dummy: """Gets the tip of the arm. Each arm is required to have a tip for path planning. :return: The tip of the arm. """ return self._ik_tip def get_jacobian(self): """Calculates the Jacobian. :return: the row-major Jacobian matix. """ self._ik_target.set_matrix(self._ik_tip.get_matrix()) sim.simCheckIkGroup(self._ik_group, [j.get_handle() for j in self.joints]) jacobian, (rows, cols) = sim.simGetIkGroupMatrix(self._ik_group, 0) jacobian = np.array(jacobian).reshape((rows, cols), order='F') return jacobian def check_arm_collision(self, obj: 'Object' = None) -> bool: """Checks whether two entities are colliding. :param obj: The other collidable object to check collision against, or None to check against all collidable objects. Note that objects must be marked as collidable! :return: If the object is colliding. """ handle = sim.sim_handle_all if obj is None else obj.get_handle() return sim.simCheckCollision(self._collision_collection, handle) == 1
class Object_(): def __init__(self, name): self.obj_frame = Dummy(name + '_frame') self.obj_visible = Shape(name + '_visible') self.obj_respondable = Shape(name + '_respondable') self.obj_mask = Shape(name + '_mask') #region transform -> self.obj_frame def get_pose(self, relative_to=None): self.obj_frame.get_pose(relative_to=relative_to) def set_pose(self, pose, relative_to=None): """ :param pose: An array containing the (X,Y,Z,Qx,Qy,Qz,Qw) pose of the object. """ self.obj_frame.set_parent(None) self.obj_respondable.set_parent(self.obj_frame) # self.obj_visible.set_parent(self.obj_respondable) self.obj_frame.set_pose(pose, relative_to=relative_to) self.obj_respondable.set_parent(None) self.obj_frame.set_parent(self.obj_respondable) # self.obj_visible.set_parent(self.obj_respondable) #endregion #region physics and shape property -> self.shape def is_dynamic(self): return self.shape.is_dynamic() def set_dynamic(self, value: bool): self.shape.set_dynamic(value) def is_respondable(self): return self.shape.is_respondable() def set_respondable(self, value: bool): self.shape.set_respondable(value) def is_collidable(self): return self.shape.is_collidable() def set_collidable(self, value: bool): self.shape.set_collidable(value) def is_detectable(self): return self.shape.is_detectable() def set_detectable(self, value: bool): self.shape.set_detectable(value) def is_renderable(self): return self.shape.is_renderable() def set_renderable(self, value: bool): self.shape.set_renderable(value) def check_collision(self, obj=None): return self.shape.check_collision(obj) #endregion def remove(self): self.shape.remove() self.obj_frame.remove()