Exemplo n.º 1
0
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;    

 }
Exemplo n.º 4
0
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;      
}   
Exemplo n.º 10
0
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;
}
Exemplo n.º 12
0
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;
}
Exemplo n.º 13
0
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");
		}
	}
}
Exemplo n.º 14
0
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;
}
Exemplo n.º 16
0
void MyController::onInit(InitEvent &evt)
{
	my = getObj(myname());
	my->getPosition(initPos);

	start = false;
	sw = false;

	// 手を下げる
	my->setJointAngle("LARM_JOINT2", DEG2RAD(-90));
	my->setJointAngle("RARM_JOINT2", DEG2RAD(90));

	stepWidth = 70;
	sleeptime = 300000;

	if((fp = fopen("motion.txt", "r")) == NULL) {
		LOG_MSG(("File do not exist."));
	}
	else{
		fscanf(fp, "%d", &motionNum);
		fscanf(fp, "%d", &sleeptime);
		for(int i=0; i<motionNum; i++){
			fscanf(fp, "%f %f %f %f %f %f %f %f %f %f %f",
					   &HEIGHT[i],
					   &LARM_JOINT1[i],
					   &LARM_JOINT3[i],
					   &RARM_JOINT1[i],
					   &RARM_JOINT3[i],
					   &LLEG_JOINT2[i],
					   &LLEG_JOINT4[i],
					   &LLEG_JOINT6[i],
					   &RLEG_JOINT2[i],
					   &RLEG_JOINT4[i],
					   &RLEG_JOINT6[i]);
		}
	}

	count = 0;
	step = 6;
}
Exemplo n.º 17
0
double AgentController::onAction(ActionEvent &evt)
{
	try {
		static int deg = 0;
		SimObj *my = getObj(myname());
		Vector3d v;
		my->getPosition(v);
		LOG_MSG(("pos = (%f, %f, %f)", v.x(), v.y(), v.z()));
		//my->setJointAngle("R_SHOULDER", DEG2RAD(deg));
		my->setJointAngle("R_ELBOW", DEG2RAD(90));
		my->setJointAngle("R_SHOULDER", DEG2RAD(deg));
		//my->setJointAngle("R_SHOULDER", DEG2RAD(deg));
		Parts *p = my->getParts("RU_ARM");
		if (p) {
			const double *pos = p->getPosition();;
			LOG_MSG(("RU_ARM(%f, %f, %f)", pos[0], pos[1], pos[2]));
			const double *q = p->getQuaternion();
			LOG_MSG(("      (%f, %f, %f, %f", q[0], q[1], q[2], q[3]));
		}

		p = my->getParts("RL_ARM");
		if (p) {
			const double *pos = p->getPosition();;
			LOG_MSG(("RL_ARM(%f, %f, %f)", pos[0], pos[1], pos[2]));
			const double *q = p->getQuaternion();
			LOG_MSG(("      (%f, %f, %f, %f", q[0], q[1], q[2], q[3]));
		}

		deg += 45;
		


	} catch(SimObj::Exception &) {
		;
	}
	return 0.1; 
}
double MyController::onAction(ActionEvent &evt)
{  
  switch(m_state){

    // 初期状態
  case 0: {

    // ゴミがある場所と名前を取得します
    if(!this->recognizeTrash(m_tpos,m_tname)){

      // ゴミが見つからないので終了
      broadcastMsgToSrv("I cannot find trash");
      m_state = 8;
    }
    // ゴミが見つかった
    else{

      // ゴミの方向に回転をはじめる
      m_time = rotateTowardObj(m_tpos, m_vel, evt.time());
      m_state = 1;
    }
    break;
  }
    // ゴミの方向に回転中
  case 1: {

    // 回転終了
    if(evt.time() >= m_time){

      // 回転を止める
      m_my->setWheelVelocity(0.0, 0.0);

      // 関節の回転を始める
      // orig
      m_my->setJointVelocity("RARM_JOINT1", -m_jvel, 0.0);

      // 50°回転
      m_time = DEG2RAD(50) / m_jvel + evt.time();

      // ゴミを取りに関節を曲げる状態に移行します
      m_state = 2;
    }
    break;
  }
    // 関節を回転中
  case 2: {

    // 関節回転終了    
    if(evt.time() >= m_time){

      m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0);

      // graspしたいパーツを取得します
      CParts *parts = m_my->getParts("RARM_LINK7");
      
      // graspします
      parts->graspObj(m_tname);
      
      // ゴミ箱の位置を取得します
      SimObj *trashbox = getObj("trashbox_1");
      Vector3d pos;
      trashbox->getPosition(pos);
      
      // ゴミ箱の方向に移動を開始します
      m_time = rotateTowardObj(pos, m_vel, evt.time());      
      m_state = 3;
    }
    
    break;
  }
    // ゴミ箱の方向に回転中
  case 3: {

    // ゴミ箱到着
    if(evt.time() >= m_time){

      // ここではゴミ箱の名前 位置は知っているものとします
      SimObj *trashbox = getObj("trashbox_1");
      Vector3d pos;
      trashbox->getPosition(pos);

      // ゴミ箱の近くに移動します
      m_time = goToObj(pos, m_vel*4, 40.0, evt.time());
      m_state = 4;
    }
    break;
  }

    // ゴミを持ってゴミ箱に向かっている状態
  case 4: {

    // ゴミ箱に到着
    if(evt.time() >= m_time){
      m_my->setWheelVelocity(0.0, 0.0);

      // grasp中のパーツを取得します
      CParts *parts = m_my->getParts("RARM_LINK7");
      
      // releaseします
      parts->releaseObj();

      // ゴミが捨てられるまで少し待つ
      sleep(1);

      // 捨てたゴミをゴミ候補から削除
      std::vector<std::string>::iterator it;
      it = std::find(m_trashes.begin(), m_trashes.end(), m_tname);
      m_trashes.erase(it);
      
      // 関節の回転を始める
      m_my->setJointVelocity("RARM_JOINT1", m_jvel, 0.0);
      m_time = DEG2RAD(50) / m_jvel + evt.time() + 1.0;      

      m_state = 5;
    }
    break;
  }
    // ゴミを捨てて関節を戻している状態
  case 5: {

    // 関節が元に戻った
    if(evt.time() >= m_time){

      // 関節の回転を止める
      m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0);

      // 最初にいた方向に体を回転させます
      m_time = rotateTowardObj(m_inipos, m_vel, evt.time());
      m_state = 6;
    }
    break;
  }
    // 元に場所に戻る方向に回転している状態
  case 6: {

    if(evt.time() >= m_time){
      m_my->setWheelVelocity(0.0, 0.0);
      
      // 最初にいた場所に移動します
      m_time = goToObj(m_inipos, m_vel*4, 5.0, evt.time());
      m_state = 7;
    }
    break;
  }

    //  元の場所に向かっている状態
  case 7: {

    // 元の場所に到着
    if(evt.time() >= m_time){
      m_my->setWheelVelocity(0.0, 0.0);
      
      // 最初の方向に回転(z軸と仮定)
      m_time = rotateTowardObj(Vector3d(0.0, 0.0, 10000.0), m_vel, evt.time());
      m_state = 8;
    }
    break;
  }
    // 元の向きに回転している状態
  case 8: {
    
    if(evt.time() >= m_time){
      // 回転を止める
      m_my->setWheelVelocity(0.0, 0.0);

      // 最初の状態に戻る
      m_state = 0;
    }
  }
  }
  return 0.1;      
}  
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" ;

// }


}  
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;    

 }
Exemplo n.º 21
0
double RobotController::onAction(ActionEvent &evt)
{
    switch(m_state){

  // 初期姿勢を設定 seting initial pose
  case 0: {
    //broadcastMsgToSrv("Let's start the clean up task\n");
    sendMsg("VoiceReco_Service","Let's start the clean up task\n");
    double angL1 =m_my->getJointAngle("LARM_JOINT1")*180.0/(PI);
    double angL4 =m_my->getJointAngle("LARM_JOINT4")*180.0/(PI);
    double angR1 =m_my->getJointAngle("RARM_JOINT1")*180.0/(PI);
    double angR4 =m_my->getJointAngle("RARM_JOINT4")*180.0/(PI);
    double thetaL1 = -20-angL1;
    double thetaL4 = -160-angL4;
    double thetaR1 = -20-angR1;
    double thetaR4 = -160-angR4;
    if(thetaL1<0) m_my->setJointVelocity("LARM_JOINT1", -m_jvel, 0.0);
    else m_my->setJointVelocity("LARM_JOINT1", m_jvel, 0.0);
    if(thetaL4<0) m_my->setJointVelocity("LARM_JOINT4", -m_jvel, 0.0);
    else m_my->setJointVelocity("LARM_JOINT4", m_jvel, 0.0);
    if(thetaR1<0) m_my->setJointVelocity("RARM_JOINT1", -m_jvel, 0.0);
    else m_my->setJointVelocity("RARM_JOINT1", m_jvel, 0.0);
    if(thetaR4<0) m_my->setJointVelocity("RARM_JOINT4", -m_jvel, 0.0);
    else m_my->setJointVelocity("RARM_JOINT4", m_jvel, 0.0);
    m_time_LA1 = DEG2RAD(abs(thetaL1))/ m_jvel + evt.time();
    m_time_LA4 = DEG2RAD(abs(thetaL4))/ m_jvel + evt.time();
    m_time_RA1 = DEG2RAD(abs(thetaR1))/ m_jvel + evt.time();
    m_time_RA4 = DEG2RAD(abs(thetaR4))/ m_jvel + evt.time();
    m_state = 1;
    break;
  }
  // 初期姿勢に移動 moving initial pose
  case 1: {
    if(evt.time() >= m_time_LA1) m_my->setJointVelocity("LARM_JOINT1", 0.0, 0.0);
    if(evt.time() >= m_time_LA4) m_my->setJointVelocity("LARM_JOINT4", 0.0, 0.0);
    if(evt.time() >= m_time_RA1) m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0);
    if(evt.time() >= m_time_RA4) m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0);
    if(evt.time() >= m_time_LA1 && evt.time() >= m_time_LA4
    && evt.time() >= m_time_RA1 && evt.time() >= m_time_RA4){
	// 位置Aの方向に回転を開始します setting position a for rotating
	//broadcastMsgToSrv("Moving to the table");
	m_time = rotateTowardObj(pos_a, m_vel, evt.time());
	m_state = 2;
    }
    break;
  }
  // 位置Aの方向に回転 rotating to position a
  case 2: {
    // 回転終了
    if(evt.time() >= m_time){
      m_my->setWheelVelocity(0.0, 0.0);
      // 位置Aに移動します setting position a for moving
      m_time = goToObj(pos_a, m_vel*4, 0.0, evt.time());
      m_state = 3;
    }
    break;
  }
  // 位置Aに移動 moving to position a
  case 3: {
    // 位置Aに到着
    if(evt.time() >= m_time){
      m_my->setWheelVelocity(0.0, 0.0);
      // 位置Bの方向に回転を開始します setting position b for rotating
      m_time = rotateTowardObj(pos_b, m_vel, evt.time());
      m_state = 4;
    }
    break;
  }
  // 位置Bの方向に回転 rotating to position b
  case 4: {
    // 回転終了
    if(evt.time() >= m_time){
      m_my->setWheelVelocity(0.0, 0.0);
      // 位置Bに移動します setting position b for moving
      m_time = goToObj(pos_b, m_vel*4, 0.0, evt.time());
      m_state = 5;
    }
    break;
  }
  // 位置Bに移動 moving to position b
  case 5: {
    // 位置Bに到着
    if(evt.time() >= m_time){
      m_my->setWheelVelocity(0.0, 0.0);
      // テーブルの方向に回転を開始します setting table position for rotating
      SimObj *table = getObj("table_0");
      Vector3d pos;
      table->getPosition(pos);
      m_time = rotateTowardObj(pos, m_vel, evt.time());
      m_state = 6;
    }
    break;
  }
  // テーブルの方向に回転 rotating to table
  case 6: {
    // 回転終了
    if(evt.time() >= m_time){
      m_my->setWheelVelocity(0.0, 0.0);
      // ゴミがある場所と名前を取得します
      // ゴミが見つからなかった
      if(!this->recognizeTrash(m_tpos,m_tname)){
        //broadcastMsgToSrv("No trash detected");
        //broadcastMsgToSrv("Task finished");
        sleep(10);
      }
      // ゴミが見つかった trash detected
      else{
        //broadcastMsgToSrv("Please show which trash to take\n");
        sendMsg("VoiceReco_Service","Please show me which object to take");
        m_state = 7;
      }
    }
    break;
  }
  // wating to point
  case 7: {
    break;
  }
  // 物体認識開始 starting object recognition
  case 8: {
    //  m_tpos object direction on object
    SimObj *target = this->getObj(m_pointedObject.c_str());
    target->getPosition(m_tpos);
    //broadcastMsgToSrv("Ok I will take it\n");
    msg_ob = "I will take " + m_pointedObject ;
    sendMsg("VoiceReco_Service",msg_ob);
    // ゴミの方向に回転をはじめる
    m_time = rotateTowardObj(m_tpos, m_vel, evt.time());
    m_state = 9;
    break;
  }
  // ゴミの方向に回転をはじめる setting trash position for rotating
  case 9: {
    m_time = rotateTowardObj(m_tpos, m_vel, evt.time());
    m_state = 10;
    break;
  }
  // ゴミの方向に回転中 rotating to trash
  case 10: {
    // 回転終了
    if(evt.time() >= m_time){
      // 回転を止める
      m_my->setWheelVelocity(0.0, 0.0);
      //ゴミの位置まで移動をはじめる setting trash position for moving
      m_time = goToObj(m_tpos, m_vel*4, 25.0, evt.time());
      m_state = 11;
    }
    break;
  }
  // ゴミの位置まで移動中 moving to trash
  case 11: {
    // 移動終了
    if(evt.time() >= m_time){
      // 移動を止める
      m_my->setWheelVelocity(0.0, 0.0);
      // 関節の回転を始める setting arm for grasping
      double angR1 =m_my->getJointAngle("RARM_JOINT1")*180.0/(PI);
      double angR4 =m_my->getJointAngle("RARM_JOINT4")*180.0/(PI);
      double thetaR1 = -30.0-angR1;
      double thetaR4 = 0.0-angR4;
      if(thetaR1<0) m_my->setJointVelocity("RARM_JOINT1", -m_jvel, 0.0);
      else m_my->setJointVelocity("RARM_JOINT1", m_jvel, 0.0);
      if(thetaR4<0) m_my->setJointVelocity("RARM_JOINT4", -m_jvel, 0.0);
      else m_my->setJointVelocity("RARM_JOINT4", m_jvel, 0.0);
      m_time_RA1 = DEG2RAD(abs(thetaR1) )/ m_jvel + evt.time();
      m_time_RA4 = DEG2RAD(abs(thetaR4) )/ m_jvel + evt.time();
      // ゴミを取りに関節を曲げる状態に移行します
      m_state = 12;
    }
    break;
  }
  // 関節を回転中 rotating arm for grasping
  case 12: {
    // 関節回転終了
    if(evt.time() >= m_time_RA1) m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0);
    if(evt.time() >= m_time_RA4) m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0);
    if(evt.time() >= m_time_RA1 && evt.time() >= m_time_RA4){
      if(m_grasp) {
        //broadcastMsgToSrv("grasping the trash");
        // 関節の回転を始める setting arm for taking
        double angR4 =m_my->getJointAngle("RARM_JOINT4")*180.0/(PI);
        double thetaR4 = -90.0-angR4;
        if(thetaR4<0) m_my->setJointVelocity("RARM_JOINT4", -m_jvel, 0.0);
        else m_my->setJointVelocity("RARM_JOINT4", m_jvel, 0.0);
        m_time_RA4 = DEG2RAD(abs(thetaR4) )/ m_jvel + evt.time();
        // 関節を戻す状態に移行します
        m_state = 13;
      }
      else{
        // graspできない
        broadcastMsgToSrv("Unreachable");
      }
    }
    break;
  }
  // 関節を回転中 rotating arm for taking
  case 13: {
    // 関節回転終了
    if(evt.time() >= m_time_RA4){
        m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0);
        // 位置Aの方向に回転を開始します setting position a for rotating
        //broadcastMsgToSrv("Moving to the trashbox");
        sendMsg("VoiceReco_Service","Now I will go to the trash boxes");
        m_time = rotateTowardObj(pos_a, m_vel, evt.time());
        m_state = 14;
    }
    break;
  }
  // 位置Aの方向に回転 rotating to position a
  case 14: {
    // 回転終了
    if(evt.time() >= m_time){
      m_my->setWheelVelocity(0.0, 0.0);
      // 位置Aに移動します setting position a for moving
      m_time = goToObj(pos_a, m_vel*4, 0.0, evt.time());
      m_state = 15;
    }
    break;
  }
  // 位置Aの位置まで移動中 movig to position a
  case 15: {
    // 移動終了
    if(evt.time() >= m_time){
     m_my->setWheelVelocity(0.0, 0.0);
     //broadcastMsgToSrv("Please tell me which trash box \n");
     sendMsg("VoiceReco_Service","Please show me which trash box to use");
     m_state = 16;
    }
    break;
  }
  // watig to point4
  case 16: {
    break;
  }
  // ゴミ箱認識 starting trash box recognitiong-g-0
  case 17: {
    //  m_tpos object direction on object
    SimObj *target_trash = this->getObj(m_pointedtrash.c_str());
    target_trash->getPosition(m_tpos);
    //broadcastMsgToSrv("Ok I will throw the trash in trash box \n");
    msg_trash = "Ok I will put "+ m_pointedObject+"in"+ m_pointedtrash + "\n";
    sendMsg("VoiceReco_Service",msg_trash);
    // ゴミの方向に回転をはじめる setting position trash box for rotating
    m_time = rotateTowardObj(m_tpos, m_vel, evt.time());
    m_state = 18;
    break;
  }
  // ゴミ箱の方向に回転中 rotating to trash box
  case 18: {
    if(evt.time() >= m_time){
      // 回転を止める
      m_my->setWheelVelocity(0.0, 0.0);
      //ゴミの位置まで移動をはじめる setting trash position for moving
      m_time = goToObj(m_tpos, m_vel*4, 30.0, evt.time());
      m_state = 19;
    }
    break;
  }
  // ゴミを持ってゴミ箱に向かっている状態 moving to trash box
  case 19: {
    // ゴミ箱に到着
    if(evt.time() >= m_time){
      m_my->setWheelVelocity(0.0, 0.0);
      // grasp中のパーツを取得します getting grasped tarts
      CParts *parts = m_my->getParts("RARM_LINK7");
      // releaseします
      parts->releaseObj();
      // ゴミが捨てられるまで少し待つ
      sleep(1);
      // 捨てたゴミをゴミ候補から削除 deleting grasped object from list
      std::vector<std::string>::iterator it;
      it = std::find(m_trashes.begin(), m_trashes.end(), m_pointedObject);
      m_trashes.erase(it);
      // grasp終了
      m_grasp = false;
      m_state = 1;
    }
    break;
  }
  }
  return 0.01;
}
Exemplo n.º 22
0
double MyController::onAction(ActionEvent &evt) 
{ 
  // サービスが使用可能か定期的にチェックする  
  bool available = checkService("RobocupReferee");  

  if(!available && m_ref != NULL) m_ref = NULL;

  // 使用可能  
  else if(available && m_ref == NULL){  
    // サービスに接続  
    m_ref = connectToService("RobocupReferee");  
  }  
 
  // 自分の位置取得
  Vector3d myPos;
  m_my->getPosition(myPos);

  // 衝突中の場合,衝突が継続しているかチェック
  if(colState){
    CParts *parts = m_my->getMainParts();
    bool state = parts->getCollisionState();
    
    // 衝突していない状態に戻す
    if(!state) colState = false;
  }
  
  int entSize = m_entities.size();
  for(int i = 0; i < entSize; i++){

    // ロボットまたはゴミ箱の場合は除く
    if(m_entities[i] == "robot_000"  ||
       m_entities[i] == "recycle" ||
       m_entities[i] == "burnable" ||
       m_entities[i] == "room" ||
       m_entities[i] == "moderator_0" ||
       m_entities[i] == "Kinect_000" ||
       m_entities[i] == "unburnable"){
      continue;
    }
    // エンティティ取得
    SimObj *ent = getObj(m_entities[i].c_str());

    // 位置取得
    Vector3d tpos;
    ent->getPosition(tpos);

    // ゴミ箱からゴミを結ぶベクトル
    Vector3d vec(tpos.x()-myPos.x(), tpos.y()-myPos.y(), tpos.z()-myPos.z());
    
    // ゴミがゴミ箱の中に入ったかどうか判定
    if(abs(vec.x()) < tboxSize_x/2.0 &&
       abs(vec.z()) < tboxSize_z/2.0 &&
       tpos.y() < tboxMax_y     &&
       tpos.y() > tboxMin_y     ){

      // ゴミがリリースされているか確認
      if(!ent->getIsGrasped()){

	// ゴミを捨てる
	tpos.y(-100);
	tpos.x(myPos.x());
	tpos.z(myPos.z());
	ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
	ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
	ent->setPosition(tpos);
	ent->setPosition(tpos);
	ent->setPosition(tpos);
	usleep(500000);
	tpos.y(-50.0);
	ent->setPosition(tpos);
	ent->setPosition(tpos);
	ent->setPosition(tpos);

	std::string msg;
	// ゴミが所定のゴミ箱に捨てられているかチェック
	// リサイクル

  /*
	if(strcmp(myname(), "recycle") == 0){
	  // 空のペットボトルのみ点が入る
	  if(strcmp(ent->name(), "petbottle") == 0 ||
	     strcmp(ent->name(), "petbottle_2") == 0 ||
	     strcmp(ent->name(), "petbottle_4") == 0 ||
	     strcmp(ent->name(), "mayonaise_1") == 0 ) {
	    msg = "RobocupReferee/Clean up succeeded" "/1000";
	  }
	  else{
	    msg = "RobocupReferee/Clean up failed" "/-600";
	  }
	}
*/

	// 燃えるゴミ

/*	
	else if(strcmp(myname(), "burnable") == 0){
	  // 燃えるゴミに入れるべきものは無い
	  msg = "RobocupReferee/Clean up failed" "/-600";
	}
	// 缶瓶
	else if(strcmp(myname(), "unburnable") == 0){
	  if(strcmp(ent->name(), "can_0") == 0 ||
	     strcmp(ent->name(), "can_1") == 0 ||
	     strcmp(ent->name(), "can") == 0 ||
	     strcmp(ent->name(), "can_3") == 0) {
	    msg = "RobocupReferee/Clean up succeeded" "/1000";
	  }
	  else {
	    msg = "RobocupReferee/Clean up succeeded" "/-600";
	  }
	}
*/
	if(m_ref != NULL) {
	//  m_ref->sendMsgToSrv(msg.c_str());
	}
	//LOG_MSG((msg.c_str()));
      }
    }
  }

  return retValue;      
}  
double MyController::onAction(ActionEvent &evt) {  

  dlopen("libpython2.7.so", RTLD_LAZY | RTLD_GLOBAL); 
  Py_Initialize();  //initialization of the python interpreter




  //return 1.0;
  SimObj *stick = getObj("robot_test");
  SimObj *box   = getObj("box_001");
  SimObj *goal_001   = getObj("checkpoint_001");
  double torque;





  // The target position
  Vector3d targetPos;
  box->getPosition(targetPos);

  // The Goal Position: The sphere is placed at Goal position. 

  Vector3d goalPos;
  goal_001->getPosition(goalPos);

  // calculating the displacement vector 

  Vector3d displacementVect;
  displacementVect.x(goalPos.x()-targetPos.x());
  displacementVect.y(goalPos.y()-targetPos.y());
  displacementVect.z(goalPos.z()-targetPos.z());



  // stick->addForce(-500,0,500);  // This adds force to on the stick tool.
  // stick->addForce(0,0,5000); 

  // Vector3d angularVel;
  // stick->getAngularVelocity(angularVel); 
  // LOG_MSG((" Current angular Velocity is  : %f %f %f ", angularVel.x(), angularVel.y(), angularVel.z() ));

 //  if ( abs(angularVel.y()) > 0.1  )
 //  {
 //      LOG_MSG((" Current angular Velocity is  : %f %f %f ", angularVel.x(), angularVel.y(), angularVel.z() ));
 //      double* ptr1 = NULL;
 //      ptr1 = controlAngularVelocity(angularVel, 3.0, 0, 1.0);
 //      torque  =  ptr1[0] * 500;
 //      // torque =  ptr1[0] * 12000 ;
 //      cout << "The torque applied for controlling angular velocity is " << torque << "   N. m" << endl; 
 //      stick->addTorque( 0 , torque, 0);
 // } 


  // to control the rotation of the tool

  // Rotation currentToolRot;
  // stick->getRotation(currentToolRot);
  // double *ptr = NULL;
  // ptr = controlRotation(initialToolRot, currentToolRot, 20.0, 0.0, 20.0);
  // torque  =  ptr[1] * 200 ;
  // cout << "The torque applied for controlling the rotation = " << torque << endl;
  // stick->addTorque(0, torque,0);





  // double massOfTool;  
  // massOfTool = stick->getMass();  
  // cout << "The mass is " << massOfTool <<endl;

  // Vector3d velocityOfTarget;
  // box->getLinearVelocity(velocityOfTarget);

  // double netVelocityTarget;
  // netVelocityTarget=( pow(velocityOfTarget.x(),2) + pow(velocityOfTarget.y(), 2) + pow(velocityOfTarget.z(), 2 ) );
  // netVelocityTarget = sqrt(netVelocityTarget);


  // stick->getPosition(pos);
  // stick->getPartsPosition(pos, "LINK1");
  // LOG_MSG((" LINK1 Position is  : %f %f %f ", pos.x(), pos.y(), pos.z() ));

  // Vector3d targetPos;
  // box->getPosition(targetPos);

  // Vector3d goalPos;
  // goal_001->getPosition(goalPos);

  // if (abs( goalPos.z() - targetPos.z()) < 1.4 ) 
  // {
  //    cout << "The distance to goal is " << abs( goalPos.z() - targetPos.z()) << endl;
  //    cout << "The goal has been reached " << endl;
  //    exit(0);
  // }

  // if (netVelocityTarget > 0.1 )

  // {

  //   double angle = atan ( (targetPos.z() - startPosition.z()) / (targetPos.x() - startPosition.x()  )  ) * 180 / PI;
  //   cout << "The angle is" << angle; 
  //   storePosition(targetPos);
  // }



  std::vector<std::string> s;

  for(std::map<std::string, CParts *>::iterator it = stick->getPartsCollection().begin(); it != stick->getPartsCollection().end(); ++it){
    if (it->first != "body")
        s.push_back(it->first);
    }
  
 std::string linkName; 
 Size si;

 cout << "The total links are  " << s.size() << endl;

 for (int i = 0; i < s.size(); i++){

  const char* linkName = s[i].c_str();
  CParts *link = stick->getParts(linkName);
  link->getPosition(pos);
  link->getRotation(linkRotation);

  if (link->getType() == 0){
            BoxParts* box = (BoxParts*) link;
            si = box->getSize();
            // cout <<  linkName << endl;
            cout << linkName << "  position : x = " << pos.x() << "  y = " << pos.y() << "  z = " << pos.z() << endl;
            // cout << linkName << "  size : x = " << si.x() << "  y = " << si.y() << "  z = " << si.z() << endl;
            // cout << linkName << "  Rotation: qw " << linkRotation.qw() << " qx = "<< linkRotation.qx() << " qy = "<< linkRotation.qy()
            // << " qz = "<< linkRotation.qz() << endl;

            try{  

               py::object main_module = py::import("__main__");
               py::object main_namespace = main_module.attr("__dict__");  
               main_module.attr("linkName") = linkName;
               main_module.attr("length")  = si.x();
               main_module.attr("height")  = si.y();
               main_module.attr("breadth") = si.z();
               main_module.attr("linkPos") = "[" + tostr(pos.x())+" , "+ tostr(pos.y())+ " , " + tostr(pos.z()) + "]";
               main_module.attr("rotation") = "[" + tostr(linkRotation.qw())+" , "+ tostr(linkRotation.qx())+ " , " + tostr(linkRotation.qy()) + " , " + tostr(linkRotation.qz()) +"]";
               
               // calculating the rotation of the tool.
               py::exec("import ast", main_namespace);
               py::exec("import transformations as T", main_namespace);
               py::exec("rotation = ast.literal_eval(rotation)", main_namespace);
               // py::exec("angles = T.euler_from_quaternion(rotation) ", main_namespace);
               // py::exec("print angles", main_namespace);

    
             
               py::exec("linkPos = ast.literal_eval(linkPos)", main_namespace);
               py::exec_file("vertices.py", main_namespace, main_namespace );
               py::exec("getNormals(linkName, linkPos, rotation, length, height, breadth)", main_namespace);

               main_module.attr("targetPos") = "[" + tostr(targetPos.x())+" , "+ tostr(targetPos.y())+ " , " + tostr(targetPos.z()) + "]";
               main_module.attr("displacementVect") = "[" + tostr(displacementVect.x())+" , "+ tostr(displacementVect.y())+ " , " + tostr(displacementVect.z()) + "]";
               py::exec_file("displacementVector.py", main_namespace, main_namespace );
          

            }
            catch(boost::python::error_already_set const &){
                // Parse and output the exception
                std::string perror_str = parse_python_exception();
                std::cout << "Error in Python: " << perror_str << std::endl;
            }
        }

  else if(link->getType() == 1){
            CylinderParts* cyl = (CylinderParts*) link;
            cout << "Cylinder Position : x = " << pos.x() << "  y = " << pos.y() << "  z = " << pos.z() << endl;
            cout << "Cylinder Length : length = " << cyl->getLength() << endl;
            cout << "Cylinder Radius : rad = " << cyl->getRad() << endl;
        }

  else if(link->getType() == 2){
            SphereParts* sph = (SphereParts*) link;
            cout << "Sphere Position : x = " << pos.x() << "  y = " << pos.y() << "  z = " << pos.z() << endl;
            cout << "Sphere Radius : rad = " << sph->getRad() << endl;
        }


 }


 
  // stick->getPartsPosition(pos, s[1]);
  // stick->getPartsPosition(pos, "LINK1");
  // LOG_MSG((" LINK1 Position is  : %f %f %f ", pos.x(), pos.y(), pos.z() ));

  // stick->getPartsPosition(pos, "LINK2");
  // LOG_MSG((" LINK2 Position is  : %f %f %f ", pos.x(), pos.y(), pos.z() ));


  // stick->getPartsPosition(pos, "LINK3");
  // LOG_MSG((" LINK3 Position is  : %f %f %f ", pos.x(), pos.y(), pos.z() ));


  

 

 return 0.00001;
  }
Exemplo n.º 24
0
double MyController::onAction(ActionEvent &evt)
{
	Vector3d pos;

	if (start == true){

		my->getPosition(pos);

		double dx = 0;
		double dz = -2.5;
		double addx = 0.0;
		double addz = 0.0;

		if(count%2){
			for(int i=0; i<motionNum; i++){
				addx += dx;
				addz += dz;
				if(motionNum)
					usleep(sleeptime);
				my->setPosition(pos.x()+addx, HEIGHT[i], pos.z()+addz);
				my->setJointAngle("LARM_JOINT1", DEG2RAD(LARM_JOINT1[i]));
				my->setJointAngle("LARM_JOINT3", DEG2RAD(LARM_JOINT3[i]));
				my->setJointAngle("RARM_JOINT1", DEG2RAD(RARM_JOINT1[i]));
				my->setJointAngle("RARM_JOINT3", DEG2RAD(RARM_JOINT3[i]));
				my->setJointAngle("LLEG_JOINT2", DEG2RAD(LLEG_JOINT2[i]));
				my->setJointAngle("LLEG_JOINT4", DEG2RAD(LLEG_JOINT4[i]));
				my->setJointAngle("LLEG_JOINT6", DEG2RAD(LLEG_JOINT6[i]));
				my->setJointAngle("RLEG_JOINT2", DEG2RAD(RLEG_JOINT2[i]));
				my->setJointAngle("RLEG_JOINT4", DEG2RAD(RLEG_JOINT4[i]));
				my->setJointAngle("RLEG_JOINT6", DEG2RAD(RLEG_JOINT6[i]));
			}
		}
		else{
			for(int i=0; i<motionNum; i++){
				addx += dx;
				addz += dz;
				usleep(sleeptime);
				my->setPosition(pos.x()+addx, HEIGHT[i], pos.z()+addz);
				my->setJointAngle("RARM_JOINT1", DEG2RAD(LARM_JOINT1[i]));
				my->setJointAngle("RARM_JOINT3", DEG2RAD(-LARM_JOINT3[i]));
				my->setJointAngle("LARM_JOINT1", DEG2RAD(RARM_JOINT1[i]));
				my->setJointAngle("LARM_JOINT3", DEG2RAD(-RARM_JOINT3[i]));
				my->setJointAngle("RLEG_JOINT2", DEG2RAD(LLEG_JOINT2[i]));
				my->setJointAngle("RLEG_JOINT4", DEG2RAD(LLEG_JOINT4[i]));
				my->setJointAngle("RLEG_JOINT6", DEG2RAD(LLEG_JOINT6[i]));
				my->setJointAngle("LLEG_JOINT2", DEG2RAD(RLEG_JOINT2[i]));
				my->setJointAngle("LLEG_JOINT4", DEG2RAD(RLEG_JOINT4[i]));
				my->setJointAngle("LLEG_JOINT6", DEG2RAD(RLEG_JOINT6[i]));
			}
		}
		count++;

		if(count==2){
			sleep(3);
		}
		if(count==4){
			sendMsg("operator","Passed_through");
		}
		if(count>step){
			start = false;
		}
	}

	return 0.1;
}
double MyController::onAction(ActionEvent &evt)
{
	int count = 0;
	Vector3d pos;

	if (start == true){
		int step = 3;
		while (count<step){
			double dx = 0;
			double dz = -2.5;
			double addx = 0.0;
			double addz = 0.0;
			my->getPosition(pos);
			for(int i=0; i<motionNum; i++){
				addx += dx;
				addz += dz;
				if(motionNum)
					usleep(sleeptime);
				my->setPosition(pos.x()+addx, HEIGHT[i], pos.z()+addz);
				my->setJointAngle("LARM_JOINT1", DEG2RAD(LARM_JOINT1[i]));
				my->setJointAngle("LARM_JOINT3", DEG2RAD(LARM_JOINT3[i]));
				my->setJointAngle("RARM_JOINT1", DEG2RAD(RARM_JOINT1[i]));
				my->setJointAngle("RARM_JOINT3", DEG2RAD(RARM_JOINT3[i]));
				my->setJointAngle("LLEG_JOINT2", DEG2RAD(LLEG_JOINT2[i]));
				my->setJointAngle("LLEG_JOINT4", DEG2RAD(LLEG_JOINT4[i]));
				my->setJointAngle("LLEG_JOINT6", DEG2RAD(LLEG_JOINT6[i]));
				my->setJointAngle("RLEG_JOINT2", DEG2RAD(RLEG_JOINT2[i]));
				my->setJointAngle("RLEG_JOINT4", DEG2RAD(RLEG_JOINT4[i]));
				my->setJointAngle("RLEG_JOINT6", DEG2RAD(RLEG_JOINT6[i]));
			}
			for(int i=0; i<motionNum; i++){
				addx += dx;
				addz += dz;
				usleep(sleeptime);
				my->setPosition(pos.x()+addx, HEIGHT[i], pos.z()+addz);
				my->setJointAngle("RARM_JOINT1", DEG2RAD(LARM_JOINT1[i]));
				my->setJointAngle("RARM_JOINT3", DEG2RAD(-LARM_JOINT3[i]));
				my->setJointAngle("LARM_JOINT1", DEG2RAD(RARM_JOINT1[i]));
				my->setJointAngle("LARM_JOINT3", DEG2RAD(-RARM_JOINT3[i]));
				my->setJointAngle("RLEG_JOINT2", DEG2RAD(LLEG_JOINT2[i]));
				my->setJointAngle("RLEG_JOINT4", DEG2RAD(LLEG_JOINT4[i]));
				my->setJointAngle("RLEG_JOINT6", DEG2RAD(LLEG_JOINT6[i]));
				my->setJointAngle("LLEG_JOINT2", DEG2RAD(RLEG_JOINT2[i]));
				my->setJointAngle("LLEG_JOINT4", DEG2RAD(RLEG_JOINT4[i]));
				my->setJointAngle("LLEG_JOINT6", DEG2RAD(RLEG_JOINT6[i]));
			}
			count++;
			if(count==1){
				usleep(3000000);
			}

			start = false;
		}
		/*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++;

			if(count==7){
				usleep(3000000);
			}

			start = false;
		}*/
	}

	return 0.1;
}
Exemplo n.º 26
0
double MyController::onAction(ActionEvent &evt)
{
	// サービスが使用可能か定期的にチェックする
	bool available = checkService("CleanUpReferee");

	if(!available && m_ref != NULL) m_ref = NULL;

	// 使用可能  
	else if(available && m_ref == NULL){
		// サービスに接続  
		m_ref = connectToService("CleanUpReferee");
	}

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

	// 衝突中の場合,衝突が継続しているかチェック
	if(colState){
		CParts *parts = m_my->getMainParts();
		bool state = parts->getCollisionState();

		// 衝突していない状態に戻す
		if(!state) colState = false;
	}

	int entSize = m_entities.size();
	for(int i = 0; i < entSize; i++){

		// ロボットまたはゴミ箱の場合は除く
		if(m_entities[i] == "robot_000"  ||
		   m_entities[i] == "trashbox_0" ||
		   m_entities[i] == "trashbox_1" ||
		   m_entities[i] == "trashbox_2"){
			continue;
		}
		// エンティティ取得
		SimObj *ent = getObj(m_entities[i].c_str());

		// 位置取得
		Vector3d tpos;
		ent->getPosition(tpos);

		// ゴミ箱からゴミを結ぶベクトル
		Vector3d vec(tpos.x()-myPos.x(), tpos.y()-myPos.y(), tpos.z()-myPos.z());

		// ゴミがゴミ箱の中に入ったかどうか判定
		if(abs(vec.x()) < tboxSize_x/2.0 &&
		   abs(vec.z()) < tboxSize_z/2.0 &&
		   tpos.y() < tboxMax_y     &&
		   tpos.y() > tboxMin_y     ){

			// ゴミがリリースされているか確認
			if(!ent->getIsGrasped()){

				std::string msg;	
				bool success = false;
				// 台の上に置く(成功)
				if(strcmp(ent->name(), "mayonaise_0") == 0 && tpos.y() != 57.85) {tpos.y(57.85); success = true;}
				else if(strcmp(ent->name(), "chigarette") == 0 && tpos.y() != 54.04){ tpos.y(54.04); success = true;}
				else if(strcmp(ent->name(), "chocolate") == 0 && tpos.y() != 51.15){ tpos.y(51.15); success = true;}
				else if(strcmp(ent->name(), "mugcup") == 0 && tpos.y() != 54.79){ tpos.y(54.79); success = true;}
				else if(strcmp(ent->name(), "petbottle_0") == 0 && tpos.y() != 67.45){ tpos.y(67.45); success = true;}
				else if(strcmp(ent->name(), "petbottle_3") == 0 && tpos.y() != 61.95){ tpos.y(61.95); success = true;}
				else if(strcmp(ent->name(), "clock") == 0 && tpos.y() != 56.150){ tpos.y(56.150); success = true;}
				else if(strcmp(ent->name(), "kettle") == 0 && tpos.y() != 60.650){ tpos.y(60.650); success = true;}
				// 台の上に置く(失敗)
				else if(strcmp(ent->name(), "petbottle_1") == 0 && tpos.y() != 67.45){ tpos.y(67.45);}
				else if(strcmp(ent->name(), "petbottle_2") == 0 && tpos.y() != 67.45){ tpos.y(67.45);}
				else if(strcmp(ent->name(), "petbottle_4") == 0 && tpos.y() != 61.95){ tpos.y(61.95);}
				else if(strcmp(ent->name(), "mayonaise_1") == 0 && tpos.y() != 57.85){ tpos.y(57.85);}
				else if(strcmp(ent->name(), "can_0") == 0 && tpos.y() != 55.335){ tpos.y(55.335);}
				else if(strcmp(ent->name(), "can_1") == 0 && tpos.y() != 55.335){ tpos.y(55.335);}
				else if(strcmp(ent->name(), "can_2") == 0 && tpos.y() != 57.050){ tpos.y(57.050);}
				else if(strcmp(ent->name(), "can_3") == 0 && tpos.y() != 57.050){ tpos.y(57.050);}
				else if(strcmp(ent->name(), "banana") == 0 && tpos.y() != 51.69){ tpos.y(51.69);}
				else if(strcmp(ent->name(), "apple") == 0 && tpos.y() != 54.675){ tpos.y(54.675);}
				else{continue;}

				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);

				if(success){
					msg = "CleanUpReferee/";
					msg += ent->name();
					msg += " succeeded/1000";
				}
				else{
					msg = "CleanUpReferee/";
					msg += ent->name();
					msg += " failed/-600";

				}
	
				if(m_ref != NULL) {
					m_ref->sendMsgToSrv(msg.c_str());
				}
				else{
					LOG_MSG((msg.c_str()));
				}
			}
		}
	}

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

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

	bool found = false;

  // 自分の位置の取得
  Vector3d myPos;
  m_my->getPosition(myPos);
	
	// 現位置に検出出来るゴミを検索する

  // ここでは乱数を使ってゴミを決定します
  int trashNum = rand() % m_trashes.size();
	name = m_trashes[trashNum];
	
	// robot stop if it cannot grab nearest object, so i didnt use find nearest obj function
	// when use while() loop, robot freeze when all object found
	// donot use while() loop xxx

	/*while(!getObj(name.c_str())) {
	//for(int trashNum = 0; trashNum < m_trashes.size(); trashNum++) {
		trashNum = rand() % m_trashes.size();
		name = m_trashes[trashNum];
	}*/

	// if cannot find object by random(), iteratate through all the name of trash.
	int objFindTry = 0;	
	if(!getObj(name.c_str())) {
		while(objFindTry < m_trashes.size()) {
			name = m_trashes[objFindTry];
			if(getObj(name.c_str())) {
				found = true;
				break;
			}
			objFindTry++;
		}
	} else if(getObj(name.c_str())) {
		found = true;
	}

	if(!found) {
		//broadcastMsgToSrv("remain known obj not found");
		return false;
	}


	if(found) {
		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());	 
	}	

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

  return found;
}