def main(): vehicle = Vehicle([0, 10, 0, 0, 0, 0]) predictions = {0: vehicle} target = 0 delta = [-10, 10, 0, 0, 0, 0] start_s = [10, 10, 0] start_d = [4, 0, 0] T = 5.0 #the require time to get to the goal best = PTG(start_s, start_d, target, delta, T, predictions) show_trajectory(best[0], best[1], best[2], vehicle)
def main(): vehicle = Vehicle([0,10,0, 0,0,0]) predictions = {0: vehicle} target = 0 # delta = [0, 0, 0, 0, 0 ,0] start_s = [10, 10, 0] start_d = [4, 0, 0] T = 5.0 # time best = PTG(start_s, start_d, target, delta, T, predictions) show_trajectory(best[0], best[1], best[2], vehicle)
def main(): vehicle = Vehicle([0,10,0, 0,0,0]) predictions = {0: vehicle} target = 0 delta = [-10, 0, 0, 0, 0 ,0] # arrive 10m behind target car start_s = [10, 10, 0] start_d = [4, 0, 0] T = 5.0 best = PTG(start_s, start_d, target, delta, T, predictions) show_trajectory(best[0], best[1], best[2], vehicle)
def main(): vehicle = Vehicle([0, 10, 0, 0, 0, 0]) predictions = {0: vehicle} target = 0 # target s-location at non-zero distance behind target vehicle delta = [-2 * VEHICLE_RADIUS, 0, 0, 0, 0, 0] start_s = [10, 10, 0] start_d = [4, 0, 0] T = 5.0 best = PTG(start_s, start_d, target, delta, T, predictions) show_trajectory(best[0], best[1], best[2], vehicle)
def main(): vehicle = Vehicle([0, 10, 0, 0, 0, 0]) predictions = {0: vehicle} target = 0 delta = [-10, 0, 0, 0, 0, 0] start_s = [10, 10, 0] start_d = [4, 0, 0] # print("start_s: ", start_s, "start_d: ", start_d) T = 5.0 best = PTG(start_s, start_d, target, delta, T, predictions) show_trajectory(best[0], best[1], best[2], vehicle)
def main(): """ vehicle state: (s, s', s'', d, d, d'') predictions: the non-ego vechicles target: target vehicle id delta: state delta we aim to follow refer to target vehicle start_s: initial state s for ego car start_d: initial state d for ego car T: the duration of maneuver in seconds """ vehicle = Vehicle([0, 10, 0, 0, 0, 0]) predictions = {0: vehicle} target = 0 delta = [0, 0, 0, 0, 0, 0] start_s = [10, 10, 0] start_d = [4, 0, 0] T = 5.0 best = PTG(start_s, start_d, target, delta, T, predictions) show_trajectory(best[0], best[1], best[2], vehicle)
def main(): #trajectory #(si, si_dot, si_ddot, d, d_dot, d_ddot,) vehicle = Vehicle([5, 10, 0, 0, 0, 0]) #(t) predictions = {0: vehicle} target = 0 #vehicle_number delta = [0, 0, 0, 0, 0, 0] #perturb parameter --> #(si, si_dot, si_ddot, d, d_dot, d_ddot,) start_s = [0, 0, 0] start_d = [4, 0, 0] T = 20 best = PTG(start_s, start_d, target, delta, T, predictions) show_trajectory(best[0], best[1], best[2], vehicle)
from compas.geometry import Frame from compas_fab.backends import RosClient from compas_fab.robots import Configuration from helpers import show_trajectory with RosClient('localhost') as client: robot = client.load_robot() group = robot.main_group_name frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) tolerance_position = 0.001 tolerance_axes = [math.radians(1)] * 3 start_configuration = Configuration.from_revolute_values([-0.042, 4.295, 0, -3.327, 4.755, 0.]) # create goal constraints from frame goal_constraints = robot.constraints_from_frame(frame, tolerance_position, tolerance_axes, group) trajectory = robot.plan_motion(goal_constraints, start_configuration, group, options=dict(planner_id='RRT')) print("Computed kinematic path with %d configurations." % len(trajectory.points)) print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start) show_trajectory(trajectory)