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

Tekkotsu v2.2.2
Generated Tue Jan 4 15:43:14 2005 by Doxygen 1.4.0