Пример #1
0
void CrowdToolState::setMoveTarget(const float* p, bool adjust)
{
	if (!m_sample) return;
	
	// Find nearest point on navmesh and set move request to that location.
	dtNavMeshQuery* navquery = m_sample->getNavMeshQuery();
	dtCrowd* crowd = m_sample->getCrowd();
	const dtQueryFilter* filter = crowd->getFilter();
	const float* ext = crowd->getQueryExtents();

	if (adjust)
	{
		float vel[3];
		// Request velocity
		if (m_agentDebug.idx != -1)
		{
			const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
			if (ag && ag->active)
			{
				calcVel(vel, ag->npos, p, ag->params.maxSpeed);
				crowd->requestMoveVelocity(m_agentDebug.idx, vel);
			}
		}
		else
		{
			for (int i = 0; i < crowd->getAgentCount(); ++i)
			{
				const dtCrowdAgent* ag = crowd->getAgent(i);
				if (!ag->active) continue;
				calcVel(vel, ag->npos, p, ag->params.maxSpeed);
				crowd->requestMoveVelocity(i, vel);
			}
		}
	}
	else
	{
		navquery->findNearestPoly(p, ext, filter, &m_targetRef, m_targetPos);
		
		if (m_agentDebug.idx != -1)
		{
			const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
			if (ag && ag->active)
				crowd->requestMoveTarget(m_agentDebug.idx, m_targetRef, m_targetPos);
		}
		else
		{
			for (int i = 0; i < crowd->getAgentCount(); ++i)
			{
				const dtCrowdAgent* ag = crowd->getAgent(i);
				if (!ag->active) continue;
				crowd->requestMoveTarget(i, m_targetRef, m_targetPos);
			}
		}
	}
}
void OgreDetourCrowd::setMoveTarget(Ogre::Vector3 position, bool adjust)
{
        // Find nearest point on navmesh and set move request to that location.
        dtNavMeshQuery* navquery = m_recast->m_navQuery;
        dtCrowd* crowd = m_crowd;
        const dtQueryFilter* filter = crowd->getFilter();
        const float* ext = crowd->getQueryExtents();
        float p[3];
        OgreRecast::OgreVect3ToFloatA(position, p);

        navquery->findNearestPoly(p, ext, filter, &m_targetRef, m_targetPos);

        // Adjust target using tiny local search. (instead of recalculating full path)
        if (adjust)
        {
                        for (int i = 0; i < crowd->getAgentCount(); ++i)
                        {
                                const dtCrowdAgent* ag = crowd->getAgent(i);
                                if (!ag->active) continue;
                                float vel[3];
                                calcVel(vel, ag->npos, p, ag->params.maxSpeed);
                                crowd->requestMoveVelocity(i, vel);
                        }
        }
        else
        {
                // Move target using path finder (recalculate a full new path)
                        for (int i = 0; i < crowd->getAgentCount(); ++i)
                        {
                                const dtCrowdAgent* ag = crowd->getAgent(i);
                                if (!ag->active) continue;
                                crowd->requestMoveTarget(i, m_targetRef, m_targetPos);
                        }
        }
}
Ogre::Vector3 OgreDetourCrowd::calcVel(Ogre::Vector3 position, Ogre::Vector3 target, Ogre::Real speed)
{
    float pos[3];
    OgreRecast::OgreVect3ToFloatA(position, pos);

    float tgt[3];
    OgreRecast::OgreVect3ToFloatA(target, tgt);

    float res[3];
    calcVel(res, pos, tgt, speed);

    Ogre::Vector3 result;
    OgreRecast::FloatAToOgreVect3(res, result);

    return result;
}
void OgreDetourCrowd::setMoveTarget(int agentId, Ogre::Vector3 position, bool adjust)
{
    // TODO extract common method
    // Find nearest point on navmesh and set move request to that location.
    dtNavMeshQuery* navquery = m_recast->m_navQuery;
    dtCrowd* crowd = m_crowd;
    const dtQueryFilter* filter = crowd->getFilter();
    const float* ext = crowd->getQueryExtents();
    float p[3];
    OgreRecast::OgreVect3ToFloatA(position, p);

    navquery->findNearestPoly(p, ext, filter, &m_targetRef, m_targetPos);
    // ----

    if (adjust) {
        const dtCrowdAgent *ag = getAgent(agentId);
        float vel[3];
        calcVel(vel, ag->npos, p, ag->params.maxSpeed);
        crowd->requestMoveVelocity(agentId, vel);
    } else {
        m_crowd->requestMoveTarget(agentId, m_targetRef, m_targetPos);
    }
}
    void update(monitor_odometry_target_data &data, monitor_odometry_target_config config)
    {
        /* protected region user update on begin */
    	//Abstand des neue KS vom Roboter (bei Nullgeschwindigkeit).

//    	config.kindOf_primary_filter = 1;
//    	config.kindOf_secondary_filter = 5;
//    	config.kindOf_target_alignment = 0;
//    	config.noise_threshold = 0.02;
//    	config.mean_time_threshold = 2;
//    	config.vel_time_threshold = 2;
//    	config.vel_threshold = 0.05;
//    	config.kindOf_stagnancy_override = 0;
//    	config.mean_vel_threshold = 0.01;
//    	config.inner_radius = 1.0;
//    	config.outer_radius = 2.0;
//    	config.max_vel = 0.30;
//    	config.min_vel = 0.1;

    	double output_z = 0;
    	double radius_of_robot = 1;

    	ROS_INFO("********************* stage I ***********************");
    	/*
    	 * stage:						(I)
    	 * input odometry data:
    	 * velocity in x;
    	 * velocity in y;
    	 * header:
    	 * timestamp stamp;
    	 * frame_id;
    	 *
    	 */
    	x_ = data.in_odometry.twist.twist.linear.x;
    	y_ = data.in_odometry.twist.twist.linear.y;
    	header.frame_id = config.base_frame_id;
    	header.stamp = data.in_odometry.header.stamp;

    	/*
    	 * info:
    	 */
    	//TODO: delete this:
    	ROS_INFO("input:");
    	ROS_INFO("x_ = %f",x_);
    	ROS_INFO("y_ = %f",y_);
    	//end delete this;

//    	std::fstream f;
//    	f.open("testlog_x.dat", std::ios::out | std::ios::app);
//    	f << x_ << std::endl;
//    	f.close();
//
//    	f.open("testlog_y.dat", std::ios::out | std::ios::app);
//    	f << y_ << std::endl;
//    	f.close();

    	//TODO:
    	ROS_INFO("not good for performace but do anyway!");

    	velocity_ = calcAbsVec(x_, y_);
    	//TODO: just for now:
    	if (velocity_ > config.max_vel) {
    		ROS_INFO("ERROR vel > max_vel");
    	}

//    	velocity_ = calcVel(x_, y_, config.max_vel);
    	velList = updateVelList(velList, velocity_, header.stamp.toSec(), config.vel_time_threshold);
    	double meanVel = calcMeanVel(velList);

    	ROS_INFO("********************* stage II **********************");
    	/*
    	 * stage:						(II)
    	 * primary filters:
    	 */
    	if (config.kindOf_primary_filter == 0) {
    		ROS_INFO("no primary filter active ...");
//    		velocity_ = calcVel(x_, y_, config.max_vel);
    	}
    	else if (config.kindOf_primary_filter == 1) {
    		ROS_INFO("noise suppression filter active ...");
    		x_ = noiseSuppression(x_ , config.noise_threshold);
    		y_ = noiseSuppression(y_ , config.noise_threshold);
//    		velocity_ = calcVel(x_,y_, config.max_vel);
    	}
    	else if (config.kindOf_primary_filter == 2) {
    		ROS_INFO("velocity based suppression filter active ...");
//    		velocity_ = calcVel(x_,y_, config.max_vel);
    		if (velocity_ < config.vel_threshold) {
    			x_ = 0.0;
    			y_ = 0.0;
    			velocity_ = 0.0;
    		}
    	}
    	else if (config.kindOf_primary_filter == 3) {
    		ROS_INFO("combined suppression filter active ...");
    		x_ = noiseSuppression(x_ , config.noise_threshold);
			y_ = noiseSuppression(y_ , config.noise_threshold);
			velocity_ = calcVel(x_,y_, config.max_vel);
			if (velocity_ < config.vel_threshold) {
				x_ = 0.0;
				y_ = 0.0;
				velocity_ = 0.0;
				//TODO: Ist das nicht gefährlich??? Der Roboter könnte sich bewegen aber die Durchschnittsgeschw.
				// über 2-3 Sekunden trotzdem immer null sein???
			}
    	}
    	else
    		ROS_WARN("Error - 0001 - primary filter parameter out of bounds!");

    	ROS_INFO("after primary filters:");
    	ROS_INFO("x_ = %f",x_);
    	ROS_INFO("y_ = %f",y_);

    	//save velocity data
//    	std::fstream f;
//		f.open("testlog_velocity.dat", std::ios::out | std::ios::app);
//		f << velocity_ << std::endl;
//		f.close();


    	ROS_INFO("********************* stage III *********************");
    	/*
    	 * stage:						(III)
    	 * target alignment:
    	 */
    	//TODO:
    	if (config.kindOf_target_alignment == 0) {
    		ROS_INFO("no target alignment active ...");
    		z_ = (double) hight;
    	}
    	else {
    		//for all cases:
    		int overrideZ = checkForOverrideZ(config.min_vel, config.max_vel, velocity_);
    		if (config.kindOf_target_alignment == 1) {
				ROS_INFO("vertical target alignment active ...");
				z_ = (velocity_ / config.max_vel) * (double) hight;
			}
			else if (config.kindOf_target_alignment == 2) {
				ROS_INFO("linear target alignment active ...");
				z_ = (velocity_ / config.max_vel) * (double) hight;
				d_ = z_ * ((config.outer_radius - config.inner_radius) / (double) hight);
			}
			else if (config.kindOf_target_alignment == 3) {
				ROS_INFO("elliptical target aligment active ...");
				z_ = -1 * sqrt( 1 - (((velocity_ / config.max_vel) / (config.outer_radius - config.inner_radius)) * ((velocity_ / config.max_vel) / (config.outer_radius - config.inner_radius)))) * (double) hight + hight;
				d_ = (velocity_ / config.max_vel) * (config.outer_radius - config.inner_radius);
			}
			else if (config.kindOf_target_alignment == 4) {
				ROS_INFO("elliptical target alignment with mean velocity active...");
				z_ = -1 * sqrt( 1 - (((meanVel / config.max_vel) / (config.outer_radius - config.inner_radius)) * ((meanVel / config.max_vel) / (config.outer_radius - config.inner_radius)))) * (double) hight + hight;
				d_ = (meanVel / config.max_vel) * (config.outer_radius - config.inner_radius);
			}
			else {
				ROS_WARN("Error - 0002 - target alignment parameter out of bounds!");
			}

    		ROS_INFO("OVERRIDE BEFORE IF_ = %u",overrideZ);
    		if (overrideZ == 0) {
    			z_ = 0.0;
    			ROS_INFO("OVI 0");
    		}
    		else if (overrideZ == 1) {
    			z_ = (double) hight;
    			ROS_INFO("OVI 1=hight");
    		}
    		else if (overrideZ == 3) {
    			ROS_ERROR("Error - 0003 - override parameter out of bounds!");
    		}
    		else {
    			ROS_INFO("OVI wahrscheinlich 2 also nix tun!");
    		}

    	}


    	/*
    	 * stage:						(IV)
    	 * secondary filters:
    	 */
    	ROS_INFO("********************* stage IV **********************");
    	if (config.kindOf_secondary_filter == 0) {
    		ROS_INFO("no secondary filter active ...");
    	}
    	else if (config.kindOf_secondary_filter == 1) {
    		ROS_INFO("mean filter active ...");
    		list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
    		std::vector<double> meanXY = calcMeanXY(list_of_values_xystamp_);
    		x_ = meanXY.at(0);
    		y_ = meanXY.at(1);
//    		ROS_INFO("Anzahl Elemente: %lu", list_of_values_xystamp_.size());
//    		ROS_INFO("meanX %f und meanY %f", meanXY.at(0), meanXY.at(1));
    	}
    	else if (config.kindOf_secondary_filter == 2) {
    		ROS_INFO("median filter active ...");
    		list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
    		std::vector<double> medianXY = calcMedianXY(list_of_values_xystamp_);
    		x_ = medianXY.at(0);
    		y_ = medianXY.at(1);
    	}
    	else if (config.kindOf_secondary_filter == 3) {
    		ROS_INFO("quantified mean filter slope active ...");
    		list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
    		std::vector<double> meanSlope = calcQuantifiedMeanSlope(list_of_values_xystamp_);
    		x_ = meanSlope.at(0);
    		y_ = meanSlope.at(1);
    	}
    	else if (config.kindOf_secondary_filter == 4) {
    		ROS_INFO("quantified mean filter stairs active ...");
    		list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
    		std::vector<double> meanStairs = calcQuantifiedMeanStairs(list_of_values_xystamp_);
    		x_ = meanStairs.at(0);
    		y_ = meanStairs.at(1);
    	}
    	else if (config.kindOf_secondary_filter == 5) {
    		ROS_INFO("quantified mean filter square active");
    		list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
    		std::vector<double> meanSquare = calcQuantifiedMeanSquare(list_of_values_xystamp_);
    		x_ = meanSquare.at(0);
    		y_ = meanSquare.at(1);
    	}
    	else if (config.kindOf_secondary_filter == 6) {
    		ROS_INFO("quantified median filter slope active");
    		list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
    		std::vector<double> medianSlope = calcQuantifiedMedianSlope(list_of_values_xystamp_);
    		x_ = medianSlope.at(0);
    		y_ = medianSlope.at(1);
    	}
    	else
    		ROS_WARN("Error - 0003 - secondary filter parameter out of bounds!");

    	ROS_INFO("after secondary filters:");
    	ROS_INFO("x_ = %f",x_);
    	ROS_INFO("y_ = %f",y_);

    	/*
    	 * stage:					(V)
    	 * output production:
    	 */
//    	yaw_ = noZeroArcTan(y_,x_);

    	//for stag test:
    	//listOfValuesForMeanYaw_ = updateListOfDouble(listOfValuesForMeanYaw_, yaw_, 100);



    	//end of stag test
//    	yaw_ = noZeroArcTan(y_,x_);

    	/*
    	 * stage:					(V)
    	 * stagnancy override:
    	 */
    	ROS_INFO("********************* stage V ***********************");
    	if (config.kindOf_stagnancy_override == 0) {
    		ROS_INFO("no stagnancyoverride active ...");
    	}
    	else if (config.kindOf_stagnancy_override == 1) {
    		ROS_INFO("stagnancy override active ...");
//    		velList = updateVelList(velList, velocity_, header.stamp.toSec());
//    		double meanVel = calcMeanVel(velList);
    		if (meanVel < config.mean_vel_threshold) {
    			stagnancy_override_counter++;
    			z_ = (double) hight;
    			d_ = 0.0;
    			//wieder zurück drehen???
    		}
    	}
    	else if (config.kindOf_stagnancy_override == 2) {
    		ROS_INFO("stagnancy override: twist back torso");
    		if (meanVel < config.mean_vel_threshold) {
//    			yaw_ = calcMean(listOfValuesForMeanYaw_);
//    			ROS_INFO("Mean yaw_ %f",yaw_);
    		}
    	}
    	else
    		ROS_ERROR("Error - 0004 - stagnancy override parameter out of bounds!");

    	/*
    	 * stage:					(VI)
    	 * output calculator:
    	 */
    	ROS_INFO("********************* stage VI **********************");
    	yaw_ = newArcTan(x_,y_);
//    	ROS_INFO("d_ = %f",d_);
    	x_ = calcX(yaw_, (config.outer_radius - config.inner_radius) + d_);
    	y_ = calcY(yaw_, (config.outer_radius - config.inner_radius) + d_);
//    	x_ = calcX(yaw_, (config.outer_radius - config.inner_radius) + d_);
//		y_ = calcY(yaw_, (config.outer_radius - config.inner_radius) + d_);

    	ROS_INFO("Output: ---------------------");
    	ROS_INFO("x_ = %f",x_);
    	ROS_INFO("y_ = %f",y_);
    	ROS_INFO("yaw_ = %f",yaw_);
    	ROS_INFO("END ....................");
    	ROS_INFO("Schwellwert: %f", config.noise_threshold);
    	if (config.testbool) {
    		ROS_INFO("testbool is true");
    	}
    	else {
    		ROS_INFO("testbool is false");
    	}


    	/*
    	 * stage:					(VII)
    	 * tf
    	 */
    	ROS_INFO("********************* stage VII *********************");
    	frame_transform_output(tf::Vector3(x_,y_,z_));
    	stamped_transform_output.stamp_ = header.stamp;
    	stamped_transform_output.frame_id_ = config.base_frame_id;
    	stamped_transform_output.child_frame_id_ = config.lookat_frame_id;
    	tf::Quaternion q;
    	q.setRPY(0, 0, yaw_);
    	stamped_transform_output.setRotation(q);
    	stamped_transform_output.setOrigin(tf::Vector3(x_,y_,z_));
    	static tf::TransformBroadcaster br;
    	br.sendTransform(stamped_transform_output);

//    	/*
//    	 * evaluations:
//    	 */
//    	if (active_) {
//        	tf::Vector3 newOrigin;
//        	newOrigin.setX(x_);
//        	newOrigin.setY(y_);
//        	newOrigin.setZ(z_);
//        	double distance = calcEuklidianDistance(newOrigin,oldOrigin);
//        	quickAndDirty++;
//        	//Problem: Sprung von Null auf den Startwert wieder löschen!
//        	if (quickAndDirty > 4 ) {
//        		distances_list.push_back(distance);
//        	}
//
//    		double max_value = calcMax(distances_list);
//    		double sum_value = calcSum(distances_list);
//    		ROS_INFO("+++++++++++++++++++++++++++++++++++++++");
//    		ROS_INFO("Auswertung: ");/
//        	ROS_INFO("Distance = %f",distance);
//        	ROS_INFO("MAX = %f ", max_value);
//        	ROS_INFO("SUM = %f", sum_value);
//    //    	showList(distances_list);
//        	oldOrigin = newOrigin;
//
//        	//TODO:
//        	//eval: yaw:
//        	double yawDistance = calcYawDistance(yaw_, oldYaw);
//        	if (quickAndDirty > 4) {
//        		yaw_distances_list.push_back(yawDistance);
//        	}
//        	double yaw_max_value = calcMax(yaw_distances_list);
//        	double yaw_sum_value = calcSum(yaw_distances_list);
//        	ROS_INFO("++++++++++++++++++++++++++++++++++++++++");
//        	ROS_INFO("YawDistance = %f", yawDistance);
//        	ROS_INFO("MAX YAW = %f", yaw_max_value);
//        	ROS_INFO("SUM YAW = %f ", yaw_sum_value);
//        	oldYaw = yaw_;
//
//        	ROS_INFO("stagnacy_override_counter = %u", stagnancy_override_counter);
//    	}

        /* protected region user update end */
    }
Пример #6
0
/**
*@brief 更新
* @param st 刻み幅
*/
void RobotArm::update(double st)
{
	
	if(pauseFalg || stopFalg || !serbo)
		return;
	//std::cout << targetPoint.end_time << "\t" << targetPoint.time << std::endl;
	if(targetPoint.end_time < targetPoint.time)
	{
		if(targetPoint.type == Point)
		{
			Vector3d pos = calcKinematics(theta);

			double dPx = Kp*(targetPoint.target_pos(0)-pos(0));
			double dPy = Kp*(targetPoint.target_pos(1)-pos(1));
			double dPz = Kp*(targetPoint.target_pos(2)-pos(2));

			Vector3d dthe = calcJointVel(Vector3d(dPx, dPy, dPz));

			updatePos(dthe(0), dthe(1), dthe(2), 0);
			theta[3] = targetPoint.target_theta;
			
			
		}
		else
		{
			for(int i=0;i < 4;i++)
			{
				theta[i] = targetPoint.target_joint_pos[i];
			}
			if (!judgeSoftLimitJoint(theta))stop();
		}

		
		setTargetPos();

		

		return;
	}
	else
	{
		if(targetPoint.type == Point)
		{
			double td = calcVel(targetPoint.target_theta, targetPoint.start_theta, targetPoint.end_time, targetPoint.time, theta[3]);

			dt = st;
			MinTime = dt;

			double dx = targetPoint.target_pos(0)-targetPoint.start_pos(0);
			double dy = targetPoint.target_pos(1)-targetPoint.start_pos(1);
			double dz = targetPoint.target_pos(2)-targetPoint.start_pos(2);

			double ST = sqrt(dx*dx+dy*dy+dz*dz);
			if(ST < 0.001)
			{
				updatePos(0, 0, 0, td);
				targetPoint.time += dt;
				return;
			}

			double A = 2*M_PI*ST/(targetPoint.end_time*targetPoint.end_time);

			double s = A*targetPoint.end_time/(2*M_PI)*(targetPoint.time - targetPoint.end_time/(2*M_PI)*sin(2*M_PI/targetPoint.end_time*targetPoint.time));

			double Px = s*dx/ST + targetPoint.start_pos(0);
			double Py = s*dy/ST + targetPoint.start_pos(1);
			double Pz = s*dz/ST + targetPoint.start_pos(2);


			double ds = A*targetPoint.end_time/(2*M_PI)*(1 - cos(2*M_PI/targetPoint.end_time*targetPoint.time));

			Vector3d pos = calcKinematics(theta);

			double dPx = ds*dx/ST + Kp*(Px-pos(0));
			double dPy = ds*dy/ST + Kp*(Py-pos(1));
			double dPz = ds*dz/ST + Kp*(Pz-pos(2));

		
			//ofs << ds << "\t" << s << std::endl;
			//std::cout << pos << std::endl;

			

			Vector3d dthe = calcJointVel(Vector3d(dPx, dPy, dPz));


			
			

			updatePos(dthe(0), dthe(1), dthe(2), td);
		}
		else
		{
			double dthe[4];
			
			for(int i=0;i < 4;i++)
			{
				
				dthe[i] = calcVel(targetPoint.target_joint_pos[i], targetPoint.start_joint_pos[i], targetPoint.end_time, targetPoint.time, theta[i]);


				
			}

			updatePos(dthe[0], dthe[1], dthe[2], dthe[3]);

			
			
		}
		targetPoint.time += dt;
	}
	
}