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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
yunfeibai
2016-10-17 13:20:31 -07:00
parent 14fc8ae8c2 292dd5ced4
commit 3ffd95fbab
32 changed files with 922 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files

206
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -31,6 +31,7 @@
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "../SoftDemo/BunnyMesh.h"
//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
btVector3 gLastPickPos(0, 0, 0);
bool gCloseToKuka=false;
@@ -305,6 +306,12 @@ struct CommandLogPlayback
}
};
struct SaveWorldObjectData
{
b3AlignedObjectArray<int> m_bodyUniqueIds;
std::string m_fileName;
};
struct PhysicsServerCommandProcessorInternalData
{
///handle management
@@ -401,6 +408,8 @@ struct PhysicsServerCommandProcessorInternalData
btMultiBody* m_kukaGripperMultiBody;
btMultiBodyPoint2Point* m_kukaGripperRevolute1;
btMultiBodyPoint2Point* m_kukaGripperRevolute2;
int m_huskyId;
int m_KukaId;
int m_sphereId;
@@ -414,7 +423,7 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
btHashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
b3AlignedObjectArray<SaveWorldObjectData> m_saveWorldBodyData;
btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
@@ -457,6 +466,10 @@ struct PhysicsServerCommandProcessorInternalData
:m_hasGround(false),
m_gripperRigidbodyFixed(0),
m_gripperMultiBody(0),
m_kukaGripperFixed(0),
m_kukaGripperMultiBody(0),
m_kukaGripperRevolute1(0),
m_kukaGripperRevolute2(0),
m_allowRealTimeSimulation(false),
m_huskyId(-1),
m_KukaId(-1),
@@ -817,6 +830,9 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
{
b3Printf("loaded %s OK!", fileName);
}
SaveWorldObjectData sd;
sd.m_fileName = fileName;
for (int m =0; m<u2b.getNumModels();m++)
{
@@ -830,6 +846,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
sd.m_bodyUniqueIds.push_back(bodyUniqueId);
u2b.setBodyUniqueId(bodyUniqueId);
{
@@ -860,6 +877,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
if (mb)
mb->setUserIndex2(bodyUniqueId);
if (mb)
{
bodyHandle->m_multiBody = mb;
@@ -904,6 +922,9 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
}
}
m_data->m_saveWorldBodyData.push_back(sd);
}
return loadOk;
}
@@ -936,6 +957,14 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
if (bodyUniqueIdPtr)
*bodyUniqueIdPtr= bodyUniqueId;
//quick prototype of 'save world' for crude world editing
{
SaveWorldObjectData sd;
sd.m_fileName = fileName;
sd.m_bodyUniqueIds.push_back(bodyUniqueId);
m_data->m_saveWorldBodyData.push_back(sd);
}
u2b.setBodyUniqueId(bodyUniqueId);
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
@@ -1342,6 +1371,154 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = true;
break;
}
case CMD_SAVE_WORLD:
{
///this is a very rudimentary way to save the state of the world, for scene authoring
///many todo's, for example save the state of motor controllers etc.
{
//saveWorld(clientCmd.m_sdfArguments.m_sdfFileName);
FILE* f = fopen(clientCmd.m_sdfArguments.m_sdfFileName,"w");
if (f)
{
char line[1024];
{
sprintf(line,"import pybullet as p\n");
int len = strlen(line);
fwrite(line,len,1,f);
}
{
sprintf(line,"p.connect(p.SHARED_MEMORY)\n");
int len = strlen(line);
fwrite(line,len,1,f);
}
//for each objects ...
for (int i=0;i<m_data->m_saveWorldBodyData.size();i++)
{
SaveWorldObjectData& sd = m_data->m_saveWorldBodyData[i];
for (int i=0;i<sd.m_bodyUniqueIds.size();i++)
{
{
int bodyUniqueId = sd.m_bodyUniqueIds[i];
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
if (body)
{
if (body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
btTransform comTr = mb->getBaseWorldTransform();
btTransform tr = comTr * body->m_rootLocalInertialFrame.inverse();
if (strstr(sd.m_fileName.c_str(),".urdf"))
{
sprintf(line,"objects = [p.loadURDF(\"%s\", %f,%f,%f,%f,%f,%f,%f)]\n",sd.m_fileName.c_str(),
tr.getOrigin()[0],tr.getOrigin()[1],tr.getOrigin()[2],
tr.getRotation()[0],tr.getRotation()[1],tr.getRotation()[2],tr.getRotation()[3]);
int len = strlen(line);
fwrite(line,len,1,f);
}
if (strstr(sd.m_fileName.c_str(),".sdf") && i==0)
{
sprintf(line,"objects = p.loadSDF(\"%s\")\n",sd.m_fileName.c_str());
int len = strlen(line);
fwrite(line,len,1,f);
}
if (strstr(sd.m_fileName.c_str(),".sdf") || ((strstr(sd.m_fileName.c_str(),".urdf")) && mb->getNumLinks()) )
{
sprintf(line,"ob = objects[%d]\n",i);
int len = strlen(line);
fwrite(line,len,1,f);
}
if (strstr(sd.m_fileName.c_str(),".sdf"))
{
sprintf(line,"p.resetBasePositionAndOrientation(ob,[%f,%f,%f],[%f,%f,%f,%f])\n",
comTr.getOrigin()[0],comTr.getOrigin()[1],comTr.getOrigin()[2],
comTr.getRotation()[0],comTr.getRotation()[1],comTr.getRotation()[2],comTr.getRotation()[3]);
int len = strlen(line);
fwrite(line,len,1,f);
}
if (mb->getNumLinks())
{
{
sprintf(line,"jointPositions=[");
int len = strlen(line);
fwrite(line,len,1,f);
}
for (int i=0;i<mb->getNumLinks();i++)
{
btScalar jointPos = mb->getJointPosMultiDof(i)[0];
if (i<mb->getNumLinks()-1)
{
sprintf(line," %f,",jointPos);
int len = strlen(line);
fwrite(line,len,1,f);
} else
{
sprintf(line," %f ",jointPos);
int len = strlen(line);
fwrite(line,len,1,f);
}
}
{
sprintf(line,"]\nfor jointIndex in range (p.getNumJoints(ob)):\n\tp.resetJointState(ob,jointIndex,jointPositions[jointIndex])\n\n");
int len = strlen(line);
fwrite(line,len,1,f);
}
}
} else
{
//todo: btRigidBody/btSoftBody etc case
}
}
}
}
//for URDF, load at origin, then reposition...
struct SaveWorldObjectData
{
b3AlignedObjectArray<int> m_bodyUniqueIds;
std::string m_fileName;
};
}
{
btVector3 grav=this->m_data->m_dynamicsWorld->getGravity();
sprintf(line,"p.setGravity(%f,%f,%f)\n",grav[0],grav[1],grav[2]);
int len = strlen(line);
fwrite(line,len,1,f);
}
{
sprintf(line,"p.stepSimulation()\np.disconnect()\n");
int len = strlen(line);
fwrite(line,len,1,f);
}
fclose(f);
}
serverStatusOut.m_type = CMD_SAVE_WORLD_COMPLETED;
hasStatus = true;
break;
}
serverStatusOut.m_type = CMD_SAVE_WORLD_FAILED;
hasStatus = true;
break;
}
case CMD_LOAD_SDF:
{
const SdfArgs& sdfArgs = clientCmd.m_sdfArguments;
@@ -1846,16 +2023,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin();
btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation();
btVector3 linkOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin();
btQuaternion linkRotation = mb->getLink(l).m_cachedWorldTransform.getRotation();
btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin();
btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkOrigin.getX();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkOrigin.getY();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkOrigin.getZ();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkRotation.x();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkRotation.y();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkRotation.z();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkRotation.w();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkCOMOrigin.getX();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkCOMOrigin.getY();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkCOMOrigin.getZ();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkCOMRotation.x();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkCOMRotation.y();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkCOMRotation.z();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkCOMRotation.w();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY();
@@ -2744,12 +2921,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
}
btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation;
btVector3 endEffectorPosWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin();
btQuaternion endEffectorOriWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getRotation();
btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin();
btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation();
btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
@@ -3000,6 +3179,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
int bodyId = 0;
if (gCreateObjectSimVR >= 0)
{
gCreateObjectSimVR = -1;
@@ -3189,7 +3369,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
//loadUrdf("rook.urdf", btVector3(-1.2, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("knight.urdf", btVector3(-1.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_huskyId = bodyId;
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));