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
}
}
}
