コード例 #1
0
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());
}
コード例 #2
0
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;
}