def __init__(self, kinematics, cycle_time=0.008, log_cart=False):
"""Initialize the trajectory generator.
Arguments:
kinematics -- The robot kinematics
Keyword arguments:
cycle_time -- The cycle time of the robot interface(default 0.008)
log_cart -- Log the cartesian position (default False)
"""
TrajectoryGenerator.__init__(self, kinematics, cycle_time=cycle_time)
self._log_cart = log_cart
self._twist_update_time = time.time()
self._start_pose = None
self._target_pose = None
self._interpolator = None
self._velocity = None
self._has_target = False
self._step_count = None
self._num_steps = None
self._path_length = None
self._initializing = False
self._last_q = None
self._init_event = threading.Event()
if self._log_cart:
if os.getenv("LOG_PATH") is not None:
self._log_cart_file = open(
os.path.join(os.getenv("LOG_PATH"), "tlg_cart.csv"), "a")
else:
self._log_cart_file = open("tlg_cart.csv", "a")
def __init__(self,
kinematics,
cycle_time=0.008,
timeout=0.1,
q_dot_max=1.5,
q_dotdot_max=10.0):
"""Initialize the trajectory generator with a timeout.
Arguments:
kinematics -- The robot kinematics
Keyword arguments:
cycle_time -- The cycle time of the robot interface(default 0.008)
timeout -- Timeout in s after the last joint velocity was sent to make
sure the robot does not move further when the motion
controller fails. (default 0.1)
q_dot_max -- Max joint speed (default 1.5)
q_dotdot_max -- Max joint acceleration (default 10.0)
"""
TrajectoryGenerator.__init__(self, kinematics, cycle_time=cycle_time)
self._joint_velocity = np.zeros(kinematics.num_of_joints)
self._timeout = timeout
self._joint_velocity_update_time = time.time()
self._log_time_stamp = None
self._q_dot_last = np.zeros(self._kinematics.num_of_joints)
self._q_dot_max = q_dot_max
self._q_dotdot_max = q_dotdot_max
self._lock = threading.Lock()
def __init__(self,
kinematics,
cycle_time=0.008,
timeout=0.1,
interpol=0,
q_dot_max=1.5):
"""Initialize the trajectory generator with a timeout.
Arguments:
kinematics -- The robot kinematics
Keyword arguments:
cycle_time -- The cycle time of the robot interface(default 0.008)
timeout -- Timeout in s after the last twist was sent to make sure
the robot does not move further when the motion controller
fails. (default 0.1)
interpol -- Interpolation steps calculation of the next step
(default 1)
q_dot_max -- Max joint speed (default 1.5)
"""
TrajectoryGenerator.__init__(self, kinematics, cycle_time=cycle_time)
self._twist = Twist()
self._timeout = timeout
self._twist_update_time = time.time()
self._log_time_stamp = None
self._interpol = interpol
self._q_dot_max = q_dot_max
self._twist_lock = threading.Lock()
self._express_frame = None
def __init__(self,
kinematics,
cycle_time=0.008,
timeout=0.1,
q_dot_max=1.5):
"""Initialize the trajectory generator with a timeout.
Arguments:
kinematics -- The robot kinematics
Keyword arguments:
cycle_time -- The cycle time of the robot interface(default 0.008)
timeout -- Timeout in s after the last joint velocity was sent to make
sure the robot does not move further when the motion
controller fails. (default 0.1)
q_dot_max -- Max joint speed (default 1.5)
"""
TrajectoryGenerator.__init__(self, kinematics, cycle_time=cycle_time)
self._joint_velocity = np.zeros(kinematics.num_of_joints)
self._joint_target = None
self._has_target = False
self._step_count = None
self._num_steps = None
self._joint_step = None
self._last_q = None
self._init_event = threading.Event()
def __init__(self, kinematics, cycle_time=0.008):
"""Initialize the Zero Velocity Generator
Arguments:
kinematics -- The robot kinematics
cycle_time -- The cycle time of the robot interface(default 0.008)
"""
TrajectoryGenerator.__init__(self, kinematics, cycle_time=cycle_time)
def generateTrajectory(self):
if not hasattr(self, 'trajectoryConfig'):
print("Config file has not been loaded")
sys.exit()
if not hasattr(self, 'waypoints'):
print("Waypoints file has not been loaded")
sys.exit()
generator = TrajectoryGenerator(self.waypoints, self.trajectoryConfig, self.splineType)
self.segments = generator.generate()
def main():
dyn = QuadDynamics(P)
# ctrl = QuadGeometricController(P)
ctrl = QuadLinearController(P)
# ctrl = QuadSE3Controller(P)
traj = TrajectoryGenerator()
traj.create_sin_traj(P.period, P.amplitude, P.phase, P.offset)
# traj.create_bspline_traj(P.degree, P.knots, P.coeffs)
traj_points = traj.get_plot_points()
view = QuadViewer(P.l)
view.addTrajectory(traj_points)
state_plots = StateViewer()
ctrl_plots = ControlsViewer()
save_data = None
lee3_mode = []
# delta = np.array([[0.5],[0.5],[0.5],[0.5]])
t = 0
last_time = time.time()
input()
while t < P.tf:
# des_state = traj.get_state(t)
des_state = DesiredState(np.array([[0.0, 0.0, 0.0]]).T, np.array([[0.0, 0.0, 0.0]]).T, np.zeros((3,1)), np.zeros((3,1)), 0.0, 0.0)
delta, commanded_state = ctrl.update(dyn.state, des_state)
dyn.update(delta)
commanded_state[7:10] = -commanded_state[7:10]
view.update(dyn.state)
state_plots.update(dyn.state, commanded_state, P.ts)
ctrl_plots.update(delta, P.ts)
if P.control_type == "lee3":
lee3_mode.append(ctrl.m_lee3)
t += P.ts
while time.time() - last_time < P.ts:
pass
last_time = time.time()
if save_data is None:
save_data = np.hstack([np.array([[t]]), dyn.state.T, commanded_state.T])
print(save_data.shape)
else:
save_data = np.vstack([save_data, np.hstack([np.array([[t]]), dyn.state.T, commanded_state.T])])
np.save("data/upside_down_lee1.npy", save_data)
print("saved")
if P.control_type == "lee3":
plt.plot(P.tf/len(lee3_mode)*np.array([i for i in range(len(lee3_mode))]), lee3_mode)
plt.show()
def __init__(self, config: dict):
"""Init the lattice generator from the user supplied config."""
self.trajectory_generator = TrajectoryGenerator(config)
self.grid_resolution = config['grid_resolution']
self.turning_radius = config['turning_radius']
self.stopping_threshold = config['stopping_threshold']
self.num_of_headings = config['num_of_headings']
self.headings = \
self._get_heading_discretization(config['num_of_headings'])
self.motion_model = self.MotionModel[config['motion_model'].upper()]
self.DISTANCE_THRESHOLD = 0.5 * self.grid_resolution
self.ROTATION_THRESHOLD = 0.5 * (2 * np.pi / self.num_of_headings)
def init():
global planning_pub, log_file
global path_decider, speed_decider, traj_generator
global mobileye_provider, chassis_provider
global localization_provider, routing_provider
path_decider = PathDecider()
speed_decider = SpeedDecider(FLAGS.max_cruise_speed)
traj_generator = TrajectoryGenerator()
mobileye_provider = MobileyeProvider()
chassis_provider = ChassisProvider()
localization_provider = LocalizationProvider()
routing_provider = RoutingProvider()
pgm_path = os.path.dirname(os.path.realpath(__file__))
log_path = pgm_path + "/logs/"
if not os.path.exists(log_path):
os.makedirs(log_path)
now = datetime.datetime.now().strftime("%Y-%m-%d_%H.%M.%S")
log_file = open(log_path + now + ".txt", "w")
rospy.init_node(FLAGS.navigation_planning_node_name, anonymous=True)
rospy.Subscriber('/apollo/sensor/mobileye', mobileye_pb2.Mobileye,
mobileye_callback)
rospy.Subscriber('/apollo/localization/pose',
localization_pb2.LocalizationEstimate,
localization_callback)
rospy.Subscriber('/apollo/canbus/chassis', chassis_pb2.Chassis,
chassis_callback)
rospy.Subscriber('/apollo/navigation/routing', String, routing_callback)
planning_pub = rospy.Publisher(FLAGS.navigation_planning_topic,
planning_pb2.ADCTrajectory,
queue_size=1)
def __init__(self, env, use_ros, unmovable_objects=set()):
self.env = env
self.robot = self.env.GetRobots()[0]
self.manip = self.robot.GetActiveManipulator()
self.traj_cache = {}
self.use_ros = use_ros
self.unmovable_objects = unmovable_objects
self.grasp_pose_generator = GraspPoseGenerator(self.env)
self.traj_generator = TrajectoryGenerator(self.env)
self.grasp_trajectory_generator = GraspTrajectoryGenerator(self.env,
unmovable_objects)
if self.use_ros:
self.pr2 = PlannerPR2(self.robot)
def read_generators(base_velocity, sampling_frequency):
files = glob.glob(os.path.join(BASE_DIR, "data/*.sim"))
generators = []
for name in files:
f = open(name)
stream = yaml.load_all(f.read())
stream = [s for s in stream][1]
model = {}
for n in stream:
model[n[":id"]] = n
generators.append(
TrajectoryGenerator(model, base_velocity, sampling_frequency))
f.close()
return generators
def action_server_cb(self, goal):
rospy.loginfo(IDSTR + "Action server cb called")
rospy.loginfo(goal)
names = [FAMILY_NAME+"/"+NAME_1,FAMILY_NAME+"/"+NAME_2,
FAMILY_NAME+"/"+NAME_3,FAMILY_NAME+"/"+NAME_4]
# get current state
cur_pose = self.hebi_fb.position
cur_elbow = kin.get_elbow(cur_pose)
# Determine requested elbow position
req_elbow = cur_elbow
if (goal.type == TYPE_TARGET or goal.type == TYPE_CAMERA):
req_elbow = goal.waypoint_1[0] >= 0
# Create trajectory generaor object
t = TrajectoryGenerator(names)
t.set_initial_pose(cur_pose)
# Change elbow position if necessary
# Will end on rest position of opposite elbow
if( False and (goal.type == TYPE_TARGET or goal.type == TYPE_CAMERA)
and not cur_elbow == req_elbow):
rospy.loginfo(IDSTR + "Switching elbow config")
# get rest pos after transition (ie opposite of current elbow)
rest_pos = []
if (cur_elbow):
rest_pos = REST_POS_ELBOW_DOWN
else:
rest_pos = REST_POS_ELBOW_UP
# time to travel from cur pose to VERT_POS
travel_time_1 = dist(kin.fk(cur_pose), VERT_POS) / SPEED_TRAVEL
# time to travel from vert_pos to rest pos
travel_time_2 = dist(VERT_POS, rest_pos) / SPEED_TRAVEL
t.addWaypoint(VERT_POS, 10, cur_elbow)
# t.addWaypoint(VERT_POS, 10, not cur_elbow)
# t.addWaypoint(VERT_POS, travel_time_1, cur_elbow)
# t.addWaypoint(VERT_POS, VERT_ELBOW_TRANSITION_TIME, not cur_elbow)
# send trajectory to switch in vert pose
hebi_goal = t.createTrajectory()
self.hebi_is_done = False
self.trajectory_client.send_goal(hebi_goal,\
self.trajectory_done_cb,\
self.trajectory_active_cb,
self.trajectory_feedback_cb)
while (not self.hebi_is_done):
pass
rospy.loginfo(IDSTR + "done switching elbow config part 1")
# send trajectory to go to rest pose
cur_pose = self.hebi_fb.position
t = TrajectoryGenerator(names)
t.set_initial_pose(cur_pose)
t.addWaypoint(VERT_POS, 10, not cur_elbow)
t.addWaypoint(rest_pos, 10, not cur_elbow)
hebi_goal = t.createTrajectory()
self.hebi_is_done = False
self.trajectory_client.send_goal(hebi_goal,\
self.trajectory_done_cb,\
self.trajectory_active_cb,
self.trajectory_feedback_cb)
while (not self.hebi_is_done):
pass
rospy.loginfo(IDSTR + "done switching elbow config part 2")
# redo intial setup
cur_pose = self.hebi_fb.position
cur_elbow = kin.get_elbow(cur_pose)
t = TrajectoryGenerator(names)
t.set_initial_pose(cur_pose)
if(goal.type == TYPE_TARGET):
# create waypoint to go to before goal.waypoint_1
approach_waypoint = list(goal.waypoint_1);
rough_approach_time = 0
if(goal.approach_from_above):
approach_waypoint[1] += VERT_APPROACH_DIST
rough_approach_time = dist(kin.fk(cur_pose), approach_waypoint) / SPEED_TRAVEL
# pre_approach_waypoint = approach_waypoint + \
# (VERT_HORZ_APPROACH_DIST * np.sign(-goal.waypoint_1[0]))
# pre_approach_time = dist(kin.fk(cur_pose), pre_approach_waypoint) / SPEED_TRAVEL
# t.addWaypoint(pre_approach_waypoint, pre_approach_time, req_elbow)
# rough_approach_time = dist(pre_approach_waypoint, approach_waypoint) / SPEED_APPROACH
else:
approach_waypoint[0] += (HORZ_APPROACH_DIST * np.sign(-goal.waypoint_1[0]))
rough_approach_time = dist(kin.fk(cur_pose), approach_waypoint) / SPEED_TRAVEL
# time to get from cur_pose to approach_waypoint
# time to get from approach_waypoint to goal.waypoint_1
fine_approach_time = dist(approach_waypoint, goal.waypoint_1) / SPEED_APPROACH
# create waypoint to go to after goal.waypoint_2
depart_waypoint = list(goal.waypoint_2);
if(goal.approach_from_above):
depart_waypoint[1] += VERT_APPROACH_DIST
else:
depart_waypoint[0] += (HORZ_APPROACH_DIST * np.sign(-goal.waypoint_2[0]))
# time to get from goal.waypoint_2 to depart_waypoint
fine_depart_time = dist(depart_waypoint, goal.waypoint_2) / SPEED_DEPART
# Add waypoints
t.addWaypoint(approach_waypoint, rough_approach_time, req_elbow)
t.addWaypoint(goal.waypoint_1,fine_approach_time, req_elbow)
t.addWaypoint(goal.waypoint_2,goal.duration, req_elbow)
t.addWaypoint(depart_waypoint, fine_depart_time, req_elbow)
if(goal.type == TYPE_REST):
if(cur_elbow):
travel_time = dist(kin.fk(cur_pose),REST_POS_ELBOW_UP) / SPEED_TRAVEL
t.addWaypoint(REST_POS_ELBOW_UP, travel_time, True)
else:
travel_time = dist(kin.fk(cur_pose),REST_POS_ELBOW_DOWN) / SPEED_TRAVEL
t.addWaypoint(REST_POS_ELBOW_DOWN, travel_time, True)
hebi_goal = None
try:
hebi_goal = t.createTrajectory()
except:
rospy.logwarn("Arm Planner Node FAILED to create trajectory")
self.action_server.setAborted()
return
# rospy.loginfo(goal)
# Send goal to action server
self.hebi_is_done = False
self.trajectory_client.send_goal(hebi_goal,\
self.trajectory_done_cb,\
self.trajectory_active_cb,
self.trajectory_feedback_cb)
# self.as_feedback.percent_complete = 100;
# self.action_server.publish_feedback(self.as_feedback)
#
while (not self.hebi_is_done):
pass
self.action_server.set_succeeded(self.as_result)
help='recurrent nonlinearity')
parser.add_argument('--weight_decay',
default=1e-4,
help='strength of weight decay on recurrent weights')
parser.add_argument('--DoG',
default=True,
help='use difference of gaussians tuning curves')
parser.add_argument('--periodic',
default=False,
help='trajectories with periodic boundary conditions')
parser.add_argument('--box_width',
default=2.2,
help='width of training environment')
parser.add_argument('--box_height',
default=2.2,
help='height of training environment')
options = parser.parse_args()
options.run_ID = generate_run_ID(options)
place_cells = PlaceCells(options)
if options.RNN_type == 'RNN':
model = RNN(options, place_cells)
elif options.RNN_type == 'LSTM':
model = LSTM(options, place_cells)
trajectory_generator = TrajectoryGenerator(options, place_cells)
trainer = Trainer(options, model, trajectory_generator)
# Train
trainer.train(n_epochs=options.n_epochs, n_steps=options.n_steps)
def setUp(self) -> None:
config = {'turning_radius': TURNING_RADIUS}
self.trajectory_generator = TrajectoryGenerator(config)
class LatticeGenerator:
"""
Handles all the logic for computing the minimal control set.
Computes the minimal control set for a vehicle given its parameters.
Includes handling the propogating and searching along wavefronts as
well as determining if a trajectory is part of the minimal set based
on previously added trajectories.
"""
class MotionModel(Enum):
"""An Enum used for determining the motion model to use."""
ACKERMANN = 1
DIFF = 2
OMNI = 3
class Flip(Enum):
"""An Enum used for determining how a trajectory should be flipped."""
X = 1
Y = 2
BOTH = 3
def __init__(self, config: dict):
"""Init the lattice generator from the user supplied config."""
self.trajectory_generator = TrajectoryGenerator(config)
self.grid_resolution = config['grid_resolution']
self.turning_radius = config['turning_radius']
self.stopping_threshold = config['stopping_threshold']
self.num_of_headings = config['num_of_headings']
self.headings = \
self._get_heading_discretization(config['num_of_headings'])
self.motion_model = self.MotionModel[config['motion_model'].upper()]
self.DISTANCE_THRESHOLD = 0.5 * self.grid_resolution
self.ROTATION_THRESHOLD = 0.5 * (2 * np.pi / self.num_of_headings)
def _get_wave_front_points(self, pos: int) -> np.array:
"""
Calculate the end points that lie on the wave front.
Uses the user supplied grid resolution to calculate the
valid end points that lie on a wave front at a discrete
interval away from the origin.
Args:
pos: int
The number of discrete intervals of grid resolution
away from the origin to generate the wave points at
Returns
-------
np.array
An array of coordinates
"""
positions = []
max_point_coord = self.grid_resolution * pos
for i in range(pos):
varying_point_coord = self.grid_resolution * i
# Change the y and keep x at max
positions.append((max_point_coord, varying_point_coord))
# Change the x and keep y at max
positions.append((varying_point_coord, max_point_coord))
# Append the corner
positions.append((max_point_coord, max_point_coord))
return np.array(positions)
def _get_heading_discretization(self, number_of_headings: int) -> list:
"""
Calculate the heading discretization based on the number of headings.
Does not uniformly generate headings but instead generates a set of
discrete headings that is better suited for straight line trajectories.
Args:
number_of_headings: int
The number of headings to discretize a 360 degree turn into
Returns
-------
list
A list of headings in radians
"""
max_val = int((((number_of_headings + 4) / 4) - 1) / 2)
outer_edge_x = []
outer_edge_y = []
# Generate points that lie on the perimeter of the surface
# of a square with sides of length max_val
for i in range(-max_val, max_val + 1):
outer_edge_x.extend([i, i])
outer_edge_y.extend([-max_val, max_val])
if i != max_val and i != -max_val:
outer_edge_x.extend([-max_val, max_val])
outer_edge_y.extend([i, i])
return sorted(
[np.arctan2(j, i) for i, j in zip(outer_edge_x, outer_edge_y)])
def _point_to_line_distance(self, p1: np.array, p2: np.array,
q: np.array) -> float:
"""
Return the shortest distance from a point to a line segment.
Args:
p1: np.array(2,)
Start point of line segment
p2: np.array(2,)
End point of line segment
q: np.array(2,)
Point to get distance away from line of
Returns
-------
float
The shortest distance between q and line segment p1p2
"""
# Get back the l2-norm without the square root
l2 = np.inner(p1 - p2, p1 - p2)
if l2 == 0:
return np.linalg.norm(p1 - q)
# Ensure t lies in [0, 1]
t = max(0, min(1, np.dot(q - p1, p2 - p1) / l2))
projected_point = p1 + t * (p2 - p1)
return np.linalg.norm(q - projected_point)
def _is_minimal_trajectory(self, trajectory: Trajectory,
prior_end_poses: index.Rtree) -> bool:
"""
Determine wheter a trajectory is a minimal trajectory.
Uses an RTree for speedup.
Args:
trajectory: Trajectory
The trajectory to check
prior_end_poses: RTree
An RTree holding the current minimal set of trajectories
Returns
-------
bool
True if the trajectory is a minimal trajectory otherwise false
"""
# Iterate over line segments in the trajectory
for x1, y1, x2, y2, yaw in zip(
trajectory.path.xs[:-1],
trajectory.path.ys[:-1],
trajectory.path.xs[1:],
trajectory.path.ys[1:],
trajectory.path.yaws[:-1],
):
p1 = np.array([x1, y1])
p2 = np.array([x2, y2])
# Create a bounding box search region
# around the line segment
left_bb = min(x1, x2) - self.DISTANCE_THRESHOLD
right_bb = max(x1, x2) + self.DISTANCE_THRESHOLD
top_bb = max(y1, y2) + self.DISTANCE_THRESHOLD
bottom_bb = min(y1, y2) - self.DISTANCE_THRESHOLD
# For any previous end points in the search region we
# check the distance to that point and the angle
# difference. If they are within threshold then this
# trajectory can be composed from a previous trajectory
for prior_end_pose in prior_end_poses.intersection(
(left_bb, bottom_bb, right_bb, top_bb), objects='raw'):
if (self._point_to_line_distance(
p1, p2, prior_end_pose[:-1]) < self.DISTANCE_THRESHOLD
and angle_difference(yaw, prior_end_pose[-1]) <
self.ROTATION_THRESHOLD):
return False
return True
def _compute_min_trajectory_length(self) -> float:
"""
Compute the minimum trajectory length for the given parameters.
The minimum trajectory length is defined as the length needed
for the sharpest possible turn to move from 0 degrees to the next
discrete heading. Since the distance between headings is not uniform
we take the smallest possible difference.
Returns
-------
float
The minimal length of a trajectory
"""
# Compute arc length for a turn that moves from 0 degrees to
# the minimum heading difference
heading_diff = [
abs(self.headings[i + 1] - self.headings[i])
for i in range(len(self.headings) - 1)
]
return self.turning_radius * min(heading_diff)
def _generate_minimal_spanning_set(self) -> dict:
"""
Generate the minimal spanning set.
Iteratves over all possible trajectories and keeps only those that
are part of the minimal set.
Returns
-------
dict
A dictionary where the key is the start_angle and the value is
a list of trajectories that begin at that angle
"""
quadrant1_end_poses = defaultdict(list)
# Since we only compute for quadrant 1 we only need headings between
# 0 and 90 degrees
initial_headings = sorted(
filter(lambda x: 0 <= x and x <= np.pi / 2, self.headings))
# Use the minimum trajectory length to find the starting wave front
min_trajectory_length = self._compute_min_trajectory_length()
wave_front_start_pos = int(
np.round(min_trajectory_length / self.grid_resolution))
for start_heading in initial_headings:
iterations_without_trajectory = 0
prior_end_poses = index.Index()
wave_front_cur_pos = wave_front_start_pos
# To get target headings: sort headings radially and remove those
# that are more than 90 degrees away
target_headings = sorted(self.headings,
key=lambda x:
(abs(x - start_heading), -x))
target_headings = list(
filter(lambda x: abs(start_heading - x) <= np.pi / 2,
target_headings))
while iterations_without_trajectory < self.stopping_threshold:
iterations_without_trajectory += 1
# Generate x,y coordinates for current wave front
positions = self._get_wave_front_points(wave_front_cur_pos)
for target_point in positions:
for target_heading in target_headings:
# Use 10% of grid separation for finer granularity
# when checking if trajectory overlaps another already
# seen trajectory
trajectory = self.trajectory_generator.generate_trajectory(
target_point,
start_heading,
target_heading,
0.1 * self.grid_resolution,
)
if trajectory is not None:
# Check if path overlaps something in minimal
# spanning set
if self._is_minimal_trajectory(
trajectory, prior_end_poses):
# Add end pose to minimal set
new_end_pose = np.array([
target_point[0], target_point[1],
target_heading
])
quadrant1_end_poses[start_heading].append(
(target_point, target_heading))
# Create a new bounding box in the RTree
# for this trajectory
left_bb = target_point[
0] - self.DISTANCE_THRESHOLD
right_bb = target_point[
0] + self.DISTANCE_THRESHOLD
bottom_bb = target_point[
1] - self.DISTANCE_THRESHOLD
top_bb = target_point[
1] + self.DISTANCE_THRESHOLD
prior_end_poses.insert(
0,
(left_bb, bottom_bb, right_bb, top_bb),
new_end_pose,
)
iterations_without_trajectory = 0
wave_front_cur_pos += 1
# Once we have found the minimal trajectory set for quadrant 1
# we can leverage symmetry to create the complete minimal set
return self._create_complete_minimal_spanning_set(quadrant1_end_poses)
def _flip_angle(self, angle: float, flip_type: Flip) -> float:
"""
Return the the appropriate flip of the angle in self.headings.
Args:
angle: float
The angle to flip
flip_type: Flip
Whether to flip acrpss X axis, Y axis, or both
Returns
-------
float
The angle in self.heading that is the appropriate flip
"""
angle_idx = self.headings.index(angle)
if flip_type == self.Flip.X:
heading_idx = (self.num_of_headings / 2 - 1) - angle_idx - 1
elif flip_type == self.Flip.Y:
heading_idx = self.num_of_headings - angle_idx - 2
elif flip_type == self.Flip.BOTH:
heading_idx = (angle_idx -
(self.num_of_headings / 2)) % self.num_of_headings
else:
raise Exception(f'Unsupported flip type: {flip_type}')
return self.headings[int(heading_idx)]
def _create_complete_minimal_spanning_set(
self, single_quadrant_minimal_set: dict) -> dict:
"""
Create the full minimal spanning set from a single quadrant set.
Exploits the symmetry between the quadrants to create the full set.
This is done by flipping every trajectory in the first quadrant across
either the X-axis, Y-axis, or both axes.
Args:
single_quadrant_minimal_set: dict
The minimal set for quadrant 1 (positive x and positive y)
Returns
-------
dict
The complete minimal spanning set containing the trajectories
in all quadrants
"""
all_trajectories = defaultdict(list)
for start_angle in single_quadrant_minimal_set.keys():
for end_point, end_angle in single_quadrant_minimal_set[
start_angle]:
x, y = end_point
# Prevent double adding trajectories that lie on axes
# (i.e. start and end angle are either both 0 or both pi/2)
if start_angle == 0 and end_angle == 0:
unflipped_start_angle = 0.0
flipped_x_start_angle = np.pi
unflipped_end_angle = 0.0
flipped_x_end_angle = np.pi
# Generate trajectories from calculated parameters
unflipped_trajectory = (
self.trajectory_generator.generate_trajectory(
np.array([x, y]),
unflipped_start_angle,
unflipped_end_angle,
self.grid_resolution,
))
flipped_x_trajectory = (
self.trajectory_generator.generate_trajectory(
np.array([-x, -y]),
flipped_x_start_angle,
flipped_x_end_angle,
self.grid_resolution,
))
all_trajectories[unflipped_trajectory.parameters.
start_angle].append(unflipped_trajectory)
all_trajectories[flipped_x_trajectory.parameters.
start_angle].append(flipped_x_trajectory)
elif abs(start_angle) == np.pi / 2 and abs(
end_angle) == np.pi / 2:
unflipped_start_angle = np.pi / 2
flipped_y_start_angle = -np.pi / 2
unflipped_end_angle = np.pi / 2
flipped_y_end_angle = -np.pi / 2
# Generate trajectories from calculated parameters
unflipped_trajectory = (
self.trajectory_generator.generate_trajectory(
np.array([-x, y]),
unflipped_start_angle,
unflipped_end_angle,
self.grid_resolution,
))
flipped_y_trajectory = (
self.trajectory_generator.generate_trajectory(
np.array([x, -y]),
flipped_y_start_angle,
flipped_y_end_angle,
self.grid_resolution,
))
all_trajectories[unflipped_trajectory.parameters.
start_angle].append(unflipped_trajectory)
all_trajectories[flipped_y_trajectory.parameters.
start_angle].append(flipped_y_trajectory)
else:
unflipped_start_angle = start_angle
flipped_x_start_angle = self._flip_angle(
start_angle, self.Flip.X)
flipped_y_start_angle = self._flip_angle(
start_angle, self.Flip.Y)
flipped_xy_start_angle = self._flip_angle(
start_angle, self.Flip.BOTH)
unflipped_end_angle = end_angle
flipped_x_end_angle = self._flip_angle(
end_angle, self.Flip.X)
flipped_y_end_angle = self._flip_angle(
end_angle, self.Flip.Y)
flipped_xy_end_angle = self._flip_angle(
end_angle, self.Flip.BOTH)
# Generate trajectories from calculated parameters
unflipped_trajectory = (
self.trajectory_generator.generate_trajectory(
np.array([x, y]),
unflipped_start_angle,
unflipped_end_angle,
self.grid_resolution,
))
flipped_x_trajectory = (
self.trajectory_generator.generate_trajectory(
np.array([-x, y]),
flipped_x_start_angle,
flipped_x_end_angle,
self.grid_resolution,
))
flipped_y_trajectory = (
self.trajectory_generator.generate_trajectory(
np.array([x, -y]),
flipped_y_start_angle,
flipped_y_end_angle,
self.grid_resolution,
))
flipped_xy_trajectory = (
self.trajectory_generator.generate_trajectory(
np.array([-x, -y]),
flipped_xy_start_angle,
flipped_xy_end_angle,
self.grid_resolution,
))
all_trajectories[unflipped_trajectory.parameters.
start_angle].append(unflipped_trajectory)
all_trajectories[flipped_x_trajectory.parameters.
start_angle].append(flipped_x_trajectory)
all_trajectories[flipped_y_trajectory.parameters.
start_angle].append(flipped_y_trajectory)
all_trajectories[flipped_xy_trajectory.parameters.
start_angle].append(flipped_xy_trajectory)
return all_trajectories
def _handle_motion_model(self, spanning_set: dict) -> dict:
"""
Add the appropriate motions for the user supplied motion model.
Ackerman: No additional trajectories
Diff: In place turns to the right and left
Omni: Diff + Sliding movements to right and left
Args:
spanning set: dict
The minimal spanning set
Returns
-------
dict
The minimal spanning set with additional trajectories based
on the motion model
"""
if self.motion_model == self.MotionModel.ACKERMANN:
return spanning_set
elif self.motion_model == self.MotionModel.DIFF:
diff_spanning_set = self._add_in_place_turns(spanning_set)
return diff_spanning_set
elif self.motion_model == self.MotionModel.OMNI:
omni_spanning_set = self._add_in_place_turns(spanning_set)
omni_spanning_set = self._add_horizontal_motions(omni_spanning_set)
return omni_spanning_set
else:
print('No handling implemented for Motion Model: ' +
f'{self.motion_model}')
raise NotImplementedError
def _add_in_place_turns(self, spanning_set: dict) -> dict:
"""
Add in place turns to the spanning set.
In place turns are trajectories with only a rotational component and
only shift a single angular heading step
Args:
spanning_set: dict
The minimal spanning set
Returns
-------
dict
The minimal spanning set containing additional in place turns
for each start angle
"""
all_angles = sorted(spanning_set.keys())
for idx, start_angle in enumerate(all_angles):
prev_angle_idx = idx - 1 if idx - 1 >= 0 else len(all_angles) - 1
next_angle_idx = idx + 1 if idx + 1 < len(all_angles) else 0
prev_angle = all_angles[prev_angle_idx]
next_angle = all_angles[next_angle_idx]
left_turn_params = TrajectoryParameters.no_arc(
end_point=np.array([0, 0]),
start_angle=start_angle,
end_angle=next_angle,
)
right_turn_params = TrajectoryParameters.no_arc(
end_point=np.array([0, 0]),
start_angle=start_angle,
end_angle=prev_angle,
)
# Calculate number of steps needed to rotate by roughly 10 degrees
# for each pose
angle_dif = angle_difference(start_angle, next_angle)
steps = int(round(angle_dif / np.deg2rad(10))) + 1
position = np.full(steps, 0)
left_yaws = interpolate_yaws(start_angle, next_angle, True, steps)
right_yaws = interpolate_yaws(start_angle, prev_angle, False,
steps)
left_turn_path = Path(xs=position, ys=position, yaws=left_yaws)
right_turn_path = Path(xs=position, ys=position, yaws=right_yaws)
left_turn = Trajectory(parameters=left_turn_params,
path=left_turn_path)
right_turn = Trajectory(parameters=right_turn_params,
path=right_turn_path)
spanning_set[start_angle].append(left_turn)
spanning_set[start_angle].append(right_turn)
return spanning_set
def _add_horizontal_motions(self, spanning_set: dict) -> dict:
"""
Add horizontal sliding motions to the spanning set.
The horizontal sliding motions are simply straight line trajectories
at 90 degrees to every start angle in the spanning set. The yaw of these
trajectories is the same as the start angle for which it is generated.
Args:
spanning_set: dict
The minimal spanning set
Returns
-------
dict
The minimal spanning set containing additional sliding motions
for each start angle
"""
# Calculate the offset in the headings list that represents an
# angle change of 90 degrees
idx_offset = int(self.num_of_headings / 4)
for idx, angle in enumerate(self.headings):
# Copy the straight line trajectory for the start angle that
# is 90 degrees to the left
left_angle_idx = int((idx + idx_offset) % self.num_of_headings)
left_angle = self.headings[left_angle_idx]
left_trajectories = spanning_set[left_angle]
left_straight_trajectory = next(
t for t in left_trajectories
if t.parameters.end_angle == left_angle)
# Copy the straight line trajectory for the start angle that
# is 90 degrees to the right
right_angle_idx = int((idx - idx_offset) % self.num_of_headings)
right_angle = self.headings[right_angle_idx]
right_trajectories = spanning_set[right_angle]
right_straight_trajectory = next(
t for t in right_trajectories
if t.parameters.end_angle == right_angle)
yaws = np.full(len(left_straight_trajectory.path.xs),
angle,
dtype=np.float64)
# Create a new set of parameters that represents
# the left sliding motion
parmas_l = left_straight_trajectory.parameters
left_motion_parameters = TrajectoryParameters(
parmas_l.turning_radius,
parmas_l.x_offset,
parmas_l.y_offset,
parmas_l.end_point,
angle,
angle,
parmas_l.left_turn,
parmas_l.arc_start_point,
parmas_l.arc_end_point,
)
# Create a new set of parameters that represents
# the right sliding motion
params_r = right_straight_trajectory.parameters
right_motion_parameters = TrajectoryParameters(
params_r.turning_radius,
params_r.x_offset,
params_r.y_offset,
params_r.end_point,
angle,
angle,
params_r.left_turn,
parmas_l.arc_start_point,
parmas_l.arc_end_point,
)
left_motion = Trajectory(
parameters=left_motion_parameters,
path=Path(
xs=left_straight_trajectory.path.xs,
ys=left_straight_trajectory.path.ys,
yaws=yaws,
),
)
right_motion = Trajectory(
parameters=right_motion_parameters,
path=Path(
xs=right_straight_trajectory.path.xs,
ys=right_straight_trajectory.path.ys,
yaws=yaws,
),
)
spanning_set[angle].append(left_motion)
spanning_set[angle].append(right_motion)
return spanning_set
def run(self):
"""
Run the lattice generator.
Returns
-------
dict
The minimal spanning set including additional motions for the
specified motion model
"""
complete_spanning_set = self._generate_minimal_spanning_set()
return self._handle_motion_model(complete_spanning_set)
class ObjectMover(object):
def __init__(self, env, use_ros, unmovable_objects=set()):
self.env = env
self.robot = self.env.GetRobots()[0]
self.manip = self.robot.GetActiveManipulator()
self.traj_cache = {}
self.use_ros = use_ros
self.unmovable_objects = unmovable_objects
self.grasp_pose_generator = GraspPoseGenerator(self.env)
self.traj_generator = TrajectoryGenerator(self.env)
self.grasp_trajectory_generator = GraspTrajectoryGenerator(self.env,
unmovable_objects)
if self.use_ros:
self.pr2 = PlannerPR2(self.robot)
def clear_cache(self):
self.traj_cache = {}
def pickup(self, obj):
if self.use_ros:
self.pr2.rgrip.open()
# always start at same place
joints = [-1.2, 0.2, -0.8, -1.8, -3.0, -0.3, 3.0]
traj, _ = self.traj_generator.traj_from_joints(joints)
self._execute_traj(traj)
# trajectory to grasp
traj = self._get_grasping_trajectory(obj)
self._execute_traj(traj)
# close gripper
self.robot.Grab(obj)
utils.exclude_robot_grabbed_collisions(self.robot, obj)
if self.use_ros:
self.pr2.rgrip.close()
# lift object
# link = self.robot.GetLink('r_gripper_tool_frame')
# mat = link.GetTransform()
# mat[2][3] += 0.010
# pose = openravepy.poseFromMatrix(mat).tolist()
# pos = pose[4:7]
# rot = pose[:4]
# traj, _ = self.traj_generator.plan(pos, rot, n_steps=2,
# collisionfree=False)
# self._execute_traj(traj)
def drop(self, obj, table):
pos1 = [0.4, -0.7, 1.1]
rot_z = [0.7071, 0, 0, -0.7071]
rot_x = [0, 1, 0, 0]
rot = openravepy.quatMult(rot_z, rot_x).tolist()
traj1, _ = self.traj_generator.traj_from_pose(pos1, rot)
with self.env:
# saving values
orig_values = self.robot.GetDOFValues(
self.robot.GetManipulator('rightarm').GetArmIndices())
self.robot.SetDOFValues(traj1[-1],
self.robot.GetManipulator('rightarm').GetArmIndices())
pos2 = [0.0, -0.7, 1.0]
traj2, _ = self.traj_generator.traj_from_pose(pos2, rot)
# reset
self.robot.SetDOFValues(orig_values,
self.robot.GetManipulator('rightarm').GetArmIndices())
self._execute_traj(traj1.tolist() + traj2.tolist())
# open gripper
self.robot.Release(obj)
if self.use_ros:
self.pr2.rgrip.open()
# transforming the object
T = obj.GetTransform()
rot_angle = (np.pi / 2., 0., 0) #got this from the model
rot_mat = openravepy.rotationMatrixFromAxisAngle(rot_angle)
T[:3, :3] = rot_mat
_, _, _, _, z = utils.get_object_limits(table)
T[2, 3] = z
obj.SetTransform(T)
def _execute_traj(self, traj):
traj_obj = utils.array_to_traj(self.robot, traj)
print("Executing trajectory...")
utils.run_trajectory(self.robot, traj)
if self.use_ros:
raw_input("Press enter to run trajectory on PR2")
self.pr2.rarm.follow_joint_trajectory(traj)
self.pr2.join_all() # Doesn't work in sim for some reason..
#raw_input("Press enter when real PR2 is done moving...") # temporary fix for above
print("Trajectory execution complete!")
def _get_grasping_trajectory(self, obj_to_grasp):
"""
Finds a valid grasping trajectory or raises an ObjectMoveError
if a valid trajectory cannot be found
Parameters:
obj_to_grasp: Object for which to compute a grasping trajectory
Returns:
An OpenRave trajectory object
"""
obj_name = obj_to_grasp.GetName()
traj = self.traj_cache.get(obj_name, None)
if traj is not None:
print "Using existing traj in cache!"
return traj
grasp_pose_list = self.grasp_pose_generator.generate_poses(obj_to_grasp)
print "Trying to find a collision-free trajectory..."
traj, _ = self.grasp_trajectory_generator.generate_grasping_traj(
obj_to_grasp, grasp_pose_list)
if traj is not None:
print "Found a collision-free trajectory!!"
return traj
print "No collision-free trajectory found!"
print "Trying to find any trajectory..."
traj, collisions = self.grasp_trajectory_generator.generate_grasping_traj(
obj_to_grasp, grasp_pose_list, collisionfree=False)
if traj is not None:
print "Trajectory found with collisions: {}".format(collisions)
self.traj_cache[obj_name] = traj
e = ObjectMoveError()
e.collision_list = [obj.GetName() for obj in collisions]
raise e
print "Object cannot be moved!"
raise
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from modules.drivers.proto import mobileye_pb2
from modules.planning.proto import planning_pb2
from modules.canbus.proto import chassis_pb2
from path_decider import PathDecider
from trajectory_generator import TrajectoryGenerator
planning_pub = None
PUB_NODE_NAME = "planning"
PUB_TOPIC = "/apollo/" + PUB_NODE_NAME
f = open("benchmark.txt", "w")
CRUISE_SPEED = 10 # m/s
path_decider = PathDecider()
traj_generator = TrajectoryGenerator()
nx = []
ny = []
def localization_callback(localization_pb):
speed_x = localization_pb.pose.linear_velocity.x
speed_y = localization_pb.pose.linear_velocity.y
acc_x = localization_pb.linear_acceleration.x
acc_y = localization_pb.linear_acceleration.y
def chassis_callback(chassis_pb):
global SPEED
SPEED = chassis_pb.speed_mps
class TestTrajectoryGenerator(unittest.TestCase):
"""Contains the unit tests for the TrajectoryGenerator."""
def setUp(self) -> None:
config = {'turning_radius': TURNING_RADIUS}
self.trajectory_generator = TrajectoryGenerator(config)
def test_generate_trajectory_only_arc(self):
# Quadrant 1
end_point = np.array([1, 1])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
# Quadrant 2
end_point = np.array([-1, 1])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
# Quadrant 3
end_point = np.array([-1, -1])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
# Quadrant 4
end_point = np.array([1, -1])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
def test_generate_trajectory_only_line(self):
# Quadrant 1
end_point = np.array([1, 1])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, np.deg2rad(45), np.deg2rad(45), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
# Quadrant 2
end_point = np.array([-1, 1])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, np.deg2rad(135), np.deg2rad(135), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
# Quadrant 3
end_point = np.array([-1, -1])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, -np.deg2rad(135), -np.deg2rad(135), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
# Quadrant 4
end_point = np.array([1, -1])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, -np.deg2rad(45), -np.deg2rad(45), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
def test_generate_trajectory_line_to_arc(self):
# Quadrant 1
end_point = np.array([2, 1])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
# Quadrant 2
end_point = np.array([-2, 1])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
# Quadrant 3
end_point = np.array([-2, -1])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
# Quadrant 1
end_point = np.array([2, -1])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
def test_generate_trajectory_line_to_end(self):
# Quadrant 1
end_point = np.array([1, 2])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
# Quadrant 2
end_point = np.array([-1, 2])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
# Quadrant 3
end_point = np.array([-1, -2])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
# Quadrant 4
end_point = np.array([1, -2])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
def test_generate_trajectory_radius_too_small(self):
# Quadrant 1
end_point = np.array([.9, .9])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE)
self.assertEqual(trajectory, None)
# Quadrant 2
end_point = np.array([-.9, -.9])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE)
self.assertEqual(trajectory, None)
# Quadrant 3
end_point = np.array([-.9, -.9])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE)
self.assertEqual(trajectory, None)
# Quadrant 4
end_point = np.array([.9, -.9])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE)
self.assertEqual(trajectory, None)
def test_generate_trajectory_parallel_lines_coincident(self):
# Quadrant 1
end_point = np.array([5, 0])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
# Quadrant 2
end_point = np.array([-5, 0])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE)
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
self.assertGreater(len(trajectory.path.xs), 0)
def test_generate_trajectory_parallel_lines_not_coincident(self):
# Quadrant 1
end_point = np.array([0, 3])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE)
self.assertEqual(trajectory, None)
# Quadrant 2
end_point = np.array([0, 3])
trajectory = self.trajectory_generator.generate_trajectory(
end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE)
self.assertEqual(trajectory, None)
# parameters for the trajectory
ArmSide: bool = True
steps: int = 5 ######
# ---------------------- Anzahl der Events: Objektbewegung, Augenbilder, Scenebilder ------------------------
start_X, end_X, armSide, NewHandCoord, NewHeadCoord = __initializeArmSide(
ArmSide) # type: float, float, str, List[float], List[float]
start_Y: float = __regulatingValuesEqualToLimit(
0.7, False, True) # StartSelector(False, True)
start_Z: float = __regulatingValuesEqualToLimit(
0.3, True, True) # StartSelector(True, True)
X: TrajectoryGenerator = TrajectoryGenerator(start_X,
end_X,
stepsize=0.05,
dtyp=np.float_,
num=steps,
linspace=True)
Y: TrajectoryGenerator = TrajectoryGenerator(start_Y,
0.8,
stepsize=0.05,
dtyp=np.float_,
num=steps,
linspace=True)
Z: TrajectoryGenerator = TrajectoryGenerator(start_Z,
0.4,
stepsize=0.05,
dtyp=np.float_,
num=steps,
linspace=True)
X_trajectory: ndarray = X.ConcatenateTrajectoryArrays()