int main()
{
Fastrak fastrak;
Eigen::Isometry3d pose;
while (true)
{
fastrak.achUpdate();
for (int i = 0; i < fastrak.getNumChannels(); i++)
{
fastrak.getPose(pose, i, false);
std::cout << "Sensor " << i << ": " << std::endl;
std::cout << pose.matrix() << std::endl;
}
boost::this_thread::sleep( boost::posix_time::milliseconds(1000) );
}
return 0;
}
int main() {
Eigen::Isometry3d pose;
while(1) {
fastrak.getPose(pose, 1);
std::cout << "pose = \n" << Eigen::Quaterniond(pose.linear()).vec().transpose() << std::endl;
}
}
int main(int argc, char **argv)
{
// Create Hubo_Tech object
Hubo_Tech hubo;
// Create Fastrak object
Fastrak fastrak;
// ofstream myfile;
// myfile.open("ik-output.txt");
// Initialize fastrak by opening fastrak ach channel
fastrak.initFastrak();
// Set fastrak scale to 1:1
fastrak.setFastrakScale( 1.0 );
hubo.setJointSpeedMax( RWP, 3.0 );
hubo.setJointSpeedMax( LWP, 3.0 );
// Local Variables
Vector6d q, current;
Eigen::Vector3d trans;
Eigen::Quaterniond quat;
double y=0, offset, dt, ptime=hubo.getTime();
int i=0, imax=4; // loop count variable
// Get joint angles for given y position of the drill
hubo.HuboDrillIK( q, y );
// Set RSR angle to -PI so his upper arm is parallel to the floor
// in order to avoid having the drill bit hit anything
q(2) -= M_PI/2.0;
// Set right arm joint angles with new RSR angle and send commands
hubo.setRightArmAngles( q, true );
// Get current right arm joint angles
hubo.getRightArmAngles( current );
// While the norm of the right arm angles is greater than 0.075
// keep waiting for arm to get to desired position
while( (current-q).norm() > 0.075 )
{
hubo.update(); // Get latest data from ach channels
hubo.getRightArmAngles( current ); // Get current right arm joint angles
}
// Set RSR angle back to initial angle
q(2) += M_PI/2.0;
// Set right arm joint angles with new RSR angle and send commands
hubo.setRightArmAngles( q, true );
// Read fastrak sensor pose for sensor 1
fastrak.getPose( trans, quat, 1, true );
// Define initial y position to be the offset,
// ie. take everything relative to that position
//
offset = trans(1);
// If this daemon hasn't quit...
while(!daemon_sig_quit)
{
// Get latest data from ach channels
hubo.update();
// Take latest time minus old time to see if a new time was received
dt = hubo.getTime() - ptime;
// Get current time in seconds
ptime = hubo.getTime();
// If new frame is received from hubo.update()
if(dt>0)
{
// Increment i and reset it to 0 if it exceeds the max
i++; if(i>imax) i=0;
// Read Fastrak sensors
fastrak.getPose( trans, quat, 1, true );
// Set relative y-position
y = trans(1) - offset;
// Get joint angles for given y position of the drill
hubo.HuboDrillIK( q, y );
// Set joint angles
hubo.setRightArmAngles( q );
// Send commands to control daemon
hubo.sendControls();
// Print out useful info
if( i==imax )
{
std::cout << "RH Mx" << hubo.getRightHandMx()
<< "RH My" << hubo.getRightHandMy()
<< "RH Angles:" << q.transpose()
<< std::endl;
}
}
}
}
int main(int argc, char **argv)
{
// hubo_plus hubo("balance-test");
/* Instantiate Hubo_Tech object from constructor */
Hubo_Tech hubo;
// std::ofstream myfile;
// myfile.open("balance-output.txt");
Fastrak fastrak;
/* Initialize Fastrak */
fastrak.initFastrak();
/* Set Left Leg Joint Nominal Accelerations */
hubo.setJointNominalAcceleration( LKN, 0.6 );
hubo.setJointNominalAcceleration( LHP, 0.6 );
hubo.setJointNominalAcceleration( LHR, 0.6 );
hubo.setJointNominalAcceleration( LAR, 0.6 );
hubo.setJointNominalAcceleration( LAP, 0.6 );
/* Set Right Leg Joint Nominal Accelerations */
hubo.setJointNominalAcceleration( RKN, 0.6 );
hubo.setJointNominalAcceleration( RHP, 0.6 );
hubo.setJointNominalAcceleration( RHR, 0.6 );
hubo.setJointNominalAcceleration( RAP, 0.6 );
hubo.setJointNominalAcceleration( RAR, 0.6 );
/* Set max joint angles for both leg hip pitch joints */
hubo.setJointAngleMax( RHP, 0 );
hubo.setJointAngleMax( LHP, 0 );
/* Local Variables */
double L1 = 2*0.3002;
double L2 = 0.28947 + 0.0795;
double height = 0;
Eigen::Vector3d fastrakOrigin;
Eigen::Vector3d trans;
Eigen::Quaterniond quat;
double leftP=0, leftR=0, rightP=0, rightR=0;
double knee, kneeVel, kneeAngleError;
double compRollGain = 0.0015;
double compPitchGain = 0.0015;
double pitchAngleGain = 0.5*M_PI/180.0;
double pitchRotVelGain = 0.0*M_PI/180;
double rollAngleGain = 0.3*M_PI/180;
double rollRotVelGain = 0.0*M_PI/180;
double integralGain = 0; //0.05*M_PI/180;
double kneeVelGain = 0.2;
double ptime, dt;
int i=0, imax=40;
/* Get latest data from the ach channels */
hubo.update();
/* Get current time */
ptime = hubo.getTime();
/* Read Fastrak sensor to get sensor origin */
fastrak.getPose(fastrakOrigin, quat, 1, true);
/* Print out sensor origin location */
// std::cout << fastrakOrigin.transpose() << std::endl;
/* Print headers for output data */
// fprintf(stdout, "Time,msgID,Height,KneeVel,KneeAngle\n");
// while(!daemon_sig_quit)
while(true)
{
/* Get latest data from the ach channels */
hubo.update();
dt = hubo.getTime() - ptime;
ptime = hubo.getTime();
if( dt > 0 )
// if( true )//dt > 0 )
{
/* Reset i to 0 when it reaches max value */
i++;
if(i>imax) i=0;
/* Read Fastrak to get translation and quaternion to sensor */
fastrak.getPose( trans, quat, 1, true ); //Using sensor 1
/* Print out location of fastrak sensor */
// std::cout << trans.transpose() << std::endl;
/* Make translation relative to initial sensor location */
trans -= fastrakOrigin;
/* Set height equal to the Fastrak z-position plus shoulder height of Hubo */
height = trans(2) + L1 + L2;
/* Keep height maximum at shoulder height */
if( height-L2 > L1 )
height = L1+L2;
/* Set minimum height to 0.2 meters above waist */
else if( height-L2 < 0.25 )
height = L1 + 0.2; // TODO: Check if this is a good value
/* Calculate knee angle required for calculated height value */
knee = acos( (height-L2)/L1 )*2;
/* Set knee angle error to (desired angle - current angle) */
kneeAngleError = knee - hubo.getJointAngle( RKN );
/* Set knee velocity to knee angle error times a gain */
kneeVel = kneeVelGain*kneeAngleError;
/* Calculate ankle roll and pitch velocities by resisting IMU angle, complying with
* the moment about the ankle (each with gains) and set the pitch velocities to
* negative half the knee velocities
*/
leftP = pitchAngleGain*hubo.getAngleY() - compPitchGain*hubo.getLeftFootMy() + (-kneeVel/2);
leftR = rollAngleGain*hubo.getAngleX() - compRollGain*hubo.getLeftFootMx();
rightP = pitchAngleGain*hubo.getAngleY() - compPitchGain*hubo.getRightFootMy() + (-kneeVel/2);
rightR = rollAngleGain*hubo.getAngleX() - compRollGain*hubo.getRightFootMx();
/* Set joint velocities */
hubo.setJointVelocity( RAP, rightP );
hubo.setJointVelocity( RAR, rightR );
hubo.setJointVelocity( LAP, leftP );
hubo.setJointVelocity( LAR, leftR );
hubo.setJointVelocity( RKN, kneeVel );
hubo.setJointVelocity( RHP, -kneeVel/2.0 );
hubo.setJointVelocity( LKN, kneeVel );
hubo.setJointVelocity( LHP, -kneeVel/2.0 );
/* Send commands to the control daemon */
hubo.sendControls();
// if( i==imax )
// fprintf(stdout, "%f,%d,%f,%f\n", ptime, hubo.msgID, height, kneeVel, hubo.getJointAngle(RKN));
if( i==imax )
{
std::cout
<< "Trans: " << trans.transpose()
// << "\nRotatCol1: " << quat.matrix().row(0)
// << "\nRotatCol2: " << quat.matrix().row(1)
// << "\nRotatCol3: " << quat.matrix().row(2)
<< std::endl;
//<< "\nQuat:" << quat.w() << "\t" << quat.x() << "\t" << quat.y() << "\t" << quat.z()
}
}
}
// myfile.close();
}
