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

Merge pull request #1683 from erwincoumans/master

Browse Source 
fix PyBullet.applyExternalTorque for links, expose jointFeedbackMode, improve  PyBullet combineUrdf.py performance
This commit is contained in:
erwincoumans
2018-05-11 20:40:46 -07:00
committed by GitHub
parent 282154f377 e5a9b42f9a
commit 5ed461f512
7 changed files with 48 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -575,6 +575,15 @@ B3_SHARED_API int b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryComma
return 0;
}
B3_SHARED_API int b3PhysicsParameterSetJointFeedbackMode(b3SharedMemoryCommandHandle commandHandle, int jointFeedbackMode)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_jointFeedbackMode = jointFeedbackMode;
command->m_updateFlags |= SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -296,6 +296,8 @@ B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCo
B3_SHARED_API int b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandle commandHandle, int enableConeFriction);
B3_SHARED_API int b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMemoryCommandHandle commandHandle, int deterministicOverlappingPairs);
B3_SHARED_API int b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryCommandHandle commandHandle, double allowedCcdPenetration);
B3_SHARED_API int b3PhysicsParameterSetJointFeedbackMode(b3SharedMemoryCommandHandle commandHandle, int jointFeedbackMode);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient);

11
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -70,6 +70,9 @@
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#endif
extern bool gJointFeedbackInWorldSpace;
extern bool gJointFeedbackInJointFrame;
int gInternalSimFlags = 0;
bool gResetSimulation = 0;
int gVRTrackingObjectUniqueId = -1;
@@ -6623,6 +6626,8 @@ bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCom
return hasStatus;
}
bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
@@ -6648,6 +6653,12 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
{
m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration = clientCmd.m_physSimParamArgs.m_allowedCcdPenetration;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE)
{
gJointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode&JOINT_FEEDBACK_IN_WORLD_SPACE)!=0;
gJointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode&JOINT_FEEDBACK_IN_JOINT_FRAME)!=0;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME)
{

1
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -447,6 +447,7 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 32768,
SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS = 65536,
SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION = 131072,
SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE = 262144,
};
enum EnumLoadSoftBodyUpdateFlags

7
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -714,6 +714,12 @@ enum eStateLoggingFlags
STATE_LOG_JOINT_TORQUES = STATE_LOG_JOINT_MOTOR_TORQUES+STATE_LOG_JOINT_USER_TORQUES,
};
enum eJointFeedbackModes
{
JOINT_FEEDBACK_IN_WORLD_SPACE=1,
JOINT_FEEDBACK_IN_JOINT_FRAME=2,
};
#define B3_MAX_PLUGIN_ARG_SIZE 128
#define B3_MAX_PLUGIN_ARG_TEXT_LEN 1024
@@ -746,6 +752,7 @@ struct b3PhysicsSimulationParameters
int m_enableConeFriction;
int m_deterministicOverlappingPairs;
double m_allowedCcdPenetration;
int m_jointFeedbackMode;
};

5
examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py
View File

@@ -10,6 +10,7 @@ p0.setAdditionalSearchPath(pybullet_data.getDataPath())
p1 = bc.BulletClient(connection_mode=pybullet.DIRECT)
p1.setAdditionalSearchPath(pybullet_data.getDataPath())
#can also connect using different modes, GUI, SHARED_MEMORY, TCP, UDP, SHARED_MEMORY_SERVER, GUI_SERVER
husky = p1.loadURDF("husky/husky.urdf", flags=p0.URDF_USE_IMPLICIT_CYLINDER)
@@ -42,11 +43,15 @@ print("p0.getNumBodies()=",p0.getNumBodies())
print("p1.getNumBodies()=",p1.getNumBodies())
pgui = bc.BulletClient(connection_mode=pybullet.GUI)
pgui.configureDebugVisualizer(pgui.COV_ENABLE_RENDERING, 0)
orn=[0,0,0,1]
ed0.createMultiBody([0,0,0],orn, pgui._client)
pgui.setRealTimeSimulation(1)
pgui.configureDebugVisualizer(pgui.COV_ENABLE_RENDERING, 1)
while (pgui.isConnected()):
pgui.getCameraImage(320,200, renderer=pgui.ER_BULLET_HARDWARE_OPENGL)
time.sleep(1./240.)

19
examples/pybullet/pybullet.c
View File

@@ -1156,12 +1156,13 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
int enableConeFriction = -1;
b3PhysicsClientHandle sm = 0;
int deterministicOverlappingPairs = -1;
int jointFeedbackMode =-1;
int physicsClientId = 0;
static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "physicsClientId", NULL};
static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiidi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiidii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &physicsClientId))
{
return NULL;
}
@@ -1245,7 +1246,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
if (allowedCcdPenetration>=0)
{
b3PhysicsParameterSetAllowedCcdPenetration(command,allowedCcdPenetration);
}
if (jointFeedbackMode>=0)
{
b3PhysicsParameterSetJointFeedbackMode(command,jointFeedbackMode);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
@@ -7599,7 +7603,7 @@ static PyObject* pybullet_applyExternalTorque(PyObject* self, PyObject* args, Py
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle command =
b3ApplyExternalForceCommandInit(sm);
b3ApplyExternalTorque(command, objectUniqueId, -1, torque, flags);
b3ApplyExternalTorque(command, objectUniqueId, linkIndex, torque, flags);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}
}
@@ -9177,6 +9181,9 @@ initpybullet(void)
PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType); // user read
PyModule_AddIntConstant(m, "JOINT_FEEDBACK_IN_WORLD_SPACE", JOINT_FEEDBACK_IN_WORLD_SPACE); // user read
PyModule_AddIntConstant(m, "JOINT_FEEDBACK_IN_JOINT_FRAME", JOINT_FEEDBACK_IN_JOINT_FRAME); // user read
PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE);
PyModule_AddIntConstant(m, "VELOCITY_CONTROL",
CONTROL_MODE_VELOCITY); // user read