Source code for pykin.planners.rrt_star_planner

import math
import numpy as np

from pykin.planners.planner import Planner
from pykin.planners.tree import Tree
from pykin.utils.fcl_utils import FclManager
from pykin.utils.kin_utils import get_robot_geom
from pykin.utils.error_utils import NotFoundError, CollisionError
from pykin.utils.transform_utils import get_homogeneous_matrix

[docs]class RRTStarPlanner(Planner): """ RRT star spath planning Args: robot(SingleArm or Bimanual): The manipulator robot type is SingleArm or Bimanual obstacles(Obstacle): The obstacles current_q(np.array or Iterable of floats): current joint angle goal_q(np.array or Iterable of floats): target joint angle obtained through Inverse Kinematics delta_distance(float): distance between nearest vertex and new vertex epsilon(float): 1-epsilon is probability of random sampling max_iter(int): maximum number of iterations gamma_RRT_star(int): factor used for search radius """ def __init__( self, robot, obstacles, current_q=None, goal_q=None, delta_distance=0.5, epsilon=0.2, max_iter=3000, gamma_RRT_star=300, # At least gamma_RRT > delta_distance, ): super(RRTStarPlanner, self).__init__(robot, obstacles) self.cur_q = current_q self.goal_q = goal_q self.delta_dis = delta_distance self.epsilon = epsilon self.max_iter = max_iter self.gamma_RRTs = gamma_RRT_star self.T = None self.cost = None self.arm = None self.dimension = self.robot.dof self.eef_name = self.robot.eef_name
[docs] def setup_start_goal_joint( self, current_q, goal_q, arm=None, init_transformation=None ): """ Setup start joints and goal joints Args: current_q(np.array or Iterable of floats): current joint angle goal_q(np.array or Iterable of floats): target joint angle obtained through Inverse Kinematics arm(str): If Manipulator types is bimanul, must put the arm type. init_transformation: Initial transformations """ if self._check_q_datas(current_q, goal_q): self.cur_q = current_q self.goal_q = goal_q self.arm = arm if init_transformation is None: init_transformation = self.robot.init_transformations self.dimension = len(current_q) self._setup_q_limits() self._setup_eef_name() self._setup_fcl_manager(init_transformation) self._check_init_collision()
@staticmethod def _check_q_datas(current_q, goal_q): """ Check input current joints and goal joints Args: current_q(np.array or Iterable of floats): current joint angle goal_q(np.array or Iterable of floats): target joint angle obtained through Inverse Kinematics Return: True(bool): Return True if everything is confirmed """ if not isinstance(current_q, (np.ndarray)): current_q = np.array(current_q) if not isinstance(goal_q, (np.ndarray)): goal_q = np.array(goal_q) if current_q.size == 0 or goal_q.size == 0: raise NotFoundError("Make sure set current or goal joints..") return True def _setup_q_limits(self): """ Setup joint limits (lower and upper) """ if self.arm is not None: self.q_limits_lower = self.robot.joint_limits_lower[self.arm] self.q_limits_upper = self.robot.joint_limits_upper[self.arm] else: self.q_limits_lower = self.robot.joint_limits_lower self.q_limits_upper = self.robot.joint_limits_upper def _check_q_in_limits(self, q_in): """ check q_in within joint limits If q_in is in joint limits, return True otherwise, return False Returns: bool(True or False) """ return np.all([q_in >= self.q_limits_lower, q_in <= self.q_limits_upper]) def _setup_eef_name(self): """ Setup end-effector name """ if self.arm is not None: self.eef_name = self.robot.eef_name[self.arm] def _setup_fcl_manager(self, transformatios): """ Setup fcl manager for collision checking """ self.fcl_manager = FclManager() self._apply_fcl_to_robot(transformatios) self._apply_fcl_to_obstacles() def _apply_fcl_to_robot(self, transformatios): """ Apply fcl to robot """ for link, transformation in transformatios.items(): name, gtype, gparam = get_robot_geom(self.robot.links[link]) transform = transformation.homogeneous_matrix self.fcl_manager.add_object(name, gtype, gparam, transform) def _apply_fcl_to_obstacles(self): """ Apply fcl to obstacles """ if self.obstacles: for key, vals in self.obstacles: obs_type = vals[0] obs_param = vals[1] obs_pos = vals[2] ob_transform = get_homogeneous_matrix(position=np.array(obs_pos)) self.fcl_manager.add_object(key, obs_type, obs_param, ob_transform) def _check_init_collision(self): """ Check collision between robot and obstacles """ is_collision, obj_names = self.fcl_manager.collision_check(return_names=True) if is_collision: for name1, name2 in obj_names: if not ("obstacle" in name1 and "obstacle" in name2): raise CollisionError(obj_names) goal_collision_free, collision_names = self.collision_free(self.goal_q, visible_name=True) if not goal_collision_free: for name1, name2 in collision_names: if ("obstacle" in name1 and "obstacle" not in name2) or \ ("obstacle" not in name1 and "obstacle" in name2): raise CollisionError(collision_names)
[docs] def generate_path(self): """ Generate planner path Returns: path(list) : result path (from start joints to goal joints) """ path = None self.T = Tree() self.cost = {} self.T.add_vertex(self.cur_q) self.cost[0] = 0 for k in range(self.max_iter): if k % 300 == 0 and k !=0: print(f"iter : {k}") rand_q = self.random_state() if not self.collision_free(rand_q): continue nearest_q, nearest_idx = self.nearest_neighbor(rand_q, self.T) new_q = self.new_state(nearest_q, rand_q) if self.collision_free(new_q) and self._check_q_in_limits(new_q): neighbor_indexes = self.get_near_neighbor_indices(new_q) min_cost = self.get_new_cost(nearest_idx, nearest_q, new_q) min_cost, nearest_idx = self.get_minimum_cost_and_index(neighbor_indexes, new_q, min_cost, nearest_idx) self.T.add_vertex(new_q) new_idx = len(self.T.vertices) - 1 self.cost[new_idx] = min_cost self.T.add_edge([nearest_idx, new_idx]) self.rewire(neighbor_indexes, new_q, new_idx) if self.reach_to_goal(new_q): path = self.find_path(self.T) return path
[docs] def random_state(self): """ sampling joints in q space within joint limits If random probability is greater than the epsilon, return random joint angles oterwise, return goal joint angles Returns: q_outs(np.array) : """ q_outs = np.zeros(self.dimension) if np.random.random() > self.epsilon: for i, (q_min, q_max) in enumerate(zip(self.q_limits_lower, self.q_limits_upper)): q_outs[i] = np.random.uniform(q_min, q_max) else: q_outs = self.goal_q return q_outs
[docs] def nearest_neighbor(self, random_q, tree): """ Find nearest neighbor vertex and index from random_q Args: random_q(np.array): sampled random joint angles tree(Tree): Trees obtained so far Returns: nearest_vertex(np.array): nearest vertex(joint angles) nearest_idx(int): nearest index """ distances = [self.distance(random_q, vertex) for vertex in tree.vertices] nearest_idx = np.argmin(distances) nearest_vertex = tree.vertices[nearest_idx] return nearest_vertex, nearest_idx
[docs] def distance(self, pointA, pointB): """ Get distance from pointA to pointB Args: pointA(np.array) pointB(np.array) Returns: Norm(float or ndarray) """ return np.linalg.norm(pointB - pointA)
[docs] def new_state(self, nearest_q, random_q): """ Get new point between nearest vertex and random vertex Args: nearest_q(np.array): nearest joint angles random_q(np.array): sampled random joint angles Returns: new_q(np.array): new joint angles """ if np.equal(nearest_q, random_q).all(): return nearest_q vector = random_q - nearest_q dist = self.distance(random_q, nearest_q) step = min(self.delta_dis, dist) unit_vector = vector / dist new_q = nearest_q + unit_vector * step return new_q
[docs] def get_near_neighbor_indices(self, q): """ Returns all neighbor indices within the search radius from the new vertex Args: q(np.array): new joint angles Returns: near_indexes(list): all neighbor indices """ card_V = len(self.T.vertices) + 1 r = self.gamma_RRTs * ((math.log(card_V) / card_V) ** (1/self.dimension)) search_radius = min(r, self.delta_dis) dist_list = [self.distance(vertex, q) for vertex in self.T.vertices] near_indexes = [] for idx, dist in enumerate(dist_list): if dist <= search_radius: near_indexes.append(idx) return near_indexes
[docs] def get_new_cost(self, idx, pointA, pointB): """ Returns new cost Args: idx(int): neighbor vertex's index A(np.array) B(np.array) Returns: cost(float) """ cost = self.cost[idx] + self.distance(pointA, pointB) return cost
[docs] def get_minimum_cost_and_index(self, neighbor_indexes, new_q, min_cost, nearest_idx): """ Returns minimum cost and neer vertex index between neighbor vertices and new vertex in search radius Args: neighbor_indexes: neighbor vertex's index new_q(int): new joint angles min_cost(np.array): minimum cost nearest_idx(np.array): nearest index Returns: min_cost(float) nearest_idx(int) """ for i in neighbor_indexes: new_cost = self.get_new_cost(i, new_q, self.T.vertices[i]) if new_cost < min_cost: min_cost = new_cost nearest_idx = i return min_cost, nearest_idx
[docs] def rewire(self, neighbor_indexes, new_q, new_idx): """ Rewire a new vertex with a neighbor vertex with minimum cost Args: neighbor_indexes: neighbor vertex's index new_q(int): new joint angles new_idx(np.array): new joint angles's index """ for i in neighbor_indexes: new_cost = self.get_new_cost(new_idx, new_q, self.T.vertices[i]) if new_cost < self.cost[i]: self.cost[i] = new_cost self.T.edges[i-1][0] = new_idx
[docs] def collision_free(self, new_q, visible_name=False): """ Check collision free between robot and obstacles If visible name is True, return collision result and collision object names otherwise, return only collision result Args: new_q(np.array): new joint angles visible_name(bool) Returns: result(bool): If collision free, return True names(set of 2-tup): The set of pairwise collisions. """ transformations = self._get_transformations(new_q) for link, transformations in transformations.items(): if link in self.fcl_manager._objs: transform = transformations.homogeneous_matrix self.fcl_manager.set_transform(name=link, transform=transform) is_collision, name = self.fcl_manager.collision_check(return_names=True, return_data=False) if visible_name: if is_collision: return False, name return True, name if is_collision: return False return True
def _get_transformations(self, q_in): """ Get transformations corresponding to q_in Args: q_in(np.array): joint angles Returns: transformations(OrderedDict) """ if self.arm is not None: transformations = self.robot.forward_kin(q_in, self.robot.desired_frames[self.arm]) else: transformations = self.robot.forward_kin(q_in, self.robot.desired_frames) return transformations
[docs] def reach_to_goal(self, point): """ Check reach to goal If reach to goal, return True Args: point(np.array): joint angles Returns: bool """ dist = self.distance(point, self.goal_q) if dist <= 0.5: return True return False
[docs] def find_path(self, tree): """ find path result from start index to goal index Args: tree(Tree): Trees obtained so far Returns: path(list) """ path = [self.goal_q] goal_idx = tree.edges[-1][1] while goal_idx != 0: if not np.allclose(path[0], tree.vertices[goal_idx]): path.append(tree.vertices[goal_idx]) parent_idx = tree.edges[goal_idx-1][0] goal_idx = parent_idx path.append(self.cur_q) return path[::-1]
[docs] def get_rrt_tree(self): """ Return obtained RRT Trees Returns: verteices(list) """ vertices = [] for edge in self.T.edges: from_node = self.T.vertices[edge[0]] goal_node = self.T.vertices[edge[1]] vertices.append((from_node, goal_node)) return vertices