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_fk() {
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();
arm->set_posture(Arm::Posture::mantis);
sleep(1);
VectorJ j = arm->get_joint_values();
Matrix4d actual_arm_pose = rave_utils::transform_from_to(brett->get_robot(), Matrix4d::Identity(), "r_gripper_tool_frame", "world");
Matrix4d fk_arm_pose = arm->get_pose(j);
std::cout << "actual_arm_pose:\n" << actual_arm_pose << "\n\n";
std::cout << "fk_arm_pose:\n" << fk_arm_pose << "\n\n";
Matrix4d actual_cam_pose = rave_utils::rave_to_eigen(cam->get_sensor()->GetTransform());
Matrix4d fk_cam_pose = cam->get_pose(j);
std::cout << "actual_cam_pose:\n" << actual_cam_pose << "\n\n";
std::cout << "fk_cam_pose:\n" << fk_cam_pose << "\n\n";
}
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);
}
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();
}
