Method stepSimulation

examples/InverseDynamics/InverseDynamicsExample.cpp:245–355

}

void InverseDynamicsExample::stepSimulation(float deltaTime)
{
	if (m_multiBody)
	{
		const int num_dofs = m_multiBody->getNumDofs();
		btInverseDynamics::vecx nu(num_dofs), qdot(num_dofs), q(num_dofs), joint_force(num_dofs);
		btInverseDynamics::vecx pd_control(num_dofs);

		// compute joint forces from one of two control laws:
		// 1) "computed torque" control, which gives perfect, decoupled,
		//    linear second order error dynamics per dof in case of a
		//    perfect model and (and negligible time discretization effects)
		// 2) decoupled PD control per joint, without a model
		for (int dof = 0; dof < num_dofs; dof++)
		{
			q(dof) = m_multiBody->getJointPos(dof);
			qdot(dof) = m_multiBody->getJointVel(dof);

			const btScalar qd_dot = 0;
			const btScalar qd_ddot = 0;
			if (m_timeSeriesCanvas)
				m_timeSeriesCanvas->insertDataAtCurrentTime(q[dof], dof, true);

			// pd_control is either desired joint torque for pd control,
			// or the feedback contribution to nu
			pd_control(dof) = kd * (qd_dot - qdot(dof)) + kp * (qd[dof] - q(dof));
			// nu is the desired joint acceleration for computed torque control
			nu(dof) = qd_ddot + pd_control(dof);
		}
		if (useInverseModel)
		{
			// calculate joint forces corresponding to desired accelerations nu
			if (m_multiBody->hasFixedBase())
			{
				if (-1 != m_inverseModel->calculateInverseDynamics(q, qdot, nu, &joint_force))
				{
					//joint_force(dof) += damping*dot_q(dof);
					// use inverse model: apply joint force corresponding to
					// desired acceleration nu

					for (int dof = 0; dof < num_dofs; dof++)
					{
						m_multiBody->addJointTorque(dof, joint_force(dof));
					}
				}
			}
			else
			{
				//the inverse dynamics model represents the 6 DOFs of the base, unlike btMultiBody.
				//append some dummy values to represent the 6 DOFs of the base
				btInverseDynamics::vecx nu6(num_dofs + 6), qdot6(num_dofs + 6), q6(num_dofs + 6), joint_force6(num_dofs + 6);
				for (int i = 0; i < num_dofs; i++)
				{
					nu6[6 + i] = nu[i];
					qdot6[6 + i] = qdot[i];
					q6[6 + i] = q[i];
					joint_force6[6 + i] = joint_force[i];
				}