Ejemplo n.º 1
2
PlaneExt::PlaneExt(but_plane_detector::Plane<float> plane) : but_plane_detector::Plane<float> (0.0, 0.0, 0.0, 0.0), planeCoefficients(new pcl::ModelCoefficients())
{
	a = plane.a;
	b = plane.b;
	c = plane.c;
	d = plane.d;
	norm = plane.norm;

	// Create a quaternion for rotation into XY plane
	Eigen::Vector3f current(a, b, c);
	Eigen::Vector3f target(0.0, 0.0, 1.0);
	Eigen::Quaternion<float> q;
	q.setFromTwoVectors(current, target);
	planeTransXY = q.toRotationMatrix();

	planeShift = -d;

	color.r = abs(a) / 2.0 + 0.2;
	color.g = abs(b) / 2.0 + 0.2;
	color.b = abs(d) / 5.0;
	color.a = 1.0;

	// Plane coefficients pre-calculation...
	planeCoefficients->values.push_back(a);
	planeCoefficients->values.push_back(b);
	planeCoefficients->values.push_back(c);
	planeCoefficients->values.push_back(d);

}
Ejemplo n.º 2
1
  bool testRawDataAcces() {
    bool passed = true;
    Eigen::Matrix<Scalar, 4, 1> raw = {0, 1, 0, 0};
    Eigen::Map<RxSO3Type const> map_of_const_rxso3(raw.data());
    SOPHUS_TEST_APPROX(passed, map_of_const_rxso3.quaternion().coeffs().eval(),
                       raw, Constants<Scalar>::epsilon());
    SOPHUS_TEST_EQUAL(passed, map_of_const_rxso3.quaternion().coeffs().data(),
                      raw.data());
    Eigen::Map<RxSO3Type const> const_shallow_copy = map_of_const_rxso3;
    SOPHUS_TEST_EQUAL(passed, const_shallow_copy.quaternion().coeffs().eval(),
                      map_of_const_rxso3.quaternion().coeffs().eval());

    Eigen::Matrix<Scalar, 4, 1> raw2 = {1, 0, 0, 0};
    Eigen::Map<RxSO3Type> map_of_rxso3(raw.data());
    Eigen::Quaternion<Scalar> quat;
    quat.coeffs() = raw2;
    map_of_rxso3.setQuaternion(quat);
    SOPHUS_TEST_APPROX(passed, map_of_rxso3.quaternion().coeffs().eval(), raw2,
                       Constants<Scalar>::epsilon());
    SOPHUS_TEST_EQUAL(passed, map_of_rxso3.quaternion().coeffs().data(),
                      raw.data());
    SOPHUS_TEST_NEQ(passed, map_of_rxso3.quaternion().coeffs().data(),
                    quat.coeffs().data());
    Eigen::Map<RxSO3Type> shallow_copy = map_of_rxso3;
    SOPHUS_TEST_EQUAL(passed, shallow_copy.quaternion().coeffs().eval(),
                      map_of_rxso3.quaternion().coeffs().eval());

    RxSO3Type const const_so3(quat);
    for (int i = 0; i < 4; ++i) {
      SOPHUS_TEST_EQUAL(passed, const_so3.data()[i], raw2.data()[i]);
    }

    RxSO3Type so3(quat);
    for (int i = 0; i < 4; ++i) {
      so3.data()[i] = raw[i];
    }

    for (int i = 0; i < 4; ++i) {
      SOPHUS_TEST_EQUAL(passed, so3.data()[i], raw.data()[i]);
    }

    for (int i = 0; i < 10; ++i) {
      Matrix3<Scalar> M = Matrix3<Scalar>::Random();
      for (Scalar scale : {Scalar(0.01), Scalar(0.99), Scalar(1), Scalar(10)}) {
        Matrix3<Scalar> R = makeRotationMatrix(M);
        Matrix3<Scalar> sR = scale * R;
        SOPHUS_TEST(passed, isScaledOrthogonalAndPositive(sR),
                    "isScaledOrthogonalAndPositive(sR): % *\n%", scale, R);
        Matrix3<Scalar> sR_cols_swapped;
        sR_cols_swapped << sR.col(1), sR.col(0), sR.col(2);
        SOPHUS_TEST(passed, !isScaledOrthogonalAndPositive(sR_cols_swapped),
                    "isScaledOrthogonalAndPositive(-sR): % *\n%", scale, R);
      }
    }
    return passed;
  }
Ejemplo n.º 3
1
 // returns the local R,t in nd0 that produces nd1
 // NOTE: returns a postfix rotation; should return a prefix
 void transformN2N(Eigen::Matrix<double,4,1> &trans, 
                   Eigen::Quaternion<double> &qr,
                   Node &nd0, Node &nd1)
 {
   Matrix<double,3,4> tfm;
   Quaterniond q0,q1;
   q0 = nd0.qrot;
   transformW2F(tfm,nd0.trans,q0);
   trans.head(3) = tfm*nd1.trans;
   trans(3) = 1.0;
   q1 = nd1.qrot;
   qr = q0.inverse()*q1;
   qr.normalize();
   if (qr.w() < 0)
     qr.coeffs() = -qr.coeffs();
 }
Ejemplo n.º 4
1
Eigen::Quaternion<T> naive_slerp(const Eigen::Quaternion<T>& input, const Eigen::Quaternion<T>& target, T amt){
  Eigen::Quaternion<T> result;
	
  if (amt==T(0)) {
    return input;
  } else if (amt==T(1)) {
    return target;
  }
	
  int bflip = 0;
  T dot_prod = input.dot(target);
  T a, b;
	
  //clamp
  dot_prod = (dot_prod < -1) ? -1 : ((dot_prod > 1) ? 1 : dot_prod);
	
  // if B is on opposite hemisphere from A, use -B instead
  if (dot_prod < 0.0) {
    dot_prod = -dot_prod;
    bflip = 1;
  }
	
  T cos_angle = acos(dot_prod);
  if(ABS(cos_angle) > QUAT_EPSILON) {
    T sine = sin(cos_angle);
    T inv_sine = 1./sine;
		
    a = sin(cos_angle*(1.-amt)) * inv_sine;
    b = sin(cos_angle*amt) * inv_sine;
		
    if (bflip) { b = -b; }
  } else {
    // nearly the same;
    // approximate without trigonometry
    a = amt;
    b = 1.-amt;
  }
	
  result.w = a*input.w + b*target.w;
  result.x = a*input.x + b*target.x;
  result.y = a*input.y + b*target.y;
  result.z = a*input.z + b*target.z;
	
  result.normalize();
  return result;
}
    inline static void setFromVectors(Rotation_& rot, const Eigen::Matrix<PrimType_, 3, 1>& v1, const Eigen::Matrix<PrimType_, 3, 1>& v2) {
        KINDR_ASSERT_TRUE(std::runtime_error,  v1.norm()*v2.norm() != static_cast<PrimType_>(0.0), "At least one vector has zero length.");

        Eigen::Quaternion<PrimType_> eigenQuat;
        eigenQuat.setFromTwoVectors(v1, v2);
        rot = kindr::rotations::eigen_impl::RotationQuaternion<PrimType_, Usage_>(eigenQuat);
//
//    const PrimType_ angle = acos(v1.dot(v2)/temp);
//    const PrimType_ tol = 1e-3;
//
//    if(0 <= angle && angle < tol) {
//      rot.setIdentity();
//    } else if(M_PI - tol < angle && angle < M_PI + tol) {
//      rot = eigen_impl::AngleAxis<PrimType_, Usage_>(angle, 1, 0, 0);
//    } else {
//      const Eigen::Matrix<PrimType_, 3, 1> axis = (v1.cross(v2)).normalized();
//      rot = eigen_impl::AngleAxis<PrimType_, Usage_>(angle, axis);
//    }
    }
Ejemplo n.º 6
1
visualization_msgs::Marker SatDetector3DMonitor::getMarker(int marker_id, Eigen::Vector4f centroid, Eigen::Matrix3f covariance_matrix){

  //------- Compute Principal Componets for Marker publishing


  //Get the principal components and the quaternion
  Eigen::Matrix3f evecs;
  Eigen::Vector3f evals;
  //pcl::eigen33 (covariance_matrix, evecs, evals);
  Eigen::EigenSolver<Eigen::Matrix3f> es(covariance_matrix);
	
  evecs = es.eigenvectors().real();
  evals = es.eigenvalues().real();
	    
  Eigen::Matrix3f rotation;
  rotation.row (0) = evecs.col (0);
  rotation.row (1) = evecs.col (1);
  rotation.row (2) = rotation.row (0).cross (rotation.row (1));
	    
  rotation.transposeInPlace ();
  Eigen::Quaternion<float> qt (rotation);
  qt.normalize ();
	    
  //Publish Marker for cluster
  visualization_msgs::Marker marker;	
		
  marker.header.frame_id = base_frame_;
  marker.header.stamp = ros::Time().now();
  marker.ns = "Perception";
  marker.action = visualization_msgs::Marker::ADD;
  marker.id = marker_id;	
  marker.lifetime = ros::Duration(1);	
		
  //centroid position
  marker.type = visualization_msgs::Marker::SPHERE;
  marker.pose.position.x = centroid[0];
  marker.pose.position.y = centroid[1];
  marker.pose.position.z = centroid[2];	
  marker.pose.orientation.x = qt.x();
  marker.pose.orientation.y = qt.y();
  marker.pose.orientation.z = qt.z();
  marker.pose.orientation.w = qt.w();			

  //std::cout << "e1: " << evals(0) << " e2: " << evals(1) << " e3: " << evals(2) << std::endl;

  marker.scale.x = sqrt(evals(0)) * 4;
  marker.scale.y = sqrt(evals(1)) * 4;
  marker.scale.z = sqrt(evals(2)) * 4;
	

  //give it some color!
  marker.color.a = 0.75;
  satToRGB(marker.color.r, marker.color.g, marker.color.b);

  //std::cout << "marker being published" << std::endl;

  return marker;
}
Ejemplo n.º 7
0
IGL_INLINE void igl::two_axis_valuator_fixed_up(
  const int w,
  const int h,
  const double speed,
  const Eigen::Quaternion<Scalardown_quat> & down_quat,
  const int down_x,
  const int down_y,
  const int mouse_x,
  const int mouse_y,
  Eigen::Quaternion<Scalarquat> & quat)
{
  using namespace Eigen;
  Matrix<Scalarquat,3,1> axis(0,1,0);
  quat = down_quat *
    Quaternion<Scalarquat>(
      AngleAxis<Scalarquat>(
        PI*((Scalarquat)(mouse_x-down_x))/(Scalarquat)w*speed/2.0,
        axis.normalized()));
  quat.normalize();
  {
    Matrix<Scalarquat,3,1> axis(1,0,0);
    if(axis.norm() != 0)
    {
      quat =
        Quaternion<Scalarquat>(
          AngleAxis<Scalarquat>(
            PI*(mouse_y-down_y)/(Scalarquat)h*speed/2.0,
            axis.normalized())) * quat;
      quat.normalize();
    }
  }
}
Ejemplo n.º 8
0
    SE3<> IMU2camWorldfromQuat(Eigen::Quaternion<double> atti){// use ini attitude info from EKF node to ini ptam pose
        Vector<4> attivec = makeVector(atti.w(), atti.x(), atti.y(), atti.z());//Rw1i
        Matrix<3> iniOrientationEKF;
        iniOrientationEKF = tag::quaternionToMatrix(attivec);

        Matrix<3> roll = TooN::Data(1.0, 0, 0,//Rww1, because the roll and pitch angles are in
                              0, -1, 0, // a world frame which pointing downward.
                              0, 0, -1);

        SE3<> camWorld = SE3<>();
        Matrix<3> rotation;
        if (tracker_->attitude_get)
            rotation = iniOrientationEKF; //
        else
            rotation = roll * iniOrientationEKF;//Rwi = Rww1*Rw1i
        camWorld.get_rotation() = rotation*se3IMUfromcam.get_rotation().get_matrix();//Rwc = (Rwi * Ric)

        Vector<3> twr = makeVector(0.0, 0.0, 0.198);// twc = twr + Rwr * trc
        Vector<3> twc = twr + rotation * se3IMUfromcam.get_translation();
        camWorld.get_translation()[0] = 0.0;//twc[0]; //twc
        camWorld.get_translation()[1] = 0.0;//twc[1];
        camWorld.get_translation()[2] = twc[2];

        camWorld = camWorld.inverse();//Tcw

        cout<< "TCW INITIALIZED. TWC: " << twc[0]<< ", " << twc[1]<< ", " << twc[2]<<endl;
//        cout<< camWorld.get_rotation().get_matrix()(2,2)<<endl;
        return camWorld;
    }
Ejemplo n.º 9
0
void HLManager::onVTTwist(const geometry_msgs::TwistStamped::ConstPtr& twist)
{
	if (mode == circle || mode == vtManual)
	{
		//\todo Generalize this 0.1 with Ts.
		s +=twist->twist.linear.x*0.1;

		//Circle
		if (s>=2*circleRadius*M_PI) s=s-2*circleRadius*M_PI;
		else if (s<0) s=2*circleRadius*M_PI-s;

		double xRabbit = point.point.x + circleRadius*cos(s/circleRadius);
		double yRabbit = point.point.y + circleRadius*sin(s/circleRadius);
		double gammaRabbit=labust::math::wrapRad(s/circleRadius)+M_PI/2;

		tf::Transform transform;
		transform.setOrigin(tf::Vector3(xRabbit, yRabbit, 0));
		Eigen::Quaternion<float> q;
		labust::tools::quaternionFromEulerZYX(0,0,gammaRabbit, q);
		transform.setRotation(tf::Quaternion(q.x(),q.y(),q.z(),q.w()));
		broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "local", "serret_frenet_frame"));

		auv_msgs::NavStsPtr msg(new auv_msgs::NavSts());
		msg->position.north = xRabbit;
		msg->position.east = yRabbit;
		msg->body_velocity.x = twist->twist.linear.x;
		msg->orientation.yaw = gammaRabbit;
		sfPub.publish(msg);
	}
}
Ejemplo n.º 10
0
void HLManager::start()
{
	ros::Rate rate(10);

	while (nh.ok())
	{
		if (fixValidated)
		{
			tf::Transform transform;
			transform.setOrigin(tf::Vector3(originLon, originLat, 0));
			transform.setRotation(tf::createQuaternionFromRPY(0,0,0));
			broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/worldLatLon", "/world"));
			transform.setOrigin(tf::Vector3(0, 0, 0));
			Eigen::Quaternion<float> q;
			labust::tools::quaternionFromEulerZYX(M_PI,0,M_PI/2,q);
			transform.setRotation(tf::Quaternion(q.x(),q.y(),q.z(),q.w()));
			broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "local"));
		}
		this->safetyTest();
		this->step();
		hlMessagePub.publish(hlDiagnostics);
		rate.sleep();
		ros::spinOnce();
	}
}
bool CloudMatcher::match(modular_cloud_matcher::MatchClouds::Request& req, modular_cloud_matcher::MatchClouds::Response& res)
{
	// get and check reference
	size_t referenceGoodCount;
	DP referenceCloud(rosMsgToPointMatcherCloud(req.reference, referenceGoodCount));
	const unsigned referencePointCount(req.reference.width * req.reference.height);
	const double referenceGoodRatio(double(referenceGoodCount) / double(referencePointCount));
	
	if (referenceGoodCount == 0)
	{
		ROS_ERROR("I found no good points in the reference cloud");
		return false;
	}
	if (referenceGoodRatio < 0.5)
	{
		ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - referenceGoodRatio*100.0 << "% of the cloud (received " << referenceGoodCount << ")");
	}
	
	// get and check reading
	size_t readingGoodCount;
	DP readingCloud(rosMsgToPointMatcherCloud(req.readings, readingGoodCount));
	const unsigned readingPointCount(req.readings.width * req.readings.height);
	const double readingGoodRatio(double(readingGoodCount) / double(readingPointCount));
	
	if (readingGoodCount == 0)
	{
		ROS_ERROR("I found no good points in the reading cloud");
		return false;
	}
	if (readingGoodRatio < 0.5)
	{
		ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - readingGoodRatio*100.0 << "% of the cloud (received " << readingGoodCount << ")");
	}
	
	// call icp
	TP transform;
	try 
	{
		transform = icp(readingCloud, referenceCloud);
		ROS_INFO_STREAM("match ratio: " << icp.errorMinimizer->getWeightedPointUsedRatio() << endl);
	}
	catch (PM::ConvergenceError error)
	{
		ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
		return false;
	}
	
	// fill return value
	res.transform.translation.x = transform.coeff(0,3);
	res.transform.translation.y = transform.coeff(1,3);
	res.transform.translation.z = transform.coeff(2,3);
	const Eigen::Quaternion<Scalar> quat(Matrix3(transform.block(0,0,3,3)));
	res.transform.rotation.x = quat.x();
	res.transform.rotation.y = quat.y();
	res.transform.rotation.z = quat.z();
	res.transform.rotation.w = quat.w();
	
	return true;
}
Ejemplo n.º 12
0
void quaternion2vector(const Eigen::Quaternion<T_qt> &quat_In, std::vector<T_vec> &quat_Vec )
{
    quat_Vec.resize(4,T_vec(0.0));
    quat_Vec[0] = T_vec( quat_In.w() );
    quat_Vec[1] = T_vec( quat_In.x() );
    quat_Vec[2] = T_vec( quat_In.y() );
    quat_Vec[3] = T_vec( quat_In.z() );
};
IGL_INLINE bool igl::snap_to_canonical_view_quat(
  const Eigen::Quaternion<Scalarq> & q,
  const double threshold,
  Eigen::Quaternion<Scalars> & s)
{
  return snap_to_canonical_view_quat<Scalars>(
    q.coeffs().data(),threshold,s.coeffs().data());
}
Ejemplo n.º 14
0
void SetTwistHandler::handleSimulation(){
    // called when the main script calls: simHandleModule
    if(!_initialized){
        _initialize();
    }

    Eigen::Quaternion < simFloat > orientation; //(x,y,z,w)
    Eigen::Matrix< simFloat, 3, 1> linVelocity((simFloat)_twistCommands.twist.linear.x,
    		(simFloat)_twistCommands.twist.linear.y,
    		(simFloat)_twistCommands.twist.linear.z);
    Eigen::Matrix< simFloat, 3, 1> angVelocity((simFloat)_twistCommands.twist.angular.x,
    		(simFloat)_twistCommands.twist.angular.y,
    		(simFloat)_twistCommands.twist.angular.z);

    // Input velocity is expected to be in body frame but V-Rep expects it to be in world frame.
    if(simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data())!=-1){
        linVelocity = orientation*linVelocity;
	    angVelocity = orientation*angVelocity;
    } else {
        std::stringstream ss;
        ss << "- [" << _associatedObjectName << "] Error getting orientation. " << std::endl;
        ConsoleHandler::printInConsole(ss);
    }

    simResetDynamicObject(_associatedObjectID);

	//Set object velocity
    if (_isStatic){
    	Eigen::Matrix<simFloat, 3, 1> position;
    	simGetObjectPosition(_associatedObjectID, -1, position.data());
    	const simFloat timeStep = simGetSimulationTimeStep();
    	position = position + timeStep*linVelocity;
    	simSetObjectPosition(_associatedObjectID, -1, position.data());
    	const simFloat angle = timeStep*angVelocity.norm();
    	if(angle > 1e-6){
    		orientation = Eigen::Quaternion< simFloat >(Eigen::AngleAxis< simFloat >(timeStep*angVelocity.norm(), angVelocity/angVelocity.norm()))*orientation;
    	}
    	simSetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data());
    } else {
		// Apply the linear velocity to the object
		if(simSetObjectFloatParameter(_associatedObjectID, 3000, linVelocity[0])
			&& simSetObjectFloatParameter(_associatedObjectID, 3001, linVelocity[1])
			&& simSetObjectFloatParameter(_associatedObjectID, 3002, linVelocity[2])==-1) {
			std::stringstream ss;
			ss << "- [" << _associatedObjectName << "] Error setting linear velocity. " << std::endl;;
			ConsoleHandler::printInConsole(ss);
		}
		// Apply the angular velocity to the object
		if(simSetObjectFloatParameter(_associatedObjectID, 3020, angVelocity[0])
			&& simSetObjectFloatParameter(_associatedObjectID, 3021, angVelocity[1])
			&& simSetObjectFloatParameter(_associatedObjectID, 3022, angVelocity[2])==-1) {
			std::stringstream ss;
			ss << "- [" << _associatedObjectName << "] Error setting angular velocity. " << std::endl;;
			ConsoleHandler::printInConsole(ss);
		}
    }

}
Ejemplo n.º 15
0
void GetTwistHandler::handleSimulation(){
    // called when the main script calls: simHandleModule
    if(!_initialized){
        _initialize();
    }

    ros::Time now = ros::Time::now();


    const simFloat currentSimulationTime = simGetSimulationTime();

    if ((currentSimulationTime-_lastPublishedObjTwistTime) >= 1.0/_ObjTwistFrequency){

    	Eigen::Quaternion< simFloat > orientation; //(x,y,z,w)
    	Eigen::Matrix<simFloat, 3, 1> linVelocity;
    	Eigen::Matrix<simFloat, 3, 1> angVelocity;
    	bool error = false;

    	// Get object velocity. If the object is not static simGetVelocity is more accurate.
    	if (_isStatic){
    		error = error || simGetObjectVelocity(_associatedObjectID, linVelocity.data(), angVelocity.data()) == -1;
    	} else {
    		error = error || simGetVelocity(_associatedObjectID, linVelocity.data(), angVelocity.data()) == -1;
    	}

    	// Get object orientation
    	error = error || simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data()) == -1;

    	if(!error){

    		linVelocity = orientation.conjugate()*linVelocity; // Express linear velocity in body frame
			angVelocity = orientation.conjugate()*angVelocity; // Express angular velocity in body frame

    		// Fill the status msg
			geometry_msgs::TwistStamped msg;

			msg.twist.linear.x = linVelocity[0];
			msg.twist.linear.y = linVelocity[1];
			msg.twist.linear.z = linVelocity[2];
			msg.twist.angular.x = angVelocity[0];
			msg.twist.angular.y = angVelocity[1];
			msg.twist.angular.z = angVelocity[2];

			msg.header.stamp = now;
			_pub.publish(msg);
			_lastPublishedObjTwistTime = currentSimulationTime;

    	} else {
    	    std::stringstream ss;
    	    ss << "- [" << _associatedObjectName << "] Error getting object velocity and/or orientation." << std::endl;;
    	    ConsoleHandler::printInConsole(ss);
    	}

    }

}
Ejemplo n.º 16
0
void YouBot::updateWheels(const ros::TimerEvent& e)
{
  try
  {
    if(q_base_.rows() != robot_->getMacroManipulatorDOF()) return;
    if(tau_base_.rows() != robot_->getMacroManipulatorDOF()) return;
    if(!gazebo_interface_wheel_->subscribed()) return;

    Eigen::VectorXd q = gazebo_interface_wheel_->getJointStates();
    robot_->updateWheel(q);

    Eigen::Vector3d base_pos;
    base_pos << q_base_[0], q_base_[1], 0.0;
    Eigen::Quaternion<double> base_ori;
    base_ori.x() = 0.0;
    base_ori.y() = 0.0;
    base_ori.z() = sin(0.5 * q_base_[2]);
    base_ori.w() = cos(0.5 * q_base_[2]);

    robot_->updateBase(base_pos, base_ori);

    Eigen::VectorXd v_base;
    controller_->computeBaseVelocityFromTorque(tau_base_, v_base, 3);

    Eigen::VectorXd v_wheel;
    controller_->computeWheelVelocityFromBaseVelocity(v_base, v_wheel);

    Eigen::VectorXd q_wheel_d = robot_->getMobility()->update_rate * v_wheel;

    std::vector<Eigen::Quaternion<double> > quat_d;
    for(unsigned int i = 0; i < q_wheel_d.rows(); ++i)
    {
      double rad = q_wheel_d[i];

      Eigen::Quaternion<double> quat;
      quat.x() = 0.0;
      quat.y() = sin(0.5 * rad);
      quat.z() = 0.0;
      quat.w() = cos(0.5 * rad);

      quat_d.push_back(quat);
    }

    gazebo_interface_wheel_->rotateLink(quat_d);
  }
  catch(ahl_robot::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
  catch(ahl_ctrl::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
}
Ejemplo n.º 17
0
 void transformF2W(Eigen::Matrix<double,3,4> &m, 
                   const Eigen::Matrix<double,4,1> &trans, 
                   const Eigen::Quaternion<double> &qrot)
 {
   m.block<3,3>(0,0) = qrot.toRotationMatrix();
   m.col(3) = trans.head(3);
 };
Ejemplo n.º 18
0
Eigen::Matrix< Tp, 4, 4 > transformationMatrix( Eigen::Quaternion< Tp > &quaternion, Eigen::Matrix< Tp, 3, 1 > &translation)
{
    Eigen::Matrix< Tp, 3, 3 > rr = quaternion.toRotationMatrix();
    Eigen::Matrix< Tp, 4, 4 > ttmm;
    ttmm << rr, translation, 0.0, 0.0, 0.0, 1.0;
    return ttmm;
}
Ejemplo n.º 19
0
    /**
     * @brief Computes the trackball's rotation, using stored initial and final position vectors.
     */
    void computeRotationAngle (void)
    {
        //Given two position vectors, corresponding to the initial and final mouse coordinates, calculate the rotation of the sphere that will keep the mouse always in the initial position.

        if(initialPosition.norm() > 0) {
            initialPosition.normalize();
        }
        if(finalPosition.norm() > 0) {
            finalPosition.normalize();
        }

        //cout << "Initial Position: " << initialPosition.transpose() << " Final Position: " << finalPosition.transpose() << endl << endl;

        Eigen::Vector3f rotationAxis = initialPosition.cross(finalPosition);

        if(rotationAxis.norm() != 0) {
            rotationAxis.normalize();
        }

        float dot = initialPosition.dot(finalPosition);

        float rotationAngle = (dot <= 1) ? acos(dot) : 0;//If, by losing floating point precision, the dot product between the initial and final positions is bigger than one, ignore the rotation.

        Eigen::Quaternion<float> q (Eigen::AngleAxis<float>(rotationAngle,rotationAxis));

        quaternion = q * quaternion;
        quaternion.normalize();
    }
Ejemplo n.º 20
0
    void rpyToQuaternion(const Eigen::Vector3d& rpy, Eigen::Quaternion<double>& q)
    {
      double a = rpy.coeff(0);
      double b = rpy.coeff(1);
      double g = rpy.coeff(2);

      double sin_b_half = sin(0.5 * b);
      double cos_b_half = cos(0.5 * b);
      double diff_a_g_half = 0.5 * (a - g);
      double sum_a_g_half = 0.5 * (a + g);

      q.x() = sin_b_half * cos(diff_a_g_half);
      q.y() = sin_b_half * sin(diff_a_g_half);
      q.z() = cos_b_half * sin(sum_a_g_half);
      q.w() = cos_b_half * cos(sum_a_g_half);
    }
Ejemplo n.º 21
0
 // transforms
 void transformW2F(Eigen::Matrix<double,3,4> &m, 
                   const Eigen::Matrix<double,4,1> &trans, 
                   const Eigen::Quaternion<double> &qrot)
 {
   m.block<3,3>(0,0) = qrot.toRotationMatrix();//.transpose();
   m.col(3).setZero();         // make sure there's no translation
   m.col(3) = -m*trans;
 };
Ejemplo n.º 22
0
  Eigen::Vector3d pose3Dto2D (geometry_msgs::Point const & translation,
			      geometry_msgs::Quaternion const & orientation)
  {
    // Can somebody please explain to me why Eigen has chosen to store
    // the parameters in (x,y,z,w) order, but provide a ctor in
    // (w,x,y,z) order? What have they been smoking?
    //
    Eigen::Quaternion<double> const rot (orientation.w,
					 orientation.x,
					 orientation.y,
					 orientation.z);
    Eigen::Vector3d ux;
    ux << 1.0, 0.0, 0.0;
    ux = rot._transformVector (ux);
    Eigen::Vector3d pose (translation.x, translation.y, atan2 (ux.y(), ux.x()));
    return pose;
  }
Ejemplo n.º 23
0
base::Vector6d AuvMotion::gravity_buoyancy(const Eigen::Quaternion<double> q)
{
    base::Vector6d gravitybuoyancy;
  
    double mass;
    control->nodes->getNodeMass(vehicle_id ,&mass);
    double buoyancy = _buoyancy_force;
    base::Vector3d pos = control->nodes->getPosition(vehicle_id);
  
    // In Quaternion form
    float e1 = q.x();
    float e2 = q.y();
    float e3 = q.z();
    float e4 = q.w();
    float xg = 0.0;
    float yg = 0.0;
    float zg = 0.0;
    float xb = centerOfBuoyancy[0];
    float yb = centerOfBuoyancy[1];
    float zb = centerOfBuoyancy[2];;

    gravitybuoyancy(0) = 0.0;
    gravitybuoyancy(1) = 0.0;
    
    if(pos[2] + centerOfBuoyancy[2] < 0.0){ //The vehicle is completly under water
      gravitybuoyancy(2) = buoyancy - mass;
    }else if(pos[2] < 0.0 && centerOfBuoyancy[2] != 0){ //The vehicle is partwise underwater -> apply buoyancy partwise
      gravitybuoyancy(2) = (buoyancy * (-pos[2]/ centerOfBuoyancy[2] ) ) - mass;
    }else{ //The vehicle is over the surface -> no buoyancy
      gravitybuoyancy(2) = -mass;
    }
    
    //Angular buoancy forces
    gravitybuoyancy(3) = ((-(e4 * e4)+(e1 * e1)+(e2 * e2)-(e3 * e3))*((yg*mass)-(yb*buoyancy)))+(2*((e4*e1)+(e2*e3))*((zg*mass)-(zb*buoyancy)));
    gravitybuoyancy(4) =-((-(e4 * e4)+(e1 * e1)+(e2 * e2)-(e3 * e3))*((xg*mass)-(xb*buoyancy)))+(2*((e4*e2)-(e1*e3))*((zg*mass)-(zb*buoyancy)));
    gravitybuoyancy(5) =-(2*((e4*e1)+(e2*e3))*((xg*mass)-(xb*buoyancy)))-(2*((e4*e2)-(e1*e3))*((yg*mass)-(yb*buoyancy)));
    
    //Convert angular forces to world frame
    gravitybuoyancy.block<3,1>(3,0) = q * gravitybuoyancy.block<3,1>(3,0);  
    
    return gravitybuoyancy;
}
Ejemplo n.º 24
0
/*
 * This small example show the conversion between Eigen types and KDL types
 *
 * The toKDLxxx() function specify the KDL type in the name because it is impossible
 * to determine the KDL type at compile time when using a dynamic eigen matrix.
 */
int main(int argc, char* argv[]) {
	using namespace KDL;
	using namespace std;
    cout << "Demonstration of simple conversion routines between KDL and Eigen types \n";
    Vector a(1.0,2.0,3.0);
    cout << "KDL Vector " << a << "\n";
    Eigen::Vector3d a_eigen;
    a_eigen = toEigen(a);
    cout << "Eigen representation " << a_eigen.transpose() << "\n";
    cout << "converted back to KDL " << toKDLVector(a_eigen) << "\n";
    cout << endl;

    Twist t(Vector(1.0,2.0,3.0),Vector(4.0,5.0,6.0));
    cout << "KDL Vector " << t << "\n";
    Eigen::Matrix<double,6,1> t_eigen;
    t_eigen = toEigen(t);
    cout << "Eigen representation " << t_eigen.transpose() << "\n";
    cout << "converted back to KDL " << toKDLTwist(t_eigen) << "\n";
    cout << endl;


    Wrench w(Vector(1.0,2.0,3.0),Vector(4.0,5.0,6.0));
    cout << "KDL Wrench " << t << "\n";
    Eigen::Matrix<double,6,1> w_eigen;
    w_eigen = toEigen(w);
    cout << "Eigen representation " << w_eigen.transpose() << "\n";
    cout << "converted back to KDL " << toKDLTwist(w_eigen) << "\n";

    Rotation R( Rotation::EulerZYX(30*deg2rad,45*deg2rad,0*deg2rad) );
    cout << "KDL Rotation " << R << "\n";
    Eigen::Matrix<double,3,3> R_eigen;
    R_eigen = toEigen( R );
    cout << "Eigen 3x3 matrix " << R_eigen << "\n";
    cout << "converted back to KDL " << toKDLRotation( R_eigen ) << "\n";
    Eigen::Quaternion<double> q;
    q = toEigenQuaternion( R );
    cout << "Eigen Quaternion " << q.coeffs() << "\n";
    cout << "converted back to KDL " << toKDLRotation( q ) << "\n"; 
    cout << endl;
	return 0;
}
Ejemplo n.º 25
0
void publishNode(unsigned int index, Eigen::Matrix<double,4,1> trans, 
                Eigen::Quaternion<double> fq,
                const frame_common::CamParams &cp,
                bool isFixed, sba::CameraNode &msg)
{ 
    msg.index = index;
    
    msg.transform.translation.x = trans.x();
    msg.transform.translation.y = trans.y();
    msg.transform.translation.z = trans.z();
    msg.transform.rotation.x = fq.x();
    msg.transform.rotation.y = fq.y();
    msg.transform.rotation.z = fq.z();
    msg.transform.rotation.w = fq.w();
    msg.fx = cp.fx;
    msg.fy = cp.fy;
    msg.cx = cp.cx;
    msg.cy = cp.cy;
    msg.baseline = cp.tx;
    msg.fixed = isFixed;
}
Ejemplo n.º 26
0
int main(int argc, char* argv[])
{
	ros::init(argc,argv,"navsts2odom");
	ros::NodeHandle nh, ph("~");

	//Get rotation between the two
	std::vector<double> rpy(3,0);
	ph.param("rpy",rpy,rpy);

	//Setup the LTP to Odom frame
	Eigen::Quaternion<double> q;
	labust::tools::quaternionFromEulerZYX(rpy[0], rpy[1], rpy[2],q);
	Eigen::Matrix3d rot = q.toRotationMatrix().transpose();

	ros::Publisher odom = nh.advertise<nav_msgs::Odometry>("uwsim_hook", 1);
	ros::Subscriber navsts = nh.subscribe<auv_msgs::NavSts>("navsts",1,
			boost::bind(&onNavSts, boost::ref(odom), boost::ref(rot), _1));

	ros::spin();
	return 0;
}
Ejemplo n.º 27
0
    bool operator()(T const* const* parameters, T* residuals) const {
        // Map spline
        UniformSpline<T> spline(spline_dt_, spline_offset_);
        for (size_t i=0; i < num_knots_; ++i) {
            spline.add_knot((T*) &parameters[i][0]);
        }

        Eigen::Matrix<T, 3, 1> landmark(T(5.0), T(1.0), T(3.0));
        Eigen::Matrix<T, 3, 1> expected(T(0.0), T(0.0), T(1.0));


        // Distance to center
        Sophus::SE3Group<T> P;
        Eigen::Matrix<T, 4, 4> dP, d2P;
        for (size_t i=0; i < eval_times_.size(); ++i) {
            spline.evaluate(T(eval_times_[i]), P, dP, d2P);
            Eigen::Matrix<T, 3, 1> X_spline = P.inverse() * landmark;
            Eigen::Matrix<T, 3, 1> diff = X_spline - expected;
            residuals[3*i + 0] = diff(0);
            residuals[3*i + 1] = diff(1);
            residuals[3*i + 2] = diff(2);
        }

        const size_t poff = 3 * eval_times_.size();

        // Constant angular difference
        for (size_t i=1; i < eval_times_.size(); ++i) {
            SE3Group<T> P0, P1, delta;
            spline.evaluate(T(eval_times_[i-1]), P0, dP, d2P);
            spline.evaluate(T(eval_times_[i]), P1, dP, d2P);
            delta = P0.inverse() * P1;
            Eigen::Quaternion<T> q = delta.unit_quaternion();
            residuals[poff + 4*(i - 1) + 0] = T(5.0) * (q.w() - T(cos(0.5 * expected_angular_difference_)));
            residuals[poff + 4*(i - 1) + 1] = T(5.0) * (q.x() - T(sin(0.5 * expected_angular_difference_)));
            residuals[poff + 4*(i - 1) + 2] = T(5.0) * q.y(); //- 0;
            residuals[poff + 4*(i - 1) + 3] = T(5.0) * q.z(); //- 0;
        }

        return true;
    }
Ejemplo n.º 28
0
  OBAPI void SetTorsion(double *c, unsigned int ref[4], double setang, std::vector<int> atoms)
  {
    unsigned int cidx[4];
    cidx[0] = (ref[0] - 1) * 3;
    cidx[1] = (ref[1] - 1) * 3;
    cidx[2] = (ref[2] - 1) * 3;
    cidx[3] = (ref[3] - 1) * 3;
    const Eigen::Vector3d va( c[cidx[0]], c[cidx[0]+1], c[cidx[0]+2] );
    const Eigen::Vector3d vb( c[cidx[1]], c[cidx[1]+1], c[cidx[1]+2] );
    const Eigen::Vector3d vc( c[cidx[2]], c[cidx[2]+1], c[cidx[2]+2] );
    const Eigen::Vector3d vd( c[cidx[3]], c[cidx[3]+1], c[cidx[3]+2] );
    // calculate the rotation angle 
    const double ang = setang - DEG_TO_RAD * VectorTorsion(va, vb, vc, vd);
     
    // the angles are the same... 
    if (fabs(ang) < 1e-5)
      return;
    
    // setup the rotation matrix
    const Eigen::Vector3d bc = (vb - vc).normalized();
    Eigen::Quaternion<double> m;
    m = Eigen::AngleAxis<double>(-ang, bc);

    // apply the rotation
    int j;
    for (int i = 0; i < atoms.size(); ++i) {
      j = (atoms[i] - 1) * 3;
        
      Eigen::Vector3d vj( c[j], c[j+1], c[j+2] );
      vj -= vb; // translate so b is at origin
      vj = m.toRotationMatrix() * vj;
      vj += vb; // translate back 

      c[j]   = vj.x();
      c[j+1] = vj.y();
      c[j+2] = vj.z();
    }
  }
      bool base_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf)
      {
        assert(q_urdf.size()==7);
        assert(q_sot.size()==6);
        // *********  RPY to Quat *********
        const double r = q_sot[3];
        const double p = q_sot[4];
        const double y = q_sot[5];
        const Eigen::AngleAxisd  rollAngle(r, Eigen::Vector3d::UnitX());
        const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
        const Eigen::AngleAxisd   yawAngle(y, Eigen::Vector3d::UnitZ());
        const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle;

        q_urdf[0 ]=q_sot[0 ]; //BASE
        q_urdf[1 ]=q_sot[1 ];
        q_urdf[2 ]=q_sot[2 ];
        q_urdf[3 ]=quat.x();
        q_urdf[4 ]=quat.y();
        q_urdf[5 ]=quat.z();
        q_urdf[6 ]=quat.w();

        return true;
      }
Ejemplo n.º 30
0
IGL_INLINE void igl::snap_to_fixed_up(
  const Eigen::Quaternion<Qtype> & q,
  Eigen::Quaternion<Qtype> & s)
{
  using namespace Eigen;
  typedef Eigen::Matrix<Qtype,3,1> Vector3Q;
  const Vector3Q up = q.matrix() * Vector3Q(0,1,0);
  Vector3Q proj_up(0,up(1),up(2));
  if(proj_up.norm() == 0)
  {
    proj_up = Vector3Q(0,1,0);
  }
  proj_up.normalize();
  Quaternion<Qtype> dq;
  dq = Quaternion<Qtype>::FromTwoVectors(up,proj_up);
  s = dq * q;
}