Tekkotsu Homepage

Demos

Overview

Downloads

Dev. Resources

Reference

Credits

HeadPointerMC.cc Go to the documentation of this file. #include <math.h> #include "HeadPointerMC.h" #include "Kinematics.h" #include "Shared/debuget.h" #include "Shared/WorldState.h" #include "MotionManager.h" #include "Shared/Config.h" #include "Wireless/Socket.h" HeadPointerMC::HeadPointerMC() : MotionCommand(), dirty(true), hold(true), tolerance(.05), targetReached(true), targetTimestamp(0), timeout(2000), headkin(::config->motion.makePath(::config->motion.kinematics),"Camera") { setWeight(1); defaultMaxSpeed(); takeSnapshot(); } void HeadPointerMC::freezeMotion() { for(unsigned int i=0; i<NumHeadJoints; i++) headTargets[i]=headCmds[i].value; dirty=false; } void HeadPointerMC::takeSnapshot() { for(unsigned int i=0; i<NumHeadJoints; i++) headTargets[i]=headCmds[i].value=state->outputs[HeadOffset+i]; dirty=true; } void HeadPointerMC::defaultMaxSpeed(float x/*=1*/) { maxSpeed[TiltOffset]=config->motion.max_head_tilt_speed*FrameTime*x/1000; maxSpeed[PanOffset]=config->motion.max_head_pan_speed*FrameTime*x/1000; maxSpeed[RollOffset]=config->motion.max_head_roll_speed*FrameTime*x/1000; } void HeadPointerMC::setWeight(float w) { for(unsigned int x=0; x<NumHeadJoints; x++) headCmds[x].weight=w; markDirty(); } void HeadPointerMC::setJoints(float j1, float j2, float j3) { headTargets[TiltOffset]=clipAngularRange(HeadOffset+TiltOffset,j1); headTargets[PanOffset]=clipAngularRange(HeadOffset+PanOffset,j2); headTargets[RollOffset]=clipAngularRange(HeadOffset+RollOffset,j3); markDirty(); } bool HeadPointerMC::lookAtPoint(float x, float y, float z) { NEWMAT::ColumnVector Pobj(4),Plink(4); Pobj(1)=x; Pobj(2)=y; Pobj(3)=z; Pobj(4)=1; Plink=0; Plink(3)=1; bool conv=false; NEWMAT::ColumnVector q=headkin.inv_kin_pos(Pobj,0,headkin.get_dof(),Plink,conv); NEWMAT::ColumnVector poE=headkin.convertLink(0,headkin.get_dof())*Pobj; poE=poE.SubMatrix(1,3,1,1); // float theta = acos(poE(3)/sqrt(poE(1)*poE(1)+poE(2)*poE(2)+poE(3)*poE(3))); // cout << "Computed:\n theta1: " << mathutils::rad2deg(theta) << " degrees\n"; NEWMAT::ColumnVector plE=Plink.SubMatrix(1,3,1,1); float plE2=plE.SumSquare(); float plE_len=sqrt(plE2); float obj_comp_link=NEWMAT::DotProduct(plE,poE)/plE_len; if(obj_comp_link<plE_len) obj_comp_link=obj_comp_link*.975; //.975 is a bit of fudge - accounts for joints moving Plink when adjusting else obj_comp_link=obj_comp_link/.975; //.975 is a bit of fudge - accounts for joints moving Plink when adjusting NEWMAT::ColumnVector obj_proj_link(4); obj_proj_link.SubMatrix(1,3,1,1)=obj_comp_link*plE/plE_len; obj_proj_link(4)=1; q=headkin.inv_kin_pos(Pobj,0,headkin.get_dof(),obj_proj_link,conv); for(unsigned int i=0; i<NumHeadJoints; i++) setJointValue(i,headkin.get_q(2+i)); // NEWMAT::ColumnVector poE2=headkin.convertLink(0,headkin.get_dof())*Pobj; // float theta2 = acos(poE2(3)/sqrt(poE2(1)*poE2(1)+poE2(2)*poE2(2)+poE2(3)*poE2(3))); // cout << " theta2: " << mathutils::rad2deg(theta2) << " degrees\n"; return isReachable(Pobj); } bool HeadPointerMC::lookAtPoint(float x, float y, float z, float d) { NEWMAT::ColumnVector Pobj(4),Plink(4); Pobj(1)=x; Pobj(2)=y; Pobj(3)=z; Pobj(4)=1; Plink=0; Plink(3)=d; Plink(4)=1; bool conv=false; NEWMAT::ColumnVector q=headkin.inv_kin_pos(Pobj,0,headkin.get_dof(),Plink,conv); for(unsigned int i=0; i<NumHeadJoints; i++) setJointValue(i,headkin.get_q(2+i)); // return conv; return isReachable(Pobj); } bool HeadPointerMC::lookInDirection(float x, float y, float z) { NEWMAT::ColumnVector Pobj(4),Plink(4); Pobj(1)=x; Pobj(2)=y; Pobj(3)=z; Pobj(4)=0; Plink=0; Plink(3)=1; bool conv=false; NEWMAT::ColumnVector q=headkin.inv_kin_pos(Pobj,0,headkin.get_dof(),Plink,conv); for(unsigned int i=0; i<NumHeadJoints; i++) setJointValue(i,headkin.get_q(2+i)); // return conv; return isReachable(Pobj); } int HeadPointerMC::updateOutputs() { int tmp=isDirty(); if(tmp || hold) { dirty=false; for(unsigned int i=0; i<NumHeadJoints; i++) { if(maxSpeed[i]<=0) { headCmds[i].value=headTargets[i]; motman->setOutput(this,i+HeadOffset,headCmds[i]); } else { // we may be trying to exceeded maxSpeed unsigned int f=0; while(headTargets[i]>headCmds[i].value+maxSpeed[i] && f<NumFrames) { headCmds[i].value+=maxSpeed[i]; motman->setOutput(this,i+HeadOffset,headCmds[i],f); f++; } while(headTargets[i]<headCmds[i].value-maxSpeed[i] && f<NumFrames) { headCmds[i].value-=maxSpeed[i]; motman->setOutput(this,i+HeadOffset,headCmds[i],f); f++; } if(f<NumFrames) { //we reached target value, fill in rest of frames headCmds[i].value=headTargets[i]; for(;f<NumFrames;f++) motman->setOutput(this,i+HeadOffset,headCmds[i],f); } else // we didn't reach target value, still dirty dirty=true; } } if(!dirty && !targetReached) { postEvent(EventBase(EventBase::motmanEGID,getID(),EventBase::statusETID)); targetReached=true; targetTimestamp=get_time(); } } return tmp; } int HeadPointerMC::isAlive() { if(dirty || !targetReached) return true; if(targetReached && (!hold || get_time()-targetTimestamp>timeout)) { //prevents a conflicted HeadPointerMC's from fighting forever if(get_time()-targetTimestamp>timeout && getAutoPrune()) serr->printf("WARNING: HeadPointerMC (mcid %d) timed out - possible joint conflict or out-of-range target\n",getID()); return false; } float maxdiff=0; for(unsigned int i=0; i<NumHeadJoints; i++) { float diff=fabsf(state->outputs[HeadOffset+i]-headTargets[i]); if(diff>maxdiff) maxdiff=diff; } return (maxdiff>tolerance); } void HeadPointerMC::markDirty() { dirty=true; targetReached=false; for(unsigned int i=0; i<NumHeadJoints; i++) headCmds[i].value=motman->getOutputCmd(HeadOffset+i).value; //not state->outputs[HeadOffset+i]; - see function documentation } bool HeadPointerMC::ensureValidJoint(unsigned int& i) { if(i<NumHeadJoints) return true; if(i>=HeadOffset && i<HeadOffset+NumHeadJoints) { i-=HeadOffset; serr->printf("WARNING: HeadPointerMC received a joint index of %d (HeadOffset+%d).\n",i+HeadOffset,i); serr->printf(" Since all parameters are assumed to be relative to HeadOffset,\n"); serr->printf(" you should just pass %d directly.\n",i); serr->printf("WARNING: Assuming you meant %d...\n",i); return true; } serr->printf("ERROR: HeadPointerMC received a joint index of %d (HeadOffset%+d).\n",i,i-HeadOffset); serr->printf("ERROR: This does not appear to be a head joint. HeadPointerMC only controls\n"); serr->printf(" head joints, and assumes its arguments are relative to HeadOffset\n"); return false; } /*! @file * @brief Implements HeadPointerMC, a class for various ways to control where the head is looking * @author ejt (Creator) * * $Author: ejt $ * $Name: tekkotsu-3_0 $ * $Revision: 1.25 $ * $State: Exp $ * $Date: 2006/08/31 21:42:02 $ */

Tekkotsu v3.0

Generated Wed Oct 4 00:03:43 2006 by Doxygen 1.4.7