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

void VO2Controller::render(HGE * hge)
{		
	Mover::Driver::render(hge);
	float size = 5 * mover->getMaster()->getSphereSize();
	Pose pose = mover->getGlobalPose();
	for(auto it=obstacles.begin();it!=obstacles.end();++it)
	{
		VelocityObstacle vo(pose.getPosition(),size,it->first.getPosition(0),it->second,it->first.velocity);		
		drawVO(hge,vo, size, 10);
	}

	drawRays(hge, pose.getPosition(), rays);
//	
//	//for(auto it=segments.begin();it!=segments.end();++it)
//	//	drawArc(object->position,object->maxVelocity*1.1,*it);	
}

bool EvaluationModule::checkAngleToPass(Vector2D targetPosition, Pose currRobotPosition, double & angleToTarget) const{

	angleToTarget = calculateAngleToTarget( currRobotPosition, Pose(targetPosition.x,targetPosition.y,0) );
	double threshold = 0.017 * currRobotPosition.getPosition().distance(targetPosition);
	//LOG_INFO(this->log,"set threshold to "<<threshold);

	if( fabs( angleToTarget ) < threshold ){//1stopien
		return true;
	}
	return false;
}

Task::status Rotate::run(void * arg, int steps){

	GameStatePtr currGameState( new GameState );
	double lastSimTime=0;
	double currSimTime=0;
	currSimTime=video.updateGameState(currGameState);
	//biezaca pozycja robota
	Pose currRobotPose = (*currGameState).getRobotPos( robot->getRobotID() );
	if(this->targetRobotId != Robot::unknown)
		targetPosition = (*currGameState).getRobotPos(this->targetRobotId).getPosition();

	while( !this->stopTask && (steps--)!=0 ){

		if( lastSimTime < ( currSimTime=video.updateGameState(currGameState) ) ){
			lastSimTime = currSimTime;

			currRobotPose=currGameState->getRobotPos( robot->getRobotID() );
			bool haveBall = false;
			double angleToTarget = 0;
			double threshold = 0.017;

			if(this->predicates & Task::pass){
				bool canPass=this->evaluationModule.checkAngleToPass(targetPosition, currRobotPose, angleToTarget);
				threshold = 0.017 * currRobotPose.getPosition().distance(this->targetPosition);
				LOG_INFO(this->log,"set threshold to "<<threshold);

				if( canPass ){
					this->robot->stop();
					LOG_INFO(log,"Rotation to pass OK robot position "<<currRobotPose<<" target "<<targetPosition<<" angle to target "<<angleToTarget);
					return Task::ok;
				}
			}
			else{
				angleToTarget = calculateAngleToTarget( currRobotPose, Pose(targetPosition.x,targetPosition.y,0) );
				double threshold = 0.017; //1 stopien
				if( fabs( angleToTarget ) < threshold ){
					this->robot->stop();
					LOG_INFO(log,"Rotation OK robot position "<<currRobotPose<<" target "<<targetPosition<<" angle to target "<<angleToTarget);
					return Task::ok;
				}
			}
			double w = robot->calculateAngularVel( currRobotPose, targetPosition, currGameState->getSimTime(), haveBall );
			LOG_INFO(log,"move robot from"<<currRobotPose<<" to "<<targetPosition<<" setVel w "<<w<<" angle to target "<<angleToTarget);
			robot->setRelativeSpeed( Vector2D(0,0), w );
		}
		usleep(100);
	}

	if(this->stopTask)
		return Task::ok;

	return Task::not_completed;
}

bool PoseOptimizationObjectiveFunction::getLocalHessian(numopt_common::SparseMatrix& hessian,
                                                        const numopt_common::Parameterization& params, bool newParams)
{
  // Numerical approach.
//  numopt_common::SparseMatrix numericalHessian(params.getLocalSize(), params.getLocalSize());
//  NonlinearObjectiveFunction::estimateLocalHessian(numericalHessian, params, 1.0e-6);
//  std::cout << "Numerical:\n" << numericalHessian << std::endl;

  // Analytical approach.
  Eigen::MatrixXd analyticalHessian(params.getLocalSize(), params.getLocalSize());
  analyticalHessian.setZero();
  const auto& poseParameterization = dynamic_cast<const PoseParameterization&>(params);
  const Pose pose = poseParameterization.getPose();
  const Eigen::Vector3d& p = pose.getPosition().vector();
  const Eigen::Matrix3d p_skew = kindr::getSkewMatrixFromVector(p);
  const RotationQuaternion Phi(pose.getRotation());

  // Default leg position.
  for (const auto& footPosition : stance_) {
    const Eigen::Vector3d& f_i = footPosition.second.vector();
    const Eigen::Matrix3d f_i_skew = kindr::getSkewMatrixFromVector(f_i);
    const Eigen::Vector3d Phi_d_i = pose.getRotation().rotate(nominalStanceInBaseFrame_.at(footPosition.first)).vector();
    const Eigen::Matrix3d Phi_d_i_skew = kindr::getSkewMatrixFromVector(Phi_d_i);
    analyticalHessian.topLeftCorner(3, 3) += Eigen::Matrix3d::Identity();
    analyticalHessian.topRightCorner(3, 3) += -Phi_d_i_skew;
    analyticalHessian.bottomLeftCorner(3, 3) += Phi_d_i_skew;
    analyticalHessian.bottomRightCorner(3, 3) += 0.5 * (p_skew * Phi_d_i_skew + Phi_d_i_skew * p_skew
                                                       -f_i_skew * Phi_d_i_skew - Phi_d_i_skew * f_i_skew);
  }

  // Center of mass.
  const Eigen::Vector3d p_bar(p.x(), p.y(), 0.0); // Projection.
  Eigen::Vector3d Phi_r_com = pose.getRotation().rotate(centerOfMassInBaseFrame_).vector();
  Phi_r_com.z() = 0.0;
  const Eigen::Matrix3d Phi_r_com_skew = kindr::getSkewMatrixFromVector(Phi_r_com);
  const grid_map::Position supportPolygonCentroid(supportRegion_.getCentroid());
  const Eigen::Vector3d r_centroid(supportPolygonCentroid.x(), supportPolygonCentroid.y(), 0.0);
  const Eigen::Matrix3d r_centroid_skew = kindr::getSkewMatrixFromVector(r_centroid);
  analyticalHessian.topLeftCorner(3, 3) += comWeight_ * Eigen::Vector3d(1.0, 1.0, 0.0).asDiagonal();
  analyticalHessian.topRightCorner(3, 3) += -comWeight_ * Phi_r_com_skew;
  analyticalHessian.bottomLeftCorner(3, 3) += comWeight_ * Phi_r_com_skew;
  analyticalHessian.bottomRightCorner(3, 3) += 0.5 * comWeight_ * (p_skew * Phi_r_com_skew + Phi_r_com_skew * p_skew
                                                                  -r_centroid_skew * Phi_r_com_skew - Phi_r_com_skew * r_centroid_skew);

  // Factorized with 2.0 (not weight!).
  analyticalHessian = 2.0 * analyticalHessian;
//  std::cout << "Analytical:\n" << analyticalHessian << std::endl << std::endl;

  // Return solution.
//  hessian = numericalHessian;
  hessian = analyticalHessian.sparseView(1e-10);
  return true;
}

geometry_msgs::Pose MessageConversions::poseToMessage(const Pose& pose)
    const {
  geometry_msgs::Pose message;
  
  const Eigen::Vector3d& position = pose.getPosition();
  message.position.x = position[0];
  message.position.y = position[1];
  message.position.z = position[2];
  
  const Eigen::Quaterniond& orientation = pose.getOrientation();
  message.orientation.x = orientation.x();
  message.orientation.y = orientation.y();
  message.orientation.z = orientation.z();
  message.orientation.w = orientation.w();
  
  return message;
}

bool PoseOptimizationObjectiveFunction::getLocalGradient(numopt_common::Vector& gradient,
                                                         const numopt_common::Parameterization& params, bool newParams)
{
  // Numercical approach.
//  numopt_common::Vector numericalGradient(params.getLocalSize());
//  NonlinearObjectiveFunction::estimateLocalGradient(numericalGradient, params, 1.0e-6);
//  std::cout << "Numerical: " << numericalGradient.transpose() << std::endl;

  // Analytical approach.
  numopt_common::Vector analyticalGradient(params.getLocalSize());
  analyticalGradient.setZero();
  const auto& poseParameterization = dynamic_cast<const PoseParameterization&>(params);
  const Pose pose = poseParameterization.getPose();
  const Eigen::Vector3d& p = pose.getPosition().vector();
  const RotationQuaternion Phi(pose.getRotation());

  // Default leg position.
  for (const auto& footPosition : stance_) {
    const Eigen::Vector3d& f_i = footPosition.second.vector();
    const Eigen::Vector3d Phi_d_i = Phi.rotate(nominalStanceInBaseFrame_.at(footPosition.first)).vector();
    const Eigen::Matrix3d Phi_d_i_skew = kindr::getSkewMatrixFromVector(Phi_d_i);
    analyticalGradient.head(3) += p + Phi_d_i - f_i;
    analyticalGradient.tail(3) += Phi_d_i_skew * p - Phi_d_i_skew * f_i;
  }

  // Center of mass.
  const Eigen::Vector3d p_bar(p.x(), p.y(), 0.0); // Projection.
  Eigen::Vector3d Phi_r_com = Phi.rotate(centerOfMassInBaseFrame_).vector();
  Phi_r_com.z() = 0.0;
  const Eigen::Matrix3d Phi_r_com_skew = kindr::getSkewMatrixFromVector(Phi_r_com);
  const grid_map::Position supportPolygonCentroid(supportRegion_.getCentroid());
  const Eigen::Vector3d r_centroid(supportPolygonCentroid.x(), supportPolygonCentroid.y(), 0.0);
  analyticalGradient.head(3) += comWeight_ * (p_bar - r_centroid + Phi_r_com);
  analyticalGradient.tail(3) += comWeight_ * (Phi_r_com_skew * p_bar - Phi_r_com_skew * r_centroid);

  // Factorized with 2.0 (not weight!).
  analyticalGradient = 2.0 * analyticalGradient;
//  std::cout << "Analytical: " << analyticalGradient.transpose() << std::endl << std::endl;

  // Return solution.
//  gradient = numericalGradient;
  gradient = analyticalGradient;
  return true;
}

void FollowLine::execute(void *){

	LOG_INFO(log,"create start FollowLine tactic. Line "<<p1<<" "<<p2);

	Task::status taskStatus = Task::not_completed;

    Pose goalPose = Pose(this->p1,0);
    Pose nextGoalPose = Pose(this->p2,0);

    //znajdz punkt na odcinku (p1,p2) bedacy najblizej pilki
    //i nie nalezacy do zadnej przeszkody

    //r-nie prostej przechodzacej przez P1 i P2
  //  double a = -( p2.y - p1.y )/( p2.x - p1.x );
 //   double b = 1;
 //   double c = (-1)*p1.x * a - p1.y;

    GameStatePtr gameState ( new GameState() );
    if( Videoserver::getInstance().updateGameState( gameState ) < 0 )
    	throw SimulationException("FollowLine::execute");


    while(true){
       {
    	LockGuard l(this->mutex);
    	if(this->stop)
    		break;
       }
	   taskStatus = Task::not_completed;

	   if( Videoserver::getInstance().updateGameState( gameState ) < 0)
		   throw SimulationException("FollowLine::execute");

	   Pose currPose = gameState->getRobotPos(this->robot.getRobotID());
	   Vector2D diff = currPose.getPosition() - goalPose.getPosition();

	   //podazaj do kranca zdefiniowanego odcinka
	   if( ( fabs(diff.x) < 0.05 ) && ( fabs(diff.y) < 0.05 ) ){
		   Pose tmp = goalPose;
		   goalPose = nextGoalPose;
		   nextGoalPose = tmp;
	   }

		Vector2D robotCurrentGlobalVel=gameState->getRobotGlobalVelocity( robot.getRobotID() );

		//robotNewVel=calculateVelocity( robotCurrentVel, Pose(targetRelPosition.x,targetRelPosition.y,0));

		bool haveBall= false;
		Vector2D robotNewGlobalVel=this->robot.calculateVelocity( robotCurrentGlobalVel, currPose, goalPose, haveBall );
		//double w = robot->calculateAngularVel(*currGameState,robot->getRobotID(), goalPose);
		double w = robot.calculateAngularVel( gameState->getRobotPos( robot.getRobotID() ), goalPose, gameState->getSimTime(), haveBall );

	   //this->robot.setRelativeSpeed( Vector2D( 1.0,0.0 ) * sgn( diff.x ) ,0 );
	   this->robot.setGlobalSpeed( robotNewGlobalVel ,w , currPose.get<2>());

	   /*
	   while( ( fabs(diff.x) > 0.05 ) && ( fabs(diff.y) > 0.05 ) ){
		  // newTask = this->currentTask->nextTask();

		   if( diff.x < 0 )
			   this->robot.setRelativeSpeed( Vector(1.0,0.0),0 );
		   if(){
				this->currentTask = TaskSharedPtr( newTask );
			}
			break;
	   }
	   */
	}
    this->finished = false;
}

bool PoseOptimizationObjectiveFunction::computeValue(numopt_common::Scalar& value,
                                                     const numopt_common::Parameterization& params,
                                                     bool newParams)
{
  const auto& poseParameterization = dynamic_cast<const PoseParameterization&>(params);
  const Pose pose = poseParameterization.getPose();
  value = 0.0;

  // Cost for default leg positions.
  for (const auto& footPosition : stance_) {
    const Position nominalFootPositionInWorld = pose.getPosition()
        + pose.getRotation().rotate(nominalStanceInBaseFrame_.at(footPosition.first)); // d_i
    value += (nominalFootPositionInWorld - footPosition.second).squaredNorm();
  }

  // Cost for foot below hip.
//  for (const auto& foot : stance_) {
//    const Position footPosition(foot.second);
//    const Position hipInWorld = pose.getPosition() + pose.getRotation().rotate(positionsBaseToHipInBaseFrame_.at(foot.first));
//    value += (footPosition - hipInWorld).vector().head(2).squaredNorm();
//  }

//  // Cost for deviation from initial position.
//  Vector positionDifference(state.getPositionWorldToBaseInWorldFrame() - initialPose_.getPosition());
//  value += 0.1 * positionDifference.vector().squaredNorm();

  // Cost for deviation from initial orientation.
//  RotationQuaternion rotationDifference(pose.getRotation() * initialPose_.getRotation().inverted());
//  const double rotationDifferenceNorm = rotationDifference.norm();
//  value += 10.0 * rotationDifferenceNorm * rotationDifferenceNorm;

  // Cost for deviation from horizontal pose.
//  RotationVector rotationDifference(pose.getRotation());
//  rotationDifference.toImplementation().z() = 0.0;
//  const double rotationDifferenceNorm = rotationDifference.vector().norm();
//  value += 1.0 * rotationDifferenceNorm;

  // Cost for CoM.
  const Position centerOfMassInWorldFrame = pose.getPosition() + pose.getRotation().rotate(centerOfMassInBaseFrame_);
  value += comWeight_ * (supportRegion_.getCentroid().head(2) - centerOfMassInWorldFrame.vector().head(2)).squaredNorm();

//  // Cost for torques.
////  state_.setPoseBaseToWorld(pose);
////  adapter_.setInternalDataFromState(state_); // To guide IK.
////
////  for (const auto& foot : stance_) {
////    const Position footPositionInBase(
////        adapter_.transformPosition(adapter_.getWorldFrameId(), adapter_.getBaseFrameId(), foot.second));
////    JointPositionsLeg jointPositions;
////    const bool success = adapter_.getLimbJointPositionsFromPositionBaseToFootInBaseFrame(footPositionInBase, foot.first, jointPositions);
////    if (success) state_.setJointPositionsForLimb(foot.first, jointPositions);
////  }
//
////  adapter_.setInternalDataFromState(state_);
//
////  adapter_.
//
//  for (const auto& foot : stance_) {
//    const Position footPosition(foot.second);
//    const Position baseToFootInBase = pose.getRotation().inverseRotate(footPosition - pose.getPosition());
//    Eigen::Vector3d jointPositions;
//    // https://ashwinnarayan.blogspot.ch/2014/07/inverse-kinematics-for-2dof-arm.html
//    const double l1 = 1.0;
//    const double l1_2 = l1 * l1;
//    const double l2 = 1.0;
//    const double l2_2 = l2 * l2;
//
//    const double x = -baseToFootInBase(2);
//    const double x2 = x * x;
//    const double y = baseToFootInBase(0);
//    const double y2 = y * y;
//    const double k = (x2 + y2 - l1_2 - l2_2) / (2 * l1 * l2);
//    jointPositions(0) = atan2(sqrt(1 - k * k), k);
//
//    const double k1 = l1 + l2 * cos(jointPositions(1));
//    const double k2 = l2 * sin(jointPositions(1));
//    const double gamma = atan2(k2, k1);
//    jointPositions(1) = atan2(y, x) - gamma;
//    value += 0.1 * jointPositions.array().sin().matrix().squaredNorm();
////    JointPositionsLeg jointPositions;
////    adapter_.getLimbJointPositionsFromPositionBaseToFootInBaseFrame(baseToFootInBase, foot.first, jointPositions);
////    value += 0.01 * jointPositions.vector().array().sin().matrix().squaredNorm();
//  }


  return true;
}

 inline static Translation inverseTransform(const Pose & pose, const Translation & position){
   return pose.getRotation().inverseRotate((position-pose.getPosition()));
 }

 inline static Translation transform(const Pose & pose, const Translation & position){
   return pose.getRotation().rotate(position) + pose.getPosition();
 }