Ejemplo n.º 1
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);
}
Ejemplo n.º 2
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;
}
double MyController::rotateTowardObj(Vector3d pos)
{
  // 自分の位置の取得
  Vector3d myPos;
  my->getPosition(myPos);

  // 自分の位置からターゲットを結ぶベクトル
  Vector3d tmpp = pos;
  tmpp -= myPos;
  
  // y方向は考えない
  tmpp.y(0);
  
  // 自分の回転を得る
  Rotation myRot;
  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));

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

  // z方向からの角度
  double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
  double targetAngle = acos(tmp);

  // 方向
  if(tmpp.x() > 0) targetAngle = -1*targetAngle;
  targetAngle += theta;

  return targetAngle;
}
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now)
{
  // 自分の位置の取得
  Vector3d myPos;
  m_my->getPosition(myPos);

  // 自分の位置からターゲットを結ぶベクトル
  Vector3d tmpp = pos;
  tmpp -= myPos;
  
  // 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));

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

  // z方向からの角度
  double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
  double targetAngle = acos(tmp);

  // 方向
  if(tmpp.x() > 0) targetAngle = -1*targetAngle;
  targetAngle += theta;

  if(targetAngle == 0.0){
    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);
    }

    return now + time;
  }
}
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;
  }
		
}
Ejemplo n.º 6
0
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now)
{
	// get own position
	Vector3d myPos;
	m_my->getPosition(myPos);

	// vector from own position to a target position
	Vector3d tmpp = pos;
	tmpp -= myPos;

	// rotation about y-axis is always 0
	tmpp.y(0);

	// get own rotation
	Rotation myRot;
	m_my->getRotation(myRot);

	// initial direction
	Vector3d iniVec(0.0, 0.0, 1.0);

	// get rotation angle about y-axis
	double qw = myRot.qw();
	double qy = myRot.qy();
	double theta = 2*acos(fabs(qw));

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

	// angle from z-axis
	double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
	double targetAngle = acos(tmp);

	// calcurate target angle
	if(tmpp.x() > 0){
		targetAngle = -1*targetAngle;
	}
	targetAngle += theta;

	if(targetAngle<-M_PI){
		 targetAngle += 2*M_PI;
	}
	else if(targetAngle>M_PI){
		targetAngle -= 2*M_PI;
	}

	if(targetAngle == 0.0){
		return 0.0;
	}
	else {
		// circumference length for rotation
		double distance = m_distance*M_PI*fabs(targetAngle)/(2*M_PI);

		// calcurate velocity from radius of wheels
		double vel = m_radius*velocity;

		// rotation time (micro second)
		double time = distance / vel;

		// start rotating
		if(targetAngle > 0.0){
			m_my->setWheelVelocity(velocity, -velocity);
		}
		else{
			m_my->setWheelVelocity(-velocity, velocity);
		}

		return now + time;
	}
}
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now)
{
	// 自分の位置の取得
	Vector3d myPos;
	m_my->getPosition(myPos);

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

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

	// 自分の回転を得る
	Rotation myRot;
	m_my->getRotation(myRot);
	printf("ロボットの現在位置: x: %lf, z %lf \n", myPos.x(), myPos.z());
	
	// エンティティの初期方向
	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));

	if(qw*qy < 0)
		theta = -1*theta;
	
	printf("ロボットが向いている角度 current theta: %lf(deg) \n",  theta * 180 / PI);
	
	// z方向からの角度
	double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
	double targetAngle = acos(tmp);

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

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

		// 車輪の半径から移動速度を得る
		double vel = m_radius*velocity;
		printf("radius: %lf, velocity: %lf, vel: %lf \n", m_radius, velocity, vel);		

		// 回転時間(u秒)
		double time = distance / vel;
		printf("rotateTime: %lf = dis: %lf / vel: %lf\n", time, distance, vel);
		
		// 車輪回転開始
		if(targetAngle > 0.0){
			 m_my->setWheelVelocity(velocity, -velocity);
		}
		else{
			m_my->setWheelVelocity(-velocity, velocity);
		}

		return now + time;
	}
}
Ejemplo n.º 8
0
void motionCallback(int x, int y)
{
  if (selectedView == FRONT)
  {
    controlPoints[selectedPoint[0]][selectedPoint[1]][0] = ToLocalX(x);
    controlPoints[selectedPoint[0]][selectedPoint[1]][1] = ToLocalY(y - height/2);
  }
  else if (selectedView == UP)
  {
    controlPoints[selectedPoint[0]][selectedPoint[1]][0] = ToLocalX(x);
    controlPoints[selectedPoint[0]][selectedPoint[1]][2] = -ToLocalY(y);
  }
  else if (selectedView == LEFT)
  {
    controlPoints[selectedPoint[0]][selectedPoint[1]][2] = -ToLocalX(x - width/2);
    controlPoints[selectedPoint[0]][selectedPoint[1]][1] = ToLocalY(y - height/2);
  }
  else if (selectedView == ROTATE)
  {
    Vector3d lastP = getMousePoint(lastX, lastY, width / 2.0f, height / 2.0f, radius);
    Vector3d currentP = getMousePoint(x - width / 2.0f, y, width / 2.0f, height / 2.0f, radius);

    if (mouseButton == GLUT_LEFT_BUTTON)
    {
      Vector3d rotateVector;
      rotateVector.cross(currentP, lastP);
      double angle = -currentP.angle(lastP) * 2;
      rotateVector = unProjectToEye(rotateVector, eye, center, upVector);

      Vector3d dEye;
      dEye.sub(center, eye);
      dEye = rotate(dEye, rotateVector, -angle);
      upVector = rotate(upVector, rotateVector, -angle);
      eye.sub(center, dEye);
    }
    else if (mouseButton == GLUT_RIGHT_BUTTON) {
      Vector3d dEye;
      dEye.sub(center, eye);
      double offset = 0.025;
      if ((y - lastY) < 0) {
        dEye.scale(1 - offset);
      }
      else {
        dEye.scale(1 + offset);
      }
      eye.sub(center, dEye);
    }
    else if (mouseButton == GLUT_MIDDLE_BUTTON) {
      double dx = x - width / 2.0f - lastX;
      double dy = y - lastY;
      if (dx != 0 || dy != 0)
      {
        Vector3d moveVector(dx, dy, 0);
        moveVector = unProjectToEye(moveVector, eye, center, upVector);
        moveVector.normalize();
        double eyeDistance = Vector3d(eye).distance(Vector3d(center));
        moveVector.scale(std::sqrt(dx*dx + dy*dy) / 1000 * eyeDistance);
        center.add(moveVector);
        eye.add(moveVector);
      }
    }
    lastX = x - width / 2;
    lastY = y;
  }

  EvaluateSurface();
  glutPostRedisplay();
}