def get_odom_sample_partial(s): # Sample srot1 = sample_rot1(u_move2d, motion_variances) trans = sample_trans(u_move2d, motion_variances) srot2 = sample_rot2(u_move2d, motion_variances) rot1 = ut.standard_rad(u_move2d.rot1 - srot1) trans = u_move2d.trans - trans rot2 = u_move2d.rot2 - srot2 # print mt.degrees(rot1), trans, mt.degrees(rot2) # Calculate new values sx = s.pos[0, 0] sy = s.pos[1, 0] x = sx + trans * mt.cos(s.angle + rot1) y = sy + trans * mt.sin(s.angle + rot1) total_rot = ut.standard_rad(s.angle + rot1 + rot2) return t2d.Pose2D(x, y, total_rot)
def get_odom_sample_partial(s): # Sample srot1 = sample_rot1 (u_move2d, motion_variances) trans = sample_trans(u_move2d, motion_variances) srot2 = sample_rot2 (u_move2d, motion_variances) rot1 = ut.standard_rad(u_move2d.rot1 - srot1) trans = u_move2d.trans - trans rot2 = u_move2d.rot2 - srot2 #print mt.degrees(rot1), trans, mt.degrees(rot2) # Calculate new values sx = s.pos[0,0] sy = s.pos[1,0] x = sx + trans * mt.cos(s.angle + rot1) y = sy + trans * mt.sin(s.angle + rot1) total_rot = ut.standard_rad(s.angle + rot1 + rot2) return t2d.Pose2D(x,y, total_rot)
def calc(self, t): dir = self.polar_node.val(t) speed = dir[0,0] turn = dir[1,0] #If it's behind us, err flip it and reverse it if (turn < (-np.pi/2.0)) or (turn > (np.pi/2.0)): dir[0,0] = speed * -1 dir[1,0] = ut.standard_rad(turn + np.pi) return dir
def calc(self, t): dir = self.polar_node.val(t) speed = dir[0, 0] turn = dir[1, 0] #If it's behind us, err flip it and reverse it if (turn < (-np.pi / 2.0)) or (turn > (np.pi / 2.0)): dir[0, 0] = speed * -1 dir[1, 0] = ut.standard_rad(turn + np.pi) return dir
def p2d_to_move2d(odom): """ Decompose odometry readings into three components initial rotation translation final rotation odom - a differential reading between this and the last time step """ if (odom.pos == np.matrix([0.0, 0.0]).T).all(): orot1 = 0.0 else: orot1 = ut.standard_rad(ut.ang_of_vec(odom.pos) - odom.angle) otran = np.linalg.norm(odom.pos) orot2 = odom.angle - orot1 return move2d(orot1, otran, orot2)
def __sub__(self, other): return numpy.linalg.norm(self.pos - other.pos) - ut.standard_rad(self.angle - other.angle)