def test4():
mystart = point(0, 0, 0)
myend = point(100, 100, pi / 2.0)
myrange = arange(0, 1.01, 0.01)
myacts = ActionsPoseToPoint(mystart, [2, 0], 100, 2 * pi / 4.0)
#myacts = ActionsPoseToPoint(mystart, [1,1], 50, pi/4.0)
plot([mystart.x], [mystart.y], 'go', markersize=15)
plot([mystart.x, 0.07 * cos(mystart.theta)],
[mystart.y, 0.07 * sin(mystart.theta)],
'r',
linewidth=3)
X1, Y1 = [], []
plt, = plot([], [])
for i in range(100):
myspline = myacts.getAction(i)
X, Y = myspline.getVals(myrange)
print "curvature", myspline.curvature()
#X1.append(X); Y1.append(Y);
plot(X, Y)
#plt.set_data(X1,Y1)
axis([-0.3, 2.1, -1.2, 1.2])
draw()
raw_input()
#print "saving -> video/splines"+num_as_str(i)+".png"
#savefig("video/splines"+num_as_str(i)+".png")
title("Action Space")
#savefig("data/action_space.eps")
show()
def test1():
mystart = point(0, 0, 0)
myend = point(10, 10, pi / 4.0)
myrange = arange(0, 1, 0.01)
ion()
for i in range(100):
myspline = Spline(mystart, myend, i, i)
X, Y = myspline.getVals(myrange)
plot(X, Y)
draw()
show()
def test2():
mystart = point(0, 0, 0)
myend = point(100, 100, pi / 2.0)
myrange = arange(0, 1, 0.01)
ion()
myacts = ActionsPoseToPose(mystart, myend, 10)
for i in range(10):
myspline = myacts.getAction(i)
X, Y = myspline.getVals(myrange)
plot(X, Y)
draw()
show()
def test4():
#create the noise models
tv_nm = motion_noise(0.0001, 0, 0.001, 0.0)
rv_nm = motion_noise(0, 0.0001, 0.0, 0.001)
slip_nm = motion_noise(0, 0, 0.0001, 0.0)
#make the papa of noise models here
nm = motion_noise_model_slip(tv_nm, rv_nm, slip_nm)
ion()
mysim = sim_carmen_map(
point(3, 3, 0),
nm,
filename=
"/home/tkollar/local/tklib/tools/trajopt/data/maps/hurdles.cmf.gz")
mysim.plot()
for i in range(180):
res = mysim.simulate_measurements()
print res
#print res
#raw_input()
X, Y = rtheta_to_xy(mysim.get_pose(), res)
plot(X, Y, 'bo')
show()
def simulate_motion(self, tv, rv, dt):
dth1 = rv*(dt/2.0)
dr = tv*dt
dth2 = rv*(dt/2.0)
ds = self.noise_model.slip_noise_model.getmean(tv, rv)
#use dth1 since otherwise we are doubling the error
N = self.noise_model.getvariance(dr, dth1)
U = self.noise_model.getmean(dr, dth1)
#print "N->", N
dr_hat, dth_hat, dslip_hat = normal_sampleC(N, U)
#use the motion model of parr to simulate the true motion
#of the robot
x_true = self.true_pose.x + (dr_hat)*cos(self.true_pose.theta + dth_hat) + dslip_hat*cos(self.true_pose.theta + dth_hat + pi/2.0)
y_true = self.true_pose.y + (dr_hat)*sin(self.true_pose.theta + dth_hat) + dslip_hat*sin(self.true_pose.theta + dth_hat + pi/2.0)
th_true = normalize_theta(self.true_pose.theta + 2.0*dth_hat)
self.true_pose.x = x_true
self.true_pose.y = y_true
self.true_pose.theta = th_true
#compute the corrupted odometry
x_pr = self.corrupted_pose.x + (dr)*cos(self.corrupted_pose.theta + dth1) + ds*cos(self.corrupted_pose.theta + dth1 + pi/2.0)
y_pr = self.corrupted_pose.y + (dr)*sin(self.corrupted_pose.theta + dth1) + ds*sin(self.corrupted_pose.theta + dth1 + pi/2.0)
th_pr = normalize_theta(self.corrupted_pose.theta + rv*dt)
self.corrupted_pose.x = x_pr
self.corrupted_pose.y = y_pr
self.corrupted_pose.theta = th_pr
return point(x_pr, y_pr, th_pr)
def x_y_to_points(x, y):
ptList = []
for i in range(len(x)):
ptList.append(datatypes.point(x[i], y[i], 0))
return ptList
def test2():
ion()
start_pose = point(0, 0, 0)
#create the noise models
tv_nm = motion_noise(1.0, 0, 0.1, 0.0)
rv_nm = motion_noise(0, 1.0, 0.0, 0.1)
slip_nm = motion_noise(0, 0, 0.0001, 0.0)
#make the papa of noise models here
nm = motion_noise_model_slip(tv_nm, rv_nm, slip_nm)
onm = observation_noise_model(0.01, 0.01, 0.1)
#generate the features
myfeat = generate_circle_features(0.0, 0.0, 1.0)
#make the simulator
sim = sim_hurdle_map(start_pose, myfeat, nm, onm)
#test motion simulator
true_pos, = plot([], [], 'go', markersize=10)
true_ang, = plot([], [], linewidth=3)
meas_pos, = plot([], [], 'ro', markersize=10)
meas_ang, = plot([], [], linewidth=3)
features_plt, = plot([], [], 'mx', markersize=5)
for i in range(1000):
sim.simulate_motion(0.5, 0.3, 0.1)
axis([-3, 3, -3, 3])
#set true pose
true_pos.set_data([sim.true_pose.x], [sim.true_pose.y])
true_ang.set_data([
sim.true_pose.x, sim.true_pose.x + 0.2 * cos(sim.true_pose.theta)
], [sim.true_pose.y, sim.true_pose.y + 0.2 * sin(sim.true_pose.theta)])
#set measured pose
meas_pos.set_data([sim.corrupted_pose.x], [sim.corrupted_pose.y])
meas_ang.set_data([
sim.corrupted_pose.x,
sim.corrupted_pose.x + 0.2 * cos(sim.corrupted_pose.theta)
], [
sim.corrupted_pose.y,
sim.corrupted_pose.y + 0.2 * sin(sim.corrupted_pose.theta)
])
R, Phi, Sig = sim.simulate_features(10.0, -pi / 2.0, pi / 2.0)
X, Y = measurement_to_global(R, Phi, sim.corrupted_pose)
features_plt.set_data(X, Y)
draw()
show()
def getAction(self, a):
if (a >= self.numActions or a < 0):
print "ActionsPoseToPoint: action out of range ->", a
sys.exit(0)
numsubactions = ceil(self.numActions / (1.0 * len(self.theta_dest)))
sub_action = mod(a, numsubactions)
super_action = (a - mod(a, numsubactions)) / (numsubactions * 1.0)
end_pose = point(
self.end_pt[0], self.end_pt[1],
self.theta_dest[int(super_action)] + self.start_pose.theta)
myactions = ActionsPoseToPose(self.start_pose, end_pose, numsubactions)
return myactions.getAction(int(sub_action))
def test3():
#create the noise models
tv_nm = motion_noise(1.0, 0, 0.1, 0.0)
rv_nm = motion_noise(0, 1.0, 0.0, 0.1)
slip_nm = motion_noise(0, 0, 0.0001, 0.0)
#make the papa of noise models here
nm = motion_noise_model_slip(tv_nm, rv_nm, slip_nm)
myogm = sim_carmen_map(point(3,3,0), nm,
filename="/home/tkollar/installs/carmen/data/thickwean.map")
#filename="/home/tkollar/local/tklib/pytools/trajopt/data/maps/hurdles.cmf.gz")
myogm.plot()
show()
def callback(self, the_type, msg):
if (the_type == "front_laser"):
if ((msg["timestamp"] - self.last_message_timestamp) < 0.01):
self.last_message_timestamp = msg["timestamp"]
return
self.last_message_timestamp = msg["timestamp"]
curr_odo = point(msg["robot_pose"][0], msg["robot_pose"][1],
msg["robot_pose"][2])
curr_laser = msg["range"]
curr_timestamp = msg["timestamp"]
if (not self.prev_timestamp == maxint):
self.dt = curr_timestamp - self.prev_timestamp
self.update_ekf(self.prev_timestamp, self.prev_odo,
curr_timestamp, curr_odo, curr_laser)
self.prev_timestamp = curr_timestamp
self.prev_odo = curr_odo
def callback(self, the_type, msg):
#print msg.keys()
#msg["robot_pose"]
#msg["range"]
pts = rtheta_to_xy(point(0, 0, 0), msg["range"])
corners = corner_extractor(pts, 5, 0.2)
pts = array(pts)
X, Y = pts
print pts
self.range_plt.set_data(X, Y)
X = pts[0].take(corners)
Y = pts[1].take(corners)
self.my_corner_plt.set_data(X, Y)
axis([-5, 15, -10, 10])
draw()
def test1():
ion()
start_pose = point(0, 0, 0)
#create the noise models
tv_nm = motion_noise(1.0, 0, 0.001, 0.0)
rv_nm = motion_noise(0, 1.0, 0.0, 0.001)
slip_nm = motion_noise(0, 0, 0.01, 0.0)
#make the papa of noise models here
nm = motion_noise_model_slip(tv_nm, rv_nm, slip_nm)
onm = observation_noise_model(0.1, 0.05, 0.1)
#make the simulator
sim = sim_hurdle_map(start_pose, [], nm, onm)
#test motion simulator
true_pos, = plot([], [], 'go', markersize=5)
true_ang, = plot([], [], linewidth=3)
meas_pos, = plot([], [], 'ro', markersize=5)
meas_ang, = plot([], [], linewidth=3)
for i in range(1000):
axis([-5, 5, -5, 5])
#sim.simulate_motion(0.5, 0.05, 0.1)
sim.simulate_motion(0.5, 0.3, 0.1)
#set true pose
true_pos.set_data([sim.true_pose.x], [sim.true_pose.y])
true_ang.set_data([
sim.true_pose.x, sim.true_pose.x + 0.2 * cos(sim.true_pose.theta)
], [sim.true_pose.y, sim.true_pose.y + 0.2 * sin(sim.true_pose.theta)])
#set measured pose
meas_pos.set_data([sim.corrupted_pose.x], [sim.corrupted_pose.y])
meas_ang.set_data([
sim.corrupted_pose.x,
sim.corrupted_pose.x + 0.2 * cos(sim.corrupted_pose.theta)
], [
sim.corrupted_pose.y,
sim.corrupted_pose.y + 0.2 * sin(sim.corrupted_pose.theta)
])
draw()
show()
def __init__(self, filename, show_gui=True):
#load the logfile and get the relevant EKF
#policies, problem, myEKF = load_problem_with_EKF(filename)
trajopt = carmen_util_load_pck(filename)
self.myEKF = trajopt.get_ekf(point(0, 0, 0))
#open up a robot and subscribe to front_laser messages
Robot.__init__(self)
fl = pyCarmen.front_laser(self)
#initialize pyTklib message passing to allow
# ekf messages to be published
pyTklib.carmen_ipc_initialize(1, ["python"])
#initialize relevant state
self.prev_odo = None
self.prev_timestamp = maxint
self.last_message_timestamp = maxint
self.dt = 0.1
self.show_gui = show_gui
if (show_gui):
self.init_plot()
def test2():
ion()
tv_nm = noise_model2D(1.0, 0, 0.1, 0.0)
rv_nm = noise_model2D(0, 1.0, 0.0, 0.1)
slip_nm = noise_model2D(0, 0, 0.01, 0.0)
#make the papa of noise models here
nm = motion_noise_model_slip(tv_nm, rv_nm, slip_nm)
onm = observation_noise_model(0.1, 0.01, 0.0001)
x, y, theta = [10, 20, 0]
ax = gca()
#make a simulator, load and plot the map
print "simulator"
mysim = simulator2D([x,y,theta], nm, onm, "../../data/maps/thickwean.map");
print "ogm"
mymap = occupancy_grid_mapper(0.8,0.8, 0.5, 100, 100, 0.1, mysim.get_pose(), 0.2)
myid = carmen_maptools.plot_map(mymap.map.get_map(), mymap.map.x_size, mymap.map.y_size);
nn_plt, = plot([], [], 'ro');
orig_plt, = plot([], [], 'bx');
icp_plt, = plot([], [], 'gx');
robot_pose_plt, = plot([], [], 'ko');
robot_orient_plt, = plot([], [], 'k');
spline_plt, = plot([], [], 'b');
spline_pltc, = plot([], [], 'g');
#do some simulation
for i in range(1000):
mysim.simulate_motion(1.0, 0.6, 0.1);
sim_meas = mysim.simulate_measurements();
mymap.update(mysim.get_pose(), sim_meas);
x, y, theta = mymap.get_pose()
if(mod(i, 5) == 0):
carmen_maptools.plot_map(mymap.map.get_map(), mymap.map.x_size, mymap.map.y_size, cmap="binary");
ax.images = [ax.images[len(ax.images)-1]]
#do some spline processing
x, y, theta = mysim.get_pose()
#spline = Spline(point(x, y, theta), point(22.0, 27.1, 0), 20.0, 20.0)
spline = Spline(point(x, y, theta), point(19.0, 20.6, pi/8.0), 30.0, 40.0)
X, Y = spline.getVals(arange(0, 1, 0.01))
spline_plt.set_data(X, Y);
#to C type
cspline = spline.toCtype()
X, Y = cspline.value(arange(0, 1, 0.01))
spline_pltc.set_data(X, Y)
isfree = mymap.map.path_free(cspline)
if(isfree == 1):
print "its free"
elif(isfree == 0):
print "hit obstacle"
elif(isfree == -1):
print "outside map"
elif(isfree == -2):
print "hit unknown"
robot_pose_plt.set_data([x], [y]);
robot_orient_plt.set_data([x, x+0.3*cos(theta)], [y, y+0.3*sin(theta)]);
draw()
show()
def atDestination(self, curr_pose, epsilon):
dist = curr_pose.distance(point(self.x1, self.y1))
if (dist < epsilon):
return True
return False
def get_end_pose(self):
return point(self.x1, self.y1, self.end_theta)
def get_start_pose(self):
return point(self.x0, self.y0, self.start_theta)