C++ Vector3::y方法代码示例

void Hand_filter::update(tf::Vector3 p, tf::Quaternion& q){

    if(b_first){
        p_filter_buffer.push_back(p);
        q_filter_buffer.push_back(q);
        if(p_filter_buffer.size() == p_filter_buffer.capacity()){
            b_first = false;
            ROS_INFO("====== hand filter ======");
           // ROS_INFO("buffer full: %d",p_filter_buffer.size());
            ROS_INFO("p: %f %f %f",p.x(),p.y(),p.z());
            ROS_INFO("q: %f %f %f %f",q.x(),q.y(),q.z(),q.w());

            k_position(0) = p.x();k_position(1) = p.y(); k_position(2) = p.z();
            kalman_filter.Init(k_position);

            q_tmp = q;
            p_tmp = p;

        }
    }else{


        /// Orientation filter
       if(jumped(q,q_tmp,q_threashold)){
            ROS_INFO("q jumped !");
            q = q_tmp;
        }

       q_attractor(q,up);
       q = q_tmp.slerp(q,0.1);


       /// Position filter
        if(!jumped(p,p_tmp,p_threashold)){

            k_measurement(0) = p.x();
            k_measurement(1) = p.y();
            k_measurement(2) = p.z();

        }else{
            ROS_INFO("p jumped !");
            k_measurement(0) = p_tmp.x();
            k_measurement(1) = p_tmp.y();
            k_measurement(2) = p_tmp.z();
        }

        kalman_filter.Update(k_measurement,0.001);
        kalman_filter.GetPosition(k_position);
        p.setValue(k_position(0),k_position(1),k_position(2));



        q_tmp = q;
        p_tmp = p;

    }


}

Socket_one::Socket_one(std::string name, const tf::Vector3& origin, const tf::Vector3 &rpy,float scale){


    //const std::string& name, const geo::fCVec3& C, const geo::fMat33& R, float r
    geo::fMat33 R;
    R.eye();

    disk_radius = 0.02  * scale;
    hole_radius = 0.002 * scale;

    std::array<geo::Disk,3> holes;
    geo::fCVec3 position;
    position.zeros();

    geo::Disk plate("plate",position,R,disk_radius);


    position.zeros();
    position(0) =   -0.01 * scale;
    holes[0]    = geo::Disk("hole_left",position,R,hole_radius);

    position.zeros();
    position(0) =    0.01 * scale;
    holes[1]    = geo::Disk("hole_right",position,R,hole_radius);

    position.zeros();
    position(1)     =  0.005 * scale;
    holes[2]        = geo::Disk("hole_up",position,R,hole_radius);

    wsocket = wobj::WSocket(name,plate,holes);


    geo::fCVec3 dim             = {{0.07,0.07,0.05}};
                dim             = dim * scale;
    geo::fCVec3 orientation     = {{0,0,0}};

    position.zeros();
    position(2) = -0.005/2 - (0.05 - 0.005)/2;
    wbox = wobj::WBox("box_socket_one",dim,position,orientation);

    // Transformation

    orientation(0) = rpy.x();
    orientation(1) = rpy.y();
    orientation(2) = rpy.z();

    geo::fCVec3 T = {{origin.x(),origin.y(),origin.z()}};

    wsocket.transform(T,orientation);
    wbox.transform(T,orientation);

}

//Create a ROS frame out of the known corners of a tag in the weird marker coord frame used by Alvar markers (x right y forward z up)
//p0-->p1 should point in Alvar's pos X direction
//p1-->p2 should point in Alvar's pos Y direction
int makeMasterTransform (const CvPoint3D64f& p0, const CvPoint3D64f& p1,
                         const CvPoint3D64f& p2, const CvPoint3D64f& p3,
                         tf::Transform &retT)
  {
    const tf::Vector3 q0(p0.x, p0.y, p0.z);
    const tf::Vector3 q1(p1.x, p1.y, p1.z);
    const tf::Vector3 q2(p2.x, p2.y, p2.z);
    const tf::Vector3 q3(p3.x, p3.y, p3.z);
  
    // (inverse) matrix with the given properties
    const tf::Vector3 v = (q1-q0).normalized();
    const tf::Vector3 w = (q2-q1).normalized();
    const tf::Vector3 n = v.cross(w);
    tf::Matrix3x3 m(v[0], v[1], v[2], w[0], w[1], w[2], n[0], n[1], n[2]);
    m = m.inverse();
    
    //Translate to quaternion
    if(m.determinant() <= 0)
        return -1;
  
    //Use Eigen for this part instead, because the ROS version of bullet appears to have a bug
    Eigen::Matrix3f eig_m;
    for(int i=0; i<3; i++){
        for(int j=0; j<3; j++){
            eig_m(i,j) = m[i][j];
        }
    }
    Eigen::Quaternion<float> eig_quat(eig_m);
    
    // Translate back to bullet
    tfScalar ex = eig_quat.x();
    tfScalar ey = eig_quat.y();
    tfScalar ez = eig_quat.z();
    tfScalar ew = eig_quat.w();
    tf::Quaternion quat(ex,ey,ez,ew);
    quat = quat.normalized();
    
    double qx = (q0.x() + q1.x() + q2.x() + q3.x()) / 4.0;
    double qy = (q0.y() + q1.y() + q2.y() + q3.y()) / 4.0;
    double qz = (q0.z() + q1.z() + q2.z() + q3.z()) / 4.0;
    tf::Vector3 origin (qx,qy,qz);
    
    tf::Transform tform (quat, origin);  //transform from master to marker
    retT = tform;
    
    return 0;
  }

geometry_msgs::Vector3 MoveBaseDoorAction::toVector(const tf::Vector3& pnt)
{
  geometry_msgs::Vector3 res;
  res.x = pnt.x();
  res.y = pnt.y();
  res.z = pnt.z();
  return res;
}

/**
 * Convert tf::Vector3 to string
 */
template<> std::string toString<tf::Vector3>(const tf::Vector3& p_vec)
{
  std::stringstream ss;

  ss << "(" << p_vec.x() << ", " << p_vec.y() << ", " << p_vec.z() << ")";

  return ss.str();
}

gm::Point toPoint (const tf::Vector3& p)
{
  gm::Point pt;
  pt.x = p.x();
  pt.y = p.y();
  pt.z = p.z();
  return pt;
}

inline void transformPointMatVec(const tf::Vector3 &origin, const tf::Matrix3x3 &basis, const geometry_msgs::Point32 &in, geometry_msgs::Point32 &out)
{
  // Use temporary variables in case &in == &out
  double x = basis[0].x() * in.x + basis[0].y() * in.y + basis[0].z() * in.z + origin.x();
  double y = basis[1].x() * in.x + basis[1].y() * in.y + basis[1].z() * in.z + origin.y();
  double z = basis[2].x() * in.x + basis[2].y() * in.y + basis[2].z() * in.z + origin.z();
  
  out.x = x; out.y = y; out.z = z;
}

bool control(PID_control::controlserver::Request  &req,
	         PID_control::controlserver::Response &ret) {
    tf::vector3MsgToTF(req.target_error,sp);
    tf::transformMsgToTF(req.transform,pose);
    tf::vector3MsgToTF(req.velocity,vel);
    if (req.reset) {
        pParam=2,iParam=0,dParam=0,vParam=-2,cumul=0,lastE=0,pAlphaE=0,pBetaE=0,psp2=0,psp1=0,prevYaw=0;
        ROS_INFO("Controller reset");
    }

	e = (pose*sp).z() - pose.getOrigin().z();
	cumul=cumul+e;
    pv=pParam*e;
    thrust=5.335+pv+iParam*cumul+dParam*(e-lastE)+vel.z()*vParam;
    lastE=e;
    // Horizontal control: 
    vx = pose*tf::Vector3(1,0,0);
    vy = pose*tf::Vector3(0,1,0);
    alphaE=(vy.z()-pose.getOrigin().z());
    alphaCorr=0.25*alphaE+2.1*(alphaE-pAlphaE);
    betaE=(vx.z()-pose.getOrigin().z());
    betaCorr=-0.25*betaE-2.1*(betaE-pBetaE);
    pAlphaE=alphaE;
    pBetaE=betaE;
    alphaCorr=alphaCorr+sp.y()*0.005+1*(sp.y()-psp2);
    betaCorr=betaCorr-sp.x()*0.005-1*(sp.x()-psp1);
    psp2=sp.y();
    psp1=sp.x();
    
    pose.getBasis().getRPY(t1, t2, yaw);
    rotCorr=yaw*0.1+2*(yaw-prevYaw);
    prevYaw = yaw;

    // Decide of the motor velocities:
    a=thrust*((double)1-alphaCorr+betaCorr+rotCorr);
    b=thrust*((double)1-alphaCorr-betaCorr-rotCorr);
    c=thrust*((double)1+alphaCorr-betaCorr+rotCorr);
    d=thrust*((double)1+alphaCorr+betaCorr-rotCorr);
    ret.a=a;
    ret.b=b;
    ret.c=c;
    ret.d=d;
}

tf::Vector3 BasePositionPid::updateControl(const tf::Vector3& commanded_pos, const tf::Vector3& actual_pos, double time_elapsed)
{
  double err_x = actual_pos.x() - commanded_pos.x() ;
  double err_y = actual_pos.y() - commanded_pos.y() ;
  double err_w = angles::shortest_angular_distance(commanded_pos.z(), actual_pos.z()) ;
  
  tf::Vector3 velocity_cmd ;
  
  double odom_cmd_x = pid_x_.updatePid(err_x, time_elapsed) ;            // Translation X in the odometric frame
  double odom_cmd_y = pid_y_.updatePid(err_y, time_elapsed) ;            // Translation Y in the odometric frame

  // Rotate the translation commands so that they're in the base frame (instead of the odom frame)
  velocity_cmd.setX( odom_cmd_x*cos(actual_pos.z()) + odom_cmd_y*sin(actual_pos.z())) ;
  velocity_cmd.setY(-odom_cmd_x*sin(actual_pos.z()) + odom_cmd_y*cos(actual_pos.z())) ;

  velocity_cmd.setZ(pid_w_.updatePid(err_w, time_elapsed)) ;             // Rotation command is same is Odom and Base frames
  
  return velocity_cmd ;
}

static carmen_vector_3D_t
tf_vector3_to_carmen_vector3(tf::Vector3 tf_vector)
{
	carmen_vector_3D_t carmen_vector;
	carmen_vector.x = tf_vector.x();
	carmen_vector.y = tf_vector.y();
	carmen_vector.z = tf_vector.z();
	
	return carmen_vector;
}

std::vector<geometry_msgs::Point> MoveBaseDoorAction::getOrientedFootprint(const tf::Vector3 pos, double theta_cost)
{
  double cos_th = cos(theta_cost);
  double sin_th = sin(theta_cost);
  std::vector<geometry_msgs::Point> oriented_footprint;
  for(unsigned int i = 0; i < footprint_.size(); ++i){
    geometry_msgs::Point new_pt;
    new_pt.x = pos.x() + (footprint_[i].x * cos_th - footprint_[i].y * sin_th);
    new_pt.y = pos.y() + (footprint_[i].x * sin_th + footprint_[i].y * cos_th);
    oriented_footprint.push_back(new_pt);
  }
  return oriented_footprint;
}

	void MetalDetectorCallback(const std_msgs::Float32 v)
	{
		static int sMarkerID=0;

		if(v.data>0.95)	// Mine detected
		{
			//ROS_INFO("Found a mine");
			// Check whether a mine at the same position has already been found
			for(auto it = m_vMinesPositions.begin(); it!= m_vMinesPositions.end(); ++it)
			{
				if(squaredDistance2D(it->x(), it->y(), m_WorldSpaceToolPosition.x(), m_WorldSpaceToolPosition.y())<0.04)
					return;
			}
			//ROS_INFO("Publishing marker");

			visualization_msgs::Marker m;
			m.header.stamp = m_MsgHeaderStamp;
			m.header.frame_id = "/world";
			m.ns = "mine";
			m.id = sMarkerID++;
			m_vMinesPositions.push_back(m_WorldSpaceToolPosition);
			m.type = visualization_msgs::Marker::CYLINDER;
			m.action = visualization_msgs::Marker::ADD;
			m.pose.position.x = m_WorldSpaceToolPosition.x();
			m.pose.position.y = m_WorldSpaceToolPosition.y();
			m.pose.position.z = m_WorldSpaceToolPosition.z();
			m.scale.x = 0.4;
			m.scale.y = 0.4;
			m.scale.z = 0.4;
			m.color.a = 0.5;
			m.color.r = 0.0;
			m.color.g = 1.0;
			m.color.b = 0.0;
			//m.frame_locked = true;
			// Finally publish the marker
			m_MineMarkerPublisher.publish(m);
		}
	}

void CameraTransformOptimization::calculateMarkerError(CalibrationState state,
		tf::Vector3 markerPoint, float& error) {
	error = 0;

	for (int i = 0; i < this->measurePoints.size(); i++) {
		float currentError = 0;
		MeasurePoint& current = this->measurePoints[i];
		tf::Transform opticalToFixed = current.opticalToFixed(state);
		tf::Vector3 transformedPoint = opticalToFixed
				* current.measuredPosition;
		currentError += pow(markerPoint.x() - transformedPoint.x(), 2);
		currentError += pow(markerPoint.y() - transformedPoint.y(), 2);
		currentError += pow(markerPoint.z() - transformedPoint.z(), 2);
		error += currentError;
	}
}

void MTMHaptics::convert_bodyForcetoSpatialForce(geometry_msgs::WrenchStamped &body_wrench){

    visualize_haptic_force(body_force_pub);
    rot_quat.setX(cur_mtm_pose.orientation.x);
    rot_quat.setY(cur_mtm_pose.orientation.y);
    rot_quat.setZ(cur_mtm_pose.orientation.z);
    rot_quat.setW(cur_mtm_pose.orientation.w);
    F7wrt0.setValue(body_wrench.wrench.force.x, body_wrench.wrench.force.y, body_wrench.wrench.force.z);
    rot_matrix.setRotation(rot_quat);
    F0wrt7 = rot_matrix.transpose() * F7wrt0;
    body_wrench.wrench.force.x = F0wrt7.x();
    body_wrench.wrench.force.y = F0wrt7.y();
    body_wrench.wrench.force.z = F0wrt7.z();
    visualize_haptic_force(spatial_force_pub);

}

void Jumps::update(tf::Vector3& origin,tf::Quaternion& orientation){

    if(bFirst){

        origin_buffer.push_back(origin);
        orientation_buffer.push_back(orientation);

        if(origin_buffer.size() == origin_buffer.capacity()){
            bFirst = false;
            ROS_INFO("====== jump filter full ======");
        }

    }else{

        origin_tmp      = origin_buffer[origin_buffer.size()-1];
        orientation_tmp = orientation_buffer[orientation_buffer.size()-1];

        if(bDebug){
            std::cout<< "=== jum debug === " << std::endl;
            std::cout<< "p    : " << origin.x() << "\t" << origin.y() << "\t" << origin.z() << std::endl;
            std::cout<< "p_tmp: " << origin_tmp.x() << "\t" << origin_tmp.y() << "\t" << origin_tmp.z() << std::endl;
            std::cout<< "p_dis: " << origin.distance(origin_tmp) << std::endl;

            std::cout<< "q    : " << orientation.x() << "\t" << orientation.y() << "\t" << orientation.z() <<  "\t" << orientation.w() << std::endl;
            std::cout<< "q_tmp: " << orientation_tmp.x() << "\t" << orientation_tmp.y() << "\t" << orientation_tmp.z() << "\t" << orientation_tmp.w() << std::endl;
            std::cout<< "q_dis: " << dist(orientation,orientation_tmp) << std::endl;
        }

        /// Position jump
        if(jumped(origin,origin_tmp,origin_threashold)){
            ROS_INFO("position jumped !");
            origin = origin_tmp;
           // exit(0);
        }else{
            origin_buffer.push_back(origin);
        }

        /// Orientation jump
        if(jumped(orientation,orientation_tmp,orientation_threashold)){
            ROS_INFO("orientation jumped !");
            orientation = orientation_tmp;
            //exit(0);
        }else{
            orientation_buffer.push_back(orientation);
        }
    }
}