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_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();
}
