C++ (Cpp) Arm::get_pose Examples

Example #1

File: old-test-pr2-system.cpp Project: gkahn13/bsp

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";
}

Example #2

File: old-test-pr2-system.cpp Project: gkahn13/bsp

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