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

add experimental pr2-gripper support in VR physics server

Browse Source 
add setErp to btMultiBodyJointMotor
This commit is contained in:
erwin coumans
2016-09-09 14:30:37 -07:00
parent f72982306e
commit 3c706306cd
7 changed files with 234 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

84
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -382,6 +382,7 @@ struct PhysicsServerCommandProcessorInternalData
bool m_hasGround;
btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
btMultiBody* m_gripperMultiBody;
CommandLogger* m_commandLogger;
CommandLogPlayback* m_logPlayback;
@@ -432,6 +433,7 @@ struct PhysicsServerCommandProcessorInternalData
PhysicsServerCommandProcessorInternalData()
:m_hasGround(false),
m_gripperRigidbodyFixed(0),
m_gripperMultiBody(0),
m_allowRealTimeSimulation(true),
m_commandLogger(0),
m_logPlayback(0),
@@ -2697,6 +2699,9 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
btVector3 gVRGripperPos(0,0,0);
btQuaternion gVRGripperOrn(0,0,0,1);
bool gVRGripperClosed = false;
int gDroppedSimulationSteps = 0;
int gNumSteps = 0;
double gDtInSec = 0.f;
@@ -2717,37 +2722,70 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
}
if (0)//m_data->m_gripperRigidbodyFixed==0)
{
int bodyId = 0;
loadUrdf("pr2_gripper.urdf",btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
InteralBodyData* parentBody = m_data->getHandle(bodyId);
if (parentBody->m_multiBody)
if (m_data->m_gripperRigidbodyFixed == 0)
{
parentBody->m_multiBody->setHasSelfCollision(1);
btVector3 pivotInParent(0,0,0);
btMatrix3x3 frameInParent;
frameInParent.setRotation(btQuaternion(0,0,0,1));
btVector3 pivotInChild(0,0,0);
btMatrix3x3 frameInChild;
frameInChild.setIdentity();
int bodyId = 0;
m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,-1,0,pivotInParent,pivotInChild,frameInParent,frameInChild);
m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(2.);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed);
if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
{
InteralBodyData* parentBody = m_data->getHandle(bodyId);
if (parentBody->m_multiBody)
{
parentBody->m_multiBody->setHasSelfCollision(1);
btVector3 pivotInParent(0, 0, 0);
btMatrix3x3 frameInParent;
frameInParent.setRotation(btQuaternion(0, 0, 0, 1));
btVector3 pivotInChild(0, 0, 0);
btMatrix3x3 frameInChild;
frameInChild.setIdentity();
m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, -1, 0, pivotInParent, pivotInChild, frameInParent, frameInChild);
m_data->m_gripperMultiBody = parentBody->m_multiBody;
if (m_data->m_gripperMultiBody->getNumLinks() > 2)
{
m_data->m_gripperMultiBody->setJointPos(0, SIMD_HALF_PI);
m_data->m_gripperMultiBody->setJointPos(2, SIMD_HALF_PI);
}
m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(2.);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed);
}
}
}
}
if (m_data->m_gripperRigidbodyFixed)
if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody)
{
m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn));
m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos);
if (m_data->m_gripperMultiBody->getNumLinks() > 2)
{
for (int i = 0; i < 2; i++)
{
if (supportsJointMotor(m_data->m_gripperMultiBody, i * 2))
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i * 2).m_userPtr;
if (motor)
{
motor->setErp(0.005);
if (gVRGripperClosed)
{
motor->setPositionTarget(0.2, 1);
}
else
{
motor->setPositionTarget(SIMD_HALF_PI, 1);
}
motor->setVelocityTarget(0, 0.1);
btScalar maxImp = 550.*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
}
}
}
}
}
int maxSteps = 3;

41
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -18,6 +18,8 @@
extern btVector3 gLastPickPos;
btVector3 gVRTeleportPos(0,0,0);
extern bool gVRGripperClosed;
bool gDebugRenderToggle = false;
void MotionThreadFunc(void* userPtr,void* lsMemory);
void* MotionlsMemoryFunc();
@@ -1116,20 +1118,29 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
gDebugRenderToggle = !gDebugRenderToggle;
}
if (button==1)
{
m_args[0].m_isVrControllerTeleporting[controllerId] = true;
}
if ((button==33))
if (controllerId == 3 && (button == 33))
{
m_args[0].m_isVrControllerPicking[controllerId] = (state!=0);
m_args[0].m_isVrControllerReleasing[controllerId] = (state==0);
gVRGripperClosed =state;
}
if ((button==33)||(button==1))
else
{
m_args[0].m_vrControllerPos[controllerId].setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]);
m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0],orn[1],orn[2],orn[3]);
if ((button == 33))
{
m_args[0].m_isVrControllerPicking[controllerId] = (state != 0);
m_args[0].m_isVrControllerReleasing[controllerId] = (state == 0);
}
if ((button == 33) || (button == 1))
{
m_args[0].m_vrControllerPos[controllerId].setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]);
}
}
}
@@ -1144,13 +1155,17 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[
printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS);
return;
}
m_args[0].m_vrControllerPos[controllerId].setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]);
m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0],orn[1],orn[2],orn[3]);
gVRGripperPos.setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]);
btQuaternion orgOrn(orn[0],orn[1],orn[2],orn[3]);
gVRGripperOrn = orgOrn*btQuaternion(btVector3(0,0,1),SIMD_HALF_PI)*btQuaternion(btVector3(0,1,0),SIMD_HALF_PI);
if (controllerId == 3)
{
gVRGripperPos.setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
btQuaternion orgOrn(orn[0], orn[1], orn[2], orn[3]);
gVRGripperOrn = orgOrn*btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI);
}
else
{
m_args[0].m_vrControllerPos[controllerId].setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]);
}
}
B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc)

6
examples/StandaloneMain/hellovr_opengl_main.cpp
View File

@@ -700,12 +700,16 @@ bool CMainApplication::HandleInput()
if (button==2)
{
glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
gDebugDrawFlags = btIDebugDraw::DBG_DrawContactPoints
///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync
//add a special debug drawer that deals with this
//gDebugDrawFlags = btIDebugDraw::DBG_DrawContactPoints
//btIDebugDraw::DBG_DrawConstraintLimits+
//btIDebugDraw::DBG_DrawConstraints
;
}
sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn);
}
else
{