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