MotionNodes.cc

Go to the documentation of this file.
00001 #include "Shared/RobotInfo.h"
00002 #ifdef TGT_HAS_ARMS
00003 
00004 #include "Crew/MotionNodes.h"
00005 #include "Crew/Grasper.h"   // needed for Grasper::curReq and Grasper::verbosity
00006 
00007 void SetJoint::moveJoint(float value) {
00008   unsigned int offset = 10;
00009   if(jointName == "ARM:shldr")
00010     offset = 0;
00011   else if(jointName == "ARM:elbow")
00012     offset = 1;
00013   else if(jointName == "ARM:wristYaw")
00014     offset = 2;
00015   else if(RobotInfo::NumArmJoints > 3) {
00016     if(jointName == "ARM:wristPitch")
00017       offset = 3;
00018     else if(jointName == "ARM:wristRoll" || jointName == "ARM:gripperLeft")
00019       offset = 4;
00020     else if(jointName == "ARM:gripper" || jointName == "ARM:gripperRight")
00021       offset = 5;
00022     else {
00023       postStateCompletion();
00024       return;
00025     }
00026   }
00027   else {
00028     postStateCompletion();
00029     return;
00030   }
00031   if ( (Grasper::curReq ? Grasper::curReq->verbosity : Grasper::verbosity) & Grasper::GVsetJoint )
00032     cout << "Setting " << jointName << " to " << value << endl;
00033   getMC()->takeSnapshot();
00034   getMC()->setMaxSpeed(offset, speed);   
00035   getMC()->setJointValue(offset, value);
00036 } 
00037 
00038 #endif