From 41f9bb89e5fdba6eb763db3e541ab4624b7ff447 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 20 Dec 2017 16:56:31 -0800 Subject: [PATCH] expose API to change the local inertia diagonal, pybullet.ChangeDynamics(objectUid, linkIndex, localInertiaDiagonal=[xx,yy,zz]) --- examples/SharedMemory/PhysicsClientC_API.cpp | 15 +++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 2 ++ .../PhysicsServerCommandProcessor.cpp | 20 ++++++++++++++++++ examples/SharedMemory/SharedMemoryCommands.h | 2 ++ examples/pybullet/examples/quadruped.py | 21 ++++++++++++++----- examples/pybullet/pybullet.c | 11 ++++++++-- 6 files changed, 64 insertions(+), 7 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 203b98669..09f6a9a2d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index fa450608d..0e6584ee1 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index df32a4928..78de50e10 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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); + } + } } } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index b3d8d8c49..60cc18c38 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -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; }; diff --git a/examples/pybullet/examples/quadruped.py b/examples/pybullet/examples/quadruped.py index 57e975f26..dfdd65892 100644 --- a/examples/pybullet/examples/quadruped.py +++ b/examples/pybullet/examples/quadruped.py @@ -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 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index c78a3c198..90bef2b32 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -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);