本文整理汇总了C++中Arm::get_pose方法的典型用法代码示例。如果您正苦于以下问题：C++ Arm::get_pose方法的具体用法？C++ Arm::get_pose怎么用？C++ Arm::get_pose使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Arm的用法示例。
在下文中一共展示了Arm::get_pose方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: test_fk
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";
}示例2: test_camera
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();
}