def move_j(self,q_target,env): _q_target = copy.copy(q_target) _q_target[6] += np.pi/4 q_start = np.array(self.get_joint_positions()) dq = (_q_target - q_start)/self.path_point_nums res = [] for i in range(self.path_point_nums): res.append(q_start + dq * i) res = np.array(res) res = res.reshape((1,-1)) path = ArmConfigurationPath(self, res.tolist()[0]) done = False while not done: done = path.step() env.step()
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_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 ) -> ArmConfigurationPath: # start joints = self.get_joint_positions() H_start = self.kine.fk(joints) # rot ~ rots = [get_rotation_part(H_start),self._rot_value(euler,quaternion)] slerp = Slerp([0,1], rots) times = [x/self.path_point_nums for x in range(self.path_point_nums+1)] interp_rots = slerp(times) # trans ~ d_position = (position - self.pose)/self.path_point_nums # ik ret_floats = [] q_guess = self.home_joints start_position = get_transition_part(H_start) for i in range(self.path_point_nums+1): H_target = set_rotation_part(np.eye(4),interp_rots[i]) H_target = set_position_part(H_target,start_position) q = self.kine.ik(H_target, q_guess) # solve_ik ret_floats.append(q) q_guess = q 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] # 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: 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)