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();
00013
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
00019 Controller::loadGUI("org.tekkotsu.mon.RoverGUI","RoverGUI",PORT);
00020
00021 head = motman->addPersistentMotion(SharedObject<HeadPointerMC>());
00022 arm = motman->addPersistentMotion(SharedObject<PostureMC>());
00023 }
00024
00025 void RoverControllerBehavior::DoStop() {
00026
00027 motman->removeMotion(head);
00028 head=MotionManager::invalid_MC_ID;
00029 motman->removeMotion(arm);
00030 arm=MotionManager::invalid_MC_ID;
00031
00032 Controller::closeGUI("RoverGUI");
00033
00034 wireless->setDaemon(cmdsock,false);
00035 wireless->close(cmdsock);
00036 BehaviorBase::DoStop();
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
00050
00051 return;
00052 #endif
00053
00054
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
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
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
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208 #endif // check for (arms or hands) and legs