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 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)
