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

Merge pull request #1 from bingjeff/master

Browse Source 
[SharedMemory] Calculate inverse dynamics now uses world gravity.
This commit is contained in:
erwincoumans
2016-09-01 11:18:02 -07:00
committed by GitHub
parent e6865a1186 1c65cae6d0
commit c7ad8f2a3d
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;