Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

[SharedMemory] Calculate inverse dynamics now uses world gravity.

Browse Source 
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
2016-09-01 10:45:14 -07:00
parent e6865a1186
commit 1c65cae6d0
1 changed files with 25 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -507,7 +507,7 @@ void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiH
} }
m_data->m_guiHelper = guiHelper; m_data->m_guiHelper = guiHelper;
} }
@@ -518,7 +518,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
createEmptyDynamicsWorld(); createEmptyDynamicsWorld();
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001; m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001;
} }
PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor() PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
@@ -887,7 +887,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
btMultiBody* mb = creation.getBulletMultiBody(); btMultiBody* mb = creation.getBulletMultiBody();
btRigidBody* rb = creation.getRigidBody(); btRigidBody* rb = creation.getRigidBody();
if (useMultiBody) 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 PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes )
{ {
bool hasStatus = false; bool hasStatus = false;
{ {
@@ -1154,7 +1154,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight; int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight;
int numPixelsCopied = 0; int numPixelsCopied = 0;
if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=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) if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD)!=0)
{ {
kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex]; kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex];
} }
motor->setVelocityTarget(desiredVelocity,kd); motor->setVelocityTarget(desiredVelocity,kd);
btScalar kp = 0.f; btScalar kp = 0.f;
motor->setPositionTarget(0,kp); motor->setPositionTarget(0,kp);
hasDesiredVelocity = true; hasDesiredVelocity = true;
@@ -1565,7 +1565,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
motor->setVelocityTarget(desiredVelocity,kd); motor->setVelocityTarget(desiredVelocity,kd);
motor->setPositionTarget(desiredPosition,kp); motor->setPositionTarget(desiredPosition,kp);
btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) 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: case CMD_STEP_FORWARD_SIMULATION:
{ {
if (m_data->m_verboseOutput) if (m_data->m_verboseOutput)
{ {
b3Printf("Step simulation request"); 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[4],
clientCmd.m_initPoseArgs.m_initialStateQ[5], clientCmd.m_initPoseArgs.m_initialStateQ[5],
clientCmd.m_initPoseArgs.m_initialStateQ[6]); clientCmd.m_initPoseArgs.m_initialStateQ[6]);
mb->setWorldToBaseRot(invOrn.inverse()); mb->setWorldToBaseRot(invOrn.inverse());
} }
if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE) if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE)
@@ -1936,10 +1936,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
m_data->m_visualConverter.resetAll(); m_data->m_visualConverter.resetAll();
} }
deleteDynamicsWorld(); deleteDynamicsWorld();
createEmptyDynamicsWorld(); createEmptyDynamicsWorld();
m_data->exitHandles(); m_data->exitHandles();
m_data->initHandles(); m_data->initHandles();
@@ -2128,7 +2128,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId); InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody) if (bodyHandle && bodyHandle->m_multiBody)
{ {
btInverseDynamics::MultiBodyTree** treePtrPtr = btInverseDynamics::MultiBodyTree** treePtrPtr =
m_data->m_inverseDynamicsBodies.find(bodyHandle->m_multiBody); m_data->m_inverseDynamicsBodies.find(bodyHandle->m_multiBody);
btInverseDynamics::MultiBodyTree* tree = 0; btInverseDynamics::MultiBodyTree* tree = 0;
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
@@ -2140,12 +2140,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
else else
{ {
btInverseDynamics::btMultiBodyTreeCreator id_creator; 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"); b3Error("error creating tree\n");
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
} }
else else
{ {
tree = btInverseDynamics::CreateMultiBodyTree(id_creator); tree = btInverseDynamics::CreateMultiBodyTree(id_creator);
m_data->m_inverseDynamicsBodies.insert(bodyHandle->m_multiBody, tree); 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]; qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
nu[i+baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i]; nu[i+baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i];
} }
// Set the gravity to correspond to the world gravity
if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) 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_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs; 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; serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
} }
} }
} }
else else
{ {
@@ -2203,7 +2205,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
btMultiBody* mb = body->m_multiBody; btMultiBody* mb = body->m_multiBody;
bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME)!=0); bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME)!=0);
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0) if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0)
{ {
btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+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+0],
clientCmd.m_externalForceArguments.m_positions[i*3+1], clientCmd.m_externalForceArguments.m_positions[i*3+1],
clientCmd.m_externalForceArguments.m_positions[i*3+2]); clientCmd.m_externalForceArguments.m_positions[i*3+2]);
if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
{ {
btVector3 forceWorld = isLinkFrame ? forceLocal : mb->getBaseWorldTransform().getBasis()*forceLocal; 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], 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+1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]);
if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
{ {
btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal; btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal;
@@ -2252,7 +2254,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
} }
} }
} }
SharedMemoryStatus& serverCmd =serverStatusOut; SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true; hasStatus = true;