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) {
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;
00054 else
00055 obj_comp_link=obj_comp_link/.975;
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 {
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) {
00109 headCmds[i].value=headTargets[i];
00110 for(;f<NumFrames;f++)
00111 motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00112 } else
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) {
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;
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
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177