C++ Arm::get_joint_values方法代码示例

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