Пример #1
0
void SerializeGrid(XFILE *fp, int mode, grid *gridp)
{
	int	i;

	SerializeInt(fp, mode, gridp->nrows);
	SerializeInt(fp, mode, gridp->ncols);
	SerializeMatrix(fp, mode, &gridp->gmatrix);
	SerializePhysicsInfo(fp, mode, &gridp->physics);
	SerializeFloat(fp, mode, gridp->square_size);
	SerializeFloat(fp, mode, gridp->planeD);
	
	for (i=0; i<MAX_GRID_POINTS; i++)
		SerializeVector(fp, mode, &gridp->gpoints[i]);

}
Пример #2
0
/////////////////////////////////////////////////////////////////////////////
// CFREDDoc serialization
void SerializeObject(XFILE *fp, int mode, object *objp)
{
	SerializeInt(fp, mode, objp->signature);
	SerializeInt(fp, mode, objp->type);
	SerializeInt(fp, mode, objp->parent);
	SerializeInt(fp, mode, objp->parent_sig);
	SerializeInt(fp, mode, objp->parent_type);
	SerializeInt(fp, mode, objp->instance);
	SerializeInt(fp, mode, objp->flags);
	SerializeFloat(fp, mode, objp->radius);
//	SerializeInt(fp, mode, objp->wing);
	SerializePhysicsInfo(fp, mode, &objp->phys_info);
	SerializeVector(fp, mode, &objp->pos);
	SerializeMatrix(fp, mode, &objp->orient);
}
Пример #3
0
/////////////////////////////////////////////////////////////////////////////
// CFREDDoc serialization
void SerializeObject(XFILE *fp, int mode, object *objp)
{
	SerializeInt(fp, mode, objp->signature);
	SerializeInt(fp, mode, objp->type);
	SerializeInt(fp, mode, objp->parent);
	SerializeInt(fp, mode, objp->parent_sig);
	SerializeInt(fp, mode, objp->parent_type);
	SerializeInt(fp, mode, objp->instance);
	SerializeInt(fp, mode, objp->flags);
	SerializeInt(fp, mode, objp->flags2);	// Goober5000 - code is obsolete, but I added this just in case
	SerializeFloat(fp, mode, objp->radius);
//	SerializeInt(fp, mode, objp->wing);
	SerializePhysicsInfo(fp, mode, &objp->phys_info);
	SerializeVector(fp, mode, &objp->pos);
	SerializeMatrix(fp, mode, &objp->orient);
}
Пример #4
0
void ContinuousPolicyLearner::TimerCallback( const ros::TimerEvent& event )
{
	// Wait until we have enough actions buffered before we begin optimization
	if( _actionBuffer.size() < _minModulesToOptimize ) 
	{
		ROS_INFO_STREAM( "Action buffer size: " << _actionBuffer.size() << 
		                 " less than min: " << _minModulesToOptimize );
		return;
	}

	// First process buffer
	ros::Time now = event.current_real;
	while( _actionBuffer.size() > 0 )
	{
		ContinuousAction action( _actionBuffer.begin()->second );

		percepto_msgs::GetCritique getCritique;
		getCritique.request.time = action.time;

		if( !_getCritiqueClient.call( getCritique ) )
		{
			ROS_WARN_STREAM( "Could not get critique for time: " << action.time );
			break;
		}
		double advantage = getCritique.response.critique;
		ROS_INFO_STREAM( "Advantage: " << advantage );

		remove_lowest( _actionBuffer );
		if( !action.input.allFinite() )
		{
			ROS_WARN_STREAM( "Received non-finite input: " << action.input.transpose() );
			continue;
		}
		if( !action.output.allFinite() )
		{
			ROS_WARN_STREAM( "Received non-finite action: " << action.output.transpose() );
			continue;
		}

		if( !std::isfinite( advantage ) )
		{
			ROS_WARN_STREAM( "Received non-finite advantage: " << advantage );
			continue;
		}
		_optimization.EmplaceModule( _policy.GetPolicyModule(),
		                             action.input, 
		                             action.output,
		                             advantage );

		ROS_INFO_STREAM( "Action: " << action.output.transpose() << 
		                 " Input: " << action.input.transpose() << 
		                 " Advantage: " << advantage );
	}

	if( _optimization.NumModules() == 0 ) 
	{
		_actionBuffer.clear();
		return;
	}

	// Perform optimization
	ROS_INFO_STREAM( "Optimizing with " << _optimization.NumModules() << " modules." );
	_optimization.ResetConstraints();
	_optimizer->ResetTerminationCheckers();
	percepto::OptimizationResults results = _optimizer->Optimize( _optimization );

	ROS_INFO_STREAM( "Result: " << results.status );
	ROS_INFO_STREAM( "Objective: " << results.finalObjective );
	ROS_INFO_STREAM( "Policy: " << *_policy.GetPolicyModule() );

	// Now that we've changed the policy, these old actions are no longer valid
	_lastOptimizationTime = ros::Time::now();
	_actionBuffer.clear();

	// Trim old data
	unsigned int targetModules = _maxModulesToKeep;
	if( _clearAfterOptimize )
	{
		targetModules = 0;
	}
	while( _optimization.NumModules() > targetModules )
	{
		_optimization.RemoveOldest();
	}

	// Set new parameters
	percepto_msgs::SetParameters srv;
	SerializeMatrix( _policy.GetParameters()->GetParamsVec(), srv.request.parameters );
	if( !_setParamsClient.call( srv ) )
	{
		throw std::runtime_error( "Could not set parameters." );
	}
}
Пример #5
0
std::ostream& operator << (std::ostream& stream, const std::vector<std::vector<T>>& matrix)
{
	SerializeMatrix(stream, matrix);
	return stream;
}