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

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;

    }


}

void HapticsPSM::compute_force_in_tip_frame(geometry_msgs::Wrench &wrench){
    rot_mat6wrt0.setRPY(group->getCurrentRPY().at(0),
                        group->getCurrentRPY().at(1),
                        group->getCurrentRPY().at(2));
    tf_vec3.setValue(wrench.force.x,wrench.force.y,wrench.force.z);
    tf_vec3 = rot_mat6wrt0.transpose() * tf_vec3;
    wrench.force.x = tf_vec3.getX();
    wrench.force.y = tf_vec3.getY();
    wrench.force.z = tf_vec3.getZ();
}

bool planning_environment::configureForAttachedBodyMask(planning_models::KinematicState& state,
        planning_environment::CollisionModels* cm,
        tf::TransformListener& tf,
        const std::string& sensor_frame,
        const ros::Time& sensor_time,
        tf::Vector3& sensor_pos)
{
    state.setKinematicStateToDefault();

    cm->bodiesLock();

    const std::map<std::string, std::map<std::string, bodies::BodyVector*> >& link_att_objects = cm->getLinkAttachedObjects();

    if(link_att_objects.empty()) {
        cm->bodiesUnlock();
        return false;
    }

    planning_environment::updateAttachedObjectBodyPoses(cm,
            state,
            tf);

    sensor_pos.setValue(0.0,0.0,0.0);

    // compute the origin of the sensor in the frame of the cloud
    if (!sensor_frame.empty()) {
        std::string err;
        try {
            tf::StampedTransform transf;
            tf.lookupTransform(cm->getWorldFrameId(), sensor_frame, sensor_time, transf);
            sensor_pos = transf.getOrigin();
        } catch(tf::TransformException& ex) {
            ROS_ERROR("Unable to lookup transform from %s to %s. Exception: %s", sensor_frame.c_str(), cm->getWorldFrameId().c_str(), ex.what());
            sensor_pos.setValue(0, 0, 0);
        }
    }
    cm->bodiesUnlock();
    return true;
}

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

}

		/********** callback for the cmd velocity from the autonomy **********/
		void cmd_vel_callback(const geometry_msgs::Twist& msg)
		{
			watchdogTimer.stop();
			
			error.setValue(msg.linear.x - body_vel.linear.x, msg.linear.y - body_vel.linear.y, msg.linear.z - body_vel.linear.z);
			//std::cout << "error x: " << error.getX() << " y: " << error.getY() << " z: " << error.getZ() << std::endl;
			//std::cout << std::abs(curr_body_vel_time.toSec() - last_body_vel_time.toSec()) << std::endl;
			error_yaw = msg.angular.z - body_vel.angular.z;
			//std::cout << "error yaw: " << error_yaw << std::endl;
			
			// if some time has passed between the last body velocity time and the current body velocity time then will calculate the (feed forward PD)
			if (std::abs(curr_body_vel_time.toSec() - last_body_vel_time.toSec()) > 0.00001)
			{	
				errorDot = (1/(curr_body_vel_time - last_body_vel_time).toSec()) * (error - last_error);
				//std::cout << "errordot x: " << errorDot.getX() << " y: " << errorDot.getY() << " z: " << errorDot.getZ() << std::endl;
				
				errorDot_yaw = (1/(curr_body_vel_time - last_body_vel_time).toSec()) * (error_yaw - last_error_yaw);
				//std::cout << "error dot yaw " << errorDot_yaw << std::endl;
				velocity_command.linear.x = cap_vel_auton(kx*msg.linear.x + (kp*error).getX() + (kd*errorDot).getX());
				velocity_command.linear.y = cap_vel_auton(ky*msg.linear.y + (kp*error).getY() + (kd*errorDot).getY());
				velocity_command.linear.z = cap_vel_auton(kz*msg.linear.z + (kp*error).getZ() + (kd*errorDot).getZ());
				velocity_command.angular.z = -1*cap_vel_auton(kyaw*msg.angular.z + kp_yaw*error_yaw + kd_yaw*errorDot_yaw); // z must be switched because bebop driver http://bebop-autonomy.readthedocs.org/en/latest/piloting.html
			}
			
			last_body_vel_time = curr_body_vel_time;// update last time body velocity was recieved
			last_error = error;
			last_error_yaw = error_yaw;
			
			error_gm.linear.x = error.getX(); error_gm.linear.y = error.getY(); error_gm.linear.z = error.getZ(); error_gm.angular.z = error_yaw;
			errorDot_gm.linear.x = errorDot.getX(); errorDot_gm.linear.y = errorDot.getY(); errorDot_gm.linear.z = errorDot.getZ(); errorDot_gm.angular.z = kyaw*msg.angular.z + kp_yaw*error_yaw + kd_yaw*errorDot_yaw;
			error_pub.publish(error_gm);
			errorDot_pub.publish(errorDot_gm);
			
			if (start_autonomous)
			{
				recieved_command_from_tracking = true;
			}
			
			watchdogTimer.start();
		}

void HapticsPSM::get_current_position(tf::Vector3 &v){
    v.setValue(group->getCurrentPose().pose.position.x,
               group->getCurrentPose().pose.position.y,
               group->getCurrentPose().pose.position.z);
}