expose PyBullet.setPhysicsEngineParameter(jointFeedbackMode)
use p.JOINT_FEEDBACK_IN_JOINT_FRAME if you want the joint feedback expressed in joint frame (instead of link inertial frame) use p.JOINT_FEEDBACK_IN_WORLD_SPACE if you want the joint feedback in world space coordinates, instead of local link/joint coordinates. Example: p.setPhysicsEngineParameter(jointFeedbackMode=p.JOINT_FEEDBACK_IN_WORLD_SPACE+p.JOINT_FEEDBACK_IN_JOINT_FRAME)
This commit is contained in:
@@ -575,6 +575,15 @@ B3_SHARED_API int b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryComma
|
|||||||
return 0;
|
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)
|
B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations)
|
||||||
{
|
{
|
||||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||||
|
|||||||
@@ -296,6 +296,8 @@ B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCo
|
|||||||
B3_SHARED_API int b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandle commandHandle, int enableConeFriction);
|
B3_SHARED_API int b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandle commandHandle, int enableConeFriction);
|
||||||
B3_SHARED_API int b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMemoryCommandHandle commandHandle, int deterministicOverlappingPairs);
|
B3_SHARED_API int b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMemoryCommandHandle commandHandle, int deterministicOverlappingPairs);
|
||||||
B3_SHARED_API int b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryCommandHandle commandHandle, double allowedCcdPenetration);
|
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);
|
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient);
|
||||||
|
|||||||
@@ -70,6 +70,9 @@
|
|||||||
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
|
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
extern bool gJointFeedbackInWorldSpace;
|
||||||
|
extern bool gJointFeedbackInJointFrame;
|
||||||
|
|
||||||
int gInternalSimFlags = 0;
|
int gInternalSimFlags = 0;
|
||||||
bool gResetSimulation = 0;
|
bool gResetSimulation = 0;
|
||||||
int gVRTrackingObjectUniqueId = -1;
|
int gVRTrackingObjectUniqueId = -1;
|
||||||
@@ -6623,6 +6626,8 @@ bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCom
|
|||||||
return hasStatus;
|
return hasStatus;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
|
bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
|
||||||
{
|
{
|
||||||
bool hasStatus = true;
|
bool hasStatus = true;
|
||||||
@@ -6648,6 +6653,12 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
|
|||||||
{
|
{
|
||||||
m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration = clientCmd.m_physSimParamArgs.m_allowedCcdPenetration;
|
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)
|
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -447,6 +447,7 @@ enum EnumSimParamUpdateFlags
|
|||||||
SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 32768,
|
SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 32768,
|
||||||
SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS = 65536,
|
SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS = 65536,
|
||||||
SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION = 131072,
|
SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION = 131072,
|
||||||
|
SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE = 262144,
|
||||||
};
|
};
|
||||||
|
|
||||||
enum EnumLoadSoftBodyUpdateFlags
|
enum EnumLoadSoftBodyUpdateFlags
|
||||||
|
|||||||
@@ -714,6 +714,12 @@ enum eStateLoggingFlags
|
|||||||
STATE_LOG_JOINT_TORQUES = STATE_LOG_JOINT_MOTOR_TORQUES+STATE_LOG_JOINT_USER_TORQUES,
|
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_SIZE 128
|
||||||
#define B3_MAX_PLUGIN_ARG_TEXT_LEN 1024
|
#define B3_MAX_PLUGIN_ARG_TEXT_LEN 1024
|
||||||
|
|
||||||
@@ -746,6 +752,7 @@ struct b3PhysicsSimulationParameters
|
|||||||
int m_enableConeFriction;
|
int m_enableConeFriction;
|
||||||
int m_deterministicOverlappingPairs;
|
int m_deterministicOverlappingPairs;
|
||||||
double m_allowedCcdPenetration;
|
double m_allowedCcdPenetration;
|
||||||
|
int m_jointFeedbackMode;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -1156,12 +1156,13 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
|
|||||||
int enableConeFriction = -1;
|
int enableConeFriction = -1;
|
||||||
b3PhysicsClientHandle sm = 0;
|
b3PhysicsClientHandle sm = 0;
|
||||||
int deterministicOverlappingPairs = -1;
|
int deterministicOverlappingPairs = -1;
|
||||||
|
int jointFeedbackMode =-1;
|
||||||
|
|
||||||
int physicsClientId = 0;
|
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,
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiidii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
|
||||||
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &physicsClientId))
|
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &physicsClientId))
|
||||||
{
|
{
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
@@ -1245,7 +1246,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
|
|||||||
if (allowedCcdPenetration>=0)
|
if (allowedCcdPenetration>=0)
|
||||||
{
|
{
|
||||||
b3PhysicsParameterSetAllowedCcdPenetration(command,allowedCcdPenetration);
|
b3PhysicsParameterSetAllowedCcdPenetration(command,allowedCcdPenetration);
|
||||||
|
}
|
||||||
|
if (jointFeedbackMode>=0)
|
||||||
|
{
|
||||||
|
b3PhysicsParameterSetJointFeedbackMode(command,jointFeedbackMode);
|
||||||
}
|
}
|
||||||
|
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||||
@@ -9177,6 +9181,9 @@ initpybullet(void)
|
|||||||
|
|
||||||
PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType); // user read
|
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, "TORQUE_CONTROL", CONTROL_MODE_TORQUE);
|
||||||
PyModule_AddIntConstant(m, "VELOCITY_CONTROL",
|
PyModule_AddIntConstant(m, "VELOCITY_CONTROL",
|
||||||
CONTROL_MODE_VELOCITY); // user read
|
CONTROL_MODE_VELOCITY); // user read
|
||||||
|
|||||||
Reference in New Issue
Block a user