From 1c65cae6d0b0e91faf93ca5bc2a06cf44b64152d Mon Sep 17 00:00:00 2001 From: Jeffrey Bingham Date: Thu, 1 Sep 2016 10:45:14 -0700 Subject: [PATCH] [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. --- .../PhysicsServerCommandProcessor.cpp | 48 ++++++++++--------- 1 file changed, 25 insertions(+), 23 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 84d4554ca..914cce573 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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;