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]);
}
/////////////////////////////////////////////////////////////////////////////
// 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);
}
/////////////////////////////////////////////////////////////////////////////
// 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);
}
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." );
}
}
std::ostream& operator << (std::ostream& stream, const std::vector<std::vector<T>>& matrix)
{
SerializeMatrix(stream, matrix);
return stream;
}
