int main( int argc, char **argv )
{
Hubo_Control hubo;
std::vector<Vector6d, Eigen::aligned_allocator<Vector6d> > angles(5); // This declares "angles" as a dynamic array of Vector6ds with a starting array length of 5
angles[0] << 0.0556916, 0.577126, 0.0816814, -0.492327, 0, 0;
angles[1] << -1.07878, 0.408266, -0.477742, -0.665062, 0, 0;
angles[2] << -1.17367, -0.0540511, -0.772141, -0.503859, 0, 0;
angles[3] << -0.518417, 0.172191, -0.566084, -0.0727671, 0, 0;
angles[4] << 0, 0, 0, 0, 0, 0;
// Obviously we could easily set up some code which reads in these values out of a pre-recorded file
Vector6d currentPosition; // This Vector6d will be used to track our current angle configuration
double tol = 0.075; // This will be the allowed tolerance before moving to the next point
int traj = 0; // This will keep track of our current target on the trajectory
while( true )
{
hubo.update(); // This grabs the latest state information
hubo.getLeftArmAngleStates( currentPosition ); // This will fill in the values of "currentPosition" by passing it in by reference
// If you are unfamiliar with "passing by reference", let me know and I can explain the concept. It's a feature of C++ but not C
if( ( currentPosition-angles[traj] ).norm() < tol ) // The class function .norm() is a feature of the Eigen libraries. Eigen has many other extremely useful functions like this.
{
traj++;
if( traj > 4 )
traj = 0;
}
hubo.setLeftArmAngles( angles[traj] ); // Notice that the second argument is not passed in, making it default to "false"
hubo.sendControls(); // This will send off all the latest control commands over ACH
}
}
void gotoFirstPosition(double referenceData[], Hubo_Control &hubo){
double tol = 0.075; // This will be the allowed tolerance before moving to the next point
ArmVector left_arm_angles; // This declares "angles" as a dynamic array of ArmVectors with a starting array length of 5
ArmVector right_arm_angles; // This declares "angles" as a dynamic array of ArmVectors with a starting array length of 5
ArmVector left_leg_angles; // This declares "angles" as a dynamic array of ArmVectors with a starting array length of 5
ArmVector right_leg_angles; // This declares "angles" as a dynamic array of ArmVectors with a starting array length of 5
left_arm_angles<< referenceData[LSP], referenceData[LSR], referenceData[LSY], referenceData[LEB], referenceData[LWY], referenceData[LWP], referenceData[LWR], 0,0,0;
right_arm_angles<< referenceData[RSP], referenceData[RSR], referenceData[RSY], referenceData[REB], referenceData[RWY], referenceData[RWP], referenceData[RWR],0,0,0;
right_leg_angles<< referenceData[RHY], referenceData[RHR], referenceData[RHP], referenceData[RKN], referenceData[RAP], referenceData[RAR],0,0,0,0;
left_leg_angles<< referenceData[LHY], referenceData[LHR], referenceData[LHP], referenceData[LKN], referenceData[LAP], referenceData[LAR],0,0,0,0;
bool left_arm_in_limit=false;
bool right_arm_in_limit=false;
bool left_leg_in_limit=false;
bool right_leg_in_limit=false;
ArmVector currentPosition; // This ArmVector will be used to track our current angle configuration
while( left_arm_in_limit && right_arm_in_limit && left_leg_in_limit && right_leg_in_limit )
{
hubo.update(true); // This grabs the latest state information
hubo.getLeftArmAngleStates( currentPosition ); // This will fill in the values of "currentPosition" by passing it in by reference
// If you are unfamiliar with "passing by reference", let me know and I can explain the concept. It's a feature of C++ but not C
if( ( currentPosition-left_arm_angles ).norm() < tol ) // The class function .norm() is a feature of the Eigen libraries. Eigen has many other extremely useful functions like this.
{
left_arm_in_limit=true;
}
hubo.getRightArmAngleStates( currentPosition ); // This will fill in the values of "currentPosition" by passing it in by reference
if( ( currentPosition-right_arm_angles).norm() < tol ) // The class function .norm() is a feature of the Eigen libraries. Eigen has many other extremely useful functions like this.
{
right_arm_in_limit=true;
}
hubo.getLeftLegAngleStates( currentPosition ); // This will fill in the values of "currentPosition" by passing it in by reference
if( ( currentPosition-left_leg_angles ).norm() < tol ) // The class function .norm() is a feature of the Eigen libraries. Eigen has many other extremely useful functions like this.
{
left_leg_in_limit=true;
}
hubo.getRightLegAngleStates( currentPosition ); // This will fill in the values of "currentPosition" by passing it in by reference
if( ( currentPosition-right_leg_angles ).norm() < tol ) // The class function .norm() is a feature of the Eigen libraries. Eigen has many other extremely useful functions like this.
{
right_leg_in_limit=true;
}
hubo.setLeftArmAngles( left_arm_angles); // Notice that the second argument is not passed in, making it default to "false"
hubo.setRightArmAngles( right_arm_angles); // Notice that the second argument is not passed in, making it default to "false"
hubo.setLeftLegAngles( left_leg_angles); // Notice that the second argument is not passed in, making it default to "false"
hubo.setRightLegAngles( right_leg_angles); // Notice that the second argument is not passed in, making it default to "false"
hubo.sendControls(); // This will send off all the latest control commands over ACH
}
}