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

void takeAccelSample(ProductManager& pm, int duration_us, const char* fileName)
{
	BARRETT_UNITS_FIXED_SIZE_TYPEDEFS;

	if ( !pm.foundForceTorqueSensor() ) {
		throw std::runtime_error("Couldn't find an FTS!");
	}
	pm.startExecutionManager();

	char tmpFile[] = "/tmp/btXXXXXX";
	if (mkstemp(tmpFile) == -1) {
		throw std::runtime_error("Couldn't create temporary file!");
	}


	systems::Ramp time(pm.getExecutionManager(), 1.0);
	FTSAccel ftsa(pm.getForceTorqueSensor());

	systems::TupleGrouper<double, ca_type> tg;
	connect(time.output, tg.getInput<0>());
	connect(ftsa.output, tg.getInput<1>());

	typedef boost::tuple<double, ca_type> tuple_type;
	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);

	time.start();
	connect(tg.output, logger.input);
	printf("Logging started.\n");


	usleep(duration_us);


	logger.closeLog();
	printf("Logging stopped.\n");

	log::Reader<tuple_type> lr(tmpFile);
	lr.exportCSV(fileName);
	printf("Output written to %s.\n", fileName);
	std::remove(tmpFile);
}

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

	systems::Ramp time(pm.getExecutionManager());

	systems::TupleGrouper<double, jp_type> jpLogTg;

	// Record at 1/10th of the loop rate
	systems::PeriodicDataLogger<jp_sample_type> jpLogger(pm.getExecutionManager(),
			new barrett::log::RealTimeWriter<jp_sample_type>(tmpFile, 10*T_s), 10);


	printf("Press [Enter] to start teaching.\n");
	waitForEnter();
	{
		// Make sure the Systems are connected on the same execution cycle
		// that the time is started. Otherwise we might record a bunch of
		// samples all having t=0; this is bad because the Spline requires time
		// to be monotonic.
		BARRETT_SCOPED_LOCK(pm.getExecutionManager()->getMutex());

		connect(time.output, jpLogTg.template getInput<0>());
		connect(wam.jpOutput, jpLogTg.template getInput<1>());
		connect(jpLogTg.output, jpLogger.input);
		time.start();
	}

	printf("Press [Enter] to stop teaching.\n");
	waitForEnter();
	jpLogger.closeLog();
	disconnect(jpLogger.input);


	// Build spline between recorded points
	log::Reader<jp_sample_type> lr(tmpFile);
	std::vector<jp_sample_type> vec;
	for (size_t i = 0; i < lr.numRecords(); ++i) {
		vec.push_back(lr.getRecord());
	}
	math::Spline<jp_type> spline(vec);

	printf("Press [Enter] to play back the recorded trajectory.\n");
	waitForEnter();

	// First, move to the starting position
	wam.moveTo(spline.eval(spline.initialS()));

	// Then play back the recorded motion
	time.stop();
	time.setOutput(spline.initialS());

	systems::Callback<double, jp_type> trajectory(boost::ref(spline));
	connect(time.output, trajectory.input);
	wam.trackReferenceSignal(trajectory.output);

	time.start();

	while (trajectory.input.getValue() < spline.finalS()) {
		usleep(100000);
	}


	printf("Press [Enter] to idle the WAM.\n");
	waitForEnter();
	wam.idle();


	std::remove(tmpFile);
	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, jv_type, ja_type, jt_type, jt_type> tuple_type;
	typedef systems::TupleGrouper<double, jp_type, jv_type, ja_type, jt_type,
			jt_type> tg_type;
	tg_type tg;

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

	double Frequency;
	double Amplitude;

	double omega1;
	double offset;

	if (argc == 2) {
		Frequency = 0.1;
		Amplitude = 0.525;
		offset = 0;
	}
	if (argc == 3) {
		Amplitude = 0.525;
		offset = 0;
	}
	if (argc == 4) {
		offset = 0;
	}

	else {
		Frequency = boost::lexical_cast<double>(argv[2]);
		Amplitude = boost::lexical_cast<double>(argv[3]);
		offset = boost::lexical_cast<double>(argv[4]);
	}





	const double JT_AMPLITUDE = Amplitude;
	const double FREQUENCY = Frequency;
	const double OFFSET = offset;

	omega1 = 120;

	wam.gravityCompensate();

	const double TRANSITION_DURATION = 0.5; // seconds

	jp_type startpos(0.0);
	startpos[3] = +3.14;
	startpos[1] = offset;

	systems::Ramp time(pm.getExecutionManager(), 1.0);
	printf("Press [Enter] to turn on torque control to go to zero position");
	wam.moveTo(startpos);

	printf("Press [Enter] to turn on torque control to joint 2.");
	waitForEnter();

	// Joint acc calculator
	systems::FirstOrderFilter<jv_type> filter;
	wam.jvFilter.setLowPass(jv_type(omega1));
	filter.setHighPass(jp_type(omega1), jp_type(omega1));
	pm.getExecutionManager()->startManaging(filter);
	systems::Gain<jv_type, double, ja_type> changeUnits1(1.0);
	systems::connect(wam.jvOutput, filter.input);
	systems::connect(filter.output, changeUnits1.input);

	//
	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);
	//

	J2control<DOF> jtc(startpos, JT_AMPLITUDE, FREQUENCY, OFFSET);
	systems::connect(tg.output, logger.input);
	systems::connect(time.output, jtc.input);
	systems::connect(time.output, tg.template getInput<0>());
	systems::connect(wam.jpOutput, tg.template getInput<1>());
	systems::connect(wam.jvOutput, tg.template getInput<2>());
	systems::connect(changeUnits1.output, tg.template getInput<3>());
	systems::connect(wam.supervisoryController.output,
			tg.template getInput<4>());
	systems::connect(wam.gravity.output, tg.template getInput<5>());

	wam.trackReferenceSignal(jtc.output);
	time.smoothStart(TRANSITION_DURATION);

	printf("Press [Enter] to stop.");
	waitForEnter();
	logger.closeLog();
//.........这里部分代码省略.........

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

    cp_type cartSetPoint;
    wam.gravityCompensate();

    /////////////////////////////////////////////////////////////////////////////////////
    // Add the following to our system to make our Cartesian controller much more robust.
    /////////////////////////////////////////////////////////////////////////////////////

    // Set up singularity avoidance springs
    jp_type joint_center(0.0); // Initialize our joint centers to 0.0 on all joints
    std::vector<double> spring_constants(DOF); // create spring constants
    std::vector<double> damping_constants(DOF); // create damping constants
    joint_center[0] = 0.0;
    joint_center[1] = 0.0;
    joint_center[2] = 0.0;
    joint_center[3] = 1.1; // J4 Joint Range Center at 1.1 Radians
    spring_constants[0] = 15.0;
    spring_constants[1] = 5.0;
    spring_constants[2] = 5.0;
    spring_constants[3] = 5.0;
    damping_constants[0] = 10.0;
    damping_constants[1] = 20.0;
    damping_constants[2] = 10.0;
    damping_constants[3] = 6.0;

    if (DOF == 7)
    {
      joint_center[4] = -1.76; // J5 Joint Range Center at -1.76 Radians
      joint_center[5] = 0.0;
      joint_center[6] = 0.0;
      spring_constants[4] = 0.5;
      spring_constants[5] = 0.25;
      spring_constants[6] = 0.25;
      damping_constants[4] = 0.05;
      damping_constants[5] = 0.05;
      damping_constants[6] = 0.0;
    }

    printf("Press Enter to Turn on Haptic Singularity Avoidance.\n");
    detail::waitForEnter();

    //Initialization Move
    jp_type wam_init = wam.getHomePosition();
    wam_init[3] -= .35;
    wam.moveTo(wam_init); // Adjust the elbow, moving the end-effector out of the haptic boundary and hold position for haptic force initialization.

    // Change decrease our tool position controller gains slightly
    cp_type cp_kp, cp_kd;
    for (size_t i = 0; i < 3; i++)
    {
      cp_kp[i] = 1500;
      cp_kd[i] = 5.0;
    }
    wam.tpController.setKp(cp_kp);
    wam.tpController.setKd(cp_kd);

    //Torque Summer from our three systems
    systems::Summer<jt_type, 4> singJTSum(true);

    // Our singularity avoidance system
    SingularityAvoid<DOF> singularityAvoid(joint_center);
    systems::connect(wam.jpOutput, singularityAvoid.input);
    systems::connect(singularityAvoid.output, singJTSum.getInput(0));

    // Attach our joint stop springs
    JointStopSprings<DOF> jointStopSprings(joint_center, spring_constants);
    systems::connect(wam.jpOutput, jointStopSprings.input);
    systems::connect(jointStopSprings.output, singJTSum.getInput(1));

    // Joint velocity damper for kinematic solutions causing velocity faults
    JVDamper<DOF> jvDamper(damping_constants);
    systems::connect(wam.jvOutput, jvDamper.input);
    systems::connect(jvDamper.output, singJTSum.getInput(2));

    // Haptic collision avoidance boundary portion
    HapticCollisionAvoid<DOF> hapticCollisionAvoid(2000);
    systems::ToolForceToJointTorques<DOF> tf2jt;
    systems::connect(wam.kinematicsBase.kinOutput, tf2jt.kinInput);
    systems::connect(wam.toolPosition.output, hapticCollisionAvoid.input);
    systems::connect(hapticCollisionAvoid.output, tf2jt.input);
    systems::connect(tf2jt.output, singJTSum.getInput(3));

    systems::connect(singJTSum.output, wam.input);

    // New system watchdogs to monitor and stop trajectories if expecting a fault
    TorqueWatchdog<DOF> torqueWatchdog;
    VelocityWatchdog<DOF> velocityWatchdog;
    pm.getExecutionManager()->startManaging(torqueWatchdog);
    pm.getExecutionManager()->startManaging(velocityWatchdog);

    systems::connect(wam.jtSum.output, torqueWatchdog.input);
    systems::connect(wam.jvOutput, velocityWatchdog.input);

    /////////////////////////////////////////////////////////////////////////////////////
    // End of robust cartesian setup
    /////////////////////////////////////////////////////////////////////////////////////

//.........这里部分代码省略.........

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

	Eigen::MatrixXd Lamda;

	Eigen::MatrixXd Coeff;
	Eigen::VectorXd Delta;
	Eigen::VectorXd Amp;
	Eigen::VectorXd Freq;
	Eigen::VectorXd StartPos;
	Eigen::VectorXd dist_K_tmp;
	Eigen::VectorXd state_intg;
	Eigen::VectorXd A_tmp;


	Sam::initEigenMat<double>(Lamda, Sam::readFile<double>("lamda.txt"));
	Sam::initEigenMat<double>(Coeff, Sam::readFile<double>("coeff.txt"));
	Sam::initEigenVec<double>(Delta, Sam::readFile<double>("delta.txt"));
	Sam::initEigenVec<double>(Amp, Sam::readFile<double>("amp.txt"));
	Sam::initEigenVec<double>(Freq, Sam::readFile<double>("freq.txt"));
	Sam::initEigenVec<double>(StartPos, Sam::readFile<double>("start.txt"));
	Sam::initEigenVec<double>(dist_K_tmp, Sam::readFile<double>("dist_K.txt"));
	Sam::initEigenVec<double>(state_intg, Sam::readFile<double>("integrator_state.txt"));
	Sam::initEigenVec<double>(A_tmp, Sam::readFile<double>("A.txt"));



	std::cout << "Lamda is" << Lamda << "\n" << "Coeff is" << Coeff << "\n"
			<< "Delta is" << Delta << "\n" << std::endl;
	typedef boost::tuple<double, jp_type, jv_type, jp_type, jv_type, jt_type,
			jt_type, double, jp_type> tuple_type;
	typedef systems::TupleGrouper<double, jp_type, jv_type, jp_type, jv_type,
			jt_type, jt_type, double, jp_type> 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;
//	double amplitude1, omega1;
	const Eigen::Matrix4d lamda = Lamda;

	const Eigen::Matrix4d coeff = Coeff;
	const Eigen::Vector4d delta = Delta;
	jp_type startpos(0.0);

	startpos[0] = StartPos[0];
	startpos[1] = StartPos[1];
	startpos[2] = StartPos[2];
	startpos[3] = StartPos[3];

	const Eigen::Vector4d JP_AMPLITUDE = Amp;
	const Eigen::Vector4d OMEGA = Freq;
	Eigen::Vector4d tmp_velocity;
	tmp_velocity[0] = Amp[0]*Freq[0];
	tmp_velocity[1] = Amp[1]*Freq[1];
	tmp_velocity[2] = Amp[2]*Freq[2];
	tmp_velocity[3] = Amp[3]*Freq[3];
	bool status = true;
	const double dist_K =  dist_K_tmp[0];
	const int state = state_intg[0];
	const Eigen::Vector4d initial_velocity = tmp_velocity;

	const float A = A_tmp[0];

	J_ref<DOF> joint_ref(JP_AMPLITUDE, OMEGA, startpos);
	Slidingmode_Controller<DOF> slide(status, lamda, coeff, delta);
	Dynamics<DOF> nilu_dynamics;
	disturbance_observer<DOF> nilu_disturbance(dist_K,state, initial_velocity,A);
	Dummy<DOF> nilu_dummy;

	wam.gravityCompensate();
	printf("Press [Enter] to turn on torque control to go to zero position");
	waitForEnter();

	wam.moveTo(startpos);
	printf("Press [Enter] to turn on torque control to joint 2.");
	waitForEnter();
	printf("Error 1 \n");

	systems::Ramp time(pm.getExecutionManager(), 1.0);
	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 2 \n");

	systems::connect(tg.output, logger.input);
	systems::connect(time.output, joint_ref.timef);
	systems::connect(wam.jpOutput, slide.feedbackjpInput);
	systems::connect(wam.jvOutput, slide.feedbackjvInput);
	systems::connect(wam.jpOutput, nilu_dynamics.jpInputDynamics);
	systems::connect(wam.jvOutput, nilu_dynamics.jvInputDynamics);
	systems::connect(nilu_dynamics.MassMAtrixOutput, slide.M);
	systems::connect(nilu_dynamics.CVectorOutput, slide.C);
	systems::connect(joint_ref.referencejpTrack, slide.referencejpInput);
	systems::connect(joint_ref.referencejvTrack, slide.referencejvInput);
//.........这里部分代码省略.........

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


	Eigen::MatrixXd Coeff;
	Eigen::VectorXd Delta;
	Eigen::VectorXd Amp;
	Eigen::VectorXd Freq;
	Eigen::VectorXd StartPos;
	Eigen::MatrixXd A;
	Eigen::MatrixXd B;
	Eigen::VectorXd P;
	Eigen::VectorXd Q;

	Sam::initEigenMat<double>(A, Sam::readFile<double>("A.txt"));
	Sam::initEigenMat<double>(B, Sam::readFile<double>("B.txt"));
	Sam::initEigenVec<double>(P, Sam::readFile<double>("P.txt"));
	Sam::initEigenVec<double>(Q, Sam::readFile<double>("Q.txt"));

	Sam::initEigenMat<double>(Coeff, Sam::readFile<double>("coeff.txt"));
	Sam::initEigenVec<double>(Delta, Sam::readFile<double>("delta.txt"));
	Sam::initEigenVec<double>(Amp, Sam::readFile<double>("amp.txt"));
	Sam::initEigenVec<double>(Freq, Sam::readFile<double>("freq.txt"));
	Sam::initEigenVec<double>(StartPos, Sam::readFile<double>("start.txt"));

	typedef boost::tuple<double, jp_type, jv_type, jp_type, jv_type, jt_type> tuple_type;
	typedef systems::TupleGrouper<double, jp_type, jv_type, jp_type, jv_type,
			jt_type> 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;
	double amplitude1, omega1;
	const Eigen::Matrix4d coeff = Coeff;
	const Eigen::Vector4d delta = Delta;
	jp_type startpos(0.0);


	startpos[0] = StartPos[0];
	startpos[1] = StartPos[1];
	startpos[2] = StartPos[2];
	startpos[3] = StartPos[3];

	const Eigen::Vector4d JP_AMPLITUDE = Amp;
	const Eigen::Vector4d OMEGA = Freq;
	bool status = true;
	const Eigen::Matrix4d A1 = A;
	const Eigen::Matrix4d B1 = B;
	const float P1 = P[0];
	const float Q1 = Q[0];

	J_ref<DOF> joint_ref(JP_AMPLITUDE, OMEGA, startpos);
	SMC_higher_order<DOF> slide(status, coeff, delta , A1, B1,P1, Q1 );
	Dynamics<DOF> nilu_dynamics;

	wam.gravityCompensate();
	printf("Press [Enter] to turn on torque control to go to zero position");
	waitForEnter();

	wam.moveTo(startpos);
	printf("Press [Enter] to turn on torque control to joint 2.");
	waitForEnter();
	printf("Error 1 \n");

	systems::Ramp time(pm.getExecutionManager(), 1.0);
	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 2 \n");

	systems::connect(tg.output, logger.input);
	systems::connect(time.output, joint_ref.timef);
	systems::connect(wam.jpOutput, slide.feedbackjpInput);
	systems::connect(wam.jvOutput, slide.feedbackjvInput);
	systems::connect(wam.jpOutput, nilu_dynamics.jpInputDynamics);
	systems::connect(wam.jvOutput, nilu_dynamics.jvInputDynamics);
	systems::connect(nilu_dynamics.MassMAtrixOutput, slide.M);
	systems::connect(nilu_dynamics.CVectorOutput, slide.C);
	systems::connect(joint_ref.referencejpTrack, slide.referencejpInput);
	systems::connect(joint_ref.referencejvTrack, slide.referencejvInput);
	systems::connect(joint_ref.referencejaTrack, slide.referencejaInput);

	wam.trackReferenceSignal(slide.controlOutput);
	printf("Error 3 \n");

	systems::connect(time.output, tg.template getInput<0>());
	systems::connect(joint_ref.referencejpTrack, tg.template getInput<1>());
	systems::connect(joint_ref.referencejvTrack, tg.template getInput<2>());
	systems::connect(wam.jpOutput, tg.template getInput<3>());
	systems::connect(wam.jvOutput, tg.template getInput<4>());
	systems::connect(slide.controlOutput, tg.template getInput<5>());
	printf("Error 4 \n");

	time.smoothStart(TRANSITION_DURATION);
//.........这里部分代码省略.........

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


    // instantiate Systems
	NetworkHaptics nh(pm.getExecutionManager(), remoteHost);

	cp_type center;
	center << 0.4, -.3, 0.0;
	systems::HapticBall ball(center, 0.2);
	center << 0.35, 0.4, 0.0;
	math::Vector<3>::type size;
	size << 0.3, 0.3, 0.3;
	systems::HapticBox box(center, size);

	systems::Summer<cf_type> dirSum;
	systems::Summer<double> depthSum;
	systems::PIDController<double, double> comp;
	systems::Constant<double> zero(0.0);
	systems::TupleGrouper<cf_type, double> tg;
	systems::Callback<boost::tuple<cf_type, double>, cf_type> mult(scale);
	systems::ToolForceToJointTorques<DOF> tf2jt;

	jt_type jtLimits(35.0);
	systems::Callback<jt_type> jtSat(boost::bind(saturateJt<DOF>, _1, jtLimits));

	// configure Systems
	comp.setKp(kp);
	comp.setKd(kd);

	// connect Systems
	connect(wam.toolPosition.output, nh.input);

	connect(wam.toolPosition.output, ball.input);
	connect(ball.directionOutput, dirSum.getInput(0));
	connect(ball.depthOutput, depthSum.getInput(0));

	connect(wam.toolPosition.output, box.input);
	connect(box.directionOutput, dirSum.getInput(1));
	connect(box.depthOutput, depthSum.getInput(1));

	connect(wam.kinematicsBase.kinOutput, tf2jt.kinInput);
	connect(dirSum.output, tg.getInput<0>());

	connect(depthSum.output, comp.referenceInput);
	connect(zero.output, comp.feedbackInput);
	connect(comp.controlOutput, tg.getInput<1>());

	connect(tg.output, mult.input);
	connect(mult.output, tf2jt.input);
	connect(tf2jt.output, jtSat.input);


	// adjust velocity fault limit
	pm.getSafetyModule()->setVelocityLimit(1.5);

	while (true) {
		wam.gravityCompensate();
		connect(jtSat.output, wam.input);

		// block until the user Shift-idles
		pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);

		systems::disconnect(wam.input);
		wam.gravityCompensate(false);

		// block until the user Shift-activates
		pm.getSafetyModule()->waitForMode(SafetyModule::ACTIVE);
	}

	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");
//.........这里部分代码省略.........