////////////////////////////////////////////////////////////////////////////// // // Title : exercise9.cpp // Project : OSCAR tutorial // Created : May 30, 1998 // Author : Mitch Pryor // Platforms : PC // Copyright : Copyright© The University of Texas at Austin, 2002. // All rights reserved. // // This software and documentation constitute an unpublished work // and contain valuable trade secrets and proprietary information // belonging to University. None of the foregoing material may be // copied or duplicated or disclosed without the express, written // permission of University. UNIVERSITY EXPRESSLY DISCLAIMS ANY // AND ALL WARRANTIES CONCERNING THIS SOFTWARE AND DOCUMENTATION, // INCLUDING ANY WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A // PARTICULAR PURPOSE, AND WARRANTIES OF PERFORMANCE, AND ANY WARRANTY // THAT MIGHT OTHERWISE ARISE FROM COURSE OF DEALING OR USAGE OF TRADE. // NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH RESPECT TO THE USE OF // THE SOFTWARE OR DOCUMENTATION. Under no circumstances shall // University be liable for incidental, special, indirect, direct or // consequential damages or loss of profits, interruption of business, // or related expenses which may arise from use of software or documentation, // including but not limited to those resulting from defects in software // and/or documentation, or loss or inaccuracy of data of any kind. // // Purpose : Solution to Exercise 9 of the OSCAR Guide // ////////////////////////////////////////////////////////////////////////////// #define JRAPATH "DataFiles/rr7.limits" #define DHFile "DataFiles/rr7.dh" #define SEARCHTYPE EXHAUSTIVE //Generate Options typedef #define SOLUTIONNO 3 //Number of generated Options #define NUMCRITERIA 2 //Number of criteria currently used #include "InfoBasedCriteria/PCRangeAvailability.h" #include "InfoBasedCriteria/PCMaximizeMOT.h" #include "InfoBasedCriteria/PCNormalizedFusion.h" #include "InfoBasedCriteria/PCUtilities.h" #include "InfoBasedCriteria/Information.h" #include "InverseKinematics/IKJPartialFast.h" #include "InverseKinematics/GenerateOptions.h" #include "ForwardKinematics/FKPosition.h" #include "FileData/OffsetAngles.h" #include "Controllers/GeneralKeyboard.h" #include "Communications/RoboTalk.h" ////////////////////////////////////////////////////////////////////////////// //declare functions ////////////////////////////////////////////////////////////////////////////// int SendSetPoint(RRVector& jointsToSend); ////////////////////////////////////////////////////////////////////////////// // These are the names given to each control point in the Mr2Arm // simulation. The file that contains Mr2Arm simulation is Rr17. ////////////////////////////////////////////////////////////////////////////// char* TagNames[] = { "rr17_torso_1","rr17_torso_2","rr17_torso_3", "rr_left_1","rr_left_2","rr_left_3","rr_left_4", "rr_left_5","rr_left_6","rr_left_7","rr_left_gripper", "rr_right_1","rr_right_2","rr_right_3","rr_right_4", "rr_right_5","rr_right_6","rr_right_7","rr_right_gripper"}; const unsigned int noTags = 19; ////////////////////////////////////////////////////////////////////////////// //Begin main ////////////////////////////////////////////////////////////////////////////// int main() { bool done = false; int stopFlag = 0; int limitFlag = 0; int kFlag, vFlag, i, solutionNumber; ////////////////////////////////////////////////////////////////////////////// //Set up Graphics Communication Information ////////////////////////////////////////////////////////////////////////////// char filename[] = "RR17"; if(Connect(filename, SELF) != NO_ERROR){ cout << Connect(filename, SELF) << endl; cout << "Error Connecting" << endl; return 0; } ////////////////////////////////////////////////////////////////////////////// //Set up the robot data ////////////////////////////////////////////////////////////////////////////// RRRot3by3 rotation; RRVector3D axis; RRXform Llocation, LlocationInverse; //These transforms are necessary when the manipulator's home position //does not align with the home position of the DH parameters. axis.Z() = 1.0; rotation.SetRotationAngles(axis, 180.0); Llocation.SetRotationMatrix(rotation); Llocation.InvertXform(LlocationInverse); RRVectorD jointsToSend(17); // right 7 DOF arm jointsToSend[0] = 20.0; jointsToSend[1] = 35.0; jointsToSend[2] = 53.0; jointsToSend[3] = 116.0; jointsToSend[4] = 130.0; jointsToSend[5] = 65.0; jointsToSend[6] = 10.0; // 3 DOF torso jointsToSend[7] = 0.01; jointsToSend[8] = -15.0; jointsToSend[9] = 0.01; // left 7DOF arm jointsToSend[10] = -30.0; jointsToSend[11] = 30.0; jointsToSend[12] = -60.0; jointsToSend[13] = 110.0; jointsToSend[14] = -110.0; jointsToSend[15] = 60.0; jointsToSend[16] = 25.0; jointsToSend *= DEGTORAD; RRVectorD previousJointsToSend(jointsToSend); RRVectorD LjointPosition(7); //set up the one serial chain we want to control, the left arm for(i=0;i<7;i++){ LjointPosition.at(i) = jointsToSend.at(i+10); } LjointPosition.at(1) += 90.0*DEGTORAD; //create the objects that will hold the command changes in EEF position RRHandPose LinputHandVector(RRHandPose::EULER_ZYZ); RRHandPose LpreviousHandVector(RRHandPose::EULER_ZYZ); RRXform hand; ////////////////////////////////////////////////////////////////////////////// //Create the Manual Controller object ////////////////////////////////////////////////////////////////////////////// RRGeneralKeyboard keyboard; //define the change in hand position from keyboard input const RRVectorD deltas(6); for(i=0;i<3;i++){deltas.at(i) = 0.4; deltas.at(i+3) = .025;} ////////////////////////////////////////////////////////////////////////////// //Determine the initial hand position from the FK object. ////////////////////////////////////////////////////////////////////////////// RRFKPosition LeftFK(DHFile); (Llocation* *LeftFK.GetHandPose(LjointPosition)).ToHandPose(LinputHandVector); LpreviousHandVector = LinputHandVector; cout << "Initial Left Hand Vector: " << LinputHandVector << endl; char cont; cin >> cont; ////////////////////////////////////////////////////////////////////////////// //Create the inverse kinematic objects ////////////////////////////////////////////////////////////////////////////// RRIKJPartialFast Linverse(DHFile, LjointPosition); RRIKJPartialFast* LinversePtr = &Linverse; ////////////////////////////////////////////////////////////////////////////// //Create and set up the criteria for redundancy resolution ////////////////////////////////////////////////////////////////////////////// const int max = SOLUTIONNO; RRVectorArray solutions(max,7); RRVector validSolutions(max); RRFKJacobian jacobian(DHFile); RRInformation info(jacobian.GetDOF(), max); info.Create(&jacobian); ////////////////////////////////////////////////////////////////////////////// //Set up criteria for testing ////////////////////////////////////////////////////////////////////////////// RRPCRangeAvailability JRA(JRAPATH, max); RRPCMaximizeMOT TEST(max); RRPerformanceCriteriaP criteria[2]; criteria[0] = &JRA; criteria[0]->SetWeight(0.5); criteria[1] = &TEST; criteria[1]->SetWeight(0.5); RRCriteriaValue rnkdJRA[max]; RRCriteriaValue rnkdTEST[max]; RRCriteriaValue fusedValue[max]; RRPCNormalizedFusion fusion(criteria, 2, max); //declare the variables necessary to generate joint solution options //setup lockValues for initial use with Right arm RRVector lockedJoints(Linverse.GetDOF() - 6); RRVector lockedJointValues(Linverse.GetDOF()-6); cout << Linverse.GetDOF() << endl; lockedJoints.at(0) = 1; lockedJointValues.at(0) = LjointPosition.at(1); Linverse.LockJoints(lockedJoints,lockedJointValues); //create the options generator RRGenerateOptions generateOptions(LinversePtr, RRGenerateOptions::SEARCHTYPE, 0.01, 0); RRVector temp(7); ////////////////////////////////////////////////////////////////////////////// //BEGIN THE OPERATIONAL LOOP ////////////////////////////////////////////////////////////////////////////// while(!stopFlag) { kFlag = keyboard.UpdateVector(LinputHandVector, deltas); if(kFlag == -4){cout<<"ESC WAS PRESSED."< 0)//at least on valid solution exists { info.SetJointOptions(solutions, validSolutions); JRA.PickSolution(info, rnkdJRA); TEST.PickSolution(info, rnkdTEST); fusion.PickSolution(info, fusedValue); done = false; solutionNumber = 0; while(!done && solutionNumber < max){ if(fusedValue[solutionNumber].validity){ LjointPosition = (info.solutions)->at(fusedValue[solutionNumber].solutionNo); generateOptions.SetJointPosition(LjointPosition); Linverse.SetJointPosition(LjointPosition); LjointPosition.at(1) -= 90.0*DEGTORAD; done = true; } solutionNumber++; }//end of while(!validityCheck && solutionNumber.....) for(i=0;i<7;i++) jointsToSend.at(i+10) = LjointPosition.at(i); //Transmit the Joint Angles to the Robot. limitFlag = SendSetPoint(jointsToSend); if(limitFlag <= 0)//joint limit or excess error { if(limitFlag < 0) cout << "Joint limit reached for joint " << -limitFlag << endl; if(limitFlag ==0) cout << "Excess error reached " << endl; LinputHandVector = LpreviousHandVector; jointsToSend = previousJointsToSend; for(i=0;i<7;i++) LjointPosition.at(i) = jointsToSend.at(i+10); LjointPosition.at(1) += 90.0*DEGTORAD; Linverse.SetJointPosition(LjointPosition); }else{//new joint angles were accepted LpreviousHandVector = LinputHandVector; previousJointsToSend = jointsToSend; } }else{//generateOptions did not find a valid solution cout << "Generate options failed to find a valid solution." << endl; LinputHandVector = LpreviousHandVector; jointsToSend = previousJointsToSend; for(i=0;i<7;i++) LjointPosition.at(i) = jointsToSend.at(i+10); LjointPosition.at(1) += 90.0*DEGTORAD; Linverse.SetJointPosition(LjointPosition); }//end of else{//generate options failed..... }//end of if(!stopFlag) }//end of while(!stopFlag) /////////////////////////////////////////////////////////// //End of Operational Loop /////////////////////////////////////////////////////////// if(Disconnect() != NO_ERROR){ cout << "Error disconnecting" << endl; } return 1; }//end of int main(int..... /////////////////////////////////////////////////////////// //FUNCTION: SendSetPoint /////////////////////////////////////////////////////////// int SendSetPoint(RRVector& jointsToSend) { int i; static int firstFlag = 1; static float tagValues[noTags]; if(firstFlag){ //set up tag values to match initial joints values for(i=0;i<7;i++){ tagValues[i+3] = jointsToSend.at(i+10)*RADTODEG; tagValues[i+11] = jointsToSend.at(i)*RADTODEG; } tagValues[10] = 0.0; //Left Gripper tagValues[19] = 0.0; //Right Gripper for(i=0;i<3;i++){ tagValues[i] = jointsToSend.at(i+7)*RADTODEG; } firstFlag = 0; } for(i=0;i<7;i++){tagValues[i+3] = jointsToSend.at(i+10)*RADTODEG;} if(SetTagValues(TagNames, tagValues, noTags) != NO_ERROR){ cout << "Error writing" << endl; return -1; } return 1; }//end of SendSetPoint