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

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
Erwin Coumans
2019-04-27 14:19:58 -07:00
parent ef9570c315 38f45d7137
commit efe73670ef
8 changed files with 341 additions and 190 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/BlockSolver/BlockSolverExample.cpp
View File

@@ -140,7 +140,7 @@ void BlockSolverExample::createMultiBodyStack()
tr.setOrigin(btVector3(0, 0, 0.1 + i * 0.2)); tr.setOrigin(btVector3(0, 0, 0.1 + i * 0.2));
btMultiBody* body = createMultiBody(mass, tr, boxShape); btMultiBody* body = createMultiBody(mass, tr, boxShape);
} }
if (0) if (/* DISABLES CODE */ 0)
{ {
btMultiBody* mb = loadRobot("cube_small.urdf"); btMultiBody* mb = loadRobot("cube_small.urdf");
btTransform tr; btTransform tr;

24
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -314,6 +314,30 @@ B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle c
return 0; return 0;
} }
B3_SHARED_API int b3LoadSoftbodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_initialPosition[0] = startPosX;
command->m_loadSoftBodyArguments.m_initialPosition[1] = startPosY;
command->m_loadSoftBodyArguments.m_initialPosition[2] = startPosZ;
command->m_updateFlags |= LOAD_SOFT_BODY_INITIAL_POSITION;
return 0;
}
B3_SHARED_API int b3LoadSoftbodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_initialOrientation[0] = startOrnX;
command->m_loadSoftBodyArguments.m_initialOrientation[1] = startOrnY;
command->m_loadSoftBodyArguments.m_initialOrientation[2] = startOrnZ;
command->m_loadSoftBodyArguments.m_initialOrientation[3] = startOrnW;
command->m_updateFlags |= LOAD_SOFT_BODY_INITIAL_ORIENTATION;
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName) B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
{ {
PhysicsClient* cl = (PhysicsClient*)physClient; PhysicsClient* cl = (PhysicsClient*)physClient;

45
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -1398,12 +1398,51 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
} }
case CMD_LOAD_SOFT_BODY_FAILED: case CMD_LOAD_SOFT_BODY_FAILED:
{ {
b3Warning("loadSoftBody failed"); B3_PROFILE("CMD_LOAD_SOFT_BODY_FAILED");
break;
if (m_data->m_verboseOutput)
{
b3Printf("Server failed loading the SoftBody...\n");
}
break;
} }
case CMD_LOAD_SOFT_BODY_COMPLETED: case CMD_LOAD_SOFT_BODY_COMPLETED:
{ {
break; B3_PROFILE("CMD_LOAD_SOFT_BODY_COMPLETED");
if (m_data->m_verboseOutput)
{
b3Printf("Server loading the SoftBody OK\n");
}
b3Assert(serverCmd.m_numDataStreamBytes);
if (serverCmd.m_numDataStreamBytes > 0)
{
bParse::btBulletFile bf(
this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor,
serverCmd.m_numDataStreamBytes);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints);
bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
bodyJoints->m_baseName = "baseLink";
if (bf.ok())
{
if (m_data->m_verboseOutput)
{
b3Printf("Received robot description ok!\n");
}
}
else
{
b3Warning("Robot description not received when loading soft body!");
}
}
break;
} }
case CMD_SYNC_USER_DATA_FAILED: case CMD_SYNC_USER_DATA_FAILED:
{ {

7
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -1079,7 +1079,12 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
} }
case CMD_LOAD_SOFT_BODY_COMPLETED: case CMD_LOAD_SOFT_BODY_COMPLETED:
{ {
break; int bodyUniqueId = serverCmd.m_loadSoftBodyResultArguments.m_objectUniqueId;
BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints);
bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
bodyJoints->m_baseName = "baseLink";
break;
} }
case CMD_SYNC_USER_DATA_FAILED: case CMD_SYNC_USER_DATA_FAILED:
{ {

328
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -122,24 +122,6 @@ btScalar gRhsClamp = 1.f;
#include "../CommonInterfaces/CommonFileIOInterface.h" #include "../CommonInterfaces/CommonFileIOInterface.h"
struct UrdfLinkNameMapUtil
{
btMultiBody* m_mb;
btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_rigidBodyJoints;
btDefaultSerializer* m_memSerializer;
UrdfLinkNameMapUtil() : m_mb(0), m_memSerializer(0)
{
}
virtual ~UrdfLinkNameMapUtil()
{
}
};
class b3ThreadPool class b3ThreadPool
{ {
public: public:
@@ -3357,71 +3339,74 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire //serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
btMultiBody* mb = bodyHandle ? bodyHandle->m_multiBody : 0; if(!bodyHandle) return 0;
if (mb) if (bodyHandle->m_multiBody)
{ {
UrdfLinkNameMapUtil utilBla; btMultiBody* mb = bodyHandle->m_multiBody;
UrdfLinkNameMapUtil* util = &utilBla;
btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient); btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient);
util->m_mb = mb; ser.startSerialization();
util->m_memSerializer = &ser;
util->m_memSerializer->startSerialization();
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them); //disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(), 0); ser.m_skipPointers.insert(mb->getBaseCollider(), 0);
if (mb->getBaseName()) if (mb->getBaseName())
{ {
util->m_memSerializer->registerNameForPointer(mb->getBaseName(), mb->getBaseName()); ser.registerNameForPointer(mb->getBaseName(), mb->getBaseName());
} }
bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
for (int i = 0; i < mb->getNumLinks(); i++) for (int i = 0; i < mb->getNumLinks(); i++)
{ {
//disable serialization of the collision objects //disable serialization of the collision objects
util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider, 0); ser.m_skipPointers.insert(mb->getLink(i).m_collider, 0);
util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_linkName, mb->getLink(i).m_linkName); ser.registerNameForPointer(mb->getLink(i).m_linkName, mb->getLink(i).m_linkName);
util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_jointName, mb->getLink(i).m_jointName); ser.registerNameForPointer(mb->getLink(i).m_jointName, mb->getLink(i).m_jointName);
} }
util->m_memSerializer->registerNameForPointer(mb->getBaseName(), mb->getBaseName()); ser.registerNameForPointer(mb->getBaseName(), mb->getBaseName());
int len = mb->calculateSerializeBufferSize(); int len = mb->calculateSerializeBufferSize();
btChunk* chunk = util->m_memSerializer->allocate(len, 1); btChunk* chunk = ser.allocate(len, 1);
const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer); const char* structType = mb->serialize(chunk->m_oldPtr, &ser);
util->m_memSerializer->finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb); ser.finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb);
streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize(); streamSizeInBytes = ser.getCurrentBufferSize();
} }
else else if(bodyHandle->m_rigidBody){
{ btRigidBody* rb = bodyHandle->m_rigidBody;
btRigidBody* rb = bodyHandle ? bodyHandle->m_rigidBody : 0;
if (rb)
{
UrdfLinkNameMapUtil utilBla;
UrdfLinkNameMapUtil* util = &utilBla;
btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient); btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient);
util->m_memSerializer = &ser; ser.startSerialization();
util->m_memSerializer->startSerialization(); ser.registerNameForPointer(bodyHandle->m_rigidBody, bodyHandle->m_bodyName.c_str());
util->m_memSerializer->registerNameForPointer(bodyHandle->m_rigidBody, bodyHandle->m_bodyName.c_str());
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them); //disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
for (int i = 0; i < bodyHandle->m_rigidBodyJoints.size(); i++) for (int i = 0; i < bodyHandle->m_rigidBodyJoints.size(); i++)
{ {
const btGeneric6DofSpring2Constraint* con = bodyHandle->m_rigidBodyJoints.at(i); const btGeneric6DofSpring2Constraint* con = bodyHandle->m_rigidBodyJoints.at(i);
util->m_memSerializer->registerNameForPointer(con, bodyHandle->m_rigidBodyJointNames[i].c_str()); ser.registerNameForPointer(con, bodyHandle->m_rigidBodyJointNames[i].c_str());
util->m_memSerializer->registerNameForPointer(&con->getRigidBodyB(), bodyHandle->m_rigidBodyLinkNames[i].c_str()); ser.registerNameForPointer(&con->getRigidBodyB(), bodyHandle->m_rigidBodyLinkNames[i].c_str());
const btRigidBody& bodyA = con->getRigidBodyA(); const btRigidBody& bodyA = con->getRigidBodyA();
int len = con->calculateSerializeBufferSize(); int len = con->calculateSerializeBufferSize();
btChunk* chunk = util->m_memSerializer->allocate(len, 1); btChunk* chunk = ser.allocate(len, 1);
const char* structType = con->serialize(chunk->m_oldPtr, util->m_memSerializer); const char* structType = con->serialize(chunk->m_oldPtr, &ser);
util->m_memSerializer->finalizeChunk(chunk, structType, BT_CONSTRAINT_CODE, (void*)con); ser.finalizeChunk(chunk, structType, BT_CONSTRAINT_CODE, (void*)con);
} }
streamSizeInBytes = ser.getCurrentBufferSize();
streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize();
} }
else if(bodyHandle->m_softBody){
//minimum serialization, registerNameForPointer
btSoftBody* sb = bodyHandle->m_softBody;
btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient);
ser.startSerialization();
ser.registerNameForPointer(sb, bodyHandle->m_bodyName.c_str());
int len = sb->calculateSerializeBufferSize();
btChunk* chunk = ser.allocate(len, 1);
const char* structType = sb->serialize(chunk->m_oldPtr, &ser);
ser.finalizeChunk(chunk, structType, BT_SOFTBODY_CODE, sb);
streamSizeInBytes = ser.getCurrentBufferSize();
} }
return streamSizeInBytes; return streamSizeInBytes;
@@ -5530,7 +5515,7 @@ bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct Shar
{ {
int usedHandle = usedHandles[i]; int usedHandle = usedHandles[i];
InternalBodyData* body = m_data->m_bodyHandles.getHandle(usedHandle); InternalBodyData* body = m_data->m_bodyHandles.getHandle(usedHandle);
if (body && (body->m_multiBody || body->m_rigidBody)) if (body && (body->m_multiBody || body->m_rigidBody || body->m_softBody))
{ {
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = usedHandle; serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = usedHandle;
} }
@@ -6551,59 +6536,100 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
hasStatus = true; hasStatus = true;
} }
else else if (body && body->m_rigidBody){
{ btRigidBody* rb = body->m_rigidBody;
if (body && body->m_rigidBody) SharedMemoryStatus& serverCmd = serverStatusOut;
{ serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
btRigidBody* rb = body->m_rigidBody;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
serverCmd.m_sendActualStateArgs.m_numLinks = 0; serverCmd.m_sendActualStateArgs.m_numLinks = 0;
serverCmd.m_sendActualStateArgs.m_stateDetails = 0; serverCmd.m_sendActualStateArgs.m_stateDetails = 0;
int totalDegreeOfFreedomQ = 0; int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 0; int totalDegreeOfFreedomU = 0;
btTransform tr = rb->getWorldTransform(); btTransform tr = rb->getWorldTransform();
//base position in world space, carthesian //base position in world space, carthesian
stateDetails->m_actualStateQ[0] = tr.getOrigin()[0]; stateDetails->m_actualStateQ[0] = tr.getOrigin()[0];
stateDetails->m_actualStateQ[1] = tr.getOrigin()[1]; stateDetails->m_actualStateQ[1] = tr.getOrigin()[1];
stateDetails->m_actualStateQ[2] = tr.getOrigin()[2]; stateDetails->m_actualStateQ[2] = tr.getOrigin()[2];
//base orientation, quaternion x,y,z,w, in world space, carthesian //base orientation, quaternion x,y,z,w, in world space, carthesian
stateDetails->m_actualStateQ[3] = tr.getRotation()[0]; stateDetails->m_actualStateQ[3] = tr.getRotation()[0];
stateDetails->m_actualStateQ[4] = tr.getRotation()[1]; stateDetails->m_actualStateQ[4] = tr.getRotation()[1];
stateDetails->m_actualStateQ[5] = tr.getRotation()[2]; stateDetails->m_actualStateQ[5] = tr.getRotation()[2];
stateDetails->m_actualStateQ[6] = tr.getRotation()[3]; stateDetails->m_actualStateQ[6] = tr.getRotation()[3];
totalDegreeOfFreedomQ += 7; //pos + quaternion totalDegreeOfFreedomQ += 7; //pos + quaternion
//base linear velocity (in world space, carthesian) //base linear velocity (in world space, carthesian)
stateDetails->m_actualStateQdot[0] = rb->getLinearVelocity()[0]; stateDetails->m_actualStateQdot[0] = rb->getLinearVelocity()[0];
stateDetails->m_actualStateQdot[1] = rb->getLinearVelocity()[1]; stateDetails->m_actualStateQdot[1] = rb->getLinearVelocity()[1];
stateDetails->m_actualStateQdot[2] = rb->getLinearVelocity()[2]; stateDetails->m_actualStateQdot[2] = rb->getLinearVelocity()[2];
//base angular velocity (in world space, carthesian) //base angular velocity (in world space, carthesian)
stateDetails->m_actualStateQdot[3] = rb->getAngularVelocity()[0]; stateDetails->m_actualStateQdot[3] = rb->getAngularVelocity()[0];
stateDetails->m_actualStateQdot[4] = rb->getAngularVelocity()[1]; stateDetails->m_actualStateQdot[4] = rb->getAngularVelocity()[1];
stateDetails->m_actualStateQdot[5] = rb->getAngularVelocity()[2]; stateDetails->m_actualStateQdot[5] = rb->getAngularVelocity()[2];
totalDegreeOfFreedomU += 6; //3 linear and 3 angular DOF totalDegreeOfFreedomU += 6; //3 linear and 3 angular DOF
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
hasStatus = true; hasStatus = true;
} }
else else if (body && body->m_softBody)
{ {
//b3Warning("Request state but no multibody or rigid body available"); btSoftBody* sb = body->m_softBody;
SharedMemoryStatus& serverCmd = serverStatusOut; SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
hasStatus = true; serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
} serverCmd.m_sendActualStateArgs.m_numLinks = 0;
} serverCmd.m_sendActualStateArgs.m_stateDetails = 0;
return hasStatus;
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] =
body->m_rootLocalInertialFrame.getOrigin()[0];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] =
body->m_rootLocalInertialFrame.getOrigin()[1];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] =
body->m_rootLocalInertialFrame.getOrigin()[2];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] =
body->m_rootLocalInertialFrame.getRotation()[0];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] =
body->m_rootLocalInertialFrame.getRotation()[1];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] =
body->m_rootLocalInertialFrame.getRotation()[2];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] =
body->m_rootLocalInertialFrame.getRotation()[3];
btTransform tr = sb->getWorldTransform();
//base position in world space, cartesian
stateDetails->m_actualStateQ[0] = tr.getOrigin()[0];
stateDetails->m_actualStateQ[1] = tr.getOrigin()[1];
stateDetails->m_actualStateQ[2] = tr.getOrigin()[2];
//base orientation, quaternion x,y,z,w, in world space, cartesian
stateDetails->m_actualStateQ[3] = tr.getRotation()[0];
stateDetails->m_actualStateQ[4] = tr.getRotation()[1];
stateDetails->m_actualStateQ[5] = tr.getRotation()[2];
stateDetails->m_actualStateQ[6] = tr.getRotation()[3];
int totalDegreeOfFreedomQ = 7; //pos + quaternion
int totalDegreeOfFreedomU = 6; //3 linear and 3 angular DOF
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
hasStatus = true;
}
else
{
//b3Warning("Request state but no multibody or rigid body available");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
hasStatus = true;
}
return hasStatus;
} }
bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
@@ -7281,17 +7307,34 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
btAssert((clientCmd.m_updateFlags & LOAD_SOFT_BODY_FILE_NAME) != 0); btAssert((clientCmd.m_updateFlags & LOAD_SOFT_BODY_FILE_NAME) != 0);
btAssert(loadSoftBodyArgs.m_fileName); btAssert(loadSoftBodyArgs.m_fileName);
btVector3 initialPos(0, 0, 0);
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_POSITION)
{
initialPos[0] = loadSoftBodyArgs.m_initialPosition[0];
initialPos[1] = loadSoftBodyArgs.m_initialPosition[1];
initialPos[2] = loadSoftBodyArgs.m_initialPosition[2];
}
btQuaternion initialOrn(0, 0, 0, 1);
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_ORIENTATION)
{
initialOrn[0] = loadSoftBodyArgs.m_initialOrientation[0];
initialOrn[1] = loadSoftBodyArgs.m_initialOrientation[1];
initialOrn[2] = loadSoftBodyArgs.m_initialOrientation[2];
initialOrn[3] = loadSoftBodyArgs.m_initialOrientation[3];
}
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_SCALE) if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_SCALE)
{ {
scale = clientCmd.m_loadSoftBodyArguments.m_scale; scale = clientCmd.m_loadSoftBodyArguments.m_scale;
} }
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_MASS) if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_MASS)
{ {
mass = clientCmd.m_loadSoftBodyArguments.m_mass; mass = clientCmd.m_loadSoftBodyArguments.m_mass;
} }
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN) if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN)
{ {
collisionMargin = clientCmd.m_loadSoftBodyArguments.m_collisionMargin; collisionMargin = clientCmd.m_loadSoftBodyArguments.m_collisionMargin;
} }
{ {
@@ -7310,7 +7353,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
bool foundFile = UrdfFindMeshFile(fileIO,pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); bool foundFile = UrdfFindMeshFile(fileIO,pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
std::vector<tinyobj::shape_t> shapes; std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str(),"",fileIO); std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str(),"",fileIO);
if (shapes.size() > 0) if (!shapes.empty())
{ {
const tinyobj::shape_t& shape = shapes[0]; const tinyobj::shape_t& shape = shapes[0];
btAlignedObjectArray<btScalar> vertices; btAlignedObjectArray<btScalar> vertices;
@@ -7334,8 +7377,8 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
psb->m_cfg.piterations = 20; psb->m_cfg.piterations = 20;
psb->m_cfg.kDF = 0.5; psb->m_cfg.kDF = 0.5;
psb->randomizeConstraints(); psb->randomizeConstraints();
psb->rotate(btQuaternion(0.70711, 0, 0, 0.70711)); psb->rotate(initialOrn);
psb->translate(btVector3(-0.05, 0, 1.0)); psb->translate(initialPos);
psb->scale(btVector3(scale, scale, scale)); psb->scale(btVector3(scale, scale, scale));
psb->setTotalMass(mass, true); psb->setTotalMass(mass, true);
@@ -7350,6 +7393,19 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId; serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED; serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED;
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
#ifdef ENABLE_LINK_MAPPER
if (m_data->m_urdfLinkNameMapper.size())
{
serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size() - 1)->m_memSerializer->getCurrentBufferSize();
}
#endif
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
b3Notification notification; b3Notification notification;
notification.m_notificationType = BODY_ADDED; notification.m_notificationType = BODY_ADDED;
notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId; notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId;
@@ -7482,6 +7538,17 @@ bool PhysicsServerCommandProcessor::processProfileTimingCommand(const struct Sha
return hasStatus; return hasStatus;
} }
void setDefaultRootWorldAABB(SharedMemoryStatus& serverCmd)
{
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1;
}
bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{ {
bool hasStatus = true; bool hasStatus = true;
@@ -7496,13 +7563,7 @@ bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const str
btMultiBody* mb = body->m_multiBody; btMultiBody* mb = body->m_multiBody;
serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED; serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED;
serverCmd.m_sendCollisionInfoArgs.m_numLinks = body->m_multiBody->getNumLinks(); serverCmd.m_sendCollisionInfoArgs.m_numLinks = body->m_multiBody->getNumLinks();
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0; setDefaultRootWorldAABB(serverCmd);
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1;
if (body->m_multiBody->getBaseCollider()) if (body->m_multiBody->getBaseCollider())
{ {
@@ -7544,21 +7605,14 @@ bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const str
} }
} }
} }
else else if (body && body->m_rigidBody)
{ {
if (body && body->m_rigidBody)
{
btRigidBody* rb = body->m_rigidBody; btRigidBody* rb = body->m_rigidBody;
SharedMemoryStatus& serverCmd = serverStatusOut; SharedMemoryStatus& serverCmd = serverStatusOut;
serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED; serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED;
serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0; serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0; setDefaultRootWorldAABB(serverCmd);
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1;
if (rb->getCollisionShape()) if (rb->getCollisionShape())
{ {
btTransform tr = rb->getWorldTransform(); btTransform tr = rb->getWorldTransform();
@@ -7573,8 +7627,14 @@ bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const str
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1]; serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1];
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2]; serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2];
} }
}
} }
else if (body && body->m_softBody){
SharedMemoryStatus& serverCmd = serverStatusOut;
serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED;
serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0;
setDefaultRootWorldAABB(serverCmd);
}
return hasStatus; return hasStatus;
} }
@@ -7815,7 +7875,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask); m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask);
} }
} }
} }
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL) if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
{ {
@@ -8171,6 +8231,12 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
} }
hasStatus = true; hasStatus = true;
} }
else if (body && body->m_softBody){
//todo: @fuchuyuan implement dynamics info
b3Warning("Softbody dynamics info not set!!!");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED;
}
return hasStatus; return hasStatus;
} }
@@ -10496,6 +10562,18 @@ bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const s
int totalNumVisualShapes = m_data->m_pluginManager.getRenderInterface()->getNumVisualShapes(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId); int totalNumVisualShapes = m_data->m_pluginManager.getRenderInterface()->getNumVisualShapes(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId);
//int totalBytesPerVisualShape = sizeof (b3VisualShapeData); //int totalBytesPerVisualShape = sizeof (b3VisualShapeData);
//int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1; //int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1;
//set serverCmd.m_sendVisualShapeArgs when totalNumVisualShapes is zero
if (totalNumVisualShapes==0) {
serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = 0;
serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 0;
serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData) * serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED;
}
else{
b3VisualShapeData* visualShapeStoragePtr = (b3VisualShapeData*)bufferServerToClient; b3VisualShapeData* visualShapeStoragePtr = (b3VisualShapeData*)bufferServerToClient;
int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
@@ -10531,6 +10609,10 @@ bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const s
serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData) * serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData) * serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED; serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED;
} }
else{
b3Warning("failed to get shape info");
}
}
} }
return hasStatus; return hasStatus;
} }

6
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -490,7 +490,9 @@ enum EnumLoadSoftBodyUpdateFlags
LOAD_SOFT_BODY_FILE_NAME = 1, LOAD_SOFT_BODY_FILE_NAME = 1,
LOAD_SOFT_BODY_UPDATE_SCALE = 2, LOAD_SOFT_BODY_UPDATE_SCALE = 2,
LOAD_SOFT_BODY_UPDATE_MASS = 4, LOAD_SOFT_BODY_UPDATE_MASS = 4,
LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN = 8 LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN = 8,
LOAD_SOFT_BODY_INITIAL_POSITION = 16,
LOAD_SOFT_BODY_INITIAL_ORIENTATION = 32
}; };
enum EnumSimParamInternalSimFlags enum EnumSimParamInternalSimFlags
@@ -507,6 +509,8 @@ struct LoadSoftBodyArgs
double m_scale; double m_scale;
double m_mass; double m_mass;
double m_collisionMargin; double m_collisionMargin;
double m_initialPosition[3];
double m_initialOrientation[4];
}; };
struct b3LoadSoftBodyResultArgs struct b3LoadSoftBodyResultArgs

117
examples/pybullet/gym/pybullet_envs/ARS/ars.py
View File

@@ -20,17 +20,17 @@ import argparse
# Setting the Hyper Parameters # Setting the Hyper Parameters
class Hp(): class Hp():
def __init__(self): def __init__(self):
self.nb_steps = 10000 self.nb_steps = 10000
self.episode_length = 1000 self.episode_length = 1000
self.learning_rate = 0.02 self.learning_rate = 0.02
self.nb_directions = 16 self.nb_directions = 16
self.nb_best_directions = 16 self.nb_best_directions = 8
assert self.nb_best_directions <= self.nb_directions assert self.nb_best_directions <= self.nb_directions
self.noise = 0.03 self.noise = 0.03
self.seed = 1 self.seed = 1
self.env_name = 'HalfCheetahBulletEnv-v0' self.env_name = 'HalfCheetahBulletEnv-v0'
# Multiprocess Exploring the policy on one specific direction and over one episode # Multiprocess Exploring the policy on one specific direction and over one episode
@@ -165,55 +165,52 @@ def explore(env, normalizer, policy, direction, delta, hp):
def train(env, policy, normalizer, hp, parentPipes, args): def train(env, policy, normalizer, hp, parentPipes, args):
for step in range(hp.nb_steps):
for step in range(hp.nb_steps):
# Initializing the perturbations deltas and the positive/negative rewards
# Initializing the perturbations deltas and the positive/negative rewards deltas = policy.sample_deltas()
deltas = policy.sample_deltas() positive_rewards = [0] * hp.nb_directions
positive_rewards = [0] * hp.nb_directions negative_rewards = [0] * hp.nb_directions
negative_rewards = [0] * hp.nb_directions
if parentPipes:
if parentPipes: for k in range(hp.nb_directions):
for k in range(hp.nb_directions): parentPipe = parentPipes[k]
parentPipe = parentPipes[k] parentPipe.send([_EXPLORE,[normalizer, policy, hp, "positive", deltas[k]]])
parentPipe.send([_EXPLORE, [normalizer, policy, hp, "positive", deltas[k]]]) for k in range(hp.nb_directions):
for k in range(hp.nb_directions): positive_rewards[k] = parentPipes[k].recv()[0]
positive_rewards[k] = parentPipes[k].recv()[0]
for k in range(hp.nb_directions):
for k in range(hp.nb_directions): parentPipe = parentPipes[k]
parentPipe = parentPipes[k] parentPipe.send([_EXPLORE,[normalizer, policy, hp, "negative", deltas[k]]])
parentPipe.send([_EXPLORE, [normalizer, policy, hp, "negative", deltas[k]]]) for k in range(hp.nb_directions):
for k in range(hp.nb_directions): negative_rewards[k] = parentPipes[k].recv()[0]
negative_rewards[k] = parentPipes[k].recv()[0]
else:
else: # Getting the positive rewards in the positive directions
# Getting the positive rewards in the positive directions for k in range(hp.nb_directions):
for k in range(hp.nb_directions): positive_rewards[k] = explore(env, normalizer, policy, "positive", deltas[k], hp)
positive_rewards[k] = explore(env, normalizer, policy, "positive", deltas[k], hp)
# Getting the negative rewards in the negative/opposite directions # Getting the negative rewards in the negative/opposite directions
for k in range(hp.nb_directions): for k in range(hp.nb_directions):
negative_rewards[k] = explore(env, normalizer, policy, "negative", deltas[k], hp) negative_rewards[k] = explore(env, normalizer, policy, "negative", deltas[k], hp)
# Gathering all the positive/negative rewards to compute the standard deviation of these rewards
all_rewards = np.array(positive_rewards + negative_rewards) # Gathering all the positive/negative rewards to compute the standard deviation of these rewards
sigma_r = all_rewards.std() all_rewards = np.array(positive_rewards + negative_rewards)
sigma_r = all_rewards.std()
# Sorting the rollouts by the max(r_pos, r_neg) and selecting the best directions
scores = { # Sorting the rollouts by the max(r_pos, r_neg) and selecting the best directions
k: max(r_pos, r_neg) scores = {k:max(r_pos, r_neg) for k,(r_pos,r_neg) in enumerate(zip(positive_rewards, negative_rewards))}
for k, (r_pos, r_neg) in enumerate(zip(positive_rewards, negative_rewards)) order = sorted(scores.keys(), key = lambda x:-scores[x])[:hp.nb_best_directions]
} rollouts = [(positive_rewards[k], negative_rewards[k], deltas[k]) for k in order]
order = sorted(scores.keys(), key=lambda x: scores[x])[:hp.nb_best_directions]
rollouts = [(positive_rewards[k], negative_rewards[k], deltas[k]) for k in order] # Updating our policy
policy.update(rollouts, sigma_r, args)
# Updating our policy
policy.update(rollouts, sigma_r, args) # Printing the final reward of the policy after the update
reward_evaluation = explore(env, normalizer, policy, None, None, hp)
# Printing the final reward of the policy after the update print('Step:', step, 'Reward:', reward_evaluation)
reward_evaluation = explore(env, normalizer, policy, None, None, hp)
print('Step:', step, 'Reward:', reward_evaluation)
# Running the main code # Running the main code

2
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -238,7 +238,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
{ {
} }
MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other) MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other)
{ {
btAssert(0); btAssert(0);
(void)other; (void)other;