Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

HeadPointerMC.cc

Go to the documentation of this file.
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), // if change default tolerance, update documentation in header
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/*=1*/) {
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   //  float theta = acos(poE(3)/sqrt(poE(1)*poE(1)+poE(2)*poE(2)+poE(3)*poE(3)));
00104   //  cout << "Computed:\n theta1: " << mathutils::rad2deg(theta) << " degrees\n";
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; //.975 is a bit of fudge - accounts for joints moving Plink when adjusting
00111   else
00112     obj_comp_link=obj_comp_link/.975; //.975 is a bit of fudge - accounts for joints moving Plink when adjusting
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   //  NEWMAT::ColumnVector poE2=headkin.convertLink(0,headkin.get_dof())*Pobj;
00122   //  float theta2 = acos(poE2(3)/sqrt(poE2(1)*poE2(1)+poE2(2)*poE2(2)+poE2(3)*poE2(3)));
00123   //  cout << " theta2: " << mathutils::rad2deg(theta2) << " degrees\n";
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   //  return conv;
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   //  return conv;
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 { // we may be trying to exceeded maxSpeed
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) { //we reached target value, fill in rest of frames
00183           headCmds[i].value=headTargets[i];
00184           for(;f<NumFrames;f++)
00185             motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00186         } else // we didn't reach target value, still dirty
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)) { //prevents a conflicted HeadPointerMC's from fighting forever
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; //not state->outputs[HeadOffset+i]; - see function documentation
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 /*! @file
00252  * @brief Implements HeadPointerMC, a class for various ways to control where the head is looking
00253  * @author ejt (Creator)
00254  *
00255  * $Author: ejt $
00256  * $Name: tekkotsu-4_0 $
00257  * $Revision: 1.29 $
00258  * $State: Exp $
00259  * $Date: 2007/11/18 06:47:03 $
00260  */

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