home *** CD-ROM | disk | FTP | other *** search
/ DOS/V Power Report 2003 March / VPR0303A.ISO / AIBO / MoNet / MotionAgents / NeutralAgent.cc < prev    next >
C/C++ Source or Header  |  2002-12-19  |  8KB  |  263 lines
  1. //
  2. // Copyright 2002 Sony Corporation 
  3. //
  4. // Permission to use, copy, modify, and redistribute this software for
  5. // non-commercial use is hereby granted.
  6. //
  7. // This software is provided "as is" without warranty of any kind,
  8. // either expressed or implied, including but not limited to the
  9. // implied warranties of fitness for a particular purpose.
  10. //
  11.  
  12. #include <OPENR/OPENRAPI.h>
  13. #include "NeutralAgent.h"
  14.  
  15. NeutralAgent::NeutralAgent() : neutralAgentState(NAS_IDLE)
  16. {
  17. }
  18.  
  19. void
  20. NeutralAgent::Init()
  21. {
  22. }
  23.  
  24. bool
  25. NeutralAgent::AreYou(MoNetAgentID agent)
  26. {
  27.     OSYSDEBUG(("NeutralAgent::AreYou(%d)\n", agent));
  28.     return (agent == monetagentNEUTRAL) ? true : false;
  29. }
  30.  
  31. void
  32. NeutralAgent::NotifyCommand(const MoNetAgentCommand& command,
  33.                             MoNetAgentResult* result)
  34. {
  35.     OSYSDEBUG(("NeutralAgent::NotifyCommand()\n"));
  36.     OSYSDEBUG(("  agent        : %d\n",   command.agent));
  37.     OSYSDEBUG(("  index        : %d\n",   command.index));
  38.     OSYSDEBUG(("  syncKey      : %08x\n", command.syncKey));
  39.     OSYSDEBUG(("  startPosture : %d\n",   command.startPosture));
  40.     OSYSDEBUG(("  endPosture   : %d\n",   command.endPosture));
  41.  
  42.     if (command.agent != monetagentNEUTRAL ||
  43.         command.index != monetagentNEUTRAL_NT2SLEEP) {
  44.  
  45.         result->agent      = command.agent;
  46.         result->index      = command.index;
  47.         result->status     = monetINVALID_ARG;
  48.         result->endPosture = monetpostureUNDEF;
  49.         return;
  50.     }
  51.  
  52.     AdjustDiffJointValue(command.syncKey);
  53.     neutralAgentState = NAS_ADJUSTING_DIFF_JOINT_VALUE;
  54.  
  55.     result->agent      = monetagentNEUTRAL;
  56.     result->index      = monetagentNEUTRAL_NT2SLEEP;
  57.     result->status     = monetSUCCESS;
  58.     result->endPosture = monetpostureUNDEF;
  59. }
  60.  
  61. void
  62. NeutralAgent::ReadyEffector(const MoNetAgentCommand& command,
  63.                             MoNetAgentResult* result)
  64. {
  65.     result->agent      = monetagentNEUTRAL;
  66.     result->index      = monetagentNEUTRAL_NT2SLEEP;
  67.     result->endPosture = monetpostureUNDEF;
  68.     
  69.     if (neutralAgentState == NAS_IDLE) {
  70.  
  71.         ; // do nothing
  72.  
  73.     } else if (neutralAgentState == NAS_ADJUSTING_DIFF_JOINT_VALUE) {
  74.  
  75.         OSYSDEBUG(("NAS_ADJUSTING_DIFF_JOINT_VALUE\n"));
  76.         SetJointGain();
  77.         MoNetStatus s = MoveToBroadBase();
  78.         neutralAgentState = NAS_MOVING_TO_BROADBASE;
  79.         result->status = monetSUCCESS;
  80.  
  81.     } else if (neutralAgentState == NAS_MOVING_TO_BROADBASE) {
  82.  
  83.         OSYSDEBUG(("NAS_MOVING_TO_BROADBASE\n"));
  84.         MoNetStatus s = MoveToBroadBase();
  85.         if (s == monetCOMPLETION) {
  86.             neutralAgentState = NAS_MOVING_TO_SLEEPING;
  87.         }
  88.         result->status = monetSUCCESS;
  89.  
  90.     } else if (neutralAgentState == NAS_MOVING_TO_SLEEPING) {
  91.  
  92.         OSYSDEBUG(("NAS_MOVING_TO_SLEEPING\n"));
  93.         MoNetStatus s = MoveToSleeping();
  94.         if (s == monetCOMPLETION) {
  95.             result->status     = monetCOMPLETION;
  96.             result->endPosture = monetpostureSLEEP;
  97.         } else {
  98.             result->status = monetSUCCESS;
  99.         }
  100.     }
  101. }
  102.  
  103. MoNetStatus
  104. NeutralAgent::AdjustDiffJointValue(OVRSyncKey syncKey)
  105. {
  106.     OJointValue current[NUM_JOINTS];
  107.     RCRegion* rgn = moNetAgentManager->FindFreeCommandRegion();
  108.     OCommandVectorData* cmdVecData = (OCommandVectorData*)rgn->Base();
  109.     cmdVecData->vectorInfo.syncKey = syncKey;
  110.  
  111.     for (int i = 0; i < NUM_JOINTS; i++) {
  112.         OJointValue current;
  113.         OPENR::GetJointValue(moNetAgentManager->PrimitiveID(i), ¤t);
  114.         SetJointValue(rgn, i,
  115.                       degrees(current.value/1000000.0),
  116.                       degrees(current.value/1000000.0));
  117.     }
  118.  
  119.     moNetAgentManager->Effector()->SetData(rgn);
  120.     moNetAgentManager->Effector()->NotifyObservers();
  121.  
  122.     return monetCOMPLETION;
  123. }
  124.  
  125. MoNetStatus
  126. NeutralAgent::MoveToBroadBase()
  127. {
  128.     static int counter = -1;
  129.     static double start[NUM_JOINTS];
  130.     static double delta[NUM_JOINTS];
  131.     double ndiv = (double)BROADBASE_MAX_COUNTER;
  132.  
  133.     if (counter == -1) {
  134.  
  135.         for (int i = 0; i < NUM_JOINTS; i++) {
  136.             OJointValue current;
  137.             OPrimitiveID jointID = moNetAgentManager->PrimitiveID(i);
  138.             OPENR::GetJointValue(jointID, ¤t);
  139.             start[i] = degrees(current.value/1000000.0);
  140.             delta[i] = (BROADBASE_ANGLE[i] - start[i]) / ndiv;
  141.         }
  142.  
  143.         counter = 0;
  144.  
  145.         RCRegion* rgn = moNetAgentManager->FindFreeCommandRegion();
  146.         for (int i = 0; i < NUM_JOINTS; i++) {
  147.             SetJointValue(rgn, i, start[i], start[i] + delta[i]);
  148.             start[i] += delta[i];
  149.         }
  150.  
  151.         moNetAgentManager->Effector()->SetData(rgn);
  152.         counter ++;
  153.     }
  154.  
  155.     RCRegion* rgn = moNetAgentManager->FindFreeCommandRegion();
  156.     for (int i = 0; i < NUM_JOINTS; i++) {
  157.         SetJointValue(rgn, i, start[i], start[i] + delta[i]);
  158.         start[i] += delta[i];
  159.     }
  160.  
  161.     moNetAgentManager->Effector()->SetData(rgn);
  162.     moNetAgentManager->Effector()->NotifyObservers();
  163.  
  164.     counter++;
  165.     return (counter == BROADBASE_MAX_COUNTER) ? monetCOMPLETION : monetSUCCESS;
  166. }
  167.  
  168. MoNetStatus
  169. NeutralAgent::MoveToSleeping()
  170. {
  171.     static int counter = -1;
  172.     static double start[NUM_JOINTS];
  173.     static double delta[NUM_JOINTS];
  174.     double ndiv = (double)SLEEPING_MAX_COUNTER;
  175.  
  176.     if (counter == -1) {
  177.  
  178.         for (int i = 0; i < NUM_JOINTS; i++) {
  179.             start[i] = BROADBASE_ANGLE[i];
  180.             delta[i] = (SLEEPING_ANGLE[i] - start[i]) / ndiv;
  181.         }
  182.  
  183.         counter = 0;
  184.     }
  185.  
  186.     RCRegion* rgn = moNetAgentManager->FindFreeCommandRegion();
  187.     for (int i = 0; i < NUM_JOINTS; i++) {
  188.         SetJointValue(rgn, i, start[i], start[i] + delta[i]);
  189.         start[i] += delta[i];
  190.     }
  191.  
  192.     moNetAgentManager->Effector()->SetData(rgn);
  193.     moNetAgentManager->Effector()->NotifyObservers();
  194.  
  195.     counter++;
  196.     return (counter == SLEEPING_MAX_COUNTER) ? monetCOMPLETION : monetSUCCESS;
  197. }
  198.  
  199. void
  200. NeutralAgent::SetJointGain()
  201. {
  202.     OPrimitiveID tiltID = moNetAgentManager->PrimitiveID(HEAD_TILT);
  203.     OPrimitiveID panID  = moNetAgentManager->PrimitiveID(HEAD_PAN);
  204.     OPrimitiveID rollID = moNetAgentManager->PrimitiveID(HEAD_ROLL);
  205.  
  206.     OPENR::EnableJointGain(tiltID);
  207.     OPENR::SetJointGain(tiltID,
  208.                         TILT_PGAIN, TILT_IGAIN, TILT_DGAIN,
  209.                         PSHIFT, ISHIFT, DSHIFT);
  210.     
  211.     OPENR::EnableJointGain(panID);
  212.     OPENR::SetJointGain(panID,
  213.                         PAN_PGAIN, PAN_IGAIN, PAN_DGAIN,
  214.                         PSHIFT, ISHIFT, DSHIFT);
  215.  
  216.     OPENR::EnableJointGain(rollID);
  217.     OPENR::SetJointGain(rollID,
  218.                         ROLL_PGAIN, ROLL_IGAIN, ROLL_DGAIN,
  219.                         PSHIFT, ISHIFT, DSHIFT);
  220.  
  221.     int base = RFLEG_J1;
  222.     for (int i = 0; i < 4; i++) {
  223.  
  224.         OPrimitiveID j1ID = moNetAgentManager->PrimitiveID(base + 3 * i);
  225.         OPrimitiveID j2ID = moNetAgentManager->PrimitiveID(base + 3 * i + 1);
  226.         OPrimitiveID j3ID = moNetAgentManager->PrimitiveID(base + 3 * i + 2);
  227.         
  228.         OPENR::EnableJointGain(j1ID);        
  229.         OPENR::SetJointGain(j1ID,
  230.                             J1_PGAIN, J1_IGAIN, J1_DGAIN,
  231.                             PSHIFT, ISHIFT, DSHIFT);
  232.  
  233.         OPENR::EnableJointGain(j2ID);
  234.         OPENR::SetJointGain(j2ID,
  235.                             J2_PGAIN, J2_IGAIN, J2_DGAIN,
  236.                             PSHIFT, ISHIFT, DSHIFT);
  237.  
  238.         OPENR::EnableJointGain(j3ID);
  239.         OPENR::SetJointGain(j3ID,
  240.                             J3_PGAIN, J3_IGAIN, J3_DGAIN,
  241.                             PSHIFT, ISHIFT, DSHIFT);
  242.     }
  243. }
  244.  
  245. void
  246. NeutralAgent::SetJointValue(RCRegion* rgn, int idx, double start, double end)
  247. {
  248.     OCommandVectorData* cmdVecData = (OCommandVectorData*)rgn->Base();
  249.  
  250.     OPrimitiveID jointID = moNetAgentManager->PrimitiveID(idx);
  251.     OCommandInfo* info = cmdVecData->GetInfo(idx);
  252.     info->Set(odataJOINT_COMMAND2, jointID, NUM_FRAMES);
  253.  
  254.     OCommandData* data = cmdVecData->GetData(idx);
  255.     OJointCommandValue2* jval = (OJointCommandValue2*)data->value;
  256.  
  257.     double delta = end - start;
  258.     for (int i = 0; i < NUM_FRAMES; i++) {
  259.         double dval = start + (delta * i) / (double)NUM_FRAMES;
  260.         jval[i].value = oradians(dval);
  261.     }
  262. }
  263.