diff --git a/examples/BlockSolver/BlockSolverExample.cpp b/examples/BlockSolver/BlockSolverExample.cpp index ba699ee64..76cb2bc5e 100644 --- a/examples/BlockSolver/BlockSolverExample.cpp +++ b/examples/BlockSolver/BlockSolverExample.cpp @@ -140,7 +140,7 @@ void BlockSolverExample::createMultiBodyStack() tr.setOrigin(btVector3(0, 0, 0.1 + i * 0.2)); btMultiBody* body = createMultiBody(mass, tr, boxShape); } - if (0) + if (/* DISABLES CODE */ 0) { btMultiBody* mb = loadRobot("cube_small.urdf"); btTransform tr; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 920a463b7..c2cf772ae 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -314,6 +314,30 @@ B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle c 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) { PhysicsClient* cl = (PhysicsClient*)physClient; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index f6cd23d1c..f0b742623 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1398,12 +1398,51 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() } case CMD_LOAD_SOFT_BODY_FAILED: { - b3Warning("loadSoftBody failed"); - break; + B3_PROFILE("CMD_LOAD_SOFT_BODY_FAILED"); + + if (m_data->m_verboseOutput) + { + b3Printf("Server failed loading the SoftBody...\n"); + } + break; } 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: { diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index dd5bbfb7f..2c3b9a43f 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -1079,7 +1079,12 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd } 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: { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b0c6697ec..1ed19e316 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -122,24 +122,6 @@ btScalar gRhsClamp = 1.f; #include "../CommonInterfaces/CommonFileIOInterface.h" - - - -struct UrdfLinkNameMapUtil -{ - btMultiBody* m_mb; - btAlignedObjectArray m_rigidBodyJoints; - - btDefaultSerializer* m_memSerializer; - - UrdfLinkNameMapUtil() : m_mb(0), m_memSerializer(0) - { - } - virtual ~UrdfLinkNameMapUtil() - { - } -}; - class b3ThreadPool { 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 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); - btMultiBody* mb = bodyHandle ? bodyHandle->m_multiBody : 0; - if (mb) + if(!bodyHandle) return 0; + if (bodyHandle->m_multiBody) { - UrdfLinkNameMapUtil utilBla; - UrdfLinkNameMapUtil* util = &utilBla; + btMultiBody* mb = bodyHandle->m_multiBody; btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient); - util->m_mb = mb; - util->m_memSerializer = &ser; - util->m_memSerializer->startSerialization(); + ser.startSerialization(); //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()) { - util->m_memSerializer->registerNameForPointer(mb->getBaseName(), mb->getBaseName()); + ser.registerNameForPointer(mb->getBaseName(), mb->getBaseName()); } bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); for (int i = 0; i < mb->getNumLinks(); i++) { //disable serialization of the collision objects - util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider, 0); - util->m_memSerializer->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.m_skipPointers.insert(mb->getLink(i).m_collider, 0); + ser.registerNameForPointer(mb->getLink(i).m_linkName, mb->getLink(i).m_linkName); + 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(); - btChunk* chunk = util->m_memSerializer->allocate(len, 1); - const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer); - util->m_memSerializer->finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb); - streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize(); + btChunk* chunk = ser.allocate(len, 1); + const char* structType = mb->serialize(chunk->m_oldPtr, &ser); + ser.finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb); + streamSizeInBytes = ser.getCurrentBufferSize(); } - else - { - btRigidBody* rb = bodyHandle ? bodyHandle->m_rigidBody : 0; - if (rb) - { - UrdfLinkNameMapUtil utilBla; - UrdfLinkNameMapUtil* util = &utilBla; + else if(bodyHandle->m_rigidBody){ + btRigidBody* rb = bodyHandle->m_rigidBody; btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient); - util->m_memSerializer = &ser; - util->m_memSerializer->startSerialization(); - util->m_memSerializer->registerNameForPointer(bodyHandle->m_rigidBody, bodyHandle->m_bodyName.c_str()); + ser.startSerialization(); + ser.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); for (int i = 0; i < bodyHandle->m_rigidBodyJoints.size(); i++) { const btGeneric6DofSpring2Constraint* con = bodyHandle->m_rigidBodyJoints.at(i); - util->m_memSerializer->registerNameForPointer(con, bodyHandle->m_rigidBodyJointNames[i].c_str()); - util->m_memSerializer->registerNameForPointer(&con->getRigidBodyB(), bodyHandle->m_rigidBodyLinkNames[i].c_str()); + ser.registerNameForPointer(con, bodyHandle->m_rigidBodyJointNames[i].c_str()); + ser.registerNameForPointer(&con->getRigidBodyB(), bodyHandle->m_rigidBodyLinkNames[i].c_str()); const btRigidBody& bodyA = con->getRigidBodyA(); int len = con->calculateSerializeBufferSize(); - btChunk* chunk = util->m_memSerializer->allocate(len, 1); - const char* structType = con->serialize(chunk->m_oldPtr, util->m_memSerializer); - util->m_memSerializer->finalizeChunk(chunk, structType, BT_CONSTRAINT_CODE, (void*)con); + btChunk* chunk = ser.allocate(len, 1); + const char* structType = con->serialize(chunk->m_oldPtr, &ser); + ser.finalizeChunk(chunk, structType, BT_CONSTRAINT_CODE, (void*)con); } - - streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize(); + streamSizeInBytes = ser.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; @@ -5530,7 +5515,7 @@ bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct Shar { int usedHandle = usedHandles[i]; 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; } @@ -6551,59 +6536,100 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc hasStatus = true; } - else - { - if (body && body->m_rigidBody) - { - btRigidBody* rb = body->m_rigidBody; - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; + else if (body && body->m_rigidBody){ + 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_numLinks = 0; - serverCmd.m_sendActualStateArgs.m_stateDetails = 0; + serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; + serverCmd.m_sendActualStateArgs.m_numLinks = 0; + serverCmd.m_sendActualStateArgs.m_stateDetails = 0; - int totalDegreeOfFreedomQ = 0; - int totalDegreeOfFreedomU = 0; + int totalDegreeOfFreedomQ = 0; + int totalDegreeOfFreedomU = 0; - btTransform tr = rb->getWorldTransform(); - //base position in world space, carthesian - stateDetails->m_actualStateQ[0] = tr.getOrigin()[0]; - stateDetails->m_actualStateQ[1] = tr.getOrigin()[1]; - stateDetails->m_actualStateQ[2] = tr.getOrigin()[2]; + btTransform tr = rb->getWorldTransform(); + //base position in world space, carthesian + 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, carthesian - 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]; - totalDegreeOfFreedomQ += 7; //pos + quaternion + //base orientation, quaternion x,y,z,w, in world space, carthesian + 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]; + totalDegreeOfFreedomQ += 7; //pos + quaternion - //base linear velocity (in world space, carthesian) - stateDetails->m_actualStateQdot[0] = rb->getLinearVelocity()[0]; - stateDetails->m_actualStateQdot[1] = rb->getLinearVelocity()[1]; - stateDetails->m_actualStateQdot[2] = rb->getLinearVelocity()[2]; + //base linear velocity (in world space, carthesian) + stateDetails->m_actualStateQdot[0] = rb->getLinearVelocity()[0]; + stateDetails->m_actualStateQdot[1] = rb->getLinearVelocity()[1]; + stateDetails->m_actualStateQdot[2] = rb->getLinearVelocity()[2]; - //base angular velocity (in world space, carthesian) - stateDetails->m_actualStateQdot[3] = rb->getAngularVelocity()[0]; - stateDetails->m_actualStateQdot[4] = rb->getAngularVelocity()[1]; - stateDetails->m_actualStateQdot[5] = rb->getAngularVelocity()[2]; - totalDegreeOfFreedomU += 6; //3 linear and 3 angular DOF + //base angular velocity (in world space, carthesian) + stateDetails->m_actualStateQdot[3] = rb->getAngularVelocity()[0]; + stateDetails->m_actualStateQdot[4] = rb->getAngularVelocity()[1]; + stateDetails->m_actualStateQdot[5] = rb->getAngularVelocity()[2]; + totalDegreeOfFreedomU += 6; //3 linear and 3 angular DOF - serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; - serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; + 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; + hasStatus = true; + } + else if (body && body->m_softBody) + { + btSoftBody* sb = body->m_softBody; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; + serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; + serverCmd.m_sendActualStateArgs.m_numLinks = 0; + serverCmd.m_sendActualStateArgs.m_stateDetails = 0; + + 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) @@ -7281,17 +7307,34 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar btAssert((clientCmd.m_updateFlags & LOAD_SOFT_BODY_FILE_NAME) != 0); 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) { - scale = clientCmd.m_loadSoftBodyArguments.m_scale; + scale = clientCmd.m_loadSoftBodyArguments.m_scale; } 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); std::vector shapes; 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]; btAlignedObjectArray vertices; @@ -7334,8 +7377,8 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar psb->m_cfg.piterations = 20; psb->m_cfg.kDF = 0.5; psb->randomizeConstraints(); - psb->rotate(btQuaternion(0.70711, 0, 0, 0.70711)); - psb->translate(btVector3(-0.05, 0, 1.0)); + psb->rotate(initialOrn); + psb->translate(initialPos); psb->scale(btVector3(scale, scale, scale)); psb->setTotalMass(mass, true); @@ -7350,6 +7393,19 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId; 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; notification.m_notificationType = BODY_ADDED; notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId; @@ -7482,6 +7538,17 @@ bool PhysicsServerCommandProcessor::processProfileTimingCommand(const struct Sha 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 hasStatus = true; @@ -7496,13 +7563,7 @@ bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const str btMultiBody* mb = body->m_multiBody; serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED; serverCmd.m_sendCollisionInfoArgs.m_numLinks = body->m_multiBody->getNumLinks(); - 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; + setDefaultRootWorldAABB(serverCmd); 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; SharedMemoryStatus& serverCmd = serverStatusOut; serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED; serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0; + setDefaultRootWorldAABB(serverCmd); - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1; if (rb->getCollisionShape()) { 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[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; } @@ -7815,7 +7875,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask); } } - + } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL) { @@ -8171,6 +8231,12 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S } 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; } @@ -10496,6 +10562,18 @@ bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const s int totalNumVisualShapes = m_data->m_pluginManager.getRenderInterface()->getNumVisualShapes(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId); //int totalBytesPerVisualShape = sizeof (b3VisualShapeData); //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; 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_type = CMD_VISUAL_SHAPE_INFO_COMPLETED; } + else{ + b3Warning("failed to get shape info"); + } + } } return hasStatus; } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 1d70a9df0..083192c48 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -490,7 +490,9 @@ enum EnumLoadSoftBodyUpdateFlags LOAD_SOFT_BODY_FILE_NAME = 1, LOAD_SOFT_BODY_UPDATE_SCALE = 2, 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 @@ -507,6 +509,8 @@ struct LoadSoftBodyArgs double m_scale; double m_mass; double m_collisionMargin; + double m_initialPosition[3]; + double m_initialOrientation[4]; }; struct b3LoadSoftBodyResultArgs diff --git a/examples/pybullet/gym/pybullet_envs/ARS/ars.py b/examples/pybullet/gym/pybullet_envs/ARS/ars.py index 9e9c2b559..52ae67b25 100644 --- a/examples/pybullet/gym/pybullet_envs/ARS/ars.py +++ b/examples/pybullet/gym/pybullet_envs/ARS/ars.py @@ -20,17 +20,17 @@ import argparse # Setting the Hyper Parameters class Hp(): - - def __init__(self): - self.nb_steps = 10000 - self.episode_length = 1000 - self.learning_rate = 0.02 - self.nb_directions = 16 - self.nb_best_directions = 16 - assert self.nb_best_directions <= self.nb_directions - self.noise = 0.03 - self.seed = 1 - self.env_name = 'HalfCheetahBulletEnv-v0' + + def __init__(self): + self.nb_steps = 10000 + self.episode_length = 1000 + self.learning_rate = 0.02 + self.nb_directions = 16 + self.nb_best_directions = 8 + assert self.nb_best_directions <= self.nb_directions + self.noise = 0.03 + self.seed = 1 + self.env_name = 'HalfCheetahBulletEnv-v0' # 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): - - for step in range(hp.nb_steps): - - # Initializing the perturbations deltas and the positive/negative rewards - deltas = policy.sample_deltas() - positive_rewards = [0] * hp.nb_directions - negative_rewards = [0] * hp.nb_directions - - if parentPipes: - for k in range(hp.nb_directions): - parentPipe = parentPipes[k] - parentPipe.send([_EXPLORE, [normalizer, policy, hp, "positive", deltas[k]]]) - for k in range(hp.nb_directions): - positive_rewards[k] = parentPipes[k].recv()[0] - - for k in range(hp.nb_directions): - parentPipe = parentPipes[k] - parentPipe.send([_EXPLORE, [normalizer, policy, hp, "negative", deltas[k]]]) - for k in range(hp.nb_directions): - negative_rewards[k] = parentPipes[k].recv()[0] - - else: - # Getting the positive rewards in the positive directions - for k in range(hp.nb_directions): - positive_rewards[k] = explore(env, normalizer, policy, "positive", deltas[k], hp) - - # Getting the negative rewards in the negative/opposite directions - for k in range(hp.nb_directions): - 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) - sigma_r = all_rewards.std() - - # Sorting the rollouts by the max(r_pos, r_neg) and selecting the best directions - scores = { - k: max(r_pos, r_neg) - 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] - - # 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) - print('Step:', step, 'Reward:', reward_evaluation) - + for step in range(hp.nb_steps): + + # Initializing the perturbations deltas and the positive/negative rewards + deltas = policy.sample_deltas() + positive_rewards = [0] * hp.nb_directions + negative_rewards = [0] * hp.nb_directions + + if parentPipes: + for k in range(hp.nb_directions): + parentPipe = parentPipes[k] + parentPipe.send([_EXPLORE,[normalizer, policy, hp, "positive", deltas[k]]]) + for k in range(hp.nb_directions): + positive_rewards[k] = parentPipes[k].recv()[0] + + for k in range(hp.nb_directions): + parentPipe = parentPipes[k] + parentPipe.send([_EXPLORE,[normalizer, policy, hp, "negative", deltas[k]]]) + for k in range(hp.nb_directions): + negative_rewards[k] = parentPipes[k].recv()[0] + + else: + # Getting the positive rewards in the positive directions + for k in range(hp.nb_directions): + positive_rewards[k] = explore(env, normalizer, policy, "positive", deltas[k], hp) + + + # Getting the negative rewards in the negative/opposite directions + for k in range(hp.nb_directions): + 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) + sigma_r = all_rewards.std() + + # Sorting the rollouts by the max(r_pos, r_neg) and selecting the best directions + scores = {k:max(r_pos, r_neg) 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] + + # 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) + print('Step:', step, 'Reward:', reward_evaluation) # Running the main code diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 2050baa75..1131e5378 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -238,7 +238,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: { } - MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other) + MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other) { btAssert(0); (void)other;