AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 22:00:05 +00:00
Code Issues Releases Wiki Activity

Merge pull request #1376 from YunfeiBai/master

Browse Source 
Modify inverse kinematics to also support robot model with fixed joint.
This commit is contained in:
erwincoumans 2017-10-09 19:17:24 -07:00 committed by GitHub
commit 0be7584eee
1 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -7667,14 +7667,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs); btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
for (int i = 0; i < numDofs; i++) int DofIndex = 0;
{ for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i) {
q_current[i] = bodyHandle->m_multiBody->getJointPos(i); if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2) { // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints.
q[i+baseDofs] = bodyHandle->m_multiBody->getJointPos(i); q_current[DofIndex] = bodyHandle->m_multiBody->getJointPos(i);
qdot[i + baseDofs] = 0; q[DofIndex+baseDofs] = bodyHandle->m_multiBody->getJointPos(i);
nu[i+baseDofs] = 0; qdot[DofIndex+baseDofs] = 0;
} nu[DofIndex+baseDofs] = 0;
// Set the gravity to correspond to the world gravity DofIndex++;
}
} // Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
if (-1 != tree->setGravityInWorldFrame(id_grav) && if (-1 != tree->setGravityInWorldFrame(id_grav) &&