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)