void CLegGrid::makeStates()
{
/*
* measurement = recieved leg detector data
* control command = leg velocity (-robot velocity)
* last state = last estimated state
*/
clearStates();
ROS_INFO_COND(DEBUG,"size of new leg msgs: %lu", grid_->polar_array.current.size());
if(diff_time_.toSec() < 1.0)
{
meas_.assign(grid_->polar_array.current.begin(), grid_->polar_array.current.end());
if(!grid_->polar_array.current.empty())
grid_->polar_array.current.clear();
}
match_meas_.resize(meas_.size(), false);
addLastStates();
addMeasurements();
filterStates();
ROS_ASSERT(cmeas_.size() == cstate_.size());
}
hkDemo::Result UnevenTerrainVsDemo::stepDemo()
{
HK_TIMER_BEGIN( "simulate multiply characters", HK_NULL );
m_world->lock();
hkVector4 up;
up.setNeg4( m_world->getGravity() );
up.normalize3();
m_tick++;
hkpCharacterInput inputRb;
hkpCharacterInput inputProxy;
// Fill in the state machine input structure for character rigid body
{
inputRb.m_atLadder = false;
inputRb.m_up = up;
// Steer the characters
inputRb.m_inputLR = 0.0f;
inputRb.m_inputUD = 1.0f;
{
// The factor 70.0f gives a turning circle which fits in the level
// for the default walk speed of 10.0f.
hkReal walkSpeedFactor = m_options.m_characterWalkSpeed / 70.0f;
hkReal time = hkReal(m_tick) * walkSpeedFactor / 60.0f;
const hkReal x = hkMath::sin(time);
const hkReal z = hkMath::cos(time);
inputRb.m_forward.set( x, 0, z );
inputRb.m_forward.normalize3();
}
inputRb.m_wantJump = false;
hkStepInfo stepInfo;
stepInfo.m_deltaTime = m_timestep;
stepInfo.m_invDeltaTime = 1.0f/m_timestep;
inputRb.m_stepInfo = stepInfo;
inputRb.m_characterGravity.set( 0.0f, m_options.m_characterGravity, 0.0f );
inputProxy = inputRb;
// character rigid body specific code
inputRb.m_velocity = m_characterRigidBody->getLinearVelocity();
inputRb.m_position = m_characterRigidBody->getPosition();
m_characterRigidBody->checkSupport(stepInfo, inputRb.m_surfaceInfo);
filterStates( m_filterRigidBody, inputRb.m_surfaceInfo );
m_supportHistoryRb <<= 1;
if ( inputRb.m_surfaceInfo.m_supportedState == hkpSurfaceInfo::SUPPORTED )
{
++m_supportHistoryRb;
}
else
{
++m_unsupportedFramesCountRb;
}
// character proxy specific code
inputProxy.m_velocity = m_characterProxy->getLinearVelocity();
inputProxy.m_position = m_characterProxy->getPosition();
hkVector4 down; down.setNeg4(UP);
m_characterProxy->checkSupport(down, inputProxy.m_surfaceInfo);
filterStates( m_filterProxy, inputProxy.m_surfaceInfo );
m_supportHistoryProxy <<= 1;
if ( inputProxy.m_surfaceInfo.m_supportedState == hkpSurfaceInfo::SUPPORTED )
{
++m_supportHistoryProxy;
}
else
{
++m_unsupportedFramesCountProxy;
}
}
hkpCharacterOutput outputRb;
hkpCharacterOutput outputProxy;
// Apply the character state machine
{
HK_TIMER_BEGIN( "update character state", HK_NULL );
m_characterContext->update(inputRb, outputRb);
m_characterProxyContext->update(inputProxy, outputProxy);
HK_TIMER_END();
}
//Apply the character controllers
{
HK_TIMER_BEGIN( "simulate character", HK_NULL );
// Feed the output velocity from state machine into character rigid body
m_characterRigidBody->setLinearVelocity(outputRb.m_velocity, m_timestep);
m_characterProxy->setLinearVelocity(outputProxy.m_velocity );
hkStepInfo si;
si.m_deltaTime = m_timestep;
si.m_invDeltaTime = 1.0f/m_timestep;
m_characterProxy->integrate( si, m_world->getGravity() );
HK_TIMER_END();
}
{
// If the character has fallen off the world we reposition them
if ( ( m_characterProxy->getPosition()(1) < -10.0f ) || ( m_characterRigidBody->getPosition()(1) < -10.0f ) )
{
m_characterProxy->setPosition( characterStart );
m_characterProxy->setLinearVelocity( hkVector4::getZero() );
m_characterRigidBody->getRigidBody()->setPosition(characterStart);
m_characterRigidBody->setLinearVelocity( hkVector4::getZero(), m_timestep );
m_tick = 0;
}
}
m_world->unlock();
// Display state
{
char historyRb[33];
char historyProxy[33];
char buffer[255];
for ( int i = 0; i < 32; ++i )
{
historyRb[i] = m_supportHistoryRb & ( 1 << i ) ? '#' : '_';
historyProxy[i] = m_supportHistoryProxy & ( 1 << i ) ? '#' : '_';
}
historyRb[32] = '\0';
historyProxy[32] = '\0';
hkString::sprintf( buffer, "Rigid body unsupported frames: %d\n"
"Proxy unsupported frames: %d\n\n"
"Rigid body support history: %s\n"
"Proxy support history: %s\n", m_unsupportedFramesCountRb, m_unsupportedFramesCountProxy, historyRb, historyProxy );
m_env->m_textDisplay->outputText( buffer, 20, 200, 0xffffffff );
}
// Step the world
{
hkDefaultPhysicsDemo::stepDemo();
}
HK_TIMER_END();
return hkDemo::DEMO_OK;
}