Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

HeadPointerMC.cc

Go to the documentation of this file.
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 #include "Wireless/Socket.h"
00008 
00009 HeadPointerMC::HeadPointerMC()
00010   : MotionCommand(), dirty(true), hold(true), tolerance(.035),
00011     targetReached(false), targetTimestamp(0), timeout(2000), 
00012     headkin(::config->motion.makePath(::config->motion.kinematics),"Camera")
00013 {
00014   setWeight(1);
00015   defaultMaxSpeed();
00016   for(unsigned int i=0; i<NumHeadJoints; i++)
00017     headTargets[i]=headCmds[i].value=state->outputs[HeadOffset+i];
00018 }
00019 
00020 void HeadPointerMC::defaultMaxSpeed(float x/*=1*/) {
00021   maxSpeed[TiltOffset]=config->motion.max_head_tilt_speed*FrameTime*x/1000;
00022   maxSpeed[PanOffset]=config->motion.max_head_pan_speed*FrameTime*x/1000;
00023   maxSpeed[RollOffset]=config->motion.max_head_roll_speed*FrameTime*x/1000;
00024 }
00025 
00026 void HeadPointerMC::setWeight(float w) {
00027   for(unsigned int x=0; x<NumHeadJoints; x++)
00028     headCmds[x].weight=w;
00029   markDirty();
00030 }
00031 
00032 void HeadPointerMC::setJoints(float j1, float j2, float j3) {
00033   headTargets[TiltOffset]=clipAngularRange(HeadOffset+TiltOffset,j1);
00034   headTargets[PanOffset]=clipAngularRange(HeadOffset+PanOffset,j2);
00035   headTargets[RollOffset]=clipAngularRange(HeadOffset+RollOffset,j3);
00036   markDirty();
00037 }
00038 
00039 bool HeadPointerMC::lookAtPoint(float x, float y, float z) {
00040   NEWMAT::ColumnVector Pobj(4),Plink(4);
00041   Pobj(1)=x; Pobj(2)=y; Pobj(3)=z; Pobj(4)=1;
00042   Plink=0; Plink(3)=1;
00043   bool conv=false;
00044   NEWMAT::ColumnVector q=headkin.inv_kin_pos(Pobj,0,headkin.get_dof(),Plink,conv);
00045 
00046   NEWMAT::ColumnVector poE=headkin.convertLink(0,headkin.get_dof())*Pobj;
00047   poE=poE.SubMatrix(1,3,1,1);
00048   NEWMAT::ColumnVector plE=Plink.SubMatrix(1,3,1,1);
00049   float plE2=plE.SumSquare();
00050   float plE_len=sqrt(plE2);
00051   float obj_comp_link=NEWMAT::DotProduct(plE,poE)/plE_len;
00052   if(obj_comp_link<plE_len)
00053     obj_comp_link=obj_comp_link*.975; //.975 is a bit of fudge - accounts for joints moving Plink when adjusting
00054   else
00055     obj_comp_link=obj_comp_link/.975; //.975 is a bit of fudge - accounts for joints moving Plink when adjusting
00056   NEWMAT::ColumnVector obj_proj_link(4);
00057   obj_proj_link.SubMatrix(1,3,1,1)=obj_comp_link*plE/plE_len;
00058   obj_proj_link(4)=1;
00059   q=headkin.inv_kin_pos(Pobj,0,headkin.get_dof(),obj_proj_link,conv);
00060 
00061   for(unsigned int i=0; i<NumHeadJoints; i++)
00062     setJointValue(i,headkin.get_q(2+i));
00063   return conv;
00064 }
00065   
00066 bool HeadPointerMC::lookAtPoint(float x, float y, float z, float d) {
00067   NEWMAT::ColumnVector Pobj(4),Plink(4);
00068   Pobj(1)=x; Pobj(2)=y; Pobj(3)=z; Pobj(4)=1;
00069   Plink=0; Plink(3)=d; Plink(4)=1;
00070   bool conv=false;
00071   NEWMAT::ColumnVector q=headkin.inv_kin_pos(Pobj,0,headkin.get_dof(),Plink,conv);
00072   for(unsigned int i=0; i<NumHeadJoints; i++)
00073     setJointValue(i,headkin.get_q(2+i));
00074   return conv;
00075 }
00076   
00077 bool HeadPointerMC::lookInDirection(float x, float y, float z) {
00078   NEWMAT::ColumnVector Pobj(4),Plink(4);
00079   Pobj(1)=x; Pobj(2)=y; Pobj(3)=z; Pobj(4)=0;
00080   Plink=0; Plink(3)=1;
00081   bool conv=false;
00082   NEWMAT::ColumnVector q=headkin.inv_kin_pos(Pobj,0,headkin.get_dof(),Plink,conv);
00083   for(unsigned int i=0; i<NumHeadJoints; i++)
00084     setJointValue(i,headkin.get_q(2+i));
00085   return conv;
00086 }
00087 
00088 int HeadPointerMC::updateOutputs() {
00089   int tmp=isDirty();
00090   if(tmp || hold) {
00091     dirty=false;
00092     for(unsigned int i=0; i<NumHeadJoints; i++) {
00093       if(maxSpeed[i]<=0) {
00094         headCmds[i].value=headTargets[i];
00095         motman->setOutput(this,i+HeadOffset,headCmds[i]);
00096       } else { // we may be trying to exceeded maxSpeed
00097         unsigned int f=0;
00098         while(headTargets[i]>headCmds[i].value+maxSpeed[i] && f<NumFrames) {
00099           headCmds[i].value+=maxSpeed[i];
00100           motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00101           f++;
00102         }
00103         while(headTargets[i]<headCmds[i].value-maxSpeed[i] && f<NumFrames) {
00104           headCmds[i].value-=maxSpeed[i];
00105           motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00106           f++;
00107         }
00108         if(f<NumFrames) { //we reached target value, fill in rest of frames
00109           headCmds[i].value=headTargets[i];
00110           for(;f<NumFrames;f++)
00111             motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00112         } else // we didn't reach target value, still dirty
00113           dirty=true;
00114       }
00115     }
00116     if(!dirty && !targetReached) {
00117       postEvent(EventBase(EventBase::motmanEGID,getID(),EventBase::statusETID));
00118       targetReached=true;
00119       targetTimestamp=get_time();
00120     }
00121   }
00122   return tmp;
00123 }
00124 
00125 int HeadPointerMC::isAlive() {
00126   if(dirty || !targetReached)
00127     return true;
00128   if(targetReached && get_time()-targetTimestamp>timeout) { //prevents a conflicted HeadPointerMC's from fighting forever
00129     if(getAutoPrune())
00130       serr->printf("WARNING: HeadPointerMC timed out - possible joint conflict or out-of-range target\n");
00131     return false;
00132   }
00133   float maxdiff=0;
00134   for(unsigned int i=0; i<NumHeadJoints; i++) {
00135     float diff=fabsf(state->outputs[HeadOffset+i]-headTargets[i]);
00136     if(diff>maxdiff)
00137       maxdiff=diff;
00138   }
00139   return (maxdiff>tolerance);
00140 }
00141 
00142 void HeadPointerMC::markDirty() {
00143   dirty=true;
00144   targetReached=false;
00145   for(unsigned int i=0; i<NumHeadJoints; i++)
00146     headCmds[i].value=motman->getOutputCmd(HeadOffset+i).value; //not state->outputs[HeadOffset+i]; - see function documentation
00147 }
00148 
00149 bool HeadPointerMC::ensureValidJoint(unsigned int& i) {
00150   if(i<NumHeadJoints)
00151     return true;
00152   if(i>=HeadOffset && i<HeadOffset+NumHeadJoints) {
00153     i-=HeadOffset;
00154     serr->printf("WARNING: HeadPointerMC received a joint index of %d (HeadOffset+%d).\n",i+HeadOffset,i);
00155     serr->printf("         Since all parameters are assumed to be relative to HeadOffset,\n");
00156     serr->printf("         you should just pass %d directly.\n",i);
00157     serr->printf("WARNING: Assuming you meant %d...\n",i);
00158     return true;
00159   }
00160   serr->printf("ERROR: HeadPointerMC received a joint index of %d (HeadOffset%+d).\n",i,i-HeadOffset);
00161   serr->printf("ERROR: This does not appear to be a head joint.  HeadPointerMC only controls\n");
00162   serr->printf("       head joints, and assumes its arguments are relative to HeadOffset\n");
00163   return false;
00164 }
00165 
00166 /*! @file
00167  * @brief Implements HeadPointerMC, a class for various ways to control where the head is looking
00168  * @author ejt (Creator)
00169  *
00170  * $Author: ejt $
00171  * $Name: tekkotsu-2_4_1 $
00172  * $Revision: 1.19 $
00173  * $State: Exp $
00174  * $Date: 2005/01/25 20:06:42 $
00175  */
00176 
00177 

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:47 2005 by Doxygen 1.4.4