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

fix the pybullet.changeDynamics linear/angular damping

Browse Source 
expose pybullet.getConstraintState
This commit is contained in:
Erwin Coumans
2017-10-18 19:15:35 -07:00
parent 618deae3e4
commit c178c101a8
8 changed files with 154 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2128,7 +2128,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHan
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linearDamping = angularDamping;
command->m_changeDynamicsInfoArgs.m_angularDamping = angularDamping;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING;
return 0;
}
@@ -2292,10 +2292,47 @@ B3_SHARED_API int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle c
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitGetUserConstraintStateCommand(b3PhysicsClientHandle physClient, int constraintUniqueId)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_USER_CONSTRAINT;
command->m_updateFlags = USER_CONSTRAINT_REQUEST_STATE;
command->m_userConstraintArguments.m_userConstraintUniqueId = constraintUniqueId;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3GetStatusUserConstraintState(b3SharedMemoryStatusHandle statusHandle, struct b3UserConstraintState* constraintState)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status)
{
btAssert(status->m_type == CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED);
if (status && status->m_type == CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED)
{
int i = 0;
constraintState->m_numDofs = status->m_userConstraintStateResultArgs.m_numDofs;
for (i = 0; i < constraintState->m_numDofs; i++)
{
constraintState->m_appliedConstraintForces[i] = status->m_userConstraintStateResultArgs.m_appliedConstraintForces[i];
}
for (; i < 6; i++)
{
constraintState->m_appliedConstraintForces[i] = 0;
}
return 1;
}
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
@@ -2341,6 +2378,7 @@ B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle s
}
B3_SHARED_API b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ)

5
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -143,6 +143,11 @@ B3_SHARED_API int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle c
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
B3_SHARED_API int b3GetNumUserConstraints(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitGetUserConstraintStateCommand(b3PhysicsClientHandle physClient, int constraintUniqueId);
B3_SHARED_API int b3GetStatusUserConstraintState(b3SharedMemoryStatusHandle statusHandle, struct b3UserConstraintState* constraintState);
B3_SHARED_API int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info);
/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() )
B3_SHARED_API int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex);

4
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -601,6 +601,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
break;
}
case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED:
{
break;
}
case CMD_USER_CONSTRAINT_INFO_COMPLETED:
{
B3_PROFILE("CMD_USER_CONSTRAINT_INFO_COMPLETED");

5
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -783,7 +783,10 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
}
break;
}
case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED:
{
break;
}
case CMD_SYNC_BODY_INFO_COMPLETED:
case CMD_MJCF_LOADING_COMPLETED:
case CMD_SDF_LOADING_COMPLETED:

44
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -5630,13 +5630,22 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
double rollingFriction = clientCmd.m_changeDynamicsInfoArgs.m_rollingFriction;
double restitution = clientCmd.m_changeDynamicsInfoArgs.m_restitution;
btAssert(bodyUniqueId >= 0);
btAssert(linkIndex >= -1);
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
{
mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
{
mb->setAngularDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
}
if (linkIndex == -1)
{
if (mb->getBaseCollider())
@@ -5645,14 +5654,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
mb->getBaseCollider()->setRestitution(restitution);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
{
mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
{
mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING)
{
@@ -7173,6 +7175,29 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED;
hasStatus = true;
if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_STATE)
{
int constraintUid = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(constraintUid);
if (userConstraintPtr)
{
serverCmd.m_userConstraintStateResultArgs.m_numDofs = 0;
for (int i = 0; i < 6; i++)
{
serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = 0;
}
if (userConstraintPtr->m_mbConstraint)
{
serverCmd.m_userConstraintStateResultArgs.m_numDofs = userConstraintPtr->m_mbConstraint->getNumRows();
for (int i = 0; i < userConstraintPtr->m_mbConstraint->getNumRows(); i++)
{
serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = userConstraintPtr->m_mbConstraint->getAppliedImpulse(i) / m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
}
serverCmd.m_type = CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED;
}
}
};
if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_INFO)
{
int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
@@ -7180,6 +7205,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (userConstraintPtr)
{
serverCmd.m_userConstraintResultArgs = userConstraintPtr->m_userConstraintData;
serverCmd.m_type = CMD_USER_CONSTRAINT_INFO_COMPLETED;
}
}

3
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -676,7 +676,7 @@ enum EnumUserConstraintFlags
USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK=256,
USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET=512,
USER_CONSTRAINT_CHANGE_ERP=1024,
USER_CONSTRAINT_REQUEST_STATE=2048,
};
enum EnumBodyChangeFlags
@@ -1039,6 +1039,7 @@ struct SharedMemoryStatus
struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
struct UserDebugDrawResultArgs m_userDebugDrawArgs;
struct b3UserConstraint m_userConstraintResultArgs;
struct b3UserConstraintState m_userConstraintStateResultArgs;
struct SendVREvents m_sendVREvents;
struct SendKeyboardEvents m_sendKeyboardEvents;
struct SendRaycastHits m_raycastHits;

11
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -5,7 +5,8 @@
///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
///my convention is year/month/day/rev
#define SHARED_MEMORY_MAGIC_NUMBER 201710050
#define SHARED_MEMORY_MAGIC_NUMBER 201710180
//#define SHARED_MEMORY_MAGIC_NUMBER 201710050
//#define SHARED_MEMORY_MAGIC_NUMBER 201708270
//#define SHARED_MEMORY_MAGIC_NUMBER 201707140
//#define SHARED_MEMORY_MAGIC_NUMBER 201706015
@@ -143,6 +144,7 @@ enum EnumSharedMemoryServerStatus
CMD_USER_DEBUG_DRAW_FAILED,
CMD_USER_CONSTRAINT_COMPLETED,
CMD_USER_CONSTRAINT_INFO_COMPLETED,
CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED,
CMD_REMOVE_USER_CONSTRAINT_COMPLETED,
CMD_CHANGE_USER_CONSTRAINT_COMPLETED,
CMD_REMOVE_USER_CONSTRAINT_FAILED,
@@ -253,7 +255,6 @@ struct b3UserConstraint
int m_gearAuxLink;
double m_relativePositionTarget;
double m_erp;
};
struct b3BodyInfo
@@ -331,6 +332,12 @@ struct b3OpenGLVisualizerCameraInfo
float m_target[3];
};
struct b3UserConstraintState
{
double m_appliedConstraintForces[6];
int m_numDofs;
};
enum b3VREventType
{
VR_CONTROLLER_MOVE_EVENT=1,