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

double MyController::onAction(ActionEvent &evt)
{
	robotObj = getObj(robotName.c_str());
	operatorObj = getObj(operatorName.c_str());

	Vector3d robotPos;
	robotObj->getPosition(robotPos);

	// 位置取得
	Vector3d operatorPos;
	operatorObj->getPosition(operatorPos);

	Vector3d vec(operatorPos.x()-robotPos.x(), operatorPos.y()-robotPos.y(), operatorPos.z()-robotPos.z());

	double distance = sqrt((vec.x()*vec.x())+(vec.z()*vec.z()));

	if(distance>=WAIT_DISTANCE){
		if(follow){
			std::string msg = "NotFollowing";
			sendMsg(operatorName.c_str(),msg);
			follow = false;
			//LOG_MSG((msg.c_str()));
		}
	}
	else{
		if(!follow){
			std::string msg = "Following";
			sendMsg(operatorName.c_str(),msg);
			follow = true;
			//LOG_MSG((msg.c_str()));
		}
	}

	return 0.1;
}

bool MyController::recognizeNearestTrash(Vector3d &pos, std::string &name)
{
  /////////////////////////////////////////////
  ///////////ここでゴミを認識します////////////
  /////////////////////////////////////////////

  // 候補のゴミが無い場合
  if(m_trashes.empty()){
    return false;
  } /*else {
		printf("m_trashes.size(): %d \n", m_trashes.size());
	}*/

	bool found = false;

  // 自分の位置の取得
  Vector3d myPos;
  m_my->getPosition(myPos);
	
	// もっと近いゴミを検索する	
  double dis_min = 10000000000000.0;
	double distance;
	double min_idx;

	// 現位置に検出出来るゴミを検索する
	for(int trashNum = 0; trashNum < m_trashes.size(); trashNum++) {
		name = m_trashes[trashNum];
		if(getObj(name.c_str())) {
			SimObj *trash = getObj(name.c_str());
			//printf("created SimObj Trash \n");

			// ゴミの位置取得
			trash->getPosition(pos);
			//printf("ゴミの位置：　%lf %lf %lf \n", pos.x(), pos.y(), pos.z());	 

			distance = (myPos.x() - pos.x()) * (myPos.x() - pos.x()) + 
								 (myPos.z() - pos.z()) * (myPos.z() - pos.z()); 
			if (distance < dis_min) {
				min_idx = trashNum;
				dis_min = distance;
			}				
			found = true;
		}	else {
			//printf("cannot find obj with such name \n");
		}
	}

	if (found == true) {
			name = m_trashes[min_idx];
			printf("nearestObj trash ^^^: %s \n", name.c_str());
			SimObj *trash = getObj(name.c_str());
			// ゴミの位置取得
			trash->getPosition(pos);
	} else {
		return false;
	}

  return found;
}

double MyController::onAction(ActionEvent &evt) {  


  int actionNumber = 5;
  int functionalFeature = 3;
  myfile << setprecision(2) << std::fixed;

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

  
  
  
  maintainOrientationOfTool(toolName, initToolRotation);

  isToolAtRest   = checkEntityMotionStatus(toolName);  // checks whether the tool is moving by calculating its velocity
  isTargetAtRest = checkEntityMotionStatus(target);    // checks whether the object is moving by calculating its velocity

  if(isToolAtRest)
  {
     toolName->getPosition(currentToolPos);
  }

      
 if (isTargetAtRest && isToolAtRest)
 {

    target->getPosition(currentTargetPos);        

 }

if (isTargetAtRest && isToolAtRest && flag )
       {
          
           myfile << actionNumber  << " , " << functionalFeature << " , " ;
           myfile << forceOnTool_x  << " , " << forceOnTool_z << " , " ;
           myfile << initToolPos.x() << " , " << initToolPos.z() << " , " ;
           myfile << initTargetPos.x() << " , " << initTargetPos.z() << " , " ;
           myfile << currentTargetPos.x() << " , " << currentTargetPos.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 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.
  

}  

void DemoRobotController::recognizeObjectPosition(Vector3d &pos, std::string &name)
{
	// get object of trash selected
	SimObj *trash = getObj(name.c_str());

	// get trash's position
	trash->getPosition(pos);
}

void MyController::onInit(InitEvent &evt) {  


SimObj *stick = getObj("robot_test");
// stick->addForce(-5000,0,5000);

stick->getPosition(startPosition);
// stick->getRotation(initialToolRot);
 
}  

void MyController::confirmThrewTrashPos(Vector3d &pos, std::string &name) {
	if(getObj(name.c_str())) {
		SimObj *trash = getObj(name.c_str());
		//printf("created SimObj Trash \n");

		// ゴミの位置取得
		trash->getPosition(pos);
		//printf("捨てた座標：　%lf %lf %lf \n", pos.x(), pos.y(), pos.z());	 
	}	else {
		//printf("cannot find trashbox with such name \n");
	}
	return;
}

double MyController::onAction(ActionEvent &evt)
{  

    int count=0;
    Vector3d pos;


    if(start==true){
                while(count<20){
                    if(sw == false){
                        my->getPosition(pos);
                        my->setPosition(pos.x(),pos.y(),pos.z()-10);
                        my->setJointAngle("LARM_JOINT1", DEG2RAD(-30)); 
                        my->setJointAngle("RLEG_JOINT2", DEG2RAD(-20));
                        my->setJointAngle("RLEG_JOINT4", DEG2RAD(10));   
                        usleep(100000);
                        my->setJointAngle("LARM_JOINT1", DEG2RAD(0));  
                        my->setJointAngle("RLEG_JOINT2", DEG2RAD(0)); 
                        my->setJointAngle("RLEG_JOINT4", DEG2RAD(0)); 
                        sw = true;
                    }else{
                        my->getPosition(pos);
                        my->setPosition(pos.x(),pos.y(),pos.z()-10);
                        my->setJointAngle("RARM_JOINT1", DEG2RAD(-30)); 
                        my->setJointAngle("LLEG_JOINT2", DEG2RAD(-20));  
                        my->setJointAngle("LLEG_JOINT4", DEG2RAD(10));  
                        usleep(100000);
                        my->setJointAngle("RARM_JOINT1", DEG2RAD(0));  
                        my->setJointAngle("LLEG_JOINT2", DEG2RAD(0)); 
                        my->setJointAngle("LLEG_JOINT4", DEG2RAD(0));  
                        sw = false;
                    }
                    count++;
                    start=false;
               }
    }

    return 0.1;      
}   

std::string RobotController::getPointedTrashName(std::string entName)
{
  // 発話者の名前からSimObjを取得します
  SimObj *tobj = getObj(entName.c_str());

  // メッセージ送信者の左肘関節の位置を取得します
  Vector3d jpos;
  if(!tobj->getJointPosition(jpos, "RARM_JOINT4")) {
    LOG_ERR(("failed to get joint position"));
    return "";
  }
  // メッセージ送信者の左肘から左手首をつなぐベクトルを取得します
  Vector3d jvec;
  if(!tobj->getPointingVector(jvec, "RARM_JOINT4", "RARM_JOINT7")) {
    LOG_ERR(("failed to get pointing vector"));
    return "";
  }

  double distance = 0.0;
  std::string objName = "";

  // 全ゴミオブジェクトでループします
  int trashboxSize = m_trashboxs.size();
  for(int i = 0; i < trashboxSize; i++) {

  // エンティティの位置を取得します
  SimObj *obj = getObj(m_trashboxs[i].c_str());
  Vector3d objVec;
  obj->getPosition(objVec);

  // エンティティと左肘関節を結ぶベクトルを作成します
  objVec -= jpos;

  // cos角度が不の場合（指差した方向と反対側にある場合)は対象から外します
  double cos = jvec.angle(objVec);
  if(cos < 0)
    continue;

  // 指差した方向ベクトルまでの最短距離の計算
  double theta = acos(cos);
  double tmp_distance = sin(theta) * objVec.length();

  // 最小距離の場合は名前、距離を保存しておく
  if(tmp_distance < distance || distance == 0.0){
    distance = tmp_distance;
    objName = obj->name();
    }
  }
  // エンティティでループして最も近いオブジェクトの名前を取得する
  return objName;
}

void MyController::onInit(InitEvent &evt) {  
	m_my = getObj(myname());

	// この範囲で判定
	checkSize_x  = 40.0;
	checkSize_z  = 80.0; 

	// 自分の位置取得
	m_my->getPosition(myPos);

	sentMsg = false;
	flag1 = false;
	flag2 = false;
}

bool RobotController::recognizeTrash(Vector3d &pos, std::string &name)
{
  // 候補のゴミが無い場合
  if(m_trashes.empty()){
    return false;
  }

  // ここでは乱数を使ってゴミを決定します
  int trashNum = rand() % m_trashes.size();

  // ゴミの名前と位置を取得します
  name = m_trashes[trashNum];
  SimObj *trash = getObj(name.c_str());

  // ゴミの位置取得
  trash->getPosition(pos);
  return true;
}

void RobotController::grasp_left_hand()
{
	Vector3d hand, object;
	SimObj *obj = getObj(m_pointedObject.c_str());

	obj->getPosition(object);
	my->getJointPosition(hand, "LARM_JOINT7");

	double distance = getDist3D(hand,object);

	if (distance < GRASPABLE_DISTANCE &&  m_grasp_left == false)
	{
		CParts * parts = my->getParts("LARM_LINK7");
		if (parts->graspObj(m_pointedObject))
		{
			m_grasp_left = true;
			//  broadcastMsg("Object_grasped");
		}
	}
}

void CameraController::onInit(InitEvent &evt)
{
  // カメラ初期方向　1:45 2:135 3:315 4:225
  // ロボット初期位置取得
  r_my = getRobotObj("robot_004");
  r_my->getPosition(r_pos);

  // カメラ番号取得
  m_my = getObj(myname());
  m_my->getPosition(m_rotatePos);
  m_pathPos = Vector3d(m_rotatePos.x() -500, m_rotatePos.y() + 1700, m_rotatePos.z());
  m_name = m_my->name();

  configuringPath = false;

  // 定点カメラをロボットの方向に回転
  // rotateTowardRobot(r_pos);
  rotateTowardGround();

}

/* ゴミの名前から、捨てるべきゴミ箱の名前を検索し、そのゴミ箱の位置を探す
 * @return pos 捨てるべきの位置
 * @param trashName ゴミの名前
 */
bool MyController::findPlace2PutObj(Vector3d &pos, std::string trashName)
{
  /////////////////////////////////////////////
  ///////////ここでゴミを認識します////////////
  /////////////////////////////////////////////

  // 候補のゴミが無い場合
  if(m_trashBoxes.empty()){
    return false;
  } else {
		//printf("m_trashBoxes.size(): %d \n", m_trashBoxes.size());
	}

	std::string trashBoxName = "";
	std::map<std::string, std::string>::iterator it = m_trashTypeMap.begin();
	trashBoxName = m_trashTypeMap.find(trashName)->second;
	std::cout << trashName << " => " << trashBoxName << '\n';
	bool trashBoxExist = false;

	for(int i = 0; i < m_trashBoxes.size(); i++) {
		if(m_trashBoxes[i] == trashBoxName) {
			trashBoxExist = true;			
		}
	}	

	if(trashBoxExist) {
		if(getObj(trashBoxName.c_str())) {
			SimObj *place = getObj(trashBoxName.c_str());
			// ゴミの位置取得
			place->getPosition(pos);
			//printf("ゴミ箱の位置：　%lf %lf %lf \n", pos.x(), pos.y(), pos.z());	 
			return true;
		} else {
			printf("can not get obj with such name \n");
		}	
	} else {
		return false;
	}

  return false;
}