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 
00008 HeadPointerMC::HeadPointerMC()
00009   : MotionCommand(), dirty(true), targetReached(false),
00010     headkin(::config->motion.makePath(::config->motion.kinematics),"Camera")
00011 {
00012   setWeight(1);
00013   defaultMaxSpeed();
00014   for(unsigned int i=0; i<NumHeadJoints; i++)
00015     headTargets[i]=headCmds[i].value=state->outputs[HeadOffset+i];
00016 }
00017 
00018 void HeadPointerMC::defaultMaxSpeed() {
00019   maxSpeed[TiltOffset]=config->motion.max_head_tilt_speed*FrameTime/1000;
00020   maxSpeed[PanOffset]=config->motion.max_head_pan_speed*FrameTime/1000;
00021   maxSpeed[RollOffset]=config->motion.max_head_roll_speed*FrameTime/1000;
00022 }
00023 
00024 void HeadPointerMC::setWeight(float w) {
00025   for(unsigned int x=0; x<NumHeadJoints; x++)
00026     headCmds[x].weight=w;
00027   markDirty();
00028 }
00029 
00030 void HeadPointerMC::setJoints(float j1, float j2, float j3) {
00031   headTargets[TiltOffset]=clipAngularRange(HeadOffset+TiltOffset,j1);
00032   headTargets[PanOffset]=clipAngularRange(HeadOffset+PanOffset,j2);
00033   headTargets[RollOffset]=clipAngularRange(HeadOffset+RollOffset,j3);
00034   markDirty();
00035 }
00036 
00037 bool HeadPointerMC::lookAtPoint(float x, float y, float z) {
00038   NEWMAT::ColumnVector Pobj(4),Plink(4);
00039   Pobj(1)=x; Pobj(2)=y; Pobj(3)=z; Pobj(4)=1;
00040   Plink=0; Plink(3)=1;
00041   bool conv=false;
00042   NEWMAT::ColumnVector q=headkin.inv_kin_pos(Pobj,0,headkin.get_dof(),Plink,conv);
00043   for(unsigned int i=0; i<NumHeadJoints; i++)
00044     setJointValue(i,headkin.get_q(2+i));
00045   return conv;
00046 }
00047   
00048 bool HeadPointerMC::lookAtPoint(float x, float y, float z, float d) {
00049   NEWMAT::ColumnVector Pobj(4),Plink(4);
00050   Pobj(1)=x; Pobj(2)=y; Pobj(3)=z; Pobj(4)=1;
00051   Plink=0; Plink(3)=d; Plink(4)=1;
00052   bool conv=false;
00053   NEWMAT::ColumnVector q=headkin.inv_kin_pos(Pobj,0,headkin.get_dof(),Plink,conv);
00054   for(unsigned int i=0; i<NumHeadJoints; i++)
00055     setJointValue(i,headkin.get_q(2+i));
00056   return conv;
00057 }
00058   
00059 bool HeadPointerMC::lookInDirection(float x, float y, float z) {
00060   NEWMAT::ColumnVector Pobj(4),Plink(4);
00061   Pobj(1)=x; Pobj(2)=y; Pobj(3)=z; Pobj(4)=0;
00062   Plink=0; Plink(3)=1;
00063   bool conv=false;
00064   NEWMAT::ColumnVector q=headkin.inv_kin_pos(Pobj,0,headkin.get_dof(),Plink,conv);
00065   for(unsigned int i=0; i<NumHeadJoints; i++)
00066     setJointValue(i,headkin.get_q(2+i));
00067   return conv;
00068 }
00069 
00070 int HeadPointerMC::updateOutputs() {
00071   int tmp=isDirty();
00072   if(tmp) {
00073     dirty=false;
00074     for(unsigned int i=0; i<NumHeadJoints; i++) {
00075       if(maxSpeed[i]<=0) {
00076         headCmds[i].value=headTargets[i];
00077         motman->setOutput(this,i+HeadOffset,headCmds[i]);
00078       } else { // we may be trying to exceeded maxSpeed
00079         unsigned int f=0;
00080         while(headTargets[i]>headCmds[i].value+maxSpeed[i] && f<NumFrames) {
00081           headCmds[i].value+=maxSpeed[i];
00082           motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00083           f++;
00084         }
00085         while(headTargets[i]<headCmds[i].value-maxSpeed[i] && f<NumFrames) {
00086           headCmds[i].value-=maxSpeed[i];
00087           motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00088           f++;
00089         }
00090         if(f<NumFrames) { //we reached target value, fill in rest of frames
00091           headCmds[i].value=headTargets[i];
00092           for(;f<NumFrames;f++)
00093             motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00094         } else // we didn't reach target value, still dirty
00095           dirty=true;
00096       }
00097     }
00098     if(!dirty && !targetReached) {
00099       postEvent(EventBase(EventBase::motmanEGID,getID(),EventBase::statusETID));
00100       targetReached=true;
00101     }
00102   }
00103   return tmp;
00104 }
00105 
00106 void HeadPointerMC::markDirty() {
00107   dirty=true;
00108   targetReached=false;
00109 }
00110 
00111 /*! @file
00112  * @brief Implements HeadPointerMC, a class for various ways to control where the head is looking
00113  * @author ejt (Creator)
00114  *
00115  * $Author: ejt $
00116  * $Name: tekkotsu-2_2_1 $
00117  * $Revision: 1.14 $
00118  * $State: Exp $
00119  * $Date: 2004/10/14 21:59:23 $
00120  */
00121 
00122 

Tekkotsu v2.2.1
Generated Tue Nov 23 16:36:38 2004 by Doxygen 1.3.9.1