OldHeadPointerMC.ccGo to the documentation of this file.00001 #include "OldHeadPointerMC.h"
00002 #include "Shared/debuget.h"
00003 #include "Shared/WorldState.h"
00004 #include "MotionManager.h"
00005 #include <math.h>
00006 #include "Shared/Config.h"
00007
00008 OldHeadPointerMC::OldHeadPointerMC() :
00009 MotionCommand(), dirty(true), active(true), targetReached(false) {
00010 setWeight(1);
00011 defaultMaxSpeed();
00012 for(unsigned int i=0; i<NumHeadJoints; i++) {
00013 headModes[i]=BodyRelative;
00014 headTargets[i]=0;
00015 headCmds[i].value=state->outputs[HeadOffset+i];
00016 }
00017 }
00018
00019 void OldHeadPointerMC::setJoints(double tilt, double pan, double roll) {
00020 setJointValue(TiltOffset,tilt);
00021 setJointValue(PanOffset,pan);
00022 setJointValue(RollOffset,roll);
00023 }
00024
00025 void OldHeadPointerMC::setWeight(double w) {
00026 for(unsigned int x=0; x<NumHeadJoints; x++)
00027 headCmds[x].weight=w;
00028 dirty=true; targetReached=false;
00029 }
00030
00031 void OldHeadPointerMC::defaultMaxSpeed() {
00032 maxSpeed[TiltOffset]=config->motion.max_head_tilt_speed*FrameTime/1000;
00033 maxSpeed[PanOffset]=config->motion.max_head_pan_speed*FrameTime/1000;
00034 maxSpeed[RollOffset]=config->motion.max_head_roll_speed*FrameTime/1000;
00035 }
00036
00037 void OldHeadPointerMC::setMode(CoordFrame_t m, bool conv) {
00038 for(unsigned int x=0; x<NumHeadJoints; x++)
00039 setJointMode((RobotInfo::TPROffset_t)x,m,conv);
00040 }
00041
00042 void OldHeadPointerMC::setJointMode(RobotInfo::TPROffset_t i, CoordFrame_t m, bool conv) {
00043 if(conv)
00044 headTargets[i]=OldHeadPointerMC::convert(i,headTargets[i],headModes[i],m);
00045 headModes[i]=m;
00046 dirty=true; targetReached=false;
00047 }
00048
00049 int OldHeadPointerMC::updateOutputs() {
00050 int tmp=isDirty();
00051 if(tmp) {
00052 dirty=false;
00053 for(unsigned int i=0; i<NumHeadJoints; i++) {
00054 float value;
00055 if(headModes[i]!=BodyRelative) {
00056 value=convToBodyRelative((RobotInfo::TPROffset_t)i,headTargets[i],headModes[i]);
00057 dirty=true;
00058 } else
00059 value=headTargets[i];
00060 if(maxSpeed[i]<=0)
00061 headCmds[i].value=value;
00062 if(value==headCmds[i].value) {
00063 motman->setOutput(this,i+HeadOffset,headCmds[i]);
00064 } else {
00065 unsigned int f=0;
00066 while(value>headCmds[i].value+maxSpeed[i] && f<NumFrames) {
00067 headCmds[i].value+=maxSpeed[i];
00068 motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00069 f++;
00070 }
00071 while(value<headCmds[i].value-maxSpeed[i] && f<NumFrames) {
00072 headCmds[i].value-=maxSpeed[i];
00073 motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00074 f++;
00075 }
00076 if(f<NumFrames) {
00077 headCmds[i].value=value;
00078 for(;f<NumFrames;f++)
00079 motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00080 } else
00081 dirty=true;
00082 }
00083 }
00084 float dist=0;
00085 for(unsigned int i=0; i<NumHeadJoints; i++) {
00086 float diff=(state->outputs[HeadOffset+i]-headTargets[i]);
00087 dist+=diff*diff;
00088 }
00089 if(dist<0.05*0.05 && !targetReached) {
00090 postEvent(EventBase(EventBase::motmanEGID,getID(),EventBase::statusETID));
00091 targetReached=true;
00092 }
00093 }
00094 return tmp;
00095 }
00096
00097 const OutputCmd& OldHeadPointerMC::getOutputCmd(unsigned int i) {
00098 i-=HeadOffset;
00099 if(i<NumHeadJoints && getActive())
00100 return headCmds[i];
00101 else
00102 return OutputCmd::unused;
00103 }
00104
00105
00106 double OldHeadPointerMC::convToBodyRelative(TPROffset_t i, double v, CoordFrame_t mode) const {
00107 switch(mode) {
00108 default:
00109 ASSERT(false,"Passed bad mode "<<mode);
00110 case BodyRelative:
00111 return v;
00112 case GravityRelative: {
00113 double bacc=state->sensors[BAccelOffset];
00114 double lacc=state->sensors[LAccelOffset];
00115 double dacc=state->sensors[DAccelOffset];
00116 switch(i) {
00117 case TiltOffset:
00118 return v+atan2(bacc,sqrt(dacc*dacc+lacc*lacc));
00119 case PanOffset:
00120 return v;
00121 case RollOffset:
00122 if(state->robotDesign&WorldState::ERS7Mask) {
00123 float pans=sin(state->outputs[HeadOffset+PanOffset]);
00124 float panc=cos(state->outputs[HeadOffset+PanOffset]);
00125 float ans=v;
00126 ans+=atan2(bacc*panc-lacc*pans,sqrt(dacc*dacc+lacc*lacc*panc*panc+bacc*bacc*pans*pans));
00127 ans-=panc*state->outputs[HeadOffset+TiltOffset];
00128 return ans;
00129 } else
00130 return v+atan2(-lacc,sqrt(dacc*dacc+bacc*bacc));
00131 default:
00132 ASSERT(false,"Passed bad offset "<<i<<" (use RobotInfo::TPROffset_t!)");
00133 return v;
00134 }
00135 }
00136 }
00137 }
00138
00139
00140 double OldHeadPointerMC::convFromBodyRelative(TPROffset_t i, double v, CoordFrame_t mode) const {
00141 switch(mode) {
00142 default:
00143 ASSERT(false,"Passed bad mode "<<mode);
00144 case BodyRelative:
00145 return v;
00146 case GravityRelative: {
00147 double bacc=state->sensors[BAccelOffset];
00148 double lacc=state->sensors[LAccelOffset];
00149 double dacc=state->sensors[DAccelOffset];
00150 switch(i) {
00151 case TiltOffset:
00152 return v-atan2(bacc,sqrt(dacc*dacc+lacc*lacc));
00153 case PanOffset:
00154 return v;
00155 case RollOffset:
00156 if(state->robotDesign&WorldState::ERS7Mask) {
00157 float pans=sin(state->outputs[HeadOffset+PanOffset]);
00158 float panc=cos(state->outputs[HeadOffset+PanOffset]);
00159 float ans=v;
00160 ans-=atan2(bacc*panc-lacc*pans,sqrt(dacc*dacc+lacc*lacc*panc*panc+bacc*bacc*pans*pans));
00161 ans+=panc*state->outputs[HeadOffset+TiltOffset];
00162 return ans;
00163 } else
00164 return v-atan2(-lacc,sqrt(dacc*dacc+bacc*bacc));
00165 default:
00166 ASSERT(false,"Passed bad offset "<<i<<" (use RobotInfo::TPROffset_t!)");
00167 return v;
00168 }
00169 }
00170 }
00171 }
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
|