def closest_point(self, f_x, f_y, s_x, s_y, obs_bearing): ''' Calculate the closest point on the line to the feature The feature is the point (probably not on the line) The line is defined by a point (state x and y) and direction (heading) This probably won't return a point that is behind the x-y-bearing. Input: f_x float (feature's x coordinate) f_y float (feature's y coordinate) s_x float (robot state's x) s_y float (robot state's y) obs_bearing float (robot state's heading) ''' origin_to_feature = (f_x - s_x, f_y - s_y, 0.0,) line_parallel = unit((cos(obs_bearing), sin(obs_bearing), 0.0)) # origin_to_feature dot line_parallel = magnitude of otf along line magmag = dot_product(origin_to_feature, line_parallel) if magmag < 0: return (s_x, s_y) scaled_line = scale(line_parallel, magmag) scaled_x = scaled_line[0] scaled_y = scaled_line[1] return (float(s_x + scaled_x), float(s_y + scaled_y))
def closest_point(self, f_x, f_y, s_x, s_y, obs_bearing): ''' Calculate the closest point on the line to the feature The feature is the point (probably not on the line) The line is defined by a point (state x and y) and direction (heading) This probably won't return a point that is behind the x-y-bearing. Input: f_x float (feature's x coordinate) f_y float (feature's y coordinate) s_x float (robot state's x) s_y float (robot state's y) obs_bearing float (robot state's heading) ''' origin_to_feature = ( f_x - s_x, f_y - s_y, 0.0, ) line_parallel = unit((cos(obs_bearing), sin(obs_bearing), 0.0)) # origin_to_feature dot line_parallel = magnitude of otf along line magmag = dot_product(origin_to_feature, line_parallel) if magmag < 0: return (s_x, s_y) scaled_line = scale(line_parallel, magmag) scaled_x = scaled_line[0] scaled_y = scaled_line[1] return (float(s_x + scaled_x), float(s_y + scaled_y))
def generate_plan(self, start, goal): # debug = None debug = rospy.loginfo if debug is not None: debug('potential generate plane\n'+str(goal)) deck = deque() deck.append(start) next_ = deepcopy(start) distance = (math.pow(next_.position.x-goal.position.x, 2) + math.pow(next_.position.y-goal.position.y, 2)) if distance < .01: debug('the start distance is very small') return [] debug('dist: %f' % (distance,)) step_size = 0.2 count = max(10, int(1.0/step_size)) while(distance > .01 and count >= 0): debug('start calculating obs_force') obs_force = self.calc_potential(next_) debug('end calculating obs_force') goal_force = self.goal_force(next_, goal) total_force = addv(obs_force, goal_force) total_force = unit(total_force) debug('%s\n%s\n%s' % (obs_force, goal_force, total_force)) dx = total_force[0]*step_size dy = total_force[1]*step_size debug('d: %f , %f' % (dx, dy,)) new_pose = Pose() new_pose.position.x = next_.position.x+dx new_pose.position.y = next_.position.y+dy new_pose.orientation = heading_to_quaternion(math.atan2(dy, dx)) deck.append(new_pose) next_ = deepcopy(new_pose) distance = (math.pow(next_.position.x-goal.position.x, 2) + math.pow(next_.position.y-goal.position.y, 2)) debug('dist: %f' % (distance,)) count += -1 if (distance > .01): debug('count break') else: deck.append(goal) return list(deck)
def goal_force(self, pose, goal): dx = goal.position.x - pose.position.x dy = goal.position.y - pose.position.y weight = 5.0 farce = (dx, dy, 0) farce = unit(farce) farce = scale(farce, weight) return farce
def off_axis_error(self, location, goal): """ calc error normal to axis defined by the goal position and direction input: two nav_msgs.msg.Odometry, current best location estimate and goal output: double distance along the axis axis is defined by a vector from the unit circle aligned with the goal heading relative position is the vector from the goal x, y to the location x, y distance is defined by subtracting the parallel vector from the total relative position vector example use: see calc_errors above """ relative_position_x = (location.pose.pose.position.x - goal.pose.pose.position.x) relative_position_y = (location.pose.pose.position.y - goal.pose.pose.position.y) # relative position of the best estimate position and the goal # vector points from the goal to the location ## relative_position = (relative_position_x, relative_position_y, 0.0) goal_heading = quaternion_to_heading(goal.pose.pose.orientation) goal_vector_x = math.cos(goal_heading) goal_vector_y = math.sin(goal_heading) # vector in the direction of the goal heading, axis of desired motion goal_vector = (goal_vector_x, goal_vector_y, 0.0) along_axis_error = self.along_axis_error(location, goal) along_axis_vec = scale(unit(goal_vector), along_axis_error) new_rel_x = relative_position_x - along_axis_vec[0] new_rel_y = relative_position_y - along_axis_vec[1] new_rel_vec = (new_rel_x, new_rel_y, 0.0) error_magnitude = math.sqrt(new_rel_x*new_rel_x + new_rel_y*new_rel_y) if error_magnitude < .0001: return 0.0 if cross_product(goal_vector, new_rel_vec)[2] >= 0.0: return error_magnitude else: return -error_magnitude
def construct_problem_mixed(spline_template, observed_accel_timestamps, observed_accel_orientations, observed_accel_readings, observed_frame_timestamps, observed_frame_orientations, observed_features, imu_to_camera=np.eye(3), camera_matrix=np.eye(3), feature_tolerance=1e-2, gravity_magnitude=9.8, max_bias_magnitude=.1): if len(observed_features) < 5: raise InsufficientObservationsError() # Sanity checks assert isinstance(spline_template, spline.SplineTemplate) assert len(observed_accel_orientations) == len(observed_accel_readings) assert len(observed_accel_timestamps) == len(observed_accel_readings) assert len(observed_frame_timestamps) == len(observed_frame_orientations) assert all(0 <= f.frame_id < len(observed_frame_timestamps) for f in observed_features) assert np.ndim(observed_accel_timestamps) == 1 assert np.ndim(observed_frame_timestamps) == 1 # Compute offsets position_offset = 0 position_len = spline_template.control_size gravity_offset = position_offset + position_len accel_bias_offset = gravity_offset + 3 structure_offset = accel_bias_offset + 3 track_ids = set(f.track_id for f in observed_features) num_aux_vars = 1 # one extra variable representing the objective num_frames = len(observed_frame_timestamps) num_tracks = max(track_ids) + 1 num_vars = structure_offset + num_tracks * 3 + num_aux_vars # Make sure each track has at least one observation counts_by_frame = np.zeros(num_frames, int) counts_by_track = np.zeros(num_tracks, int) for f in observed_features: counts_by_frame[f.frame_id] += 1 counts_by_track[f.track_id] += 1 if not np.all(counts_by_frame > 0): raise InsufficientObservationsError( 'These frames had zero features: ' + ','.join(map(str, np.flatnonzero(counts_by_frame == 0)))) if not np.all(counts_by_track > 0): raise InsufficientObservationsError( 'These tracks had zero features: ' + ','.join(map(str, np.flatnonzero(counts_by_track == 0)))) # Track IDs should be exactly 0..n-1 assert all(track_id < num_tracks for track_id in track_ids) # Initialize the problem objective = utils.unit(num_vars-1, num_vars) # the last variable is the objective we minimize problem = socp.SocpProblem(objective) # Construct accel constraints print 'Constructing constraints for %d accel readings...' % len(observed_accel_readings) accel_coefficients = spline_template.coefficients_d2(observed_accel_timestamps) accel_j_blocks = [] accel_r_blocks = [] for r, a, c in zip(observed_accel_orientations, observed_accel_readings, accel_coefficients): amat = spline.diagify(c, 3) j = np.zeros((3, num_vars)) j[:, :position_len] = np.dot(r, amat) j[:, gravity_offset:gravity_offset+3] = r j[:, accel_bias_offset:accel_bias_offset+3] = np.eye(3) accel_j_blocks.append(j) accel_r_blocks.append(a) # Form the least squares objective || J*x + r ||^2 accel_j = np.vstack(accel_j_blocks) accel_r = np.hstack(accel_r_blocks) # Form the quadratic objective: x' J' J x + b' x + c <= objective ("objective" is the variable we minimize) accel_c = np.dot(accel_r, accel_r) accel_b = -2. * np.dot(accel_j.T, accel_r) accel_b[-1] = -1. # Convert to an SOCP objective problem.add_constraint(*soc_constraint_from_quadratic_constraint(accel_j, accel_b, accel_c)) # Construct gravity constraints a_gravity = np.zeros((3, num_vars)) a_gravity[:, gravity_offset:gravity_offset+3] = np.eye(3) d_gravity = gravity_magnitude problem.add_constraint(a=a_gravity, d=d_gravity) # Construct accel bias constraints a_bias = np.zeros((3, num_vars)) a_bias[:, accel_bias_offset:accel_bias_offset+3] = np.eye(3) d_bias = max_bias_magnitude problem.add_constraint(a=a_bias, d=d_bias) # Construct vision constraints print 'Constructing constraints for %d features...' % len(observed_features) pos_coefficients = spline_template.coefficients(observed_frame_timestamps) pos_multidim_coefs = [spline.diagify(x, 3) for x in pos_coefficients] for feature in observed_features: r = observed_frame_orientations[feature.frame_id] pmat = pos_multidim_coefs[feature.frame_id] point_offset = structure_offset + feature.track_id*3 assert point_offset + 3 <= num_vars, 'track id was %d, num vars was %d' % (feature.track_id, num_vars) k_rc_r = np.dot(camera_matrix, np.dot(imu_to_camera, r)) ymat = np.zeros((3, num_vars)) ymat[:, :position_len] = -np.dot(k_rc_r, pmat) ymat[:, point_offset:point_offset+3] = k_rc_r a_feature = ymat[:2] - np.outer(feature.position, ymat[2]) c_feature = ymat[2] * feature_tolerance problem.add_constraint(a=a_feature, c=c_feature) return problem