void test_figtree() {
Vector3d object(3.35, -1.11, 0.8);
Arm::ArmType arm_type = Arm::ArmType::right;
bool view = true;
PR2System sys(object, arm_type, view);
PR2* brett = sys.get_brett();
Arm* arm = sys.get_arm();
rave::EnvironmentBasePtr env = brett->get_env();
arm->set_posture(Arm::Posture::mantis);
sleep(1);
MatrixP P = setup_particles(env);
std::vector<ParticleGaussian> particle_gmm;
tc.start("figtree");
sys.fit_gaussians_to_pf(P, particle_gmm);
tc.stop("figtree");
std::cout << "particle_gmm size: " << particle_gmm.size() << "\n";
for(int i=0; i < particle_gmm.size(); ++i) {
std::cout << "mean: " << particle_gmm[i].mean.transpose() << "\n";
std::cout << "cov diag: " << particle_gmm[i].cov.diagonal().transpose() << "\n";
std::cout << "pct: " << particle_gmm[i].pct << "\n";
}
tc.print_all_elapsed();
sys.display(arm->get_joint_values(), particle_gmm);
}
void test_particle_update() {
Vector3d object(3.35, -1.11, 0.8);
Arm::ArmType arm_type = Arm::ArmType::right;
bool view = true;
PR2System sys(object, arm_type, view);
PR2* brett = sys.get_brett();
Arm* arm = sys.get_arm();
rave::EnvironmentBasePtr env = brett->get_env();
sleep(2);
arm->set_posture(Arm::Posture::mantis);
// setup scenario
VectorJ j_t_real = arm->get_joint_values(), j_tp1_real;
VectorJ j_t = j_t_real, j_tp1; // i.e. no belief == actual
VectorU u_t = VectorU::Zero();
MatrixP P_t = setup_particles(env), P_tp1;
LOG_INFO("Origin particles");
std::vector<ParticleGaussian> particle_gmm_t;
sys.fit_gaussians_to_pf(P_t, particle_gmm_t);
sys.display(j_t, particle_gmm_t);
sys.execute_control_step(j_t_real, j_t, u_t, P_t, j_tp1_real, j_tp1, P_tp1);
LOG_INFO("New particles")
std::vector<ParticleGaussian> particle_gmm_tp1;
sys.fit_gaussians_to_pf(P_tp1, particle_gmm_tp1);
sys.display(j_tp1, particle_gmm_tp1);
}
void test_pr2_system() {
Vector3d object(3.35, -1.11, 0.8);
Arm::ArmType arm_type = Arm::ArmType::right;
bool view = true;
PR2System sys(object, arm_type, view);
PR2* brett = sys.get_brett();
Arm* arm = sys.get_arm();
rave::EnvironmentBasePtr env = brett->get_env();
sleep(2);
arm->set_posture(Arm::Posture::mantis);
// setup particles
MatrixP P = setup_particles(env);
// test plotting
VectorJ j = arm->get_joint_values();
std::vector<ParticleGaussian> particle_gmm;
particle_gmm.push_back(ParticleGaussian(Vector3d::Zero(), Matrix3d::Identity(), P.leftCols(M_DIM/2), 1));
particle_gmm.push_back(ParticleGaussian(Vector3d::Zero(), Matrix3d::Identity(), P.rightCols(M_DIM/2), 1));
sys.display(j, particle_gmm);
}
// The start of the Application
int App::start(const std::vector<std::string> &args)
{
clan::DisplayWindowDescription win_desc;
//win_desc.set_version(3, 2, false);
win_desc.set_allow_resize(true);
win_desc.set_title("Point Sprite Example");
win_desc.set_size(clan::Size( 800, 480 ), false);
clan::DisplayWindow window(win_desc);
clan::SlotContainer cc;
cc.connect(window.sig_window_close(), clan::bind_member(this, &App::on_window_close));
cc.connect(window.get_ic().get_keyboard().sig_key_up(), clan::bind_member(this, &App::on_input_up));
std::string theme;
if (clan::FileHelp::file_exists("../../../Resources/GUIThemeAero/theme.css"))
theme = "../../../Resources/GUIThemeAero";
else if (clan::FileHelp::file_exists("../../../Resources/GUIThemeBasic/theme.css"))
theme = "../../../Resources/GUIThemeBasic";
else
throw clan::Exception("No themes found");
clan::GUIWindowManagerTexture wm(window);
clan::GUIManager gui(wm, theme);
clan::Canvas canvas(window);
// Deleted automatically by the GUI
Options *options = new Options(gui, clan::Rect(0, 0, canvas.get_size()));
clan::Image image_grid(canvas, "../Blend/Resources/grid.png");
clan::Texture2D texture_particle(canvas, "Resources/particle.png");
float grid_width = (float) image_grid.get_width();
float grid_height = (float) image_grid.get_height();
grid_space = (float) (image_grid.get_width());
setup_particles();
clan::ShaderObject vertex_shader(canvas, clan::shadertype_vertex, text_shader_vertex);
if(!vertex_shader.compile())
{
throw clan::Exception(clan::string_format("Unable to compile vertex shader object: %1", vertex_shader.get_info_log()));
}
clan::ShaderObject fragment_shader(canvas, clan::shadertype_fragment, text_shader_fragment);
if(!fragment_shader.compile())
{
throw clan::Exception(clan::string_format("Unable to compile fragment shader object: %1", fragment_shader.get_info_log()));
}
clan::ProgramObject program_object(canvas);
program_object.attach(vertex_shader);
program_object.attach(fragment_shader);
program_object.bind_attribute_location(0, "InPosition");
program_object.bind_attribute_location(1, "InColor");
if (!program_object.link())
{
throw clan::Exception(clan::string_format("Unable to link program object: %1", program_object.get_info_log()));
}
program_object.set_uniform1i("Texture0", 0);
options->request_repaint();
clan::BlendStateDescription blend_state_desc;
blend_state_desc.enable_blending(true);
blend_state_desc.set_blend_function(clan::blend_src_alpha, clan::blend_one, clan::blend_src_alpha, clan::blend_one);
clan::BlendState blend_state(canvas, blend_state_desc);
clan::GameTime game_time;
while (!quit)
{
game_time.update();
wm.process();
wm.draw_windows(canvas);
int num_particles = options->num_particles;
if (num_particles > max_particles)
num_particles = max_particles;
move_particles(game_time.get_time_elapsed(), num_particles);
const float grid_xpos = 10.0f;
const float grid_ypos = 10.0f;
// Draw the grid
image_grid.draw(canvas, grid_xpos, grid_ypos);
if (num_particles > 0)
{
std::vector<clan::Vec2f> positions;
std::vector<clan::Colorf> colors;
positions.resize(num_particles);
colors.resize(num_particles);
for (int cnt=0; cnt<num_particles; cnt++)
{
positions[cnt] = clan::Vec2f(grid_xpos + particles[cnt].xpos, grid_ypos + particles[cnt].ypos);
switch (cnt % 3)
{
case 0:
colors[cnt] = clan::Colorf(1.0f, 0.0f, 0.0f, 1.0f);
break;
case 1:
colors[cnt] = clan::Colorf(0.0f, 1.0f, 0.0f, 1.0f);
break;
case 2:
colors[cnt] = clan::Colorf(0.0f, 0.0f, 1.0f, 1.0f);
break;
}
};
canvas.flush();
clan::GraphicContext gc = canvas.get_gc();
canvas.set_blend_state(blend_state);
clan::RasterizerStateDescription raster_state_desc;
raster_state_desc.set_point_size(options->point_size);
raster_state_desc.set_point_sprite_origin(clan::origin_upper_left);
clan::RasterizerState raster_state(canvas, raster_state_desc);
canvas.set_rasterizer_state(raster_state);
clan::PrimitivesArray primarray(gc);
clan::VertexArrayVector<clan::Vec2f> gpu_positions = clan::VertexArrayVector<clan::Vec2f>(gc, &positions[0], positions.size());
clan::VertexArrayVector<clan::Colorf> gpu_colors = clan::VertexArrayVector<clan::Colorf>(gc, &colors[0], colors.size());
primarray.set_attributes(0, gpu_positions);
primarray.set_attributes(1, gpu_colors);
ProgramUniforms buffer;
buffer.cl_ModelViewProjectionMatrix = canvas.get_projection() * canvas.get_modelview();
clan::UniformVector<ProgramUniforms> uniform_vector(gc, &buffer, 1);
gc.set_uniform_buffer(0, uniform_vector);
gc.set_texture(0, texture_particle);
gc.set_program_object(program_object);
gc.draw_primitives(clan::type_points, num_particles, primarray);
gc.reset_program_object();
gc.reset_texture(0);
gc.reset_blend_state();
gc.reset_rasterizer_state();
}
window.flip(1);
clan::KeepAlive::process();
}
return 0;
}
App::App()
{
clan::OpenGLTarget::enable();
clan::DisplayWindowDescription win_desc;
//win_desc.set_version(3, 2, false);
win_desc.set_allow_resize(true);
win_desc.set_title("Point Sprite Example");
win_desc.set_size(clan::Size( 900, 480 ), false);
window = clan::DisplayWindow(win_desc);
sc.connect(window.sig_window_close(), clan::bind_member(this, &App::on_window_close));
sc.connect(window.get_ic().get_keyboard().sig_key_up(), clan::bind_member(this, &App::on_input_up));
canvas = clan::Canvas(window);
clan::FileResourceDocument doc(clan::FileSystem("../../ThemeAero"));
clan::ResourceManager resources = clan::FileResourceManager::create(doc);
ui_thread = clan::UIThread(resources);
options = std::make_shared<Options>(canvas);
options->set_event_window(window);
options->set_cursor_window(window);
image_grid = clan::Image(canvas, "../Blend/Resources/grid.png");
texture_particle = clan::Texture2D(canvas, "Resources/particle.png");
float grid_width = (float) image_grid.get_width();
float grid_height = (float) image_grid.get_height();
grid_space = (float) (image_grid.get_width());
setup_particles();
clan::ShaderObject vertex_shader(canvas, clan::shadertype_vertex, text_shader_vertex);
if(!vertex_shader.compile())
{
throw clan::Exception(clan::string_format("Unable to compile vertex shader object: %1", vertex_shader.get_info_log()));
}
clan::ShaderObject fragment_shader(canvas, clan::shadertype_fragment, text_shader_fragment);
if(!fragment_shader.compile())
{
throw clan::Exception(clan::string_format("Unable to compile fragment shader object: %1", fragment_shader.get_info_log()));
}
program_object = clan::ProgramObject(canvas);
program_object.attach(vertex_shader);
program_object.attach(fragment_shader);
program_object.bind_attribute_location(0, "InPosition");
program_object.bind_attribute_location(1, "InColor");
if (!program_object.link())
{
throw clan::Exception(clan::string_format("Unable to link program object: %1", program_object.get_info_log()));
}
program_object.set_uniform1i("Texture0", 0);
clan::BlendStateDescription blend_state_desc;
blend_state_desc.enable_blending(true);
blend_state_desc.set_blend_function(clan::blend_src_alpha, clan::blend_one, clan::blend_src_alpha, clan::blend_one);
blend_state = clan::BlendState(canvas, blend_state_desc);
game_time.reset();
}
// The start of the Application
int App::start(const std::vector<CL_String> &args)
{
CL_OpenGLWindowDescription win_desc;
//win_desc.set_version(3, 2, false);
win_desc.set_allow_resize(true);
win_desc.set_title("Point Sprite Example");
win_desc.set_size(CL_Size( 800, 480 ), false);
CL_DisplayWindow window(win_desc);
CL_Slot slot_quit = window.sig_window_close().connect(this, &App::on_window_close);
CL_Slot slot_input_up = (window.get_ic().get_keyboard()).sig_key_up().connect(this, &App::on_input_up);
CL_String theme;
if (CL_FileHelp::file_exists("../../../Resources/GUIThemeAero/theme.css"))
theme = "../../../Resources/GUIThemeAero";
else if (CL_FileHelp::file_exists("../../../Resources/GUIThemeBasic/theme.css"))
theme = "../../../Resources/GUIThemeBasic";
else
throw CL_Exception("No themes found");
CL_GUIWindowManagerTexture wm(window);
CL_GUIManager gui(wm, theme);
CL_GraphicContext gc = window.get_gc();
// Deleted automatically by the GUI
Options *options = new Options(gui, CL_Rect(0, 0, gc.get_size()));
CL_Image image_grid(gc, "../Blend/Resources/grid.png");
CL_Texture texture_particle(gc, "Resources/particle.png");
float grid_width = (float) image_grid.get_width();
float grid_height = (float) image_grid.get_height();
grid_space = (float) (image_grid.get_width());
setup_particles();
CL_ShaderObject vertex_shader(gc, cl_shadertype_vertex, text_shader_vertex);
if(!vertex_shader.compile())
{
throw CL_Exception(cl_format("Unable to compile vertex shader object: %1", vertex_shader.get_info_log()));
}
CL_ShaderObject fragment_shader(gc, cl_shadertype_fragment, text_shader_fragment);
if(!fragment_shader.compile())
{
throw CL_Exception(cl_format("Unable to compile fragment shader object: %1", fragment_shader.get_info_log()));
}
CL_ProgramObject program_object(gc);
program_object.attach(vertex_shader);
program_object.attach(fragment_shader);
program_object.bind_attribute_location(0, "InPosition");
program_object.bind_attribute_location(1, "InColor");
if (!program_object.link())
{
throw CL_Exception(cl_format("Unable to link program object: %1", program_object.get_info_log()));
}
options->request_repaint();
unsigned int time_last = CL_System::get_time();
while (!quit)
{
unsigned int time_now = CL_System::get_time();
float time_diff = (float) (time_now - time_last);
time_last = time_now;
wm.process();
wm.draw_windows(gc);
int num_particles = options->num_particles;
if (num_particles > max_particles)
num_particles = max_particles;
move_particles(time_diff, num_particles);
const float grid_xpos = 10.0f;
const float grid_ypos = 10.0f;
// Draw the grid
image_grid.draw(gc, grid_xpos, grid_ypos);
if (num_particles > 0)
{
std::vector<CL_Vec2f> positions;
std::vector<CL_Vec4f> colors;
positions.resize(num_particles);
colors.resize(num_particles);
for (int cnt=0; cnt<num_particles; cnt++)
{
positions[cnt] = CL_Vec2f(grid_xpos + particles[cnt].xpos, grid_ypos + particles[cnt].ypos);
switch (cnt % 3)
{
case 0:
colors[cnt] = CL_Vec4f(1.0f, 0.0f, 0.0f, 1.0f);
break;
case 1:
colors[cnt] = CL_Vec4f(0.0f, 1.0f, 0.0f, 1.0f);
break;
case 2:
colors[cnt] = CL_Vec4f(0.0f, 0.0f, 1.0f, 1.0f);
break;
}
};
CL_BlendMode blend;
blend.enable_blending(true);
blend.set_blend_function(cl_blend_src_alpha, cl_blend_one, cl_blend_src_alpha, cl_blend_one);
gc.set_blend_mode(blend);
CL_Pen pen;
pen.enable_point_sprite(true);
pen.set_point_size(options->point_size);
pen.set_point_sprite_origin(cl_point_sprite_origin_upper_left);
gc.set_pen(pen);
program_object.set_uniform1i("Texture0", 0);
gc.set_texture(0, texture_particle);
CL_PrimitivesArray prim_array(gc);
prim_array.set_attributes(0, &positions[0]);
prim_array.set_attributes(1, &colors[0]);
gc.set_program_object(program_object);
gc.draw_primitives(cl_points, num_particles, prim_array);
gc.reset_program_object();
gc.reset_texture(0);
gc.reset_blend_mode();
gc.reset_pen();
}
window.flip(1);
CL_KeepAlive::process();
}
return 0;
}
void test_camera() {
Vector3d object(3.35, -1.11, 0.8);
Arm::ArmType arm_type = Arm::ArmType::right;
bool view = true;
PR2System sys(object, arm_type, view);
PR2* brett = sys.get_brett();
Arm* arm = sys.get_arm();
Camera* cam = sys.get_camera();
rave::EnvironmentBasePtr env = brett->get_env();
MatrixP P = setup_particles(env);
arm->set_posture(Arm::Posture::mantis);
sleep(2);
VectorJ j = arm->get_joint_values();
StdVector3d pcl = cam->get_pcl(j);
cam->plot_pcl(pcl);
// std::cout << "Displaying env mesh. Teleop and then get FOV\n";
arm->teleop();
std::vector<std::vector<Beam3d> > beams = cam->get_beams(j, pcl);
rave_utils::clear_plots();
cam->plot_fov(beams);
std::vector<Triangle3d> border = cam->get_border(beams);
// tc.start("sd");
// for(int m=0; m < M_DIM; ++m) {
// double sd = cam->signed_distance(P.col(m), beams, border);
// }
// tc.stop("sd");
Vector3d p(3.35, -2.5, 0.8);
rave_utils::plot_point(env, p, Vector3d(1,0,0));
while(true) {
VectorU grad = cost_grad(sys, j, p, VectorU::Zero(), .01);
std::cout << "grad:\n" << grad << "\n";
if (grad.norm() < epsilon) {
std::cout << "crap\n";
exit(0);
}
VectorJ j_new = j - (M_PI/32)*grad/grad.norm();
// arm->set_joint_values(j_new);
beams = cam->get_beams(j_new, pcl);
rave_utils::clear_plots();
rave_utils::plot_transform(env, rave_utils::eigen_to_rave(arm->get_pose(j_new)));
cam->plot_fov(beams);
rave_utils::plot_point(env, p, Vector3d(1,0,0));
j = j_new;
std::cin.ignore();
}
tc.print_all_elapsed();
std::cout << "Displaying current fov beams. Press enter to exit\n";
std::cin.ignore();
}
