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

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

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

void DemoRobotController::onCollision(CollisionEvent &evt)
{
	if (m_grasp == false) {
		typedef CollisionEvent::WithC C;
		// Get name of entity which is touched by the robot
		const std::vector<std::string> & with = evt.getWith();
		// Get parts of the robot which is touched by the entity
		const std::vector<std::string> & mparts = evt.getMyParts();

		// loop for every collided entities
		for(int i = 0; i < with.size(); i++) {
			if(m_graspObjectName == with[i]) {
				// If the right hand touches the entity
				if(mparts[i] == "RARM_LINK7") {
					SimObj *my = getObj(myname());
					CParts * parts = my->getParts("RARM_LINK7");
					if(parts->graspObj(with[i])) m_grasp = true;
				}
			}
		}
	}
}

double MyController::rotateTowardGrabPos(Vector3d pos, double velocity, double now)
{

	printf("start rotate %lf \n", now);
	//自分を取得  
	SimObj *my = getObj(myname());  
	
	//printf("向く座標 %lf %lf %lf \n", pos.x(), pos.y(), pos.z());
  // 自分の位置の取得
  Vector3d myPos;
  m_my->getPosition(myPos);



	//自分の手のパーツを得ます  
	CParts * parts = my->getParts("RARM_LINK7");  

	Vector3d partPos;
	parts->getPosition(partPos);


  // 自分の位置からターゲットを結ぶベクトル
  Vector3d tmpp = pos;
  //tmpp -= myPos;
	tmpp -= partPos;

  // y方向は考えない
  tmpp.y(0);
  
  // 自分の回転を得る
  Rotation myRot;
  m_my->getRotation(myRot);

  // エンティティの初期方向
  Vector3d iniVec(0.0, 0.0, 1.0);
      
  // y軸の回転角度を得る(x,z方向の回転は無いと仮定)
  double qw = myRot.qw();
  double qy = myRot.qy();
      
  double theta = 2*acos(fabs(qw));
	//printf("qw: %lf theta: %lf \n", qw, theta);

  if(qw*qy < 0) {
		//printf("qw * qy < 0 \n");
    theta = -1*theta;		
	}

  // z方向からの角度
 	//printf("結ぶベクトル　座標 %lf %lf %lf \n", tmpp.x(), tmpp.y(), tmpp.z());
  double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
	//printf("tmp: %lf \n", tmp);
  double targetAngle = acos(tmp);
	//printf("targetAngle: %lf ---> %lf \n", targetAngle, targetAngle*180.0/PI);

  // 方向
	//printf("tmpp.x() %lf \n", tmpp.x());
  if(tmpp.x() > 0) {
		targetAngle = -1*targetAngle;
		//printf("targetAngle: %lf deg \n", targetAngle*180.0/PI);
	}
  targetAngle += theta;
	//printf("targetAngle: %lf <--- %lf \n", targetAngle, theta);

	//printf("qw: %lf qy: %lf theta: %lf tmp: %lf targetAngle: %lf \n", qw, qy, theta, tmp, targetAngle);
	
  if(targetAngle == 0.0){
		//printf("donot need rotate \n");
    return 0.0;
  }
  else 
	{
    // 回転すべき円周距離
    double distance = m_distance*PI*fabs(targetAngle)/(2*PI);

    // 車輪の半径から移動速度を得る
    double vel = m_radius*velocity;
    
    // 回転時間(u秒)
    double time = distance / vel;
    
    // 車輪回転開始
    if(targetAngle > 0.0){
      m_my->setWheelVelocity(velocity, -velocity);
    }
    else
		{
      m_my->setWheelVelocity(-velocity, velocity);
    }

		//printf("distance: %lf vel: %lf time: %lf \n", distance, vel, time);
		printf("rotate time: %lf, time to stop: %lf \n", time, now + time);
    return now + time;
  }
		
}

//.........这里部分代码省略.........
  // 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();