From 7a1378e2293d32e7e320265b051fd9491aead847 Mon Sep 17 00:00:00 2001 From: Monica Sekoyan Date: Tue, 11 Nov 2025 13:42:00 +0000 Subject: [PATCH 1/2] add qp with bezier curves Signed-off-by: Monica Sekoyan --- bezier.py | 2 +- control.py | 43 ++++++---- inverse_geometry.py | 4 +- path.py | 8 +- qp_utils.py | 190 ++++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 223 insertions(+), 24 deletions(-) create mode 100644 qp_utils.py diff --git a/bezier.py b/bezier.py index 62375bc..2e392c1 100644 --- a/bezier.py +++ b/bezier.py @@ -7,7 +7,7 @@ def __init__(self, pointlist, t_min=0.0, t_max=1.0, mult_t=1.0): self.T_min_ = t_min self.T_max_ = t_max self.mult_T_ = mult_t - self.size_ = len(pointlist)- 1 + self.size_ = len(pointlist) self.degree_ = self.size_ - 1 self.control_points_ = pointlist if (self.size_ < 1 or self.T_max_ <= self.T_min_): diff --git a/control.py b/control.py index 597e42d..f88ae53 100644 --- a/control.py +++ b/control.py @@ -6,6 +6,7 @@ @author: stonneau """ +import pickle import numpy as np from zmq.constants import THREAD_AFFINITY_CPU_REMOVE @@ -15,7 +16,7 @@ from cube_trajectory_wrapper import GlobalMinJerkLinearJTraj from tools import setcubeplacement from config import LEFT_HAND, RIGHT_HAND - +from qp_utils import get_bezier_control_points import pybullet as pyb # in my solution these gains were good enough for all joints but you might want to tune this. @@ -26,7 +27,15 @@ Dx_lin = 5 * np.sqrt(Kx_lin) Kx_rot = 1000 -Dx_rot = 10 +Dx_rot = 10 + +# Whether to use Bezier curve for trajectory generation +USE_BEZIER = False + +if USE_BEZIER: + Kp = 1000 # proportional gain (P of PD) + Kv = 30 * np.sqrt(Kp) # derivative gain (D of PD) + def controllaw(sim, robot, trajs, tcurrent): """Joint trajectory tracking with end-effector force correction""" @@ -125,9 +134,9 @@ def controllaw(sim, robot, trajs, tcurrent): q0, successinit = computeqgrasppose(robot, robot.q0, cube, CUBE_PLACEMENT, None) qe, successend = computeqgrasppose(robot, robot.q0, cube, CUBE_PLACEMENT_TARGET, None) + if not successinit or not successend: - print('Failed to compute grasp pose') - exit() + raise RuntimeError('Failed to successfully compute grasp pose!') path, cube_placements = computepath( q0, @@ -141,8 +150,7 @@ def controllaw(sim, robot, trajs, tcurrent): sim.setqsim(q0) - def maketraj(q0, q1, path, T): #TODO compute a real trajectory ! - points = [q0, q0] + path + [q1, q1] + def maketraj(points, T): q_of_t = Bezier(points,t_max=T) vq_of_t = q_of_t.derivative(1) vvq_of_t = vq_of_t.derivative(1) @@ -163,17 +171,18 @@ def maketraj_with_joint_space_linear( joint_space_trajectory.derivative(2) ) - total_time=10. - - trajs = maketraj_with_joint_space_linear( - waypoints=path, - T=total_time - ) - - cube_trajs = maketraj_with_joint_space_linear( - waypoints=[p.translation for p in cube_placements], - T=total_time - )[0] + total_time=10.0 + + if USE_BEZIER: + control_points = get_bezier_control_points(path) + if control_points is None: + raise RuntimeError('Failed to successfully compute Bezier control points!') + trajs = maketraj(control_points, total_time) + else: + trajs = maketraj_with_joint_space_linear( + waypoints=path, + T=total_time + ) tcur = 0. diff --git a/inverse_geometry.py b/inverse_geometry.py index ba4810f..1bca68d 100644 --- a/inverse_geometry.py +++ b/inverse_geometry.py @@ -9,7 +9,7 @@ import pinocchio as pin import numpy as np from numpy.linalg import pinv,inv,norm,svd,eig -from tools import collision, getcubeplacement, setcubeplacement, projecttojointlimits +from tools import collision, getcubeplacement, setcubeplacement, jointlimitsviolated from config import LEFT_HOOK, RIGHT_HOOK, LEFT_HAND, RIGHT_HAND, EPSILON from config import CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET @@ -125,7 +125,7 @@ def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None): right_hook_close_enough = rotation_to_right_hook <= EPSILON and translation_to_right_hook <= EPSILON / 10 if left_hook_close_enough and right_hook_close_enough: - if not collision(robot, q): + if not collision(robot, q) and not jointlimitsviolated(robot, q): q_opt_found = True break diff --git a/path.py b/path.py index 3129510..31b75e2 100644 --- a/path.py +++ b/path.py @@ -15,7 +15,7 @@ from typing import Callable, Tuple, List from pinocchio.utils import rotate -from tools import setcubeplacement +from tools import setcubeplacement, distanceToObstacle #### Helper Functions for Random Cube Placement Sampling #### @@ -215,7 +215,7 @@ def get_q_new(self, cube=self.cube, cubetarget=q, ) - if not found_grasping_pose: + if not found_grasping_pose or (found_grasping_pose and distanceToObstacle(self.robot, grasping_q) < 2e-3): if i - 1 == 0: q_new_found = False return self.lerp( @@ -423,10 +423,10 @@ def computepath(qinit, table = table or globals().get("table") computeqgrasppose = computeqgrasppose or globals().get("computeqgrasppose") - NUM_ITER = 200 + NUM_ITER = 500 RANDOM_SAMPLER = lambda: random_cube_placement(robot=robot, cube=cube, table=table) MAX_DELTA_Q = None - DISCRETISATION_STEPS = 100 + DISCRETISATION_STEPS = 200 GET_GRAPSING_POSEQ = computeqgrasppose print('Searching for a valid path...(this may take some time)') diff --git a/qp_utils.py b/qp_utils.py new file mode 100644 index 0000000..17193f1 --- /dev/null +++ b/qp_utils.py @@ -0,0 +1,190 @@ +import numpy as np +import quadprog +from typing import List, Dict, Tuple + +def assign_time_to_intermediate_points(path: List[np.array]) -> Dict[int, float]: + # assign time to each point in the path + # so we can use it in QP cost function + # compute time proportional to distance between points + + total_dist = 0 + point_idx_time = {} + + for i in range(1, len(path)-1): + dist = np.linalg.norm(path[i] - path[i-1]) + total_dist += dist + point_idx_time[i] = dist + + total_dist += np.linalg.norm(path[-1] - path[-2]) + norm_dist = np.cumsum(list(point_idx_time.values()) / total_dist) + for point_idx, time in point_idx_time.items(): + point_idx_time[point_idx] = norm_dist[point_idx-1] + + return point_idx_time + + +def quadprog_solve_qp(H, q, G=None, h=None, C=None, d=None, verbose=False): + """ + min (1/2)x' P x + q' x + subject to G x <= h + subject to C x = d + """ + qp_G = 0.5 * (H + H.T) # make sure P is symmetric + qp_a = -q + qp_C = None + qp_b = None + meq = 0 + if C is not None: + if G is not None: + qp_C = -np.vstack([C, G]).T + qp_b = -np.hstack([d, h]) + else: + qp_C = -C.transpose() + qp_b = -d + meq = C.shape[0] + elif G is not None: # no equality constraint + qp_C = -G.T + qp_b = -h + res = quadprog.solve_qp(qp_G, qp_a, qp_C, qp_b, meq) + if verbose: + return res + return res[0] + + +def bernstein_deg4(t: float) -> np.array: + """Quartic Bernstein basis [B0..B4] at scalar t.""" + # as the equation looks like this + # p(t) = (1-t)^4*p0 + 4(1-t)^3*t*p1 + 6(1-t)^2*t^2*p2 + 4(1-t)*t^3*p3 + t^4*p4 + # so we need to compute the coefficients for each term + # B0 = (1-t)^4 + # B1 = 4*(1-t)^3*t + # B2 = 6*(1-t)^2*t^2 + # B3 = 4*(1-t)*t^3 + # B4 = t^4 + tt = t + omt = 1.0 - tt + B0 = omt**4 + B1 = 4 * omt**3 * tt + B2 = 6 * omt**2 * tt**2 + B3 = 4 * omt * tt**3 + B4 = tt**4 + return np.array([B0, B1, B2, B3, B4], dtype=np.float64) + +def build_H_q_quartic_bezier( + q0: np.array, + qe: np.array, + t_samples: List[float], + g_samples: List[np.array], + l2_reg: float = 0.0 +) -> Tuple[np.array, np.array]: + + + """ + t_samples: (N,) in [0,1] + g_samples: (N, 15) targets + Returns H (45x45), q (45,) + """ + + ## p(t)=P0​B0​ + (P1​B1 ​+ P2​B2 ​+ P3​B3 ​) + P4​B4 + ## need to be p(t) = Ax + b + ## where b = P0​B0​ + P4​B4 + + ## variables are [P1_dimensions, P2_dimensions, P3_dimensions] + ## and A = [ + ## [B1, 0, 0, 0, ...., B2, 0, 0, 0, ...., B3, 0, 0, 0,0...], + ## [0, B1, 0, 0, ...., 0, B2, 0, 0, ...., 0, B3, 0, 0, ...., 0], + ## [0, 0, B1, 0, ...., 0, 0, B2, 0, ...., 0, 0, B3, 0, ...., 0], + ## ..... + ## [0, 0, 0, B1, ...., 0, 0, 0, B2, ...., 0, 0, 0, B3, ...., 0], + ## [0, 0, 0, 0, ...., B3, 0, 0, 0, ...., 0, 0, 0, 0, ...., B3] + ## ] + + # we need to compute the coefficients for each term + t_samples = np.asarray(t_samples, dtype=np.float64).ravel() + g_samples = np.asarray(g_samples, dtype=np.float64) + assert g_samples.shape == (t_samples.size, 15) + + d = 15 + n_ctrl = 3 + nvars = d * n_ctrl + + H = np.zeros((nvars, nvars), dtype=np.float64) + q = np.zeros(nvars, dtype=np.float64) + + # samples in the computed trajectory we want to fit + # Should look like this + # ||p(t) - g(t)||^2 + # ||A x + b - g(t)||^2 + # ||A x - (g(t) - b)||^2 + # 1/2 x.T A.T A x + (-A.T (g(t) - b)) x + # 1/2 x.T A.T A x + (A.T (b - g(t))) x + + for ti, gi in zip(t_samples, g_samples): + B = bernstein_deg4(ti) + b = B[0] * q0 + B[-1] * qe + A_i = np.concatenate([np.eye(d) * b for b in B[1:-1]], axis=1)# (15, 45): per-dim applies same B across control blocks + H += A_i.T @ A_i # accumulate quadratic term + q += A_i.T @ (b - gi) # accumulate linear term + + if l2_reg > 0.0: + # Optional small ridge for PD Hessian (helps quadprog) + H += 2.0 * l2_reg * np.eye(nvars, dtype=np.float64) + + return H, q + + +def accel_equalities_known_endpoints( + q0: np.array, + qe: np.array +) -> Tuple[np.array, np.array]: + d = 15 + C_start = np.concatenate([np.eye(d) * i for i in [-2., 1., 0.]], axis=1) # P2 - 2P1 = -P0 + C_end = np.concatenate([np.eye(d) * i for i in [ 0., 1., -2.]], axis=1) # P2 - 2P3 = -P4 + C = np.vstack([C_start, C_end]) # (30,45) + dvec = np.concatenate([-q0, -qe]) # (30,) + return C, dvec + + +def construct_collision_constraints( + discretization_steps, + distance_to_obstacle_function, + distance_to_obstacle_threshold): + + ## we need to construct inequality constraints for the collision avoidance + ## form of inequality constraint is + ## distance_to_obstacle_function(x) <= distance_to_obstacle_threshold + ## distance_function(p(t)) <= distance_to_obstacle_threshold + ## distance_function(Ax+b) <= distance_to_obstacle_threshold + + pass + + +def get_bezier_control_points( + path: List[np.array]) -> List[np.array]: + + # assign specific times to intermediate points + # this is based on the distance between points in the path + point_times = assign_time_to_intermediate_points(path) + + q0 = path[0] + qe = path[-1] + + t_samples = np.array(list(point_times.values())) + g_samples = path[1:-1] + + l2_reg = 1e-6 + + H, q = build_H_q_quartic_bezier(q0, qe, t_samples, g_samples, l2_reg=l2_reg) + C, d = accel_equalities_known_endpoints(q0, qe) + res = quadprog_solve_qp(H, q, C=C, d=d) + + if res is None or res.size == 0: + return + + control_points = [] + for i in range(0, len(res), 15): + control_points.append(res[i:i+15]) + + return [q0] + control_points + [qe] + + From fe0caa6409db2ca90c9f03e4072fadbd389fb19c Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Tue, 11 Nov 2025 14:07:39 +0000 Subject: [PATCH 2/2] Finish merge --- inverse_geometry.py | 4 - path.py | 398 -------------------------------------------- 2 files changed, 402 deletions(-) diff --git a/inverse_geometry.py b/inverse_geometry.py index a8ebc72..53409ae 100644 --- a/inverse_geometry.py +++ b/inverse_geometry.py @@ -9,12 +9,8 @@ import pinocchio as pin import numpy as np from numpy.linalg import pinv,inv,norm,svd,eig -<<<<<<< HEAD from tools import collision, getcubeplacement, setcubeplacement, jointlimitsviolated -======= import time -from tools import collision, getcubeplacement, setcubeplacement, projecttojointlimits ->>>>>>> main from config import LEFT_HOOK, RIGHT_HOOK, LEFT_HAND, RIGHT_HAND, EPSILON from config import CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET diff --git a/path.py b/path.py index 4e508b8..157caf5 100644 --- a/path.py +++ b/path.py @@ -30,399 +30,10 @@ logger = logging.getLogger(__name__) -<<<<<<< HEAD from typing import Callable, Tuple, List from pinocchio.utils import rotate from tools import setcubeplacement, distanceToObstacle - -#### Helper Functions for Random Cube Placement Sampling #### - -def get_table_borders(table: pin.robot_wrapper.RobotWrapper) -> Tuple[np.array, np.array]: - """ Returns the minimum and maximum x, y, z coordinates of the table. - Args: - table: table object - Returns: - xyz_min: minimum x, y, z coordinates of the table's leftmost and bottommost corner - xyz_max: maximum x, y, z coordinates of the table's rightmost and topmost corner - """ - pin.updateGeometryPlacements(table.model, table.data, table.collision_model, table.collision_data) - table_surface = table.collision_model.geometryObjects[0].geometry - oMtable_surface = table.collision_data.oMg[0] - R, t = oMtable_surface.rotation, oMtable_surface.translation - - - half = table_surface.halfSide - local_corners = np.array([[sx, sy, sz] - for sx in [+1, -1] - for sy in [+1, -1] - for sz in [+1, -1]]) * half - world_corners = (R @ local_corners.T).T + t - - xyz_min = world_corners.min(axis=0) - xyz_max = world_corners.max(axis=0) - - return xyz_min, xyz_max - - - -def random_cube_placement( - robot: pin.robot_wrapper.RobotWrapper, - cube: pin.robot_wrapper.RobotWrapper, - table: pin.robot_wrapper.RobotWrapper, - check_collisions: bool = True - ) -> pin.pinocchio_pywrap_default.SE3: - """ Returns a random cube placement on the table that is collision free. - Args: - robot: robot object - cube: cube object - table: table object - check_collisions: whether to check for collisions - Returns: - valid_placement: random cube placement on the table that is collision free - """ - xyz_min, xyz_max = get_table_borders(table) - - collision = True - valid_placement = None - - while collision: - x = np.random.uniform(xyz_min[0], xyz_max[0]) - y = np.random.uniform(xyz_min[1], xyz_max[1]) - # TODO: change harcoded max z to a more clever bounding - z = np.random.uniform(0.93, 2) - - placement = pin.SE3(rotate('z', 0.), np.array([x, y, z])) - setcubeplacement(robot, cube, placement) - if check_collisions: - collision = pin.computeCollisions(cube.collision_model, cube.collision_data, False) - valid_placement = placement if not collision else valid_placement - else: - valid_placement = placement - collision = False - - return valid_placement - - -#### Helper Functions and Classes for RRT #### - -class Vertex: - def __init__(self, q, grasping_q, parent): - self.q = q - self.grasping_q = grasping_q - self.parent = parent - -class RRT: - def __init__(self, - root_q: pin.pinocchio_pywrap_default.SE3, - root_grasping_q: np.array, - robot: pin.robot_wrapper.RobotWrapper, - cube: pin.robot_wrapper.RobotWrapper, - get_grasping_poseq: Callable): - - self.robot = robot - self.cube = cube - self.get_grasping_poseq = get_grasping_poseq - - # initialize the tree - self.nodes = [ - Vertex(q=root_q, grasping_q=root_grasping_q, parent=None) - ] - - @staticmethod - def calc_distance( - q1: np.array, - q2: np.array) -> float: - '''Return the euclidian distance between two configurations''' - return np.linalg.norm(q2 - q1) - - @staticmethod - def lerp( - q0: np.array, - q1: np.array, - t: float - ) -> np.array: - """ - Performs linear interpolation between the given points and for the step t - """ - return RRT.get_placement_from_translation((1 - t) * q0 + t * q1) - - @staticmethod - def get_placement_from_translation(translation): - return pin.SE3(rotate('z', 0.), np.array(translation)) - - def add_edge(self, - parent_idx: int, - new_q: pin.pinocchio_pywrap_default.SE3, - new_grasping_q: np.array) -> None: - - """ - Constructs and adds a new vertex to the tree. - """ - - self.nodes.append( - Vertex(q=new_q, - grasping_q=new_grasping_q, - parent=self.nodes[parent_idx]) - ) - - def get_nearest_vertex(self, - q: pin.pinocchio_pywrap_default.SE3 - ) -> Tuple[int, Vertex]: - - """ - Args: - q: random configuration of the cube (SE3) - Returns: - nearest_vertex_idx: nearest vertex index in the tree - nearest_vertex: nearest vertex - """ - nearest_vertex_idx = None - smallest_distance = None - - for i, vertex in enumerate(self.nodes): - dist = self.calc_distance(vertex.q.translation, q.translation) - if not smallest_distance or dist <= smallest_distance: - smallest_distance = dist - nearest_vertex_idx = i - - return nearest_vertex_idx, self.nodes[nearest_vertex_idx] - - def get_q_new(self, - q_nearest_vertex: Vertex, - q_rand: pin.pinocchio_pywrap_default.SE3, - discretisation_steps: int, - max_delta_q: int = None, - ) -> Tuple[pin.pinocchio_pywrap_default.SE3, np.array, bool]: - - """ Returns a new configuration that is the closest to the random configuration `q_rand` that can be grasped and is collison free. - Args: - q_nearest_vertex: nearest vertex - q_rand: random configuration - discretisation_steps: number of discretisation steps - max_delta_q: maximum delta q - Returns: - q_new: new configuration - grasping_q: grasping pose for the new configuration - q_new_found: True if the new configuration (other than the `q_nearest_vertex`) is found, False otherwise - """ - - q_new_found = True - q_end = q_rand.copy() - - dist = self.calc_distance( - q_nearest_vertex.q.translation, - q_rand.translation) - - if max_delta_q is not None and dist > max_delta_q: - q_end = self.lerp( - q_nearest_vertex.q.translation, - q_rand.translation, - max_delta_q/dist) - - if self.get_grasping_poseq is not None: - dt = 1 / discretisation_steps - for i in range(1, discretisation_steps): - q = self.lerp( - q_nearest_vertex.q.translation, - q_end.translation, - dt*i) - grasping_q, found_grasping_pose = self.get_grasping_poseq( - robot=self.robot, - qcurrent=q_nearest_vertex.grasping_q, - cube=self.cube, - cubetarget=q, - ) - if not found_grasping_pose or (found_grasping_pose and distanceToObstacle(self.robot, grasping_q) < 2e-3): - if i - 1 == 0: - q_new_found = False - return self.lerp( - q_nearest_vertex.q.translation, - q_end.translation, - dt*(i-1)), grasping_q, q_new_found - else: - grasping_q = None - - return q_end, grasping_q, q_new_found - - def check_edge( - self, - q_latest_new: pin.pinocchio_pywrap_default.SE3, - q_latest_new_grasping_pose: np.array, - q_goal: pin.pinocchio_pywrap_default.SE3, - discretisation_steps: int = 1000, - ) -> bool: - - - """ Checks if there is a valid edge between the latest new configuration and the goal configuration. - Args: - q_latest_new: latest new configuration - q_latest_new_grasping_pose: grasping pose for the latest new configuration - q_goal: goal configuration - discretisation_steps: number of discretisation steps - Returns: - valid_edge: True if there is a valid edge, False otherwise - """ - - valid_edge = False - q_latest_new_vertex = Vertex( - q=q_latest_new, - grasping_q=q_latest_new_grasping_pose, - parent=None - ) - - # check if we can get to the q_goal from the q_new directly - q_new, q_new_grasping_q, q_new_found = self.get_q_new( - q_nearest_vertex=q_latest_new_vertex, - q_rand=q_goal, - discretisation_steps=discretisation_steps, - max_delta_q=None - ) - - if q_new_found: - distance_to_target = self.calc_distance(q_new.translation, q_goal.translation) - if distance_to_target < 1e-3: - valid_edge = True - - return valid_edge - - def get_path_from_rrt(self) -> List[Vertex]: - """ Returns a list of vertices from the root to the latest vertex. - If there was a valid edge between the latest vertex and the goal configuration, the latest vertex is the vertex with the goal configuration. - Returns: - vertices: list of vertices - """ - vertices = [] - last_vertex = self.nodes[-1] - - while last_vertex.parent is not None: - vertices.insert(0, last_vertex) - last_vertex = last_vertex.parent - - vertices.insert(0, last_vertex) - - return vertices - - def get_shortcut_from_rrt_path(self, - discretisation_steps: int = 100, - ) -> List[Vertex]: - - """ Returns a shortcut path from the root to the latest vertex. - Args: - discretisation_steps: number of discretisation steps - Returns: - short_path: list of vertices from the root to the latest vertex that has no redundant intermediate vertices - """ - - path_vertices = self.get_path_from_rrt() - short_path = [] - start_idx = 0 - - while start_idx < len(path_vertices) - 3: - current_vertex = path_vertices[start_idx] - short_path.append(current_vertex) - - # search in the next nodes starting from the furthest one - next_vertex_idx = len(path_vertices) - 1 - while next_vertex_idx > start_idx + 1: - next_vertex = path_vertices[next_vertex_idx] - is_valid_edge = self.check_edge( - q_latest_new=current_vertex.q, - q_latest_new_grasping_pose=current_vertex.grasping_q, - q_goal=next_vertex.q, - discretisation_steps=discretisation_steps - ) - if is_valid_edge: - start_idx = next_vertex_idx - break - next_vertex_idx -= 1 - - if not is_valid_edge: - start_idx += 1 - - short_path.extend(path_vertices[start_idx:]) - - return short_path - - - -#### The main function for constructing the RRT #### - -def construct_rrt(robot: pin.robot_wrapper.RobotWrapper, - cube: pin.robot_wrapper.RobotWrapper, - q_init: np.array, - q_goal: np.array, - cubeplacementq0: pin.pinocchio_pywrap_default.SE3, - cubeplacementqgoal: pin.pinocchio_pywrap_default.SE3, - num_iter: int, - radnom_sampler: Callable, - max_delta_q: float = None, - discretisation_steps: int = 100, - get_grasping_poseq: Callable = None, - ): - - """ Constructs the RRT from the root to the goal configuration. - Args: - robot: robot object - cube: cube object - q_init: initial grasping pose - q_goal: goal grasping pose - cubeplacementq0: initial cube placement - cubeplacementqgoal: goal cube placement - num_iter: number of iterations of sampling random cube placements - radnom_sampler: random cube placement sampler - max_delta_q: maximum delta q between the current and the end configuration used to get q_new configuration - discretisation_steps: number of discretisation steps used in the get_q_new and check_edge function - get_grasping_poseq: grasping pose calculator from the cube placement (inverse geometry). - Returns: - rrt: RRT object - """ - rrt = RRT( - root_q=cubeplacementq0, - root_grasping_q=q_init, - robot=robot, - cube=cube, - get_grasping_poseq=get_grasping_poseq - ) - - for _ in range(num_iter): - random_cp_q = radnom_sampler() - cp_q_nearest_idx, cp_q_nearest_vertex = rrt.get_nearest_vertex(random_cp_q) - - cp_q_new, cp_q_new_grasping_pose, cp_q_new_found = rrt.get_q_new( - q_nearest_vertex=cp_q_nearest_vertex, - q_rand=random_cp_q, - discretisation_steps=discretisation_steps, - max_delta_q=max_delta_q) - - if cp_q_new_found: - rrt.add_edge( - parent_idx=cp_q_nearest_idx, - new_q=cp_q_new, - new_grasping_q=cp_q_new_grasping_pose, - ) - - is_valid_edge = rrt.check_edge( - q_latest_new=cp_q_new, - q_latest_new_grasping_pose=cp_q_new_grasping_pose, - q_goal=cubeplacementqgoal, - discretisation_steps=discretisation_steps) - - if is_valid_edge: - rrt.add_edge(parent_idx=len(rrt.nodes) - 1, - new_q=cubeplacementqgoal, - new_grasping_q=q_goal) - - return rrt - - return None - -#returns a collision free path from qinit to qgoal under grasping constraints -#the path is expressed as a list of configurations -def computepath(qinit, - qgoal, - cubeplacementq0, -======= __all__ = [ "random_cube_placement", "Vertex", @@ -430,11 +41,9 @@ def computepath(qinit, "construct_rrt", ] - def computepath(qinit, qgoal, cubeplacementq0, ->>>>>>> main cubeplacementqgoal, robot=None, cube=None, @@ -463,12 +72,6 @@ def computepath(qinit, cube = cube or globals().get("cube") computeqgrasppose = computeqgrasppose or globals().get("computeqgrasppose") -<<<<<<< HEAD - NUM_ITER = 500 - RANDOM_SAMPLER = lambda: random_cube_placement(robot=robot, cube=cube, table=table) - MAX_DELTA_Q = None - DISCRETISATION_STEPS = 200 -======= # If no sampler supplied, build a constrained sampler on the vertical plane # that intersects the initial and final cube placements. sampling_box_ranges = None @@ -524,7 +127,6 @@ def plane_sampler(): else: RANDOM_SAMPLER = random_sampler MAX_DELTA_Q = None ->>>>>>> main GET_GRAPSING_POSEQ = computeqgrasppose print('Searching for a valid path...(this may take some time)')