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

pybullet vr_kuka_setup.py: add a gear joint, to keep the gripper centered,

Browse Source 
vr_kuka_control.py: control all joints, use analogue button to close gripper
remove some debug warnings/prints
pybullet, avoid crash in changeUserConstraint if not passing a [list]
allow some gym environments (pybullet_pendulum and locomotors) to re-use an existing physics client connection.
This commit is contained in:
erwincoumans
2017-11-12 10:36:30 -08:00
parent 99f9584a2c
commit 6be7e34dd6
10 changed files with 101 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -688,7 +688,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
case CMD_CHANGE_USER_CONSTRAINT_FAILED:
{
B3_PROFILE("CMD_CHANGE_USER_CONSTRAINT_FAILED");
b3Warning("changeConstraint failed");
//b3Warning("changeConstraint failed");
break;
}
case CMD_ACTUAL_STATE_UPDATE_FAILED:

2
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -903,7 +903,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
}
case CMD_CHANGE_USER_CONSTRAINT_FAILED:
{
b3Warning("changeConstraint failed");
//b3Warning("changeConstraint failed");
break;
}

2
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -5542,7 +5542,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = true;
} else
{
b3Warning("Request state but no multibody or rigid body available");
//b3Warning("Request state but no multibody or rigid body available");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
hasStatus = true;

52
examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp
View File

@@ -62,33 +62,41 @@ B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* con
{
if (obj->m_constraintId>=0)
{
//this is basically equivalent to doing this in Python/pybullet:
//p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=...)
b3SharedMemoryCommandHandle commandHandle;
int userConstraintUniqueId = obj->m_constraintId;
commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, userConstraintUniqueId);
double pos[4] = {event.m_pos[0],event.m_pos[1],event.m_pos[2],1};
b3InitChangeUserConstraintSetPivotInB(commandHandle, pos);
double orn[4] = {event.m_orn[0],event.m_orn[1],event.m_orn[2],event.m_orn[3]};
b3InitChangeUserConstraintSetFrameInB(commandHandle, orn);
b3InitChangeUserConstraintSetMaxForce(commandHandle, obj->m_maxForce);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
struct b3UserConstraint constraintInfo;
if (b3GetUserConstraintInfo(context->m_physClient, obj->m_constraintId, &constraintInfo))
{
//this is basically equivalent to doing this in Python/pybullet:
//p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=...)
b3SharedMemoryCommandHandle commandHandle;
int userConstraintUniqueId = obj->m_constraintId;
commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, userConstraintUniqueId);
double pos[4] = {event.m_pos[0],event.m_pos[1],event.m_pos[2],1};
b3InitChangeUserConstraintSetPivotInB(commandHandle, pos);
double orn[4] = {event.m_orn[0],event.m_orn[1],event.m_orn[2],event.m_orn[3]};
b3InitChangeUserConstraintSetFrameInB(commandHandle, orn);
b3InitChangeUserConstraintSetMaxForce(commandHandle, obj->m_maxForce);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
}
}
// apply the analogue button to close the constraint, using a gear constraint with position target
if (obj->m_constraintId2>=0)
{
//this block is similar to
//p.changeConstraint(c,gearRatio=1, erp=..., relativePositionTarget=relPosTarget, maxForce=...)
//printf("obj->m_constraintId2=%d\n", obj->m_constraintId2);
b3SharedMemoryCommandHandle commandHandle;
commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, obj->m_constraintId2);
struct b3UserConstraint constraintInfo;
if (b3GetUserConstraintInfo(context->m_physClient, obj->m_constraintId2, &constraintInfo))
{
//this block is similar to
//p.changeConstraint(c,gearRatio=1, erp=..., relativePositionTarget=relPosTarget, maxForce=...)
//printf("obj->m_constraintId2=%d\n", obj->m_constraintId2);
b3SharedMemoryCommandHandle commandHandle;
commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, obj->m_constraintId2);
//0 -> open, 1 = closed
double openPos = 1.;
double relPosTarget = openPos - (event.m_analogAxis*openPos);
b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, relPosTarget);
b3InitChangeUserConstraintSetERP(commandHandle,1);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
//0 -> open, 1 = closed
double openPos = 1.;
double relPosTarget = openPos - (event.m_analogAxis*openPos);
b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, relPosTarget);
b3InitChangeUserConstraintSetERP(commandHandle,1);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
}
}
//printf("event.m_analogAxis=%f\n", event.m_analogAxis);