C++ SimObj::getRotation方法代码示例

void CameraController::rotateTowardRobot(Vector3d r_pos)
{
  m_my->setPosition(m_rotatePos);
  // カメラの位置を得る
  m_my->getPosition(m_pos);

  // カメラの位置からロボットを結ぶベクトル
  Vector3d tmpp = r_pos;
  tmpp -= m_pos;

  // y方向は考えない
  tmpp.y(0);

  // カメラの回転角度を得る
  Rotation myRot;
  m_my->getRotation(myRot);

  // カメラのy軸の回転角度を得る(x,z方向の回転は無いと仮定)
  double qw = myRot.qw();
  double qy = myRot.qy();
  double theta = 2*acos(fabs(qw));

  if(qw*qy < 0)
    theta = -1*theta;

  // ロボットまでの回転角度を得る
  double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
  double targetAngle = acos(tmp);
  if(tmpp.x() > 0) targetAngle = -1*targetAngle;

  // 角度差から回転量を得る
  targetAngle += theta;
  m_my->setAxisAndAngle(0, 1, 0, -targetAngle, 0);
}

void MyController::onInit(InitEvent &evt) {  

  // handle of target and tool
  SimObj *toolName  = getObj("TShapeTool_7");
  SimObj *target = getObj("box_7");

  toolName->setMass(10.0); // mass of all the tools should be set uniformly
  
  target->getPosition(initTargetPos);  // initial target position
  target->getRotation(initTargetRotation); // initial target rotation in quaternion

  toolName->getPosition(initToolPos);  // initial tool position
  toolName->getRotation(initToolRotation); // initial tool rotation in quaternion

  isTargetAtRest = true;     // target and tool both are at rest initially.
  isToolAtRest = true;  
  insideTimer = true; 
  f_x = 0 ;
  f_z = 0 ;

  int xForceVariance = 4000;
  int zForceVariance = 4000;

  flag = true;
  Colli = false; 
  counterOfCollision = 0; 
  counterOfAction = 0; 


  // Reset the forces applied
  double r;
  r = ((double) rand() / (RAND_MAX)) ;
  f_x = r * int(xForceVariance);
  f_z = r * int(zForceVariance);

  forceOnTool_z = 8000 + f_z;
  forceOnTool_x = 0;

  forceOnTool.set(forceOnTool_x, 0 , forceOnTool_z);

  toolName->addForce(forceOnTool.x(), forceOnTool.y(), forceOnTool.z());
  toolName->getVelocity(appliedToolVel);

  myfile.flush(); // I have uncommented the file, because I want to overwrite the file.
  

}  

double MyController::onAction(ActionEvent &evt) {  

  int actionNumber = 2;
  int functionalFeature = 1;
  int targetType = 3;

  myfile << setprecision(2) << std::fixed;

  // handle of target and tool
  SimObj *target = getObj("box_004");
  SimObj *toolName = getObj("TShapeTool_004");


  if (evt.time() < 5.0)
  {
    // cout << "Time" << endl;
    cout << evt.time() << endl;

    toolName->getPosition(currentToolPos); // get the current tool position
    toolName->getRotation(finalToolRotation);
    toolName->getVelocity(finalToolVel);
    isToolAtRest   = checkEntityMotionStatus(toolName);  // checks whether the tool is moving by calculating its velocity

    target->getPosition(currentTargetPos);
    target->getRotation(finalTargetRotation);
    target->getVelocity(finalTargetVel);
    isTargetAtRest = checkEntityMotionStatus(target);    // checks whether the object is moving by calculating its velocity
    
  }

  if (evt.time() > 5.0)
  {
     insideTimer = false;
     counterOfAction ++ ; 
  }

  if(!insideTimer && counterOfAction == 1 )
  {
           myfile << actionNumber  << " , " << functionalFeature << " , " ;
           myfile << initToolRotation.qw() <<  " , " << initToolRotation.qx() <<  " , " << initToolRotation.qy() <<  " , " << initToolRotation.qz() << " , " ;
           myfile << initTargetRotation.qw() <<  " , " << initTargetRotation.qx() <<  " , " << initTargetRotation.qy() <<  " , " << initTargetRotation.qz() << " , " ;
           myfile << finalTargetRotation.qw() <<  " , " << finalTargetRotation.qx() <<  " , " << finalToolRotation.qy() <<  " , " << finalToolRotation.qz() << " , " ;
           myfile << initToolPos.x() << " , " << initToolPos.z() << " , " ;
           myfile << initTargetPos.x() << " , " << initTargetPos.z() << " , " ;
           myfile << forceOnTool_x  << " , " << forceOnTool_z << " , " ;
           myfile << appliedToolVel.x()  << " , " << appliedToolVel.z() << " , " ;
           myfile << toolVelAtHit.x() << " , " << toolVelAtHit.z() << " , " ;
           myfile << targetVelAtHit.x() << " , " << targetVelAtHit.z() << " , " ;
           myfile << currentToolPos.x() << " , " << currentToolPos.z() << " , " ;
           myfile << currentTargetPos.x() << " , " << currentTargetPos.z() << " , " ;
           myfile << finalToolVel.x() << " , " << finalToolVel.z() << " , " ;
           myfile << finalTargetVel.x() << " , " << finalTargetVel.z() << " , " ;
           myfile   << isToolAtRest <<  " , " << isTargetAtRest << " , " ;
           myfile << currentToolPos.x() -  initToolPos.x() << " , " << currentToolPos.z() - initToolPos.z() << " , " ;
           myfile << currentTargetPos.x() -  initTargetPos.x() << " , " << currentTargetPos.z() - initTargetPos.z();
           myfile << "\n"; 
           cout << "The simulation for " << actionNumber << " , " << functionalFeature << " has been recorded" << endl;
           // exit(0);
           flag = false;  
  }

      
  return 0.01;    

 }

void MyController::onInit(InitEvent &evt) {  

 
  SimObj *target = getObj("box_015");
  SimObj *toolName = getObj("TShapeTool_015");


  toolName->setMass(10.0);
  
  target->getPosition(initTargetPos);
  target->getRotation(initTargetRotation);

  toolName->getPosition(initToolPos);
  toolName->getRotation(initToolRotation);

  isTargetAtRest = true;   
  isToolAtRest = true;  
  f_x = 0 ;
  f_z = 0 ;

  int xForceVariance = 2000;
  int zForceVariance = 2000;


  // Reset the forces applied
  double r;
  r = ((double) rand() / (RAND_MAX)) ;
  f_x = r * int(xForceVariance);
  f_z = r * int(zForceVariance);

  flag = true;
  Colli = false; 
  counter = 0; 

  forceOnTool_x = 5000 + f_z;
  forceOnTool_z = 5000 + f_z;

  // cout << "The applied force " << forceOnTool_x <<  " , " << forceOnTool_z << endl;


  forceOnTool.set(forceOnTool_x, 0 , forceOnTool_z);
  tapWithTool(toolName, initToolRotation, forceOnTool);

  myfile.flush(); // I have uncommented the file, because I want to overwrite the file. 


//   if (myfile.is_open())
//   {
//      myfile << "Action" << " , " << "FunctionalFeature" << " , " ;
//      myfile << "forceOnTool_X" << " , " << "forceOnTool_Z " <<  " , ";
//      myfile << "toolVelAtHit_X" << " , " << "toolVelAtHit_Z" << " , ";  
//      myfile << "InitialToolPos_X" << " , " << "InitialToolPos_Z" << " , ";
//      myfile << "InitialTargetPos_X" << " , " << "InitialTargetPos_Z" << " , ";
//      myfile <<  "targetFinalPos_X" << " , " << "targetFinalPos_Z"  << " , ";
//      myfile << "targetDisplacement_X" << " , " << "targetDisplacement_Z";
//      myfile <<  "\n" ;

// }


}