本文整理汇总了C++中Arm类的典型用法代码示例。如果您正苦于以下问题：C++ Arm类的具体用法？C++ Arm怎么用？C++ Arm使用的例子？那么, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了Arm类的15个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: test_particle_update
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);
}示例2: GamepadArmControl
void GamepadArmControl(Gamepad *gp)
{
// arm shoulder control
if ( gp->GetButton(Gamepad::kBottomButton) )
_arm->lowerShoulder();
else if ( gp->GetButton(Gamepad::kTopButton) )
_arm->raiseShoulder();
// claw control w/top right trigger
if ( gp->GetButton(Gamepad::kRightTopTrigger) )
_claw->open();
else
_claw->close();
double stick = gp->GetLeftY(); // FIXME: is the minus sign necessary????
SmartDashboard::Log(stick, "Elbow Control Stick");
double elbowPower = stick * -ELBOW_MAX_POWER;
_arm->_elbowController->Set(elbowPower);
}示例3: test_pr2_system
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);
}示例4: 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";
}示例5: update2
void update2(double value) {
//goal = Point3f(cos(angle*PI / 180.0f), sin(angle*PI / 180.0f), (sin(zangle*PI / 180.0f)));
//Point3f goal(cos(angle*PI/180.0f), sin(angle*PI/180.0f), 0);
//goal.normalize();
//goal *= 5;
//goal += Vector3f(-3, 0, -1);
//goal = Vector3f(0, 0, 7);
//mainArm.solve(goal, 100);
//Point3f secGoal = Point3f(goal[0], goal[1], -goal[2]);
//secArm.solve(secGoal, 100);
//display();
for (int i = 0; i < 50; i++) {
if (i>20) {
goal = Point3f(3, 0, 1);
//goal += Vector3f(20, 4, 4);
mainArm.solve(goal, 100);
display();
}
}
for (int i = 0; i < 50;i++){
if(i>25){
goal = Vector3f(value+3, 0, 3);
mainArm.solve(goal, 100);
display();
}
}
}示例6: test_figtree
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);
}示例7: Base
/* ************************************************************************** */
Pose2MobileArm::Pose2MobileArm(const Arm& arm, const gtsam::Pose3& base_T_arm) :
Base(arm.dof() + 3, arm.dof() + 1), base_T_arm_(base_T_arm), arm_(arm) {
// check arm base pose, warn if non-zero
if (!arm_.base_pose().equals(Pose3::identity(), 1e-6))
cout << "[Pose2MobileArm] WARNING: Arm has non-zero base pose, this base pose will be override. "
"Set the base_T_arm in Pose2MobileArm." << endl;
}示例8: test_move
/* Move arm to various places */
void test_move(Arm &arm) {
arm.move_to( iPair(4,2) );
arm.move_to( iPair(3,0) );
arm.move_to( iPair(2,2) );
arm.move_to( iPair(0,0) );
}示例9: Base
/* ************************************************************************** */
Pose2MobileVetLinArm::Pose2MobileVetLinArm(const Arm& arm, const gtsam::Pose3& base_T_torso,
const gtsam::Pose3& torso_T_arm, bool reverse_linact) :
Base(arm.dof() + 4, arm.dof() + 2), base_T_torso_(base_T_torso), torso_T_arm_(torso_T_arm),
reverse_linact_(reverse_linact), arm_(arm) {
// check arm base pose, warn if non-zero
if (!arm_.base_pose().equals(Pose3::identity(), 1e-6))
cout << "[Pose2MobileArm] WARNING: Arm has non-zero base pose, this base pose will be override. "
"Set the base_T_torso and torso_T_arm in Pose2MobileArm." << endl;
}示例10: CMN_UNUSED
bool mtsIntuitiveResearchKitConsole::AddArm(mtsComponent * genericArm, const mtsIntuitiveResearchKitConsole::Arm::ArmType CMN_UNUSED(armType))
{
// create new required interfaces to communicate with the components we created
Arm * newArm = new Arm(genericArm->GetName(), "");
if (SetupAndConnectInterfaces(newArm)) {
mArms.push_back(newArm);
return true;
}
CMN_LOG_CLASS_INIT_ERROR << GetName() << ": AddArm, unable to add new arm. Are you adding two arms with the same name? "
<< newArm->Name() << std::endl;
delete newArm;
return false;
}示例11: test_board_move
void test_board_move(Arm &arm) {
Move move(iPair(4,1), iPair(4, 2));
/* Move arm according to move */
arm.execute_move(move);
/* Move arm to 0,0) */
arm.move_to(iPair(0,0));
/* move = Move(iPair(5, 0), iPair(4,0)); */
/* arm.execute_move(move); */
/* arm.move_to( iPair(3,0) ); */
/* arm.move_to(iPair(0,0) ); */
}示例12: CMN_UNUSED
bool mtsIntuitiveResearchKitConsole::AddArm(mtsComponent * genericArm, const mtsIntuitiveResearchKitConsole::Arm::ArmType CMN_UNUSED(armType))
{
// create new required interfaces to communicate with the components we created
Arm * newArm = new Arm(genericArm->GetName(), "");
if (AddArmInterfaces(newArm)) {
ArmList::iterator armIterator = mArms.find(newArm->mName);
if (armIterator != mArms.end()) {
mArms[newArm->mName] = newArm;
return true;
}
}
CMN_LOG_CLASS_INIT_ERROR << GetName() << ": AddArm, unable to add new arm. Are you adding two arms with the same name? "
<< newArm->Name() << std::endl;
delete newArm;
return false;
}示例13: update
void update() {
angle += 360.0 / 60.0;
zangle += 360.0f / 100.0f;
//goal = Point3f(cos(angle*PI / 180.0f), sin(angle*PI / 180.0f), (sin(zangle*PI / 180.0f)));
//Point3f goal(cos(angle*PI/180.0f), sin(angle*PI/180.0f), 0);
//goal.normalize();
//goal *= 5;
//goal += Vector3f(-3, 0, -1);
//goal = Vector3f(0, 0, 7);
//mainArm.solve(goal, 100);
//Point3f secGoal = Point3f(goal[0], goal[1], -goal[2]);
//secArm.solve(secGoal, 100);
//display();
goal = Vector3f(20, 4, 4);
mainArm.solve(goal, 100);
display();
}示例14: main
int main() {
cout << "Begin" << endl;
//test_sensors();
/* Arm back to 0,0 position */
Arm arm;
arm.reset();
/* Move arm */
test_board_move(arm);
//test_move(arm);
/* Arm back to 0,0 position */
arm.reset();
cout << "Done." << endl;
return 0;
}示例15: main
int main(int argc, char *argv[])
{
signal(SIGINT, stopsignal);
// Connect to the arm
start_arm();
sleep(1);
vec values = arm.sense();
arm.set_pose(values(0), values(1), values(2), values(3), values(4), values(5));
sense = values;
values += vec({ 90, 90, 0, 90, 90, 0 });
iValue[0] = (values(0));
iValue[1] = (values(1));
iValue[2] = (values(2));
iValue[3] = (values(3));
iValue[4] = (values(4));
iValue[5] = (values(5));
// Run loop
thread ui_thread(run_ui);
ui_thread.join(); // wait until the ui is finished
for (int i = 0; i < 100; i++)
{
arm.set_pose(0, 0, 0, 0, 0, 0, false);
}
stop_arm();
return 0;
}