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

Merge pull request #1483 from erwincoumans/master

Browse Source 
expose API to change the local inertia diagonal, pybullet.ChangeDynam…
This commit is contained in:
erwincoumans 2017-12-20 17:46:36 -08:00 committed by GitHub
commit 60d2af8ed2
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 64 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -2092,6 +2092,21 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle comman
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetLocalInertiaDiagonal(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double localInertiaDiagonal[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_localInertiaDiagonal[0] = localInertiaDiagonal[0];
command->m_changeDynamicsInfoArgs.m_localInertiaDiagonal[1] = localInertiaDiagonal[1];
command->m_changeDynamicsInfoArgs.m_localInertiaDiagonal[2] = localInertiaDiagonal[2];
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL;
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -116,6 +116,8 @@ B3_SHARED_API int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, str
B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass);
B3_SHARED_API int b3ChangeDynamicsInfoSetLocalInertiaDiagonal(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double localInertiaDiagonal[3]);
B3_SHARED_API int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction);
B3_SHARED_API int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
B3_SHARED_API int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);

20
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -5933,6 +5933,10 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
double spinningFriction = clientCmd.m_changeDynamicsInfoArgs.m_spinningFriction;
double rollingFriction = clientCmd.m_changeDynamicsInfoArgs.m_rollingFriction;
double restitution = clientCmd.m_changeDynamicsInfoArgs.m_restitution;
btVector3 newLocalInertiaDiagonal(clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[0],
clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[1],
clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[2]);
btAssert(bodyUniqueId >= 0);
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
@ -5998,6 +6002,10 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
mb->setBaseInertia(localInertia);
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
{
mb->setBaseInertia(newLocalInertiaDiagonal);
}
}
else
{
@ -6052,6 +6060,10 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
mb->getLink(linkIndex).m_inertiaLocal = localInertia;
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
{
mb->getLink(linkIndex).m_inertiaLocal = newLocalInertiaDiagonal;
}
}
}
} else
@ -6110,6 +6122,14 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
}
body->m_rigidBody->setMassProps(mass,localInertia);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
{
btScalar orgMass = body->m_rigidBody->getInvMass();
if (orgMass>0)
{
body->m_rigidBody->setMassProps(mass,newLocalInertiaDiagonal);
}
}
}
}

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -151,6 +151,7 @@ enum EnumChangeDynamicsInfoFlags
CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING=128,
CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING=256,
CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR = 512,
CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL = 1024,
};
@ -168,6 +169,7 @@ struct ChangeDynamicsInfoArgs
double m_angularDamping;
double m_contactStiffness;
double m_contactDamping;
double m_localInertiaDiagonal[3];
int m_frictionAnchor;
};

21
examples/pybullet/examples/quadruped.py
View File

@ -114,11 +114,6 @@ motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
drawInertiaBox(quadruped,-1, [1,0,0])
#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
for i in range (nJoints):
drawInertiaBox(quadruped,i, [0,1,0])
#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])
@ -128,6 +123,22 @@ halfpi = 1.57079632679
twopi = 4*halfpi
kneeangle = -2.1834
mass, friction, localInertiaDiagonal = p.getDynamicsInfo(quadruped,-1, flags=p.DYNAMICS_INFO_REPORT_INERTIA )
print("localInertiaDiagonal",localInertiaDiagonal)
#this is a no-op, just to show the API
p.changeDynamics(quadruped,-1,localInertiaDiagonal=localInertiaDiagonal)
#for i in range (nJoints):
# p.changeDynamics(quadruped,i,localInertiaDiagonal=[0.000001,0.000001,0.000001])
drawInertiaBox(quadruped,-1, [1,0,0])
#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
for i in range (nJoints):
drawInertiaBox(quadruped,i, [0,1,0])
if (useMaximalCoordinates):
steps = 400

11
examples/pybullet/pybullet.c
View File

@ -737,12 +737,13 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
double contactStiffness = -1;
double contactDamping = -1;
int frictionAnchor = -1;
PyObject* localInertiaDiagonalObj=0;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddii", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &physicsClientId))
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &physicsClientId))
{
return NULL;
}
@ -762,6 +763,12 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
{
b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, mass);
}
if (localInertiaDiagonalObj)
{
double localInertiaDiagonal[3];
pybullet_internalSetVectord(localInertiaDiagonalObj, localInertiaDiagonal);
b3ChangeDynamicsInfoSetLocalInertiaDiagonal(command, bodyUniqueId, linkIndex, localInertiaDiagonal);
}
if (lateralFriction >= 0)
{
b3ChangeDynamicsInfoSetLateralFriction(command, bodyUniqueId, linkIndex, lateralFriction);