Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

RoverControllerBehavior.cc

Go to the documentation of this file.
00001 #include "Shared/RobotInfo.h"
00002 #if ( defined(TGT_HAS_ARMS) || defined(TGT_HAS_LEGS) ) && defined(TGT_HAS_HEAD)
00003 
00004 #include "RoverControllerBehavior.h"
00005 
00006 #include "Motion/MMAccessor.h"
00007 #include <sstream>
00008 
00009 using namespace std; 
00010 
00011 void RoverControllerBehavior::DoStart() {
00012   BehaviorBase::DoStart(); // do this first (required)
00013   // open server socket
00014   cmdsock=wireless->socket(Socket::SOCK_STREAM, 2048, 2048);
00015   wireless->setReceiver(cmdsock, callback);
00016   wireless->setDaemon(cmdsock,true);
00017   wireless->listen(cmdsock, PORT);
00018   // Open the gui on the desktop
00019   Controller::loadGUI("org.tekkotsu.mon.RoverGUI","RoverGUI",PORT);
00020   // set up motion commands
00021   head = motman->addPersistentMotion(SharedObject<HeadPointerMC>());
00022   arm = motman->addPersistentMotion(SharedObject<PostureMC>());
00023 }
00024 
00025 void RoverControllerBehavior::DoStop() {
00026   // remove motion commands
00027   motman->removeMotion(head);
00028   head=MotionManager::invalid_MC_ID;
00029   motman->removeMotion(arm);
00030   arm=MotionManager::invalid_MC_ID;
00031   // Close the GUI
00032   Controller::closeGUI("RoverGUI");
00033   // close server socket
00034   wireless->setDaemon(cmdsock,false);
00035   wireless->close(cmdsock);
00036   BehaviorBase::DoStop(); // do this last (required)
00037 }
00038 
00039 void RoverControllerBehavior::initScale() {
00040   PostureEngine e;
00041   unsigned int gripperFrame=-1U, armBaseFrame=-1U;
00042 #ifdef TGT_HAS_ARMS
00043   gripperFrame=ArmOffset+NumArmJoints-1;
00044   armBaseFrame=ArmOffset;
00045 #elif defined(TGT_HAS_LEGS)
00046   gripperFrame=PawFrameOffset;
00047   armBaseFrame=LFrLegOffset;
00048 #else
00049   // RoverControllerBehavior can't find manipulator, won't run
00050   // (user should've seen the compiler warning when including the header...)
00051   return;
00052 #endif
00053   
00054   // find something resembling range of motion
00055   const float BIGNUM=1000000;
00056   e.solveLinkPosition(BIGNUM,0,0, gripperFrame, 0,0,0);
00057   NEWMAT::ColumnVector x = e.jointToBase(gripperFrame).SubMatrix(1,4,4,4);
00058   float max=ceil(x(1)/x(4));
00059   x = e.jointToBase(armBaseFrame).SubMatrix(1,4,4,4);
00060   tgtXOff=x(1)/x(4);
00061   tgtScale=(max-tgtXOff)/2;
00062   tgtXOff+=tgtScale;
00063   x = e.jointToBase(armBaseFrame+1).SubMatrix(1,4,4,4);
00064   tgtYOff=x(2)/x(4);
00065   std::cout << "Gripper XY reach (-1 to 1): " << tgtXOff-tgtScale << " to " << tgtXOff+tgtScale << std::endl;
00066   std::cout << "Gripper XY center: (" << tgtXOff << ',' << tgtYOff <<')' << std::endl;
00067   
00068 #ifdef TGT_HAS_ARMS
00069   // arm just goes up
00070   e.solveLinkPosition(0,0,BIGNUM, gripperFrame, 0,0,0);
00071   x = e.jointToBase(gripperFrame).SubMatrix(1,4,4,4);
00072   max=ceil(x(3)/x(4));
00073   x = e.jointToBase(armBaseFrame).SubMatrix(1,4,4,4);
00074   heightOff=x(3)/x(4);
00075   heightScale=max-heightOff;
00076 #elif defined(TGT_HAS_LEGS)
00077   // legs start at bottom
00078   e.solveLinkPosition(0,0,-BIGNUM, gripperFrame, 0,0,0);
00079   x = e.jointToBase(gripperFrame).SubMatrix(1,4,4,4);
00080   heightOff=floor(x(3)/x(4));
00081   x = e.jointToBase(armBaseFrame).SubMatrix(1,4,4,4);
00082   heightScale=(x(3)/x(4)-heightOff)*2;
00083 #endif
00084   std::cout << "Gripper Z reach (0 to 1):" << heightOff << " to " << heightOff+heightScale << std::endl;
00085   
00086   e.solveLinkPosition(0,0,-BIGNUM, gripperFrame, 0,0,0);
00087   e.solveLinkPosition(0,0,BIGNUM, CameraFrameOffset, 0,0,0);
00088   float ydist=e.jointToJoint(gripperFrame,CameraFrameOffset).SubMatrix(1,3,4,4).NormFrobenius();
00089   e.solveLinkPosition(BIGNUM,0,0, gripperFrame, 0,0,0);
00090   e.solveLinkPosition(-BIGNUM,0,0, CameraFrameOffset, 0,0,0);
00091   float xdist=e.jointToJoint(gripperFrame,CameraFrameOffset).SubMatrix(1,3,4,4).NormFrobenius();
00092   distanceScale = ceil(xdist>ydist ? xdist : ydist);
00093   std::cout << "Max camera distance is " << distanceScale << std::endl;
00094 }
00095 
00096 int RoverControllerBehavior::update(char *buf, int bytes) {
00097   stringstream cmd(string(buf,bytes));
00098   
00099   bool armDirty=false, headDirty=false;
00100   
00101   while(cmd) {
00102     string t;
00103     cmd >> t;
00104     bool gotArm=false, gotHead=false;
00105     if(gotHead=(t=="autoPerspective"))
00106       cmd >> autoPerspective;
00107     else if(gotArm=(t=="autoAngle"))
00108       cmd >> autoGripperAngle;
00109     else if(gotHead=(t=="autoTrack"))
00110       cmd >> autoTrackGripper;
00111     else if(gotHead=(t=="maxDist"))
00112       cmd >> maxDist;
00113     
00114     else if(t=="gripper") {
00115       float v;
00116       cmd >> v;
00117       unsigned int idx=-1U;
00118 #ifdef TGT_HAS_ARMS
00119       idx=ArmOffset+JointsPerArm-1;
00120 #elif defined(TGT_HAS_MOUTH)
00121       if(NumMouthJoints>0)
00122         idx=MouthOffset;
00123 #endif
00124       if(idx!=-1U) {
00125         float range = outputRanges[idx][MaxRange] - outputRanges[idx][MinRange];
00126         v = v*range + outputRanges[idx][MinRange];
00127         MMAccessor<PostureMC>(arm)->setOutputCmd(idx,v);
00128       }
00129     }
00130     else if(gotArm=(t=="gripperAngle"))
00131       cmd >> gripperAngle;
00132     else if(gotHead=(t=="perspective"))
00133       cmd >> perspective;
00134     else if(gotArm=(t=="gripperHeight")) {
00135       cmd >> tgtZ;
00136       tgtZ*=heightScale;
00137       tgtZ+=heightOff;
00138     }
00139     else if(gotHead=(t=="cameraDistance")) {
00140       cmd >> lookD;
00141       lookD*=distanceScale;
00142     }
00143 
00144     else if(gotArm=(t=="tgt")) {
00145       cmd >> tgtY >> tgtX;
00146       tgtY=-tgtY;
00147       tgtX*=tgtScale;
00148       tgtX+=tgtXOff;
00149       tgtY*=tgtScale;
00150       tgtY+=tgtYOff;
00151     }
00152     else if(gotHead=(t=="look")) {
00153       cmd >> lookY >> lookX;
00154       lookY=-lookY;
00155       lookX*=tgtScale;
00156       lookX+=tgtXOff;
00157       lookY*=tgtScale;
00158       lookY+=tgtYOff;
00159     }
00160     
00161     armDirty = armDirty || gotArm;
00162     headDirty = headDirty || gotHead;
00163   }
00164   if(autoTrackGripper) {
00165     lookX=tgtX;
00166     lookY=tgtY;
00167     headDirty = headDirty || armDirty;
00168   }
00169   
00170   if(armDirty)
00171     updateArm();
00172   
00173   if(headDirty)
00174     updateHead();
00175   
00176   return 0;
00177 }
00178 
00179 RoverControllerBehavior * RoverControllerBehavior::theOne=NULL;
00180 
00181 void RoverControllerBehavior::updateArm() {
00182 #ifdef TGT_HAS_ARMS
00183   MMAccessor<PostureMC>(arm)->solveLinkPosition(tgtX,tgtY,tgtZ,ArmOffset+NumArmJoints-1,0,0,0);
00184 #elif defined(TGT_HAS_LEGS)
00185   MMAccessor<PostureMC>(arm)->solveLinkPosition(tgtX,tgtY,tgtZ,PawFrameOffset,0,0,0);
00186 #endif
00187 }
00188 
00189 void RoverControllerBehavior::updateHead() {
00190   if(maxDist)
00191     MMAccessor<HeadPointerMC>(head)->lookAtPoint(lookX,lookY,tgtZ);
00192   else
00193     MMAccessor<HeadPointerMC>(head)->lookAtPoint(lookX,lookY,tgtZ,lookD);
00194 }
00195 
00196 
00197 /*! @file
00198  * @brief 
00199  * @author Ethan Tira-Thompson (ejt) (Creator)
00200  *
00201  * $Author: ejt $
00202  * $Name: tekkotsu-4_0 $
00203  * $Revision: 1.5 $
00204  * $State: Exp $
00205  * $Date: 2007/11/18 06:47:01 $
00206  */
00207 
00208 #endif // check for (arms or hands) and legs

Tekkotsu v4.0
Generated Thu Nov 22 00:54:55 2007 by Doxygen 1.5.4