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;
00053 else
00054 obj_comp_link=obj_comp_link/.975;
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 {
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) {
00108 headCmds[i].value=headTargets[i];
00109 for(;f<NumFrames;f++)
00110 motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00111 } else
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
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156