C++ SimObj类代码示例

void MyController::onCollision(CollisionEvent &evt) { 

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

  if(Colli && counterOfCollision == 1 )
  {
      toolName->getVelocity(toolVelAtHit);
      target->getVelocity(targetVelAtHit);
      cout << "The tool velocity is " << toolVelAtHit.x() << " , " << toolVelAtHit.z() << endl;
      cout << "The target velocity is " << targetVelAtHit.x() << " , " << targetVelAtHit.z() << endl;
      // myfile << toolVelAtHit.x() << " , " << toolVelAtHit.z() << " , " ;
      // myfile << targetVelAtHit.x() << " , " << targetVelAtHit.z() << " , " ;
      Colli = false;

  }



}

double NiiRobotOkonomi::onAction(ActionEvent &evt)
{

	SimObj *my = getObj(myname());

	static int s_cnt = 0;

	int step = s_cnt % STEP_NUM;
	for (int i=0; i<JOINT_NUM; i++) {
		const char * jointName = jointNames[i];
		if (!jointName) { continue; }
		double angle = jointAngles[step][i];
		double rad = DEG2RAD(angle);
		my->setJointAngle(jointName, rad); 
	}
	s_cnt++;

	/*
	double zz = cos(s_cnt*5*3.14/180);
	double xx = sin(s_cnt*5*3.14/180);
	my->setPosition(xx, 0.5, zz);

	my->setAxisAndAngle(0.0, 1.0, 0.0, s_cnt*5*3.14/180);
	*/

	return 0.1;
}

bool RecvEntitiesEvent::set(int packetNum, int seq, char *data, int n)
{
	char *p = data;

	m_time = BINARY_GET_DOUBLE_INCR(p);

	for (;;) {
		int head = p - data;
		int left = n - head;
		if (left <= 0) {
			fprintf(stderr, "RecvEntitiesEvent : packet too small left = %d\n", left);
			return false;
		}
		
		if (CommData::isPacketEnd(p)) {
			break;
		}
		
		SimObj *obj = new SimObj();
		int r = obj->setBinary(p, left);
		if (r < 0) { return false; }
		push(obj);
		p += r;
	}
	return true;
}

void MyController::onRecvMsg(RecvMsgEvent &evt) {

  //取得したメッセージを表示します
  string msg = evt.getMsg();
  //LOG_MSG(("msg : %s", msg.c_str()));
  SimObj *my = getObj(myname());

  if(strstr(msg.c_str()," "))
    {
      // phi theta に分ける 
      int n = 0;
      n = msg.find(" ");
      string phi_s = msg.substr(0,n);
      string theta_s = msg.substr(n+1);

      double phi = atof(theta_s.c_str());
      double theta = atof(phi_s.c_str());

      // 目玉関節を回転させる
      my->setJointAngle("LEYE_JOINT1",DEG2RAD(phi));
      my->setJointAngle("REYE_JOINT1",DEG2RAD(phi));
      my->setJointAngle("LEYE_JOINT0",DEG2RAD(theta));
      my->setJointAngle("REYE_JOINT0",DEG2RAD(theta));
    }
}

void MyController::onCollision(CollisionEvent &evt) 
{
  if (m_grasp == false){  
    typedef CollisionEvent::WithC C;  
  
    //触れたエンティティの名前を得ます  
    const std::vector<std::string> & with = evt.getWith();  
  
    // 衝突した自分のパーツを得ます  
    const std::vector<std::string> & mparts = evt.getMyParts();  
  
    //　衝突したエンティティでループします  
    for(int i = 0; i < with.size(); i++){  
  
      //右手に衝突した場合  
      if(mparts[i] == "RARM_LINK7"){  
  
				//自分を取得  
				SimObj *my = getObj(myname());  
				
				//自分の手のパーツを得ます  
				CParts * parts = my->getParts("RARM_LINK7");  
				parts->graspObj(with[i]);  
	
        m_grasp = true;  
      }  
    }  
  }  
}

void UserController::onInit(InitEvent &evt)
{
	robotName = "robot_000";
    
	//m_kinect = connectToService("SIGKINECT");
	//m_hmd = connectToService("SIGHMD");
	parseFile("command.txt");
    Mission_complete = false;
	//printf("Reslutat %s", rooms[2]);
	rooms.clear();
	objects.clear();
	m_message = 10 ;

	cycle = 0;
    m_state =20;
	srand(time(NULL));

	//初期位置の設定
	SimObj *my = this->getObj(this->myname());
	m_posx = my->x();
	m_posy = my->y();
	m_posz = my->z();
	m_range = 0.1;
	m_maxsize = 15;
}

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

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

double RobotController::onAction(ActionEvent &evt)
{
	SimObj *obj = getObj(myname());
	if (obj) {
		obj->setAccel(0.0, 100.0, 0.0);
	}

	return 5.0;
}

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 MyController::onInit(InitEvent &evt) {  


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

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

void WheelControllerByMyo::onRecvMsg(RecvMsgEvent &evt)
{
	const std::string allMsg = evt.getMsg();

//	std::cout << allMsg << std::endl;

	std::map<std::string, std::vector<std::string> > sensorDataMap = MyoSensorData::decodeSensorData(allMsg);

	if (sensorDataMap.find(MSG_KEY_DEV_TYPE) == sensorDataMap.end()){ return; }

	if(sensorDataMap[MSG_KEY_DEV_TYPE][0]     !=this->myoDeviceManager.deviceType    ){ return; }
	if(sensorDataMap[MSG_KEY_DEV_UNIQUE_ID][0]!=this->myoDeviceManager.deviceUniqueID){ return; }

	MyoSensorData sensorData;
	sensorData.setSensorData(sensorDataMap);

//	std::cout << "roll=" << sensorData.roll << ",pitch=" << sensorData.pitch << ",yaw=" << sensorData.yaw << ",pose=" << sensorData.poseStr << std::endl;
//	std::cout << "emg1=" << sensorData.emgData[0] << ",emg2=" << sensorData.emgData[1] << ", emg3=" << sensorData.emgData[2] << std::endl;

	SimObj *obj = getObj(myname());

	float vel = 0.0;

	for(int i=0; i<MyoSensorData::EMG_SENSOR_NUM; i++)
	{
		vel += std::abs(sensorData.emgData[i]);
	}

	double rvel=0.0, lvel=0.0;

	// Turn left.
	if(sensorData.poseStr==MyoSensorData::mapPoseType2Str.at(MyoSensorData::WaveIn))
	{
		rvel = 3.0*vel;
		lvel = 0.05*vel;
	}
	// Turn right.
	if(sensorData.poseStr==MyoSensorData::mapPoseType2Str.at(MyoSensorData::WaveOut))
	{
		rvel = 0.05*vel;
		lvel = 3.0*vel;
	}
	// Brake.
	if(sensorData.poseStr==MyoSensorData::mapPoseType2Str.at(MyoSensorData::Fist))
	{
		rvel = -3.0*vel;
		lvel = -3.0*vel;
	}

//	std::cout << "rvel=" << rvel << ",lvel=" << lvel << std::endl;

	obj->setJointVelocity("JOINT_RWHEEL", rvel, 500.0);
	obj->setJointVelocity("JOINT_LWHEEL", lvel, 500.0);
}

void UserController::moveRightArm(int task)
{
	SimObj *my = getObj(myname());

	my->setJointAngle("RARM_JOINT0", tab_joint_right[task][0]);
	my->setJointAngle("RARM_JOINT1", tab_joint_right[task][1]);
	my->setJointAngle("RARM_JOINT2", tab_joint_right[task][2]);
	my->setJointAngle("RARM_JOINT3", tab_joint_right[task][3]);
	my->setJointAngle("RARM_JOINT4", tab_joint_right[task][4]);
	my->setJointAngle("RARM_JOINT5", tab_joint_right[task][5]);
	my->setJointAngle("RARM_JOINT6", tab_joint_right[task][6]);
	my->setJointAngle("RARM_JOINT7", tab_joint_right[task][7]);
}

void Simulation::cycle()
{
    //if ((mSimulationTime % 1000) == 0) Gridbrain::debugMutationsCount();

    bool draw = mDrawGraphics && mDrawThisCycle;

    list<SimObj*>::iterator iterObj;

    for (iterObj = mObjectsToKill.begin();
        iterObj != mObjectsToKill.end();
        iterObj++)
    {
        mPopulationDynamics->onOrganismDeath(*iterObj);
    }
    mObjectsToKill.clear();

    if (draw)
    {
        drawBeforeObjects();
    }

    onCycle();

    for (iterObj = mObjects.begin(); iterObj != mObjects.end(); ++iterObj)
    {
        SimObj* obj = *iterObj;

        obj->process(); 
    }

    for (iterObj = mObjects.begin(); iterObj != mObjects.end(); ++iterObj)
    {
        SimObj* obj = *iterObj;

        if (obj->mType == SimObj::TYPE_AGENT)
        {
            obj->perceive();
            obj->emptyMessageList();
            obj->compute();
            obj->act();
        }
    
        if (draw)
        {
            obj->draw();
        }
    }

    if (draw)
    {
        drawAfterObjects();
    }

    mPopulationDynamics->onCycle(mSimulationTime, art_getTime());

    mSimulationTime++;
}

double DetectEntitiesController::onAction(ActionEvent &evt)
{
	std::vector<std::string> agents;
	std::vector<std::string>::iterator i;
	static int count = 0;

	//	LOG_MSG(("bear onAction(count=%d)\n", count));

	bool b = detectEntities(agents);
	if (b)
	{
		int n = agents.size();
		if (n>0)
		{
			LOG_MSG(("%d entities detected", n));
			for (int i=0; i<n; i++)
			{
				std::string name = agents[i];
				LOG_MSG(("[%d] (%s)", i, name.c_str()));
			}
		}
	}
	else
	{
		LOG_MSG(("detectEntities failed"));
	}

	try {
		SimObj *o = getObj(myname());
		if (o && !o->dynamics())
		{
			double deg = count * 10.0;
			double rad = -3.141592/180*deg;

			o->setAxisAndAngle(0.0, 1.0, 0.0, rad);
		}
	} catch(SimObj::NoAttributeException &) {
	} catch(SimObj::AttributeReadOnlyException &) {
	} catch(SimObj::Exception &) {
	}

	count++;

	return 2.0;
}