C++ ProductManager::getHand方法代码示例

int main(int argc, char** argv)
{
  ros::init(argc, argv, "bhand_node");

  ProductManager pm;
  if (!pm.foundHand())
  {
    printf("ERROR: No Hand found on bus!\n");
    return 1;
  }

  Hand* hand = pm.getHand();

  BHandNode bhand_node(hand);

  bhand_node.init();

  ros::Rate pub_rate(PUBLISH_FREQ);

  while (bhand_node.n_.ok())
  {
    ros::spinOnce();
    bhand_node.publish();
    pub_rate.sleep();
  }

  hand->idle();

  return 0;
}

int wam_main(int argc, char** argv, ProductManager& pm,
		systems::Wam<DOF>& wam) {
	BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);

	std::string filename(argv[1]);

	printf("\nMoving to start configuration \n");

	jp_type jp(0.0);
	jp[1] = -1.967;
	jp[3] = 2.5;
	jp[5] = -0.5;
	wam.moveTo(jp);

	printf("Opening hands\n");

	// Open hands.
	barrett::Hand& hand = *pm.getHand();
	hand.initialize();

	// wam.idle();

	Teach<DOF> teach(wam, pm, filename);

	teach.init();

	printf("\nPress [Enter] to start teaching.\n");
	waitForEnter();
	teach.record();
	//boost::thread t(&Teach<DOF>::display, &teach);

	printf("Press [Enter] to stop teaching.\n");
	waitForEnter();
	teach.createSpline();

	// Move to start and close hands.
	wam.moveTo(jp);
	hand.close();
	hand.idle();

	wam.idle();

	pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);

	return 0;
}

int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
	BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
	typedef boost::tuple<double, jp_type> jp_sample_type;


	char tmpFile[] = "/tmp/btXXXXXX";
	if (mkstemp(tmpFile) == -1) {
		printf("ERROR: Couldn't create temporary file!\n");
		return 1;
	}

	const double T_s = pm.getExecutionManager()->getPeriod();


	//wam.gravityCompensate();
	boost::thread displayThread(displayEntryPoint, pm.getHand());



	std::remove(tmpFile);
	pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);

	return 0;
}

void AutoTension<DOF>::init(ProductManager& pm, std::vector<int> args) {
	wam.gravityCompensate(true); // Turning on Gravity Compenstation

	// Make sure all tangs are released correctly
	for (size_t m = 0; m < args.size(); m++) {
		if (args[m] == 6) // Motor 6 tensions using the single tang for 5 & 6
			puck[4]->setProperty(Puck::TENSION, false);
		else
			puck[args[m] - 1]->setProperty(Puck::TENSION, false);
	}

	wam.moveHome(); // Move the WAM to home position

	// Tell our EM to start managing
	pm.getExecutionManager()->startManaging(motorRamp); //starting ramp manager
	pm.getExecutionManager()->startManaging(watchdog);
	pm.getExecutionManager()->startManaging(exposedGravity);

	// Some DOF dependent items
	jpInitial.resize(DOF);
	jpStart.resize(DOF);
	jpSlack1.resize(DOF);
	jpSlack2.resize(DOF);

	// Initialize hand if present
	if (pm.foundHand()) {
		jp_type handBufJT = wam.getJointPositions();
		if (DOF == 4)
			handBufJT[3] -= 0.35; // Lift the elbow to give room for BHand HI
		else
			handBufJT[5] = jpStopLow[5]; // Set J6 on its positive stop
		wam.moveTo(handBufJT);
		hand = pm.getHand();
		hand->initialize();
		hand->close(Hand::GRASP);
	}

	// Joint 1 Tensioning will be added in and run simultaneously if present.
	jpInitial[0] = wam.getJointPositions();

	// Joint 2
	jpInitial[1] = wam.getJointPositions();
	jpStart[1] = jpInitial[1];
	jpStart[1][1] = jpStopHigh[1] - tangBuffer[1];
	jpStart[1][2] = jpStopLow[2] + tangBuffer[2];
	jpStart[1][3] = jpStopHigh[3] - stopBuffer[3];
	jpSlack1[1] = jpStart[1];
	jpSlack1[1][1] = jpStopHigh[1] - stopBuffer[1]; // This is so we dont drive into and wear the joint stops
	jpSlack1[1][2] = jpStopLow[2] + stopBuffer[2];
	jpSlack2[1] = jpSlack1[1];
	jpSlack2[1][1] = jpStopLow[1] + stopBuffer[1];
	jpSlack2[1][2] = jpStopHigh[2] - stopBuffer[2];
	jpSlack2[1][3] = jpStopLow[3] + stopBuffer[3];

	// Joint 3
	jpInitial[2] = wam.getJointPositions();
	jpInitial[2][1] = 0.0;
	jpStart[2] = jpInitial[2];
	jpStart[2][1] = jpStopLow[1] + tangBuffer[1];
	jpStart[2][2] = jpStopLow[2] + tangBuffer[2];
	jpStart[2][3] = jpStopLow[3] + stopBuffer[3];
	jpSlack1[2] = jpStart[2];
	jpSlack1[2][1] = jpStopLow[1] + stopBuffer[1];
	jpSlack1[2][2] = jpStopLow[2] + stopBuffer[1];
	jpSlack2[2] = jpSlack1[2];
	jpSlack2[2][1] = jpStopHigh[1] - stopBuffer[1];
	jpSlack2[2][2] = jpStopHigh[2] - stopBuffer[2];
	jpSlack2[2][3] = jpStopHigh[3] - stopBuffer[3];

	// Joint 4
	jpInitial[3] = wam.getJointPositions();
	jpStart[3] = jpInitial[3];
	jpStart[3][1] = 0.0;
	jpStart[3][3] = jpStopLow[3] + tangBuffer[3];
	jpSlack1[3] = jpStart[3];
	jpSlack1[3][3] = jpStopLow[3] + stopBuffer[3];
	jpSlack2[3] = jpSlack1[3];
	jpSlack2[3][3] = jpStopHigh[3] - stopBuffer[3];

	if (DOF == 7) {
		// Joint 5
		jpInitial[4] = wam.getJointPositions();
		jpStart[4] = jpInitial[4];
		jpStart[4][3] = M_PI / 2.0;
		jpStart[4][4] = jpStopHigh[4] - tangBuffer[4];
		jpStart[4][5] = jpStopLow[5] + tangBuffer[5];
		jpSlack1[4] = jpStart[4];
		jpSlack1[4][4] = jpStopHigh[4] - stopBuffer[4];
		jpSlack1[4][5] = jpStopLow[5] + stopBuffer[5];
		jpSlack2[4] = jpSlack1[4];
		jpSlack2[4][4] = jpStopLow[4] + stopBuffer[4];
		jpSlack2[4][5] = jpStopHigh[5] - stopBuffer[5];

		// Joint 6
		jpInitial[5] = wam.getJointPositions();
		jpStart[5] = jpInitial[5];
		jpStart[5][3] = M_PI / 2.0;
		jpStart[5][4] = jpStopHigh[4] - tangBuffer[4];
		jpStart[5][5] = jpStopHigh[5] - tangBuffer[5];
		jpSlack1[5] = jpStart[5];
//.........这里部分代码省略.........

int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {

	BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);

	// These vectors are fixed sized, stack allocated, and zero-initialized.
	jp_type jp;  // jp is a DOFx1 column vector of joint positions
	cp_type cp;  // cp is a 3x1 vector representing a Cartesian position

	wam.gravityCompensate();
	printMenu();
	Hand* hand = pm.getHand();

	std::string line;
	bool going = true;
	while (going) {
		printf(">>> ");
		std::getline(std::cin, line);

		switch (line[0]) {

		case 'p':
			moveToStr(wam, &cp, "tool position", line.substr(1));
			break;

		case 'j':
					moveToStr(wam, &jp, "joint positions", line.substr(1));
					break;

		case 'h':
			std::cout << "Moving to home position: "
					<< wam.getHomePosition() << std::endl;
			wam.moveHome();
			break;

		case 'i':
			printf("WAM idled.\n");
			wam.idle();
			break;

		case 'g':
			hand->trapezoidalMove(Hand::jp_type(M_PI/3.0), Hand::SPREAD);
			break;


		case 'o':
			hand->initialize();
//			hand->close(Hand::GRASP);
//			hand->open(Hand::GRASP);
//		   	hand->close(Hand::SPREAD);
//		   	hand->trapezoidalMove(Hand::jp_type(M_PI/2.0),1, Hand::GRASP);
//		   	hand->trapezoidalMove(Hand::jp_type(M_PI/2.0), Hand::GRASP);
		   	break;

		case 'a':
//			hand->initialize();
			hand->close(Hand::SPREAD);
//			hand->close(Hand::GRASP);
			break;

		case 'c':
//			hand->initialize();
			hand->close(Hand::GRASP);
			break;

//		case 'f':
//			while (std::getline(file_jp,line_jp))
//			{
//			moveToStr(wam, &jp, "joint positions", line_jp);
//			waitForEnter();
//			}
//			break;

		case 'd':
//					hand->initialize();
					hand->open(Hand::SPREAD);
					break;

		case 'e':
//							hand->initialize();
							hand->open(Hand::GRASP);
							break;

		case 'b':
			hand->trapezoidalMove(Hand::jp_type(M_PI/2.0), Hand::GRASP);
			break;

		case 't':
			hand->trapezoidalMove(Hand::jp_type((M_PI)/6.0), Hand::GRASP);
//			std::cout << "sending back a message..." << std::endl;
			break;

		case 'm':
//			temp_str = line.substr(2);
			iss2.clear();
			iss2.str("");
			iss2.str(line.substr(2)) ;
		    iss2 >> angle_degree;
//			std::cin >> angle_degree;
			angle_radian = angle_degree*(M_PI/180);
			hand->trapezoidalMove(Hand::jp_type(angle_radian), Hand::GRASP);
//.........这里部分代码省略.........

int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
    BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
    typedef Hand::jp_type hjp_t;
    const char* path = pm.getWamDefaultConfigPath();
    std::cout<<"got configuration "<<std::endl;
    size_t i=0;
    while(path[i]!='\0'){
      std::cout<<path[i];
      i++;
    }
    jp_type zero(0.0);

    double O = 0.0;
    double C = 2.4;
    double SC = M_PI;
    hjp_t open(O);
    hjp_t closed(C);
    closed[3] = SC;

    jp_type PickPos, placePos;
    PickPos<<-0.002,  1.200,  0.002,  1.536, -0.054, -1.262, -1.517;
    placePos<<0.545,  1.199,  0.532,  1.481, -0.478, -0.917, -1.879;
    double OR = -0.75;
    double CR = 0.75;
    Hand::jv_type opening(OR);
    Hand::jv_type closing(CR);
    wam.gravityCompensate();
    wam.moveTo(zero);

    if ( !pm.foundHand() ) {
        printf("ERROR: No Hand found on bus!\n");
        return 1;
    }
    Hand& hand = *pm.getHand();
    std::cout<<"[Enter] to initialize Hand"<<std::endl;
    detail::waitForEnter();
    hand.initialize();


    std::cout<<"operate on Hand"<<std::endl;
    detail::waitForEnter();
    {
            assertPosition(hand, open);
            std::cout<<"[Enter] to close Hand"<<std::endl;
            detail::waitForEnter();
            hand.close();
            assertPosition(hand, closed);
            std::cout<<"[Enter] to open Hand"<<std::endl;
            detail::waitForEnter();
            hand.open();
            assertPosition(hand, open);
            std::cout<<"[Enter] to close spread Hand"<<std::endl;
            detail::waitForEnter();
            hand.close(Hand::SPREAD);
            assertPosition(hand, hjp_t(O,O,O,SC));
            std::cout<<"[Enter] to close grasp Hand"<<std::endl;
            detail::waitForEnter();
            hand.close(Hand::GRASP);
            assertPosition(hand, closed);
            std::cout<<"[Enter] to open spread Hand"<<std::endl;
            detail::waitForEnter();
            hand.open(Hand::GRASP, false);
            btsleep(0.5);
            assertPosition(hand, hjp_t(1.6,1.6,1.6,SC));
            std::cout<<"[Enter] to open Hand"<<std::endl;
            detail::waitForEnter();
            hand.open();
            assertPosition(hand, open);
            wam.moveTo(PickPos );
            btsleep(0.5);
            assertPosition(hand, open);
            std::cout<<"[Enter] to close Hand"<<std::endl;
            detail::waitForEnter();
            hand.close();
        }
    std::cout<<"[Enter] to move home"<<std::endl;
    detail::waitForEnter();

    wam.moveHome();


    std::cout<<"configuration should be printed"<<std::endl;
    pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
    return 0;
}

int wam_main(int argc, char** argv, ProductManager& pm,
		systems::Wam<DOF>& wam) {
	BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);

	typedef Hand::jp_type hjp_t;

	typedef boost::tuple<double, hjp_t> tuple_type;
	typedef systems::TupleGrouper<double, hjp_t> tg_type;
	tg_type tg;
	char tmpFile[] = "btXXXXXX";
	if (mkstemp(tmpFile) == -1) {
		printf("ERROR: Couldn't create temporary file!\n");
		return 1;
	}
	const double TRANSITION_DURATION = 0.5;

	wam.gravityCompensate();

//	printf("Press [Enter] to  go to given position");
//	waitForEnter();
//	jp_type startpos(0.0); // TODO : change here
//	wam.moveTo(startpos);

// Is an FTS attached?
	ForceTorqueSensor* fts = NULL;
	if (pm.foundForceTorqueSensor()) {
		fts = pm.getForceTorqueSensor();
		fts->tare();
	}

	// Is a Hand attached?
	Hand* hand = NULL;
	std::vector<TactilePuck*> tps;
	if (pm.foundHand()) {
		hand = pm.getHand();

		printf(
				">>> Press [Enter] to initialize Hand. (Make sure it has room!)");
		waitForEnter();
		hand->initialize();
		hand->trapezoidalMove(Hand::jp_type((1.0 / 3.0) * M_PI), Hand::SPREAD);
		hand->trapezoidalMove(Hand::jp_type((1.0 / 3.0) * M_PI), Hand::GRASP);
		hand->trapezoidalMove(Hand::jp_type((0.0) * M_PI), Hand::GRASP);

	}
	printf("Error 1 \n");
	tps = hand->getTactilePucks();
	// TODO write some error statement
	bool Release_Mode = 0;
	double delta_step = 0.002; //pm.getExecutionManager()->getPeriod();
	std::string input_angle_string;
	input_angle_string = argv[1];

	double input_angle = atoi(input_angle_string.c_str());
	double spread_angle = (input_angle / 180.0) * M_PI;
//	std::string threshold_impulse_str;
//	std::cout << "Enter the inpulse threshold limit: ";
//	std::cin >> threshold_impulse_str;
//	std::cout << "\n" << std::endl;
	std::string threshold_impulse_string;
	threshold_impulse_string = argv[2];

	double threshold_impulse = atof(threshold_impulse_string.c_str());

	printf("Press [Enter] to turn on the system");
	waitForEnter();
	printf("Error 2 \n");

	systems::Ramp time(pm.getExecutionManager(), 1.0);
//	const size_t PERIOD_MULTIPLIER = 1;
	const size_t PERIOD_MULTIPLIER = 1;
	systems::PeriodicDataLogger<tuple_type> logger(pm.getExecutionManager(),
			new log::RealTimeWriter<tuple_type>(tmpFile,
					PERIOD_MULTIPLIER * pm.getExecutionManager()->getPeriod()),
			PERIOD_MULTIPLIER);
	printf("Error 3 \n");

//	Hand_forcetorque_sense<DOF> hand_ft(hand, fts);
	Hand_tactile_sense hand_tact(hand, tps);
	main_processor<DOF> brain(hand, delta_step, spread_angle, threshold_impulse,
			Release_Mode);
	Hand_full_move hand_move(hand);

	systems::connect(tg.output, logger.input);
	systems::connect(time.output, brain.Current_time);
//	systems::connect(hand_ft.Force_hand, brain.Force_hand);
//	systems::connect(hand_ft.Torque_hand, brain.Torque_hand);
//	systems::connect(hand_ft.Acceleration_hand, brain.Acceleration_hand);
	systems::connect(hand_tact.Finger_Tactile_1, brain.Finger_Tactile_1);
	systems::connect(hand_tact.Finger_Tactile_2, brain.Finger_Tactile_2);
	systems::connect(hand_tact.Finger_Tactile_3, brain.Finger_Tactile_3);
	systems::connect(hand_tact.Finger_Tactile_4, brain.Finger_Tactile_4);
	systems::connect(brain.Desired_Finger_Angles, hand_move.Finger_Angles);

	systems::connect(time.output, tg.template getInput<0>());
	systems::connect(brain.Desired_Finger_Angles, tg.template getInput<1>());
//	systems::connect(hand_ft.Force_hand_cf, tg.template getInput<1>());


	printf("Error 4 \n");
//.........这里部分代码省略.........

int main() {
	typedef Hand::jp_type hjp_t;

	ProductManager pm;
	if ( !pm.foundHand() ) {
		printf("ERROR: No Hand found on bus!\n");
		return 1;
	}
	Hand& hand = *pm.getHand();
	hand.initialize();


	double O = 0.0;
	double C = 2.4;
	double SC = M_PI;
	hjp_t open(O);
	hjp_t closed(C);
	closed[3] = SC;

	double OR = -0.75;
	double CR = 0.75;
	Hand::jv_type opening(OR);
	Hand::jv_type closing(CR);



	{
		assertPosition(hand, open);
		hand.close();
		assertPosition(hand, closed);
		hand.open();
		assertPosition(hand, open);
		hand.close(Hand::SPREAD);
		assertPosition(hand, hjp_t(O,O,O,SC));
		hand.close(Hand::GRASP);
		assertPosition(hand, closed);
		hand.open(Hand::GRASP, false);
		btsleep(0.5);
		assertPosition(hand, hjp_t(1.6,1.6,1.6,SC));
		hand.open();
		assertPosition(hand, open);
	}

	{
		// Original interface preserved? Should move all 4 motors.
		hand.trapezoidalMove(closed);
		assertPosition(hand, closed);
		hand.trapezoidalMove(open, false);
		hand.waitUntilDoneMoving();
		assertPosition(hand, open);

		// New interface
		hand.trapezoidalMove(closed, Hand::SPREAD);
		assertPosition(hand, hjp_t(O,O,O,SC));
		hand.trapezoidalMove(closed, Hand::F1);
		assertPosition(hand, hjp_t(C,O,O,SC));
		hand.trapezoidalMove(closed, Hand::F2);
		assertPosition(hand, hjp_t(C,C,O,SC));
		hand.trapezoidalMove(closed, Hand::F3);
		assertPosition(hand, closed);
		hand.trapezoidalMove(open, Hand::GRASP);
		assertPosition(hand, hjp_t(O,O,O,SC));
		hand.trapezoidalMove(open, Hand::SPREAD);
		assertPosition(hand, open);
		hand.trapezoidalMove(closed, Hand::F3 | Hand::SPREAD);
		assertPosition(hand, hjp_t(O,O,C,SC));
		hand.trapezoidalMove(open, Hand::WHOLE_HAND);
		assertPosition(hand, open);
	}

	{
		double t = 0.0;

		// Original interface preserved? Should move all 4 motors.
		hand.velocityMove(closing);
		btsleep(1);
		t = 1.0;
		assertPosition(hand, hjp_t(CR*t), 0.2);

		// New interface
		hand.velocityMove(opening, Hand::GRASP);
		btsleep(1);
		t = 2.0;
		assertPosition(hand, hjp_t(O,O,O,CR*t), 0.4);
		hand.velocityMove(opening, Hand::WHOLE_HAND);
		hand.waitUntilDoneMoving();
		assertPosition(hand, open);
	}

	{
		double t = 0.0;

		// Original interface preserved? Should move all 4 motors.
		hand.velocityMove(closing);
		btsleep(1);
		t = 1.0;
		assertPosition(hand, hjp_t(CR*t), 0.2);

		// SPREAD should continue its velocity move
		hand.trapezoidalMove(open, Hand::F1);  // Only blocks for F1
//.........这里部分代码省略.........