int main(int argc, char** argv) { ros::init(argc, argv, "Teleop"); TeleOpAction teleop(ros::this_node::getName()); ros::spin(); return 0; }
task main() { while(true) { teleop(); } }
task usercontrol() { // User control code here, inside the loop while (true) { // This is the main execution loop for the user control program. Each time through the loop // your program should update motor + servo values based on feedback from the joysticks. // ..................................................................................... // Insert user code here. This is where you use the joystick values to update your motors, etc. // ..................................................................................... teleop(); //UserControlCodePlaceholderForTesting(); // Remove this function call once you have "real" code. } }
int main(int argc, char** argv) { // Initialize some global data Aria::init(); // If you want ArLog to print "Verbose" level messages uncomment this: //ArLog::init(ArLog::StdOut, ArLog::Verbose); // This object parses program options from the command line ArArgumentParser parser(&argc, argv); // Load some default values for command line arguments from /etc/Aria.args // (Linux) or the ARIAARGS environment variable. parser.loadDefaultArguments(); // Central object that is an interface to the robot and its integrated // devices, and which manages control of the robot by the rest of the program. ArRobot robot; // Object that connects to the robot or simulator using program options ArRobotConnector robotConnector(&parser, &robot); // If the robot has an Analog Gyro, this object will activate it, and // if the robot does not automatically use the gyro to correct heading, // this object reads data from it and corrects the pose in ArRobot ArAnalogGyro gyro(&robot); // Connect to the robot, get some initial data from it such as type and name, // and then load parameter files for this robot. if (!robotConnector.connectRobot()) { // Error connecting: // if the user gave the -help argumentp, then just print out what happened, // and continue so options can be displayed later. if (!parser.checkHelpAndWarnUnparsed()) { ArLog::log(ArLog::Terse, "Could not connect to robot, will not have parameter file so options displayed later may not include everything"); } // otherwise abort else { ArLog::log(ArLog::Terse, "Error, could not connect to robot."); Aria::logOptions(); Aria::exit(1); } } // Connector for laser rangefinders ArLaserConnector laserConnector(&parser, &robot, &robotConnector); // Connector for compasses ArCompassConnector compassConnector(&parser); // Parse the command line options. Fail and print the help message if the parsing fails // or if the help was requested with the -help option if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); exit(1); } // Used to access and process sonar range data ArSonarDevice sonarDev; // Used to perform actions when keyboard keys are pressed ArKeyHandler keyHandler; Aria::setKeyHandler(&keyHandler); // ArRobot contains an exit action for the Escape key. It also // stores a pointer to the keyhandler so that other parts of the program can // use the same keyhandler. robot.attachKeyHandler(&keyHandler); printf("You may press escape to exit\n"); // Attach sonarDev to the robot so it gets data from it. robot.addRangeDevice(&sonarDev); // Start the robot task loop running in a new background thread. The 'true' argument means if it loses // connection the task loop stops and the thread exits. robot.runAsync(true); // Connect to the laser(s) if lasers were configured in this robot's parameter // file or on the command line, and run laser processing thread if applicable // for that laser class. (For the purposes of this demo, add all // possible lasers to ArRobot's list rather than just the ones that were // specified with the connectLaser option (so when you enter laser mode, you // can then interactively choose which laser to use from the list which will // show both connected and unconnected lasers.) if (!laserConnector.connectLasers(false, false, true)) { printf("Could not connect to lasers... exiting\n"); Aria::exit(2); } // Create and connect to the compass if the robot has one. ArTCM2 *compass = compassConnector.create(&robot); if(compass && !compass->blockingConnect()) { compass = NULL; } // Sleep for a second so some messages from the initial responses // from robots and cameras and such can catch up ArUtil::sleep(1000); // We need to lock the robot since we'll be setting up these modes // while the robot task loop thread is already running, and they // need to access some shared data in ArRobot. robot.lock(); // now add all the modes for this demo // these classes are defined in ArModes.cpp in ARIA's source code. ArModeLaser laser(&robot, "laser", 'l', 'L'); ArModeTeleop teleop(&robot, "teleop", 't', 'T'); ArModeUnguardedTeleop unguardedTeleop(&robot, "unguarded teleop", 'u', 'U'); ArModeWander wander(&robot, "wander", 'w', 'W'); ArModeGripper gripper(&robot, "gripper", 'g', 'G'); ArModeCamera camera(&robot, "camera", 'c', 'C'); ArModeSonar sonar(&robot, "sonar", 's', 'S'); ArModeBumps bumps(&robot, "bumps", 'b', 'B'); ArModePosition position(&robot, "position", 'p', 'P', &gyro); ArModeIO io(&robot, "io", 'i', 'I'); ArModeActs actsMode(&robot, "acts", 'a', 'A'); ArModeCommand command(&robot, "command", 'd', 'D'); ArModeTCM2 tcm2(&robot, "tcm2", 'm', 'M', compass); // activate the default mode teleop.activate(); // turn on the motors robot.comInt(ArCommands::ENABLE, 1); robot.unlock(); // Block execution of the main thread here and wait for the robot's task loop // thread to exit (e.g. by robot disconnecting, escape key pressed, or OS // signal) robot.waitForRunExit(); Aria::exit(0); }
/** * @function: main(int argc, char **argv) * @brief: Main function that loops reading the sensors and commanding * Hubo's arm joints based on the poses of the foots */ int main(int argc, char **argv) { // check if no arguments given, if not report usage if (argc < 2) { usage(std::cerr); return 1; } // command line argument variables bool left = false; // whether to set left arm angles bool right = false; // whether to set right arm angles bool print = false; // whether to print output or not bool send = true; // whether to send commands or not int leftSensorNumberDefault = 3; // default left foot sensor number int rightSensorNumberDefault = 4; // default right foot sensor number int leftSensorNumber = leftSensorNumberDefault; // left foot sensor number int rightSensorNumber = rightSensorNumberDefault; // right foot sensor number const char *teleopDeviceName = "liberty"; // name of teleop device // local variables LegVector lActualAngles, lLegAnglesNext, lLegAnglesCurrent; LegVector rActualAngles, rLegAnglesNext, rLegAnglesCurrent; Vector3d lFootOrigin, lSensorChange, lSensorOrigin, lSensorPos; Vector3d rFootOrigin, rSensorChange, rSensorOrigin, rSensorPos; Eigen::Matrix3d lRotInitial, rRotInitial, lSensorRot, rSensorRot; Eigen::Isometry3d lFootInitialPose, lFootPoseCurrent, lFootPoseDesired; Eigen::Isometry3d rFootInitialPose, rFootPoseCurrent, rFootPoseDesired; LegVector speeds; speeds << 0.75, 0.75, 0.75, 0.75, 0.75, 0.75; LegVector accels; accels << 0.40, 0.40, 0.40, 0.40, 0.40, 0.40; double initialFootHeight = 0.1; double dt, ptime; int counter=0, counterMax=50; bool updateRight; // command line long options const struct option long_options[] = { { "left", optional_argument, 0, 'l' }, { "right", optional_argument, 0, 'r' }, { "nosend", no_argument, 0, 'n' }, { "device", optional_argument, 0, 'd' }, { "verbose", no_argument, 0, 'V' }, { "help", no_argument, 0, 'H' }, { 0, 0, 0, 0 }, }; // command line short options const char* short_options = "l::r::nd::VH"; // command line option and option index number int opt, option_index; // loop through command line options and set values accordingly while ( (opt = getopt_long(argc, argv, short_options, long_options, &option_index)) != -1 ) { switch (opt) { case 'l': left = true; if(NULL != optarg) leftSensorNumber = getSensorNumber(optarg); break; case 'r': right = true; if(NULL != optarg) rightSensorNumber = getSensorNumber(optarg); break; case 'n': send = false; break; case 'd': if(NULL != optarg) teleopDeviceName = getDeviceName(optarg); break; case 'V': print = true; break; case 'H': usage(std::cout); exit(0); break; default: usage(std::cerr); exit(1); break; } } // check to see if there are any invalid arguments on command line if (optind < argc) { std::cerr << "Error: extra arguments on command line.\n\n"; usage(std::cerr); exit(1); } // make sure the sensor numbers are not the same for both feet if(leftSensorNumber == rightSensorNumber) { if(left == true && right == true) { std::cerr << "Error!\nSensor #'s are the same.\n" << "Default sensor #'s are \n\tLEFT: " << leftSensorNumberDefault << "\n\tRIGHT: " << rightSensorNumberDefault << ".\nPlease choose different sensor numbers.\n\n"; usage(std::cerr); exit(1); } } Hubo_Control hubo; // Create Hubo_Control object // Hubo_Control hubo("teleop-arms"); // Create Hubo_Control object and daemonize program Collision_Checker collisionChecker; // Create Collision_Checker object // Create Teleop object Teleop teleop(teleopDeviceName); // Create Teleop object if (left == true) // if using the left arm { teleop.getPose( lSensorOrigin, lRotInitial, leftSensorNumber, true ); // get initial sensor pose hubo.setLeftLegNomSpeeds( speeds ); // Set left arm nominal joint speeds hubo.setLeftLegNomAcc( accels ); // Set left arm nominal joint accelerations } if (right == true) // if using the right arm { if(left == true) updateRight = false; else updateRight = true; teleop.getPose( rSensorOrigin, rRotInitial, rightSensorNumber, updateRight ); // get initial sensor pose hubo.setRightLegNomSpeeds( speeds ); // Set right arm nominal joint speeds hubo.setRightLegNomAcc( accels ); // Set right arm nomimal joint accelerations } if(send == true) // if user wants to send commands hubo.sendControls(); // send commands to the control daemon if(left == true) { hubo.getLeftLegAngles(lLegAnglesNext); hubo.huboLegFK(lFootInitialPose, lLegAnglesNext, LEFT); // Get left foot pose lFootOrigin = lFootInitialPose.translation(); // Set relative zero for foot location } if(right == true) { hubo.getRightLegAngles(rLegAnglesNext); hubo.huboLegFK(rFootInitialPose, rLegAnglesNext, RIGHT); // Get right foot pose rFootOrigin = rFootInitialPose.translation(); // Set relative zero for foot location } // while the daemon is running while(!daemon_sig_quit) { hubo.update(); // Get latest state info from Hubo dt = hubo.getTime() - ptime; // compute change in time ptime = hubo.getTime(); // get current time if(dt>0 || (send == false && print == true)); // if new data was received over ach { if(left == true) // if using left arm { hubo.getLeftLegAngles(lLegAnglesCurrent); // get left arm joint angles hubo.huboLegFK(lFootPoseCurrent, lLegAnglesCurrent, LEFT); // get left foot pose teleop.getPose(lSensorPos, lSensorRot, leftSensorNumber, true); // get teleop data lSensorChange = lSensorPos - lSensorOrigin; // compute teleop relative translation lFootPoseDesired = Eigen::Matrix4d::Identity(); // create 4d identity matrix lFootPoseDesired.translate(lSensorChange + lFootOrigin); // pretranslate relative translation // make sure feet don't cross sagittal plane if(lFootPoseDesired(1,3) - FOOT_WIDTH/2 < 0) lFootPoseDesired(1,3) = FOOT_WIDTH/2; lFootPoseDesired.rotate(lSensorRot); // add rotation to top-left of TF matrix hubo.huboLegIK( lLegAnglesNext, lFootPoseDesired, lLegAnglesCurrent, LEFT ); // get joint angles for desired TF hubo.setLeftLegAngles( lLegAnglesNext, false ); // set joint angles hubo.getLeftLegAngles( lActualAngles ); // get current joint angles } if( right==true ) // if using right arm { if(left == true) updateRight = false; else updateRight = true; hubo.getRightLegAngles(rLegAnglesCurrent); // get right arm joint angles hubo.huboLegFK(rFootPoseCurrent, rLegAnglesCurrent, RIGHT); // get right foot pose teleop.getPose(rSensorPos, rSensorRot, rightSensorNumber, updateRight); // get teleop data rSensorChange = rSensorPos - rSensorOrigin; // compute teleop relative translation rFootPoseDesired = Eigen::Matrix4d::Identity(); // create 4d identity matrix rFootPoseDesired.translate(rSensorChange + rFootOrigin); // pretranslation by relative translation // make sure feet don't cross sagittal plane if(rFootPoseDesired(1,3) + FOOT_WIDTH/2 > 0) rFootPoseDesired(1,3) = -FOOT_WIDTH/2; rFootPoseDesired.rotate(rSensorRot); // add rotation to top-left corner of TF matrix hubo.huboLegIK( rLegAnglesNext, rFootPoseDesired, rLegAnglesCurrent, RIGHT ); // get joint angles for desired TF hubo.setRightLegAngles( rLegAnglesNext, false ); // set joint angles hubo.getRightLegAngles( rActualAngles ); // get current joint angles } if( send == true ) // if user wants to send commands the control boards hubo.sendControls(); // send reference commands set above if( counter>=counterMax && print==true ) // if user wants output, print output every imax cycles { std::cout << "Teleop Position Lt(m): " << lSensorChange.transpose() << "\nTeleop Rotation Lt: \n" << lSensorRot << "\nTeleop Position Rt(m): " << rSensorChange.transpose() << "\nTeleop Rotation Rt: \n" << rSensorRot << "\nLeft Leg Actual Angles (rad): " << lActualAngles.transpose() << "\nLeft Leg Desired Angles(rad): " << lLegAnglesNext.transpose() << "\nRight Leg Actual Angles (rad): " << rActualAngles.transpose() << "\nRight Leg Desired Angles(rad): " << rLegAnglesNext.transpose() << "\nRight Foot Desired Pose: \n" << rFootPoseDesired.matrix() << "\nLeft Foot Desired Pose: \n" << lFootPoseDesired.matrix() << "\nRight foot torques(N-m)(Mx,My): " << hubo.getRightFootMx() << ", " << hubo.getRightFootMy() << "\nLeft foot torques(N-m)(Mx,My): " << hubo.getLeftFootMx() << ", " << hubo.getLeftFootMy() << std::endl; } if(counter>=counterMax) counter=0; counter++; // reset counter if it reaches counterMax } } }