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)
