Linkage& Robot::linkage(size_t linkageIndex)
{ // FIXME: Remove assert
if(linkageIndex < nJoints())
return *linkages_[linkageIndex];
Linkage* invalidLinkage = new Linkage;
invalidLinkage->name("invalid");
return *invalidLinkage;
}
Linkage& Robot::linkage(string linkageName)
{
map<string,size_t>::iterator j = linkageNameToIndex_.find(linkageName);
if( j != linkageNameToIndex_.end() )
return *linkages_.at(j->second);
Linkage* invalidLinkage = new Linkage;
invalidLinkage->name("invalid");
return *invalidLinkage;
}
void PCHierarchy::addNetwork( PCNetwork* net, string parent, Linkage link ) {
// Check if parent exists
if( hasNetwork( parent ) ) {
// Check if parent's predictions line up with child's hypotheses
if( link.checkPredictions( getNetwork( parent ) ) && link.checkHypotheses( net ) ) {
links.push_back( link );
}
}
else {
//TODO: error
}
}
void replaceLinkage()
{
std::string link = "233A102603000BBA00000000000670FA13";
//link = "00000000006507D13";
//event->setLinkage(link);
static boost::regex regex("((.{4})(.{4})(.{4})(.{4})(.{1}))");
static const int WHOLE_MATCH_POS = 1;
static const int NET_ID_POS = 2;
static const int LCN_POS = 4;
static const int EVENT_ID_POS = 5;
static const int FLAGS_POS = 6;
boost::smatch results;
std::string::const_iterator itStart = link.begin();
std::string::const_iterator itEnd = link.end();
while( boost::regex_search(itStart, itEnd, results, regex) )
{
itStart = results[1].second;
unsigned int nId = 0;
std::stringstream ss;
ss << std::hex << results[NET_ID_POS];
ss >> nId;
if (nId == 0) {
std::stringstream lcns;
lcns << std::hex << results[LCN_POS];
lcns >> nId;
Linkage l;
l.origLinkage = results[WHOLE_MATCH_POS];
l.constPart = results[EVENT_ID_POS] + results[FLAGS_POS];
l.lcn = boost::lexical_cast<std::string>(nId);
std::cout << l.toString();
//l.event = event;
//m_linksForUpdate.push(l);
}
}
uint16_t
ProtoThread::dispatch(bool flag)
{
uint16_t count = 0;
// Check if events should be processed and the run queue is empty
if (flag && runq.is_empty()) {
Event event;
Event::queue.await(&event);
event.dispatch();
count += 1;
}
// Iterate once through the run queue and call all threads run method
Linkage* link = runq.get_succ();
while (link != &runq) {
Linkage* succ = link->get_succ();
ProtoThread* thread = (ProtoThread*) link;
thread->m_state = RUNNING;
thread->run();
if (thread->m_state == RUNNING) {
thread->m_state = READY;
}
link = succ;
count += 1;
// Check if events should be processed
if (flag) {
while (Event::queue.available()) {
Event event;
Event::queue.dequeue(&event);
event.dispatch();
count += 1;
}
}
}
// Return total number of function dispatch
return (count);
}
//------------------------------------------------------------------------------
// Function Definitions
//------------------------------------------------------------------------------
void tutorial()
{
cout << "---------------------------" << endl;
cout << "| Creating A Hubo Robot |" << endl;
cout << "---------------------------" << endl << endl;
// Create an instance of a Hubo
Hubo hubo;
// Print info about hubo
cout << "The robot " << hubo.name() << " consist of " << hubo.nLinkages() << " linkages and " << hubo.nJoints() << " joints all together." << endl << endl;
cout << "-------------------------" << endl;
cout << "| Linkages and Joints |" << endl;
cout << "-------------------------" << endl << endl;
cout << "The linkages are: " << endl;
cout << "(ID, Name, Parent)" << endl;
for (vector<Linkage>::iterator linkageIt = hubo.linkages().begin(); linkageIt != hubo.linkages().end(); ++linkageIt) {
if (linkageIt->parentLinkage() == 0) {
cout << linkageIt->id() << ", " << linkageIt->name() << " <- " << hubo.name() << endl;
} else {
cout << linkageIt->id() << ", " << linkageIt->name() << " <- " << linkageIt->parentLinkage()->name() << endl;
}
}
cout << endl;
size_t rightArmIndex = hubo.linkageIndex("RIGHT_ARM"); // Get index to right arm or can refer to right arm directly (see line below).
cout << "Each linkage consist of joints." << endl << endl;
cout << "For example the " << hubo.linkage(rightArmIndex).name() << " linkage has " << hubo.linkage(rightArmIndex).nJoints() << " joints." << endl << endl;
cout << "The joints for the " << hubo.linkage(rightArmIndex).name() << " are:" << endl;
cout << "(ID, Name, Value): " << endl;
// Notice that there is a const_joints() as well as a joints() method (this exist for the robot class well), which allows for when the "this" pointer is const as is the case with the const iterator below.
for (vector<Linkage::Joint>::const_iterator jointIt = hubo.linkage("RIGHT_ARM").const_joints().begin();
jointIt != hubo.linkage("RIGHT_ARM").const_joints().end(); ++jointIt) {
cout << jointIt->id() << ", " << jointIt->name() << ", " << jointIt->value() << endl;
}
cout << endl;
cout << "---------------------------" << endl;
cout << "| Changing joint values |" << endl;
cout << "---------------------------" << endl << endl;
cout << "Let's change the joint values." << endl << endl;
// Set the joints all at once
cout << "Set all the joints to have a value of PI/2." << endl;
Linkage* rightArm = &hubo.linkage("RIGHT_ARM"); // Notice that this is a pointer
VectorXd q = M_PI/2 * VectorXd::Ones(rightArm->nJoints(), 1);
rightArm->values(q); // Set all joints at once
cout << rightArm->values() << endl << endl;
// Set the joints individually
cout << "Set the joints individually." << endl;
Linkage::Joint* joint0 = &rightArm->joint(0); // Again notice that this is a pointer
Linkage::Joint joint1 = rightArm->joint(1); // This is not a pointer but return a const reference, but updating this joint will not update the robot
Linkage::Joint* joint2 = &rightArm->joint("RSY"); // Just like linkages, joints can be index by name
joint0->value(0.1);
joint1.value(0.2);
joint2->value(0.3);
hubo.joint("REP")->value(0.4); // Notice that the joint() method of the robot class returns a pointer but the linkage() method returned a reference
hubo.joint(23)->value(0.4); // Notice that the joint index in the robot is different than the joint index in the linkage
cout << hubo.linkage("RIGHT_ARM").values() << endl << endl;
// Reset all to position minus offset
cout << "Reset joint values minus offsets." << endl;
hubo.linkage("RIGHT_ARM").values(-hubo.rightArmOffsets);
cout << hubo.linkage("RIGHT_ARM").values() << endl << endl;
cout << "------------" << endl;
cout << "| Frames |" << endl;
cout << "------------" << endl << endl;
cout << "Robots, Linkages, Joints, and Tools are all frames which have homogenous transformation with respect to differenct frames associated with them." << endl << endl;
Linkage::Tool* rightHand = &rightArm->tool();
cout << "For example the " << rightHand->name() << " tool at the end of the " << rightArm->name() << " linkage has the following homogenous transformations associated with it." << endl << endl;
rightHand->printInfo();
cout << "Every frame has a HG transformation with respect to a fixed frame and the world frame." << endl << endl;
cout << "For example, the " << hubo.name() << " robot has the following: " << endl << endl;
hubo.Frame::printInfo();
cout << "Some types of frames (e.g. Linkage::Joint) have more HG transformations associated with their frame." << endl << endl;
cout << "For example, the " << joint2->name() << " joint has the following: " << endl << endl;
joint2->printInfo();
cout << "These frames automatically get updated when the joint values change." << endl << endl;
cout << "For example, let's move the right arm straight out in front of the robot." << endl << endl;
cout << "Joint values of the " << rightArm->name() << " are:" << endl;
joint0->value(-M_PI/2);
cout << hubo.linkage("RIGHT_ARM").values() << endl << endl;
cout << "The " << rightHand->name() << " tool now has the following values:" << endl;
cout << "Respect to fixed frame (" << rightHand->parentJoint()->name() << " joint)" << endl;
cout << rightHand->respectToFixed().matrix() << endl << endl;
cout << "Respect to linkage frame (" << rightHand->parentLinkage()->name() << " linkage)" << endl;
cout << rightHand->respectToLinkage().matrix() << endl << endl;
cout << "Respect to robot frame (" << rightHand->parentRobot()->name() << " robot)" << endl;
cout << rightHand->respectToRobot().matrix() << endl << endl;
cout << "Notice the transform with respect to the fixed frame did not change but the others did." << endl << endl;
cout << "Let's now twist the robot at the waist." << endl << endl;
cout << "Joint values of the " << hubo.linkage("TORSO").name() << " are:" << endl;
hubo.linkage("TORSO").joint(0).value(M_PI/2);
cout << hubo.linkage("TORSO").values() << endl << endl;
cout << "The " << rightHand->name() << " tool now has the following values:" << endl;
cout << "Respect to fixed frame (" << rightHand->parentJoint()->name() << " joint)" << endl;
cout << rightHand->respectToFixed().matrix() << endl << endl;
cout << "Respect to linkage frame (" << rightHand->parentLinkage()->name() << " linkage)" << endl;
cout << rightHand->respectToLinkage().matrix() << endl << endl;
cout << "Respect to robot frame (" << rightHand->parentRobot()->name() << " robot)" << endl;
cout << rightHand->respectToRobot().matrix() << endl << endl;
cout << "Notice the transform with respect to the fixed frame and the linkage frame did not change but the with respect to the robot frame did." << endl << endl;
cout << "---------------" << endl;
cout << "| Jacobians |" << endl;
cout << "---------------" << endl << endl;
cout << "Jacobians for each linkage and the entire robot can be obtained easily." << endl << endl;
cout << "For example, let's get the Jacobian for the " << rightArm->name() << " linkage where the location of the Jacobian is at " << rightArm->tool().name() << " and referenced with respect to the " << rightArm->name() << " linkage base coordinate frame." << endl << endl;
rightArm->joint(0).value(0);
cout << "Current joint values are:" << endl;
cout << rightArm->values() << endl << endl;
MatrixXd J;
rightArm->jacobian(J,
rightArm->const_joints(),
rightArm->const_tool().respectToLinkage().translation(),
&hubo.linkage("RIGHT_ARM"));
cout << "Jacobian is:" << endl;
cout << J.matrix() << endl << endl;
cout << "Or we can get the Jacobian from the " << hubo.linkage("RIGHT_LEG").tool().name() << " to the " << hubo.linkage("RIGHT_ARM").tool().name() << " and referenced with respect to " << hubo.name() << "." << endl << endl;
vector<Linkage::Joint> joints;
for (vector<Linkage::Joint>::reverse_iterator jointIt = hubo.linkage("RIGHT_LEG").joints().rbegin(); jointIt != hubo.linkage("RIGHT_LEG").joints().rend(); ++jointIt) {
joints.push_back(*jointIt);
}
for (vector<Linkage::Joint>::iterator jointIt = hubo.linkage("TORSO").joints().begin(); jointIt != hubo.linkage("TORSO").joints().end(); ++jointIt) {
joints.push_back(*jointIt);
}
for (vector<Linkage::Joint>::iterator jointIt = hubo.linkage("RIGHT_ARM").joints().begin(); jointIt != hubo.linkage("RIGHT_ARM").joints().end(); ++jointIt) {
joints.push_back(*jointIt);
}
cout << "There are " << joints.size() << " joints envolved in this Jacobian with the values of" << endl;
cout << "(Name, Value)" << endl;
for (vector<Linkage::Joint>::iterator jointIt = joints.begin(); jointIt != joints.end(); ++jointIt) {
cout << jointIt->name() << ", " << jointIt->value() << endl;
}
cout << endl;
hubo.jacobian(J,
joints,
hubo.linkage("RIGHT_ARM").const_tool().respectToRobot().translation(),
&hubo);
cout << "Jacobian is:" << endl;
cout << J.matrix() << endl << endl;
cout << "--------------------------------------" << endl;
cout << "| Analytical Arm Inverse Kinematics |" << endl;
cout << "--------------------------------------" << endl << endl;
cout << "Inverse kinematics of a linkage can also easily be obtained." << endl << endl;
cout << "For example, let's set the joint values of the " << rightArm->name() << " to the following:" << endl;
Isometry3d B0;
VectorXd q0(6), q1(6);
q0 <<
-.1,
-.2,
-.3,
-.4,
-.5,
-.6;
rightArm->values(q0);
cout << rightArm->values() << endl << endl;
cout << "Then the " << rightArm->tool().name() << " with respect to the linkage has a pose of " << endl;
B0 = rightArm->tool().respectToLinkage();
cout << B0.matrix() << endl << endl;
cout << "This HG transformation can be given to the IK to get the following joint values:" << endl;
hubo.rightArmAnalyticalIK(q1, B0, q0);
cout << q1 << endl << endl;
Linkage* rightLeg = &hubo.linkage("RIGHT_LEG");
cout << "Let's now show the IK for the " << rightLeg->name() << ". Let's set the joint values to the following:" << endl;
q0 <<
.11,
.22,
.13,
.24,
.15,
.16;
rightLeg->values(q0);
cout << rightLeg->values() << endl << endl;
cout << "Then the " << rightLeg->tool().name() << " with respect to the linkage has a pose of " << endl;
B0 = rightLeg->tool().respectToLinkage();
cout << B0.matrix() << endl << endl;
cout << "This HG transformation can be given to the IK to get the following joint values:" << endl;
hubo.rightLegAnalyticalIK(q1, B0, q0);
cout << q1 << endl << endl;
cout << "------------------" << endl;
cout << "| Still to come |" << endl;
cout << "------------------" << endl << endl;
cout << "Things that still need to be added or fixed are the following:" << endl;
cout << "-- Numerical Inverse Kinematics" << endl;
cout << "-- Documentation has to be written" << endl;
cout << "-- Possibly fix how joints and linkages are stored so everything is reference by a pointer" << endl;
cout << "-- Add methods to add and remove joints" << endl;
}
/**************************************************
* Just home the motor and do a bunch of random
* moves.
**************************************************/
int main( void )
{
// The libraries define one global object of type
// CopleyMotionLibraries named cml.
//
// This object has a couple handy member functions
// including this one which enables the generation of
// a log file for debugging
cml.SetDebugLevel( LOG_EVERYTHING );
// Create an object used to access the low level CAN network.
// This examples assumes that we're using the Copley PCI CAN card.
#if defined( USE_CAN )
CopleyCAN hw( "CAN0" );
hw.SetBaud( canBPS );
#elif defined( WIN32 )
WinUdpEcatHardware hw( "eth0" );
#else
LinuxEcatHardware hw( "eth0" );
#endif
// Open the network object
#if defined( USE_CAN )
CanOpen net;
#else
EtherCAT net;
#endif
const Error *err = net.Open( hw );
showerr( err, "Opening network" );
// Initialize the amplifiers using default settings
Amp amp[AMPCT];
AmpSettings set;
set.guardTime = 0;
printf( "Doing init\n" );
int i;
for( i=0; i<AMPCT; i++ )
{
printf( "Initing %d\n", canNodeID+i );
err = amp[i].Init( net, canNodeID+i, set );
showerr( err, "Initting amp" );
MtrInfo mtrInfo;
err = amp[i].GetMtrInfo( mtrInfo );
showerr( err, "Getting motor info\n" );
err = amp[i].SetCountsPerUnit( mtrInfo.ctsPerRev );
showerr( err, "Setting cpr\n" );
}
// Create a linkage object holding these amps
Linkage link;
err = link.Init( AMPCT, amp );
showerr( err, "Linkage init" );
// Home the amps
HomeConfig hcfg;
hcfg.method = CHM_NONE;
hcfg.offset = 0;
for( i=0; i<AMPCT; i++ )
{
// Home the amp. Notice that the linkage object may be used
// like an array to reference the amplifiers.
err = link[i].GoHome( hcfg );
showerr( err, "Going home" );
}
// Wait for all amplifiers to finish the home by waiting on the
// linkage object itself.
printf( "Waiting for home to finish...\n" );
err = link.WaitMoveDone( 20000 );
showerr( err, "waiting on home" );
// Setup the velocity, acceleration, deceleration & jerk limits
// for multi-axis moves using the linkage object
err = link.SetMoveLimits( 10, 10, 10, 100 );
showerr( err, "setting move limits" );
// Do a bunch of random moves.
for( int j=0; j<5000; j++ )
{
// Create an N dimensional position to move to
Point<AMPCT> pos;
printf( "%d: moving to ", j );
for( i=0; i<AMPCT; i++ )
{
pos[i] = (rand() % 100000) / 100000.0;
if( i ) pos[i] = pos[0];
printf( "%.3lf ", pos[i] );
}
printf( "\n" );
// This function will cause the linkage object to create a
// multi-axis S-curve move to the new position. This
// trajectory will be passed down to the amplifiers using
// the PVT trajectory mode
err = link.MoveTo( pos );
showerr( err, "Moving linkage" );
// Wait for the move to finish
err = link.WaitMoveDone( 1000 * 30 );
showerr( err, "waiting on linkage done" );
}
return 0;
}
int main( void )
{
////////////////////////////////////////////////////////////////////////////////////////// VVV Initialization and Homing VVV
//cout << "ayy lmao\n"; //For debugging
////////////////////Set up Copley Libraries///////////
// The libraries define one global object of type
// CopleyMotionLibraries named cml.
//
// This object has a couple handy member functions
// including this one which enables the generation of
// a log file for debugging
cml.SetDebugLevel( LOG_EVERYTHING );
// Create an object used to access the low level CAN network.
// This examples assumes that we're using the Copley PCI CAN card.
#if defined( USE_CAN )
KvaserCAN hw( "CAN0" );
hw.SetBaud( canBPS );
#elif defined( WIN32 )
WinUdpEcatHardware hw( "eth0" );
#else
LinuxEcatHardware hw( "eth0" );
#endif
// Open the network object
#if defined( USE_CAN )
CanOpen net;
#else
EtherCAT net;
#endif
const Error *err;
int i;
Amp amp[AMPCT];
if(robotPlugged){
err = net.Open( hw );
showerr( err, "Opening network" );
// Initialize the amplifiers using default settings
AmpSettings set;
set.guardTime = 0;
cout << "Doing initialization"<<endl;
for( i=0; i<AMPCT; i++ )
{
//printf( "Initiating Amplifier %d\n", canNodeID+i );
cout << "Initializing Amplifier " << (canNodeID+i) << endl;
err = amp[i].Init( net, canNodeID+i, set );
showerr( err, "Initting amp" );
MtrInfo mtrInfo;
err = amp[i].GetMtrInfo( mtrInfo );
showerr( err, "Getting motor info\n" );
// err = amp[i].SetCountsPerUnit( mtrInfo.ctsPerRev );
// printf( "CountsPerRev %d\n", mtrInfo.ctsPerRev );
err = amp[i].SetCountsPerUnit( 8000.0/5.0); // User Units are now in mm
showerr( err, "Setting cpr\n" );
}
}
// Create a linkage object holding these amps
Linkage link;
if(robotPlugged){
err = link.Init( AMPCT, amp );
showerr( err, "Linkage init" );
// Home the amps
HomeConfig hcfg;
err = link[0].GetHomeConfig(hcfg);
showerr(err, "get Home Config");
hcfg.velFast = 5;
hcfg.velSlow = 1;
hcfg.accel = 10;
hcfg.method = CHM_NHOME_INDX; // CHM_NONE;
hcfg.offset = 0;
printf( "Home Velocity %f \n", hcfg.velFast );
printf( "Home Velocity Slow %f \n", hcfg.velSlow );
// Setup the velocity, acceleration, deceleration & jerk limits
// for multi-axis moves using the linkage object
err = link.SetMoveLimits( 75, 75, 75, 100 );
showerr( err, "setting move limits" );
// Create an N dimensional position to move to
Point<AMPCT> act;
for( i=0; i<AMPCT; i++ )
{
// Assuming we are low to the ground and centered, first move forward 30mm.
//Notice that the linkage object may be used like an array to reference the amplifiers.
double currentPosition;
err = link[i].GetPositionActual(currentPosition);
printf( "Current Position %f \n", currentPosition );
showerr( err, "ActualPosition" );
act[i]= currentPosition+50; //in mm
// if(currentPosition<0){
// act[i]= currentPosition+(0-currentPosition)+2.5; //in mm
// }else{
// act[i]= 2.5;
// }
printf( "Goal Position %f mm \n", act[i]);
}
err = link.MoveTo( act );
showerr( err, "Moving linkage" );
// Wait for all amplifiers to finish the initial move by waiting on the
// linkage object itself.
printf( "Waiting for move up to finish...\n" );
err = link.WaitMoveDone( 20000 );
showerr( err, "waiting on initial move" );
for( i=0; i<AMPCT; i++ )
{
// Home the amp. Notice that the linkage object may be used
// like an array to reference the amplifiers.
if(i==0){
hcfg.offset = 2;
}else if(i==2 || i==3){
hcfg.offset = -0.75;
}else if(i==4){
hcfg.offset = -1;
}else{
hcfg.offset = 0;
}
err = link[i].GoHome(hcfg); // hcfg
showerr( err, "Going home" );
}
// Wait for all amplifiers to finish the home by waiting on the
// linkage object itself.
printf( "Waiting for home to finish...\n" );
err = link.WaitMoveDone( 20000 );
showerr( err, "waiting on home" );
}
////////////////////////////////////////////////////////////////////////////////////////// ^^^ Initialization and Homing Complete. ^^^
// Create an N dimensional position to move to
Point<AMPCT> act;
act[0] = 99-1;
act[1] = 99-2;
act[2] = 99-2.5;
act[3] = 99-1;
act[4] = 99-2;
act[5] = 99-.5;
// Z is 45.75 inches
if(robotPlugged){
err = link.MoveTo( act );
showerr( err, "Moving linkage" );
// Wait for all amplifiers to finish the initial move by waiting on the
// linkage object itself.
printf( "Waiting for move up to finish...\n" );
err = link.WaitMoveDone( 20000 );
showerr( err, "waiting on initial move" );
}
// Create an N dimensional slide vector
// Point<AMPCT> q;
double q[AMPCT];
char msgBack;
char qMsg[sizeof(double)];
double qTemp;
////////////////////////////////////////////////////////////////////////////////////////////////////////////////Main Function
while(isRunning){
std::cout<< "RUN CHECK\n";
msgBack = std::cin.get();
std::cout<< "CPP Receieves Message!\n";
isRunning = msgBack-48;
if(isRunning){
std::cout<< "RUN OK\n";
//Check if valid q. If not, then isRunning = false, break. Or, try again.
int checker = 0;
for (int i = 0; i<AMPCT; i++){
std::cout<< "GIMME\n";
for (int j = 0; j<sizeof(double); j++){
qMsg[j] = std::cin.get();
std::cout<< qMsg[j] <<endl;
}
//printf("Received:%s\n", qMsg );
memcpy(&qTemp,&qMsg,sizeof(double));
printf( "Converted %f \n", qTemp);
q[i] = qTemp;
if(SIGMA2ACTUATOR - q[i]<=250 && SIGMA2ACTUATOR - q[i] >= 0){
checker++;
}
}
for (int i = 0; i<AMPCT; i+=2){
if(sqrt(q[i]*q[i]+q[i]*q[i+1]+q[i+1]*q[i+1])<=MAXWIDTH){
checker+=2;
}
}
if(checker == 2*AMPCT){
checker = 0;
std::cout<< "GOOD Q\n";
std::cout<< "Moving to point...\n";
//If all safe, Assign vector act[] with the new coords. If not, stay.
//Convert from q (sigma coords) to actuator coords (0 to 300mm)
act[0] = SIGMA2ACTUATOR - q[0];
act[1] = SIGMA2ACTUATOR - q[1];
act[2] = SIGMA2ACTUATOR - q[2];
act[3] = SIGMA2ACTUATOR - q[3];
act[4] = SIGMA2ACTUATOR - q[4];
act[5] = SIGMA2ACTUATOR - q[5];
if(robotPlugged){
err = link.MoveTo( act );
showerr( err, "Moving linkage" );
// Wait for all amplifiers to finish the initial move by waiting on the
// linkage object itself.
printf( "Waiting for move up to finish...\n" );
err = link.WaitMoveDone( 20000 );
showerr( err, "waiting on initial move" );
}
}else{
checker = 0;
std::cout<< "BAD Q\n";
std::cout<< "Please try again...\n";
}
// tell Python we're done with this move.
std::cout<<"Move Done.\n";
}else{
std::cout<<"Ending Program...\n";
}
}
//////////////////////////////////////////////////////////////////////////////////////Go to home position before ending
act[0] = 0;
act[1] = 0;
act[2] = 0;
act[3] = 0;
act[4] = 0;
act[5] = 0;
//cout << "Press ENTER to coninue...";
//std::cin.ignore();
if(robotPlugged){
err = link.MoveTo( act );
showerr( err, "Moving linkage" );
// Wait for all amplifiers to finish the initial move by waiting on the
// linkage object itself.
printf( "Waiting for move up to finish...\n" );
err = link.WaitMoveDone( 20000 );
showerr( err, "waiting on initial move" );
}
return 0;
}
