[SharedMemory] Calculate inverse dynamics now uses world gravity.
Small change that takes the world gravity and applies it to the body during the call to inverse dynamics in the shared memory interface. Otherwise the gravity vector is left at the default value and there is currently no interface to set the gravity for the inverse dynamics system.
This commit is contained in:
Jeffrey Bingham
@@ -507,7 +507,7 @@ void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiH
|
||||
}
|
||||
m_data->m_guiHelper = guiHelper;
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -518,7 +518,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
|
||||
|
||||
createEmptyDynamicsWorld();
|
||||
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001;
|
||||
|
||||
|
||||
}
|
||||
|
||||
PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
|
||||
@@ -887,7 +887,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
|
||||
|
||||
btMultiBody* mb = creation.getBulletMultiBody();
|
||||
btRigidBody* rb = creation.getRigidBody();
|
||||
|
||||
|
||||
if (useMultiBody)
|
||||
{
|
||||
|
||||
@@ -1024,7 +1024,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
|
||||
|
||||
bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes )
|
||||
{
|
||||
|
||||
|
||||
bool hasStatus = false;
|
||||
|
||||
{
|
||||
@@ -1154,7 +1154,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight;
|
||||
int numPixelsCopied = 0;
|
||||
|
||||
|
||||
|
||||
|
||||
if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0)
|
||||
{
|
||||
@@ -1481,10 +1481,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD)!=0)
|
||||
{
|
||||
kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
motor->setVelocityTarget(desiredVelocity,kd);
|
||||
|
||||
|
||||
btScalar kp = 0.f;
|
||||
motor->setPositionTarget(0,kp);
|
||||
hasDesiredVelocity = true;
|
||||
@@ -1565,7 +1565,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
|
||||
motor->setVelocityTarget(desiredVelocity,kd);
|
||||
motor->setPositionTarget(desiredPosition,kp);
|
||||
|
||||
|
||||
btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
|
||||
|
||||
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
|
||||
@@ -1805,7 +1805,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
}
|
||||
case CMD_STEP_FORWARD_SIMULATION:
|
||||
{
|
||||
|
||||
|
||||
if (m_data->m_verboseOutput)
|
||||
{
|
||||
b3Printf("Step simulation request");
|
||||
@@ -1897,7 +1897,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
clientCmd.m_initPoseArgs.m_initialStateQ[4],
|
||||
clientCmd.m_initPoseArgs.m_initialStateQ[5],
|
||||
clientCmd.m_initPoseArgs.m_initialStateQ[6]);
|
||||
|
||||
|
||||
mb->setWorldToBaseRot(invOrn.inverse());
|
||||
}
|
||||
if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE)
|
||||
@@ -1936,10 +1936,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
{
|
||||
m_data->m_visualConverter.resetAll();
|
||||
}
|
||||
|
||||
|
||||
deleteDynamicsWorld();
|
||||
createEmptyDynamicsWorld();
|
||||
|
||||
|
||||
m_data->exitHandles();
|
||||
m_data->initHandles();
|
||||
|
||||
@@ -2128,7 +2128,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
|
||||
if (bodyHandle && bodyHandle->m_multiBody)
|
||||
{
|
||||
btInverseDynamics::MultiBodyTree** treePtrPtr =
|
||||
btInverseDynamics::MultiBodyTree** treePtrPtr =
|
||||
m_data->m_inverseDynamicsBodies.find(bodyHandle->m_multiBody);
|
||||
btInverseDynamics::MultiBodyTree* tree = 0;
|
||||
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
|
||||
@@ -2140,12 +2140,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
else
|
||||
{
|
||||
btInverseDynamics::btMultiBodyTreeCreator id_creator;
|
||||
if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false))
|
||||
if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false))
|
||||
{
|
||||
b3Error("error creating tree\n");
|
||||
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
|
||||
}
|
||||
else
|
||||
else
|
||||
{
|
||||
tree = btInverseDynamics::CreateMultiBodyTree(id_creator);
|
||||
m_data->m_inverseDynamicsBodies.insert(bodyHandle->m_multiBody, tree);
|
||||
@@ -2163,8 +2163,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
|
||||
nu[i+baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i];
|
||||
}
|
||||
|
||||
if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
|
||||
// Set the gravity to correspond to the world gravity
|
||||
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
|
||||
if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force) &&
|
||||
-1 != tree->setGravityInWorldFrame(id_grav))
|
||||
{
|
||||
serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
|
||||
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs;
|
||||
@@ -2179,7 +2181,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -2203,7 +2205,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
{
|
||||
btMultiBody* mb = body->m_multiBody;
|
||||
bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME)!=0);
|
||||
|
||||
|
||||
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0)
|
||||
{
|
||||
btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0],
|
||||
@@ -2213,7 +2215,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
clientCmd.m_externalForceArguments.m_positions[i*3+0],
|
||||
clientCmd.m_externalForceArguments.m_positions[i*3+1],
|
||||
clientCmd.m_externalForceArguments.m_positions[i*3+2]);
|
||||
|
||||
|
||||
if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
|
||||
{
|
||||
btVector3 forceWorld = isLinkFrame ? forceLocal : mb->getBaseWorldTransform().getBasis()*forceLocal;
|
||||
@@ -2236,7 +2238,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0],
|
||||
clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1],
|
||||
clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]);
|
||||
|
||||
|
||||
if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
|
||||
{
|
||||
btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal;
|
||||
@@ -2252,7 +2254,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
SharedMemoryStatus& serverCmd =serverStatusOut;
|
||||
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
|
||||
hasStatus = true;
|
||||
|
||||
Reference in New Issue
Block a user