Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-08-07 13:59:24 +00:00
Code Issues Packages Projects Releases Wiki Activity

Cleaned-up/fixed version of this Pull Request #3239, thanks to Wenlong Lu

Browse Source
This commit is contained in:
erwin coumans
2021-04-05 11:40:45 -07:00
parent 4d34ba7310
commit 8b8c1af6a4
9 changed files with 358 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

122
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -7143,8 +7143,8 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
motor->setRhsClamp(clientCmd.m_sendDesiredStateCommandArgument.m_rhsClamp[velIndex]);
}
bool hasDesiredPosOrVel = false;
btScalar kp = 0.f;
btScalar kd = 0.f;
btVector3 kp(0, 0, 0);
btVector3 kd(0, 0, 0);
btVector3 desiredVelocity(0, 0, 0);
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT) != 0)
{
@@ -7153,7 +7153,7 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex + 0],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex + 1],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex + 2]);
kd = 0.1;
kd.setValue(0.1, 0.1, 0.1);
}
btQuaternion desiredPosition(0, 0, 0, 1);
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q) != 0)
@@ -7164,38 +7164,122 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 1],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 2],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 3]);
kp = 0.1;
kp.setValue(0.1, 0.1, 0.1);
}
if (hasDesiredPosOrVel)
{
bool useMultiDof = true;
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KP) != 0)
{
kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
kp.setValue(
clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 0],
clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 0],
clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 0]);
}
if (((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+0] & SIM_DESIRED_STATE_HAS_KP) != 0) &&
((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+1] & SIM_DESIRED_STATE_HAS_KP) != 0) &&
((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+2] & SIM_DESIRED_STATE_HAS_KP) != 0)
)
{
kp.setValue(
clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 0],
clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 1],
clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 2]);
} else
{
useMultiDof = false;
}
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KD) != 0)
{
kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
kd.setValue(
clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 0],
clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 0],
clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 0]);
}
motor->setVelocityTarget(desiredVelocity, kd);
//todo: instead of clamping, combine the motor and limit
//and combine handling of limit force and motor force.
if (((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+0] & SIM_DESIRED_STATE_HAS_KD) != 0) &&
((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+1] & SIM_DESIRED_STATE_HAS_KD) != 0) &&
((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+2] & SIM_DESIRED_STATE_HAS_KD) != 0))
{
kd.setValue(
clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 0],
clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 1],
clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 2]);
} else
{
useMultiDof = false;
}
//clamp position
//if (mb->getLink(link).m_jointLowerLimit <= mb->getLink(link).m_jointUpperLimit)
//{
// btClamp(desiredPosition, mb->getLink(link).m_jointLowerLimit, mb->getLink(link).m_jointUpperLimit);
//}
motor->setPositionTarget(desiredPosition, kp);
btVector3 maxImp(
1000000.f * m_data->m_physicsDeltaTime,
1000000.f * m_data->m_physicsDeltaTime,
1000000.f * m_data->m_physicsDeltaTime);
btScalar maxImp = 1000000.f * m_data->m_physicsDeltaTime;
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
{
maxImp.setValue(
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 0] * m_data->m_physicsDeltaTime,
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 0] * m_data->m_physicsDeltaTime,
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 0] * m_data->m_physicsDeltaTime);
}
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0)
maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex] * m_data->m_physicsDeltaTime;
if (((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+0] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) &&
((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+1] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) &&
((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+2] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0))
{
maxImp.setValue(
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 0] * m_data->m_physicsDeltaTime,
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 1] * m_data->m_physicsDeltaTime,
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 2] * m_data->m_physicsDeltaTime);
} else
{
useMultiDof = false;
}
if (useMultiDof)
{
motor->setVelocityTargetMultiDof(desiredVelocity, kd);
motor->setPositionTargetMultiDof(desiredPosition, kp);
motor->setMaxAppliedImpulseMultiDof(maxImp);
} else
{
motor->setVelocityTarget(desiredVelocity, kd[0]);
//todo: instead of clamping, combine the motor and limit
//and combine handling of limit force and motor force.
motor->setMaxAppliedImpulse(maxImp);
//clamp position
//if (mb->getLink(link).m_jointLowerLimit <= mb->getLink(link).m_jointUpperLimit)
//{
// btClamp(desiredPosition, mb->getLink(link).m_jointLowerLimit, mb->getLink(link).m_jointUpperLimit);
//}
motor->setPositionTarget(desiredPosition, kp[0]);
motor->setMaxAppliedImpulse(maxImp[0]);
}
btVector3 damping(1.f, 1.f, 1.f);
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_DAMPING) != 0) {
if (
(clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+0] & SIM_DESIRED_STATE_HAS_DAMPING)&&
(clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+1] & SIM_DESIRED_STATE_HAS_DAMPING)&&
(clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+2] & SIM_DESIRED_STATE_HAS_DAMPING)
)
{
damping.setValue(
clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 0],
clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 1],
clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 2]);
} else
{
damping.setValue(
clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 0],
clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 0],
clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 0]);
}
}
motor->setDamping(damping);
}
numMotors++;
}