00001 #include "HeadPointerMC.h"
00002 #include "Kinematics.h"
00003 #include "Shared/debuget.h"
00004 #include "Shared/WorldState.h"
00005 #include "MotionManager.h"
00006 #include "Shared/Config.h"
00007 #include "Wireless/Socket.h"
00008 #include "Shared/ERS7Info.h"
00009 #include "Shared/ERS210Info.h"
00010 #include <math.h>
00011
00012 HeadPointerMC::HeadPointerMC()
00013 : MotionCommand(), dirty(true), hold(true), tolerance(.05),
00014 targetReached(true), targetTimestamp(0), timeout(2000),
00015 headkin(::config->motion.makePath(::config->motion.kinematics),"Camera")
00016 {
00017 setWeight(1);
00018 defaultMaxSpeed();
00019 takeSnapshot();
00020 }
00021
00022 void HeadPointerMC::freezeMotion() {
00023 #ifdef TGT_HAS_HEAD
00024 for(unsigned int i=0; i<NumHeadJoints; i++)
00025 headTargets[i]=headCmds[i].value;
00026 dirty=false;
00027 #endif
00028 }
00029
00030 void HeadPointerMC::takeSnapshot() {
00031 #ifdef TGT_HAS_HEAD
00032 for(unsigned int i=0; i<NumHeadJoints; i++)
00033 headTargets[i]=headCmds[i].value=state->outputs[HeadOffset+i];
00034 dirty=true;
00035 #endif
00036 }
00037
00038 void HeadPointerMC::defaultMaxSpeed(float x) {
00039 #ifdef TGT_HAS_HEAD
00040 const char* n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::TiltOffset];
00041 unsigned int i = capabilities.findOutputOffset(n);
00042 if(i!=-1U)
00043 maxSpeed[i-HeadOffset]=config->motion.max_head_tilt_speed*FrameTime*x/1000;
00044 n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::PanOffset];
00045 i = capabilities.findOutputOffset(n);
00046 if(i!=-1U)
00047 maxSpeed[i-HeadOffset]=config->motion.max_head_pan_speed*FrameTime*x/1000;
00048 n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::NodOffset];
00049 i = capabilities.findOutputOffset(n);
00050 if(i!=-1U)
00051 maxSpeed[i-HeadOffset]=config->motion.max_head_roll_speed*FrameTime*x/1000;
00052 n = ERS210Info::outputNames[ERS210Info::HeadOffset+ERS210Info::RollOffset];
00053 i = capabilities.findOutputOffset(n);
00054 if(i!=-1U)
00055 maxSpeed[i-HeadOffset]=config->motion.max_head_roll_speed*FrameTime*x/1000;
00056 #endif
00057 }
00058
00059 void HeadPointerMC::setWeight(float w) {
00060 #ifdef TGT_HAS_HEAD
00061 for(unsigned int x=0; x<NumHeadJoints; x++)
00062 headCmds[x].weight=w;
00063 markDirty();
00064 #endif
00065 }
00066
00067 void HeadPointerMC::setJoints(float tilt1, float pan, float tilt2) {
00068 #ifdef TGT_HAS_HEAD
00069 #ifdef TGT_IS_AIBO
00070 headTargets[TiltOffset]=clipAngularRange(HeadOffset+TiltOffset,tilt1);
00071 headTargets[PanOffset]=clipAngularRange(HeadOffset+PanOffset,pan);
00072 headTargets[NodOffset]=clipAngularRange(HeadOffset+NodOffset,tilt2);
00073 #else
00074 const char* n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::TiltOffset];
00075 unsigned int i = capabilities.findOutputOffset(n);
00076 if(i!=-1U)
00077 headTargets[i-HeadOffset]=clipAngularRange(i,tilt1);
00078 n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::PanOffset];
00079 i = capabilities.findOutputOffset(n);
00080 if(i!=-1U)
00081 headTargets[i-HeadOffset]=clipAngularRange(i,pan);
00082 n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::NodOffset];
00083 i = capabilities.findOutputOffset(n);
00084 if(i!=-1U)
00085 headTargets[i-HeadOffset]=clipAngularRange(i,tilt2);
00086 #endif
00087 markDirty();
00088 #endif
00089 }
00090
00091 bool HeadPointerMC::lookAtPoint(float x, float y, float z) {
00092 #ifndef TGT_HAS_HEAD
00093 return false;
00094 #else
00095 NEWMAT::ColumnVector Pobj(4),Plink(4);
00096 Pobj(1)=x; Pobj(2)=y; Pobj(3)=z; Pobj(4)=1;
00097 Plink=0; Plink(3)=1;
00098 bool conv=false;
00099 NEWMAT::ColumnVector q=headkin.inv_kin_pos(Pobj,0,headkin.get_dof(),Plink,conv);
00100
00101 NEWMAT::ColumnVector poE=headkin.convertLink(0,headkin.get_dof())*Pobj;
00102 poE=poE.SubMatrix(1,3,1,1);
00103
00104
00105 NEWMAT::ColumnVector plE=Plink.SubMatrix(1,3,1,1);
00106 float plE2=plE.SumSquare();
00107 float plE_len=sqrt(plE2);
00108 float obj_comp_link=NEWMAT::DotProduct(plE,poE)/plE_len;
00109 if(obj_comp_link<plE_len)
00110 obj_comp_link=obj_comp_link*.975;
00111 else
00112 obj_comp_link=obj_comp_link/.975;
00113 NEWMAT::ColumnVector obj_proj_link(4);
00114 obj_proj_link.SubMatrix(1,3,1,1)=obj_comp_link*plE/plE_len;
00115 obj_proj_link(4)=1;
00116 q=headkin.inv_kin_pos(Pobj,0,headkin.get_dof(),obj_proj_link,conv);
00117
00118 for(unsigned int i=0; i<NumHeadJoints; i++)
00119 setJointValue(i,headkin.get_q(2+i));
00120
00121
00122
00123
00124 return isReachable(Pobj);
00125 #endif
00126 }
00127
00128 bool HeadPointerMC::lookAtPoint(float x, float y, float z, float d) {
00129 #ifndef TGT_HAS_HEAD
00130 return false;
00131 #else
00132 NEWMAT::ColumnVector Pobj(4),Plink(4);
00133 Pobj(1)=x; Pobj(2)=y; Pobj(3)=z; Pobj(4)=1;
00134 Plink=0; Plink(3)=d; Plink(4)=1;
00135 bool conv=false;
00136 NEWMAT::ColumnVector q=headkin.inv_kin_pos(Pobj,0,headkin.get_dof(),Plink,conv);
00137 for(unsigned int i=0; i<NumHeadJoints; i++)
00138 setJointValue(i,headkin.get_q(2+i));
00139
00140 return isReachable(Pobj);
00141 #endif
00142 }
00143
00144 bool HeadPointerMC::lookInDirection(float x, float y, float z) {
00145 #ifndef TGT_HAS_HEAD
00146 return false;
00147 #else
00148 NEWMAT::ColumnVector Pobj(4),Plink(4);
00149 Pobj(1)=x; Pobj(2)=y; Pobj(3)=z; Pobj(4)=0;
00150 Plink=0; Plink(3)=1;
00151 bool conv=false;
00152 NEWMAT::ColumnVector q=headkin.inv_kin_pos(Pobj,0,headkin.get_dof(),Plink,conv);
00153 for(unsigned int i=0; i<NumHeadJoints; i++)
00154 setJointValue(i,headkin.get_q(2+i));
00155
00156 return isReachable(Pobj);
00157 #endif
00158 }
00159
00160
00161 int HeadPointerMC::updateOutputs() {
00162 int tmp=isDirty();
00163 if(tmp || hold) {
00164 dirty=false;
00165 #ifdef TGT_HAS_HEAD
00166 for(unsigned int i=0; i<NumHeadJoints; i++) {
00167 if(maxSpeed[i]<=0) {
00168 headCmds[i].value=headTargets[i];
00169 motman->setOutput(this,i+HeadOffset,headCmds[i]);
00170 } else {
00171 unsigned int f=0;
00172 while(headTargets[i]>headCmds[i].value+maxSpeed[i] && f<NumFrames) {
00173 headCmds[i].value+=maxSpeed[i];
00174 motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00175 f++;
00176 }
00177 while(headTargets[i]<headCmds[i].value-maxSpeed[i] && f<NumFrames) {
00178 headCmds[i].value-=maxSpeed[i];
00179 motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00180 f++;
00181 }
00182 if(f<NumFrames) {
00183 headCmds[i].value=headTargets[i];
00184 for(;f<NumFrames;f++)
00185 motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00186 } else
00187 dirty=true;
00188 }
00189 }
00190 #endif
00191 if(!dirty && !targetReached) {
00192 postEvent(EventBase(EventBase::motmanEGID,getID(),EventBase::statusETID));
00193 targetReached=true;
00194 targetTimestamp=get_time();
00195 }
00196 }
00197 return tmp;
00198 }
00199
00200 int HeadPointerMC::isAlive() {
00201 #ifndef TGT_HAS_HEAD
00202 return false;
00203 #else
00204 if(dirty || !targetReached)
00205 return true;
00206 if(targetReached && (!hold || get_time()-targetTimestamp>timeout)) {
00207 if(get_time()-targetTimestamp>timeout && getAutoPrune())
00208 serr->printf("WARNING: HeadPointerMC (mcid %d) timed out - possible joint conflict or out-of-range target\n",getID());
00209 return false;
00210 }
00211 float maxdiff=0;
00212 for(unsigned int i=0; i<NumHeadJoints; i++) {
00213 float diff=fabsf(state->outputs[HeadOffset+i]-headTargets[i]);
00214 if(diff>maxdiff)
00215 maxdiff=diff;
00216 }
00217 return (maxdiff>tolerance);
00218 #endif
00219 }
00220
00221 void HeadPointerMC::markDirty() {
00222 dirty=true;
00223 targetReached=false;
00224 #ifdef TGT_HAS_HEAD
00225 for(unsigned int i=0; i<NumHeadJoints; i++)
00226 headCmds[i].value=motman->getOutputCmd(HeadOffset+i).value;
00227 #endif
00228 }
00229
00230 bool HeadPointerMC::ensureValidJoint(unsigned int& i) {
00231 #ifndef TGT_HAS_HEAD
00232 serr->printf("ERROR: HeadPointerMC received a joint index of %d on headless target.\n",i);
00233 #else
00234 if(i<NumHeadJoints)
00235 return true;
00236 if(i>=HeadOffset && i<HeadOffset+NumHeadJoints) {
00237 i-=HeadOffset;
00238 serr->printf("WARNING: HeadPointerMC received a joint index of %d (HeadOffset+%d).\n",i+HeadOffset,i);
00239 serr->printf(" Since all parameters are assumed to be relative to HeadOffset,\n");
00240 serr->printf(" you should just pass %d directly.\n",i);
00241 serr->printf("WARNING: Assuming you meant %d...\n",i);
00242 return true;
00243 }
00244 serr->printf("ERROR: HeadPointerMC received a joint index of %d (HeadOffset%+d).\n",i,i-HeadOffset);
00245 serr->printf("ERROR: This does not appear to be a head joint. HeadPointerMC only controls\n");
00246 serr->printf(" head joints, and assumes its arguments are relative to HeadOffset\n");
00247 #endif
00248 return false;
00249 }
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260