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 {
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) {
00091 headCmds[i].value=headTargets[i];
00092 for(;f<NumFrames;f++)
00093 motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00094 } else
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
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122