diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 611a75655..ee506c143 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -338,6 +338,24 @@ B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle return 0; } +B3_SHARED_API int b3LoadSoftBodyUpdateSimMesh(b3SharedMemoryCommandHandle commandHandle, const char* filename) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); + int len = strlen(filename); + if (len < MAX_FILENAME_LENGTH) + { + strcpy(command->m_loadSoftBodyArguments.m_simFileName, filename); + } + else + { + command->m_loadSoftBodyArguments.m_simFileName[0] = 0; + } + command->m_updateFlags |= LOAD_SOFT_BODY_SIM_MESH; + return 0; + +} + B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 220e5582f..c67f79d9c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -375,7 +375,6 @@ extern "C" B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient); B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand2(b3SharedMemoryCommandHandle commandHandle); B3_SHARED_API int b3InitResetSimulationSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); - ///Load a robot from a URDF file. Status type will CMD_URDF_LOADING_COMPLETED. ///Access the robot from the unique body index, through b3GetStatusBodyIndex(statusHandle); B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName); @@ -634,7 +633,9 @@ extern "C" B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin); B3_SHARED_API int b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ); B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW); + B3_SHARED_API int b3LoadSoftBodyUpdateSimMesh(b3SharedMemoryCommandHandle commandHandle, const char* filename); B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda); + B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda); B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda, double NeoHookeanDamping); B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness); B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1e6a0f62a..d3565c4d5 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1709,7 +1709,6 @@ struct PhysicsServerCommandProcessorInternalData m_dispatcher(0), m_solver(0), m_collisionConfiguration(0), - #ifndef SKIP_DEFORMABLE_BODY m_deformablebodySolver(0), #endif @@ -2651,23 +2650,25 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags) bv->setVelocityPrediction(0); m_data->m_broadphase = bv; } - - if (flags & RESET_USE_DEFORMABLE_WORLD) + + if (flags & RESET_USE_DEFORMABLE_WORLD) { #ifndef SKIP_DEFORMABLE_BODY - m_data->m_deformablebodySolver = new btDeformableBodySolver(); - btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver; - m_data->m_solver = solver; - solver->setDeformableSolver(m_data->m_deformablebodySolver); - m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver); + m_data->m_deformablebodySolver = new btDeformableBodySolver(); + btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver; + m_data->m_solver = solver; + solver->setDeformableSolver(m_data->m_deformablebodySolver); + m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver); #endif } - if ((0==m_data->m_dynamicsWorld) && (0==(flags&RESET_USE_DISCRETE_DYNAMICS_WORLD))) + if ((0==m_data->m_dynamicsWorld) && (0==(flags&RESET_USE_DISCRETE_DYNAMICS_WORLD))) { + m_data->m_solver = new btMultiBodyConstraintSolver; #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - m_data->m_solver = new btMultiBodyConstraintSolver; - m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); + m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); +#else + m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); #endif } @@ -5126,24 +5127,24 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S { btSoftBody* psb = bodyHandle->m_softBody; int totalBytesPerVertex = sizeof(btVector3); - bool separateRenderMesh = (psb->m_renderNodes.size() != 0); - int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size(); + bool separateRenderMesh = (psb->m_renderNodes.size() != 0); + int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size(); int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1; int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex; int verticesCopied = btMin(maxNumVertices, numVerticesRemaining); btVector3* verticesOut = (btVector3*)bufferServerToClient; for (int i = 0; i < verticesCopied; ++i) { - if (separateRenderMesh) - { - const btSoftBody::Node& n = psb->m_renderNodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex]; - verticesOut[i] = n.m_x; - } - else - { - const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex]; - verticesOut[i] = n.m_x; - } + if (separateRenderMesh) + { + const btSoftBody::Node& n = psb->m_renderNodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex]; + verticesOut[i] = n.m_x; + } + else + { + const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex]; + verticesOut[i] = n.m_x; + } } serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED; @@ -8069,7 +8070,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar { collisionMargin = clientCmd.m_loadSoftBodyArguments.m_collisionMargin; } - + { btSoftBody* psb = NULL; @@ -8083,27 +8084,21 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); } - const std::string& error_message_prefix = ""; - std::string out_found_filename; - std::string out_found_sim_filename; + std::string out_found_filename, out_found_sim_filename; int out_type(0), out_sim_type(0); - bool render_mesh_is_sim_mesh = true; - bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); - if (out_type == UrdfGeometry::FILE_OBJ) - { - foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type); - render_mesh_is_sim_mesh = !foundFile; - } - if (render_mesh_is_sim_mesh) + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SIM_MESH) { - out_sim_type = out_type; - out_found_sim_filename = out_found_filename; + bool foundSimMesh = UrdfFindMeshFile(fileIO, pathPrefix, loadSoftBodyArgs.m_simFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type); + } + else + { + out_sim_type = out_type; + out_found_sim_filename = out_found_filename; } - if (out_sim_type == UrdfGeometry::FILE_OBJ) { std::vector shapes; @@ -8192,6 +8187,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar deformWorld->addForce(psb, springForce); m_data->m_lf.push_back(springForce); } + } #endif } @@ -8199,58 +8195,55 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar if (psb != NULL) { #ifndef SKIP_DEFORMABLE_BODY - btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); if (deformWorld) { - if (!render_mesh_is_sim_mesh) - { - // load render mesh + // load render mesh + if (out_found_sim_filename != out_found_filename) + { + // load render mesh + { + tinyobj::attrib_t attribute; + std::vector shapes; + + std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface()); + for (int s = 0; s < (int)shapes.size(); s++) + { + tinyobj::shape_t& shape = shapes[s]; + int faceCount = shape.mesh.indices.size(); + int vertexCount = attribute.vertices.size()/3; + for (int v=0;vm_renderNodes.push_back(n); + } - { - - tinyobj::attrib_t attribute; - std::vector shapes; - - std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface()); - - for (int s = 0; s < (int)shapes.size(); s++) - { - tinyobj::shape_t& shape = shapes[s]; - int faceCount = shape.mesh.indices.size(); - int vertexCount = attribute.vertices.size()/3; - for (int v=0;vm_renderNodes.push_back(n); - } - - for (int f = 0; f < faceCount; f += 3) - { - if (f < 0 && f >= int(shape.mesh.indices.size())) - { - continue; - } - tinyobj::index_t v_0 = shape.mesh.indices[f]; - tinyobj::index_t v_1 = shape.mesh.indices[f + 1]; - tinyobj::index_t v_2 = shape.mesh.indices[f + 2]; - btSoftBody::Face ff; - ff.m_n[0] = &psb->m_renderNodes[v_0.vertex_index]; - ff.m_n[1] = &psb->m_renderNodes[v_1.vertex_index]; - ff.m_n[2] = &psb->m_renderNodes[v_2.vertex_index]; - psb->m_renderFaces.push_back(ff); - } - } - - } - - btSoftBodyHelpers::interpolateBarycentricWeights(psb); - } - else - { - psb->m_renderNodes.resize(0); - } + for (int f = 0; f < faceCount; f += 3) + { + if (f < 0 && f >= int(shape.mesh.indices.size())) + { + continue; + } + tinyobj::index_t v_0 = shape.mesh.indices[f]; + tinyobj::index_t v_1 = shape.mesh.indices[f + 1]; + tinyobj::index_t v_2 = shape.mesh.indices[f + 2]; + btSoftBody::Face ff; + ff.m_n[0] = &psb->m_renderNodes[v_0.vertex_index]; + ff.m_n[1] = &psb->m_renderNodes[v_1.vertex_index]; + ff.m_n[2] = &psb->m_renderNodes[v_2.vertex_index]; + psb->m_renderFaces.push_back(ff); + } + } + + } + btSoftBodyHelpers::interpolateBarycentricWeights(psb); + } + else + { + psb->m_renderNodes.resize(0); + } btVector3 gravity = m_data->m_dynamicsWorld->getGravity(); btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity); deformWorld->addForce(psb, gravityForce); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index ab39daaa0..ccb6357fe 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -505,6 +505,7 @@ enum EnumLoadSoftBodyUpdateFlags LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE = 1<<12, LOAD_SOFT_BODY_USE_SELF_COLLISION = 1<<13, LOAD_SOFT_BODY_USE_FACE_CONTACT = 1<<14, + LOAD_SOFT_BODY_SIM_MESH = 1<<15, }; enum EnumSimParamInternalSimFlags @@ -535,6 +536,7 @@ struct LoadSoftBodyArgs double m_NeoHookeanLambda; double m_NeoHookeanDamping; int m_useFaceContact; + char m_simFileName[MAX_FILENAME_LENGTH]; }; struct b3LoadSoftBodyResultArgs diff --git a/examples/pybullet/examples/deformable_anchor.py b/examples/pybullet/examples/deformable_anchor.py index 135a55c71..666334628 100644 --- a/examples/pybullet/examples/deformable_anchor.py +++ b/examples/pybullet/examples/deformable_anchor.py @@ -2,8 +2,8 @@ import pybullet as p from time import sleep physicsClient = p.connect(p.GUI) - p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD) + gravZ=-10 p.setGravity(0, 0, gravZ) diff --git a/examples/pybullet/examples/deformable_ball.py b/examples/pybullet/examples/deformable_ball.py index 8803e2fb2..4ff81b613 100644 --- a/examples/pybullet/examples/deformable_ball.py +++ b/examples/pybullet/examples/deformable_ball.py @@ -24,4 +24,4 @@ while p.isConnected(): sleep(1./240.) #p.resetSimulation() -#p.stopStateLogging(logId) \ No newline at end of file +#p.stopStateLogging(logId) diff --git a/examples/pybullet/examples/test_inertia.py b/examples/pybullet/examples/test_inertia.py new file mode 100644 index 000000000..a290f3bac --- /dev/null +++ b/examples/pybullet/examples/test_inertia.py @@ -0,0 +1,28 @@ +import pybullet as p +from time import sleep + +physicsClient = p.connect(p.GUI) + +useDeformable = True +if useDeformable: + p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD) + +gravZ=-10 +p.setGravity(0, 0, gravZ) + +planeOrn = [0,0,0,1]#p.getQuaternionFromEuler([0.3,0,0]) +planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn) + +boxId = p.loadURDF("cube.urdf", [0,1,2],useMaximalCoordinates = True) + +clothId = p.loadSoftBody("bunny.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1, useMassSpring=1, springElasticStiffness=100, springDampingStiffness=.001, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1) + +p.setTimeStep(0.0005) + +p.setRealTimeSimulation(1) + + +while p.isConnected(): + p.setGravity(0,0,gravZ) + sleep(1./240.) +