diff --git a/data/reduced_cube/reduced_cube.urdf b/data/reduced_cube/reduced_cube.urdf index b6be6e6ab..eebd7d9e6 100644 --- a/data/reduced_cube/reduced_cube.urdf +++ b/data/reduced_cube/reduced_cube.urdf @@ -5,7 +5,10 @@ + + + diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index bc6bb2a89..ce03e1c16 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -1345,6 +1345,45 @@ bool UrdfParser::parseReducedDeformable(UrdfModel& model, tinyxml2::XMLElement* } } + { + XMLElement* erp_xml = config->FirstChildElement("erp"); + if (erp_xml) + { + if (!erp_xml->Attribute("value")) + { + logger->reportError("friction element must have value attribute"); + return false; + } + reduced_deformable.m_erp = urdfLexicalCast(erp_xml->Attribute("value")); + } + } + + { + XMLElement* cfm_xml = config->FirstChildElement("cfm"); + if (cfm_xml) + { + if (!cfm_xml->Attribute("value")) + { + logger->reportError("cfm element must have value attribute"); + return false; + } + reduced_deformable.m_cfm = urdfLexicalCast(cfm_xml->Attribute("value")); + } + } + + { + XMLElement* damping_xml = config->FirstChildElement("damping_coefficient"); + if (damping_xml) + { + if (!damping_xml->Attribute("value")) + { + logger->reportError("damping_coefficient element must have value attribute"); + return false; + } + reduced_deformable.m_damping = urdfLexicalCast(damping_xml->Attribute("value")); + } + } + { XMLElement* friction_xml = config->FirstChildElement("friction"); if (friction_xml) diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index dc2c18941..7ddaf42bd 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -257,6 +257,7 @@ struct UrdfReducedDeformable double m_cfm; double m_friction; double m_collisionMargin; + double m_damping; std::string m_visualFileName; std::string m_simFileName; @@ -271,6 +272,7 @@ struct UrdfReducedDeformable m_mass(1), m_stiffnessScale(100), m_collisionMargin(0.02), + m_damping(0), m_visualFileName(""), m_simFileName("") {} diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index e3f91e360..d26d7e2a2 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8955,7 +8955,6 @@ void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDe bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision) { - std::cout << "---process dformable!!\n"; #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD btSoftBody* psb = NULL; CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface()); @@ -9473,13 +9472,11 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo m_data->m_pluginManager.addNotification(notification); } #endif - std::cout << "---deformable processed!!\n"; return true; } bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDeformable& reduced_deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision) { - std::cout << "---process reduced deformable!!\n"; #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD btReducedSoftBody* rsb = NULL; CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface()); @@ -9494,9 +9491,6 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe std::string out_found_filename, out_found_sim_filename; int out_type(0), out_sim_type(0); - // std::cout << relativeFileName << "\n"; - std::cout << pathPrefix << "\n"; - bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); if (!reduced_deformable.m_simFileName.empty()) { @@ -9510,64 +9504,8 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe if (out_sim_type == UrdfGeometry::FILE_OBJ) { -// std::vector shapes; -// tinyobj::attrib_t attribute; -// std::string err = tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO); -// if (!shapes.empty()) -// { -// const tinyobj::shape_t& shape = shapes[0]; -// btAlignedObjectArray vertices; -// btAlignedObjectArray indices; -// for (int i = 0; i < attribute.vertices.size(); i++) -// { -// vertices.push_back(attribute.vertices[i]); -// } -// for (int i = 0; i < shape.mesh.indices.size(); i++) -// { -// indices.push_back(shape.mesh.indices[i].vertex_index); -// } -// int numTris = shape.mesh.indices.size() / 3; -// if (numTris > 0) -// { -// { -// btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); -// if (softWorld) -// { -// psb = btSoftBodyHelpers::CreateFromTriMesh(softWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); -// if (!psb) -// { -// printf("Load deformable failed\n"); -// return false; -// } -// } -// } -// { -// btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); -// if (deformWorld) -// { -// psb = btSoftBodyHelpers::CreateFromTriMesh(deformWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); -// if (!psb) -// { -// printf("Load deformable failed\n"); -// return false; -// } -// } -// } -// } -// } -// #ifndef SKIP_DEFORMABLE_BODY -// btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); -// if (deformWorld && deformable.m_springCoefficients.elastic_stiffness > 0.) -// { -// btDeformableLagrangianForce* springForce = -// new btDeformableMassSpringForce(deformable.m_springCoefficients.elastic_stiffness, -// deformable.m_springCoefficients.damping_stiffness, -// !deformable.m_springCoefficients.damp_all_directions, -// deformable.m_springCoefficients.bending_stiffness); -// deformWorld->addForce(psb, springForce); -// m_data->m_lf.push_back(springForce); -// } -// #endif + printf("Obj file is currently unsupported\n"); + return false; } else if (out_sim_type == UrdfGeometry::FILE_VTK) { @@ -9575,7 +9513,6 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); if (deformWorld) { - std::cout << "out_found_sim_filename = " << out_found_sim_filename << "\n"; rsb = btReducedSoftBodyHelpers::createFromVtkFile(deformWorld->getWorldInfo(), out_found_sim_filename.c_str()); if (!rsb) { @@ -9584,40 +9521,10 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe } // load modes, reduced stiffness matrix - rsb->setReducedModes(reduced_deformable.m_startMode, reduced_deformable.m_numModes, rsb->m_nodes.size()); // TODO: add modes info to URDF - rsb->setStiffnessScale(100); + rsb->setReducedModes(reduced_deformable.m_startMode, reduced_deformable.m_numModes, rsb->m_nodes.size()); + rsb->setStiffnessScale(reduced_deformable.m_stiffnessScale); + rsb->setDamping(0, reduced_deformable.m_damping); // damping alpha is set to 0 by default btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(rsb, pathPrefix); - - // btScalar corotated_mu(0.), corotated_lambda(0.); - // corotated_mu = deformable.m_corotatedCoefficients.mu; - // corotated_lambda = deformable.m_corotatedCoefficients.lambda; - // if (corotated_mu > 0 || corotated_lambda > 0) - // { - // btDeformableLagrangianForce* corotatedForce = new btDeformableCorotatedForce(corotated_mu, corotated_lambda); - // deformWorld->addForce(psb, corotatedForce); - // m_data->m_lf.push_back(corotatedForce); - // } - // btScalar neohookean_mu, neohookean_lambda, neohookean_damping; - // neohookean_mu = deformable.m_neohookeanCoefficients.mu; - // neohookean_lambda = deformable.m_neohookeanCoefficients.lambda; - // neohookean_damping = deformable.m_neohookeanCoefficients.damping; - // if (neohookean_mu > 0 || neohookean_lambda > 0) - // { - // btDeformableLagrangianForce* neohookeanForce = new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda, neohookean_damping); - // deformWorld->addForce(psb, neohookeanForce); - // m_data->m_lf.push_back(neohookeanForce); - // } - - // btScalar spring_elastic_stiffness, spring_damping_stiffness, spring_bending_stiffness; - // spring_elastic_stiffness = deformable.m_springCoefficients.elastic_stiffness; - // spring_damping_stiffness = deformable.m_springCoefficients.damping_stiffness; - // spring_bending_stiffness = deformable.m_springCoefficients.bending_stiffness; - // if (spring_elastic_stiffness > 0.) - // { - // btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true, spring_bending_stiffness); - // deformWorld->addForce(psb, springForce); - // m_data->m_lf.push_back(springForce); - // } } #endif } @@ -9717,11 +9624,6 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); if (deformWorld) { - - // btVector3 gravity = m_data->m_dynamicsWorld->getGravity(); - // btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity); - // deformWorld->addForce(rsb, gravityForce); - // m_data->m_lf.push_back(gravityForce); btScalar collision_hardness = 1; rsb->m_cfg.kKHR = collision_hardness; rsb->m_cfg.kCHR = collision_hardness; @@ -9762,8 +9664,6 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe // } // #endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD rsb->scale(btVector3(scale, scale, scale)); - // rsb->rotate(orn); - // rsb->translate(pos); btTransform init_transform; init_transform.setOrigin(pos); init_transform.setRotation(orn); @@ -9789,7 +9689,6 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe } *bodyUniqueId = m_data->m_bodyHandles.allocHandle(); - std::cout << "bodyUniqueId: " << *bodyUniqueId; InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(*bodyUniqueId); bodyHandle->m_softBody = rsb; rsb->setUserIndex2(*bodyUniqueId); @@ -10008,8 +9907,6 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe m_data->m_pluginManager.addNotification(notification); } #endif - std::cout << "---reduced deformable processed!!\n"; - // exit(123); return true; } diff --git a/examples/pybullet/examples/reduced_deformable_cube.py b/examples/pybullet/examples/reduced_deformable_cube.py index 218c5ccb7..6cb241c79 100644 --- a/examples/pybullet/examples/reduced_deformable_cube.py +++ b/examples/pybullet/examples/reduced_deformable_cube.py @@ -7,19 +7,15 @@ physicsClient = p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD) -p.resetDebugVisualizerCamera(3,-420,-30,[0.3,0.9,-2]) +p.resetDebugVisualizerCamera(4,-40,-30,[0, 0, 0]) p.setGravity(0, 0, -10) tex = p.loadTexture("uvmap.png") planeId = p.loadURDF("plane.urdf", [0,0,-2]) -# boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True) - -# bunnyId = p.loadSoftBody("torus/torus_textured.obj", simFileName="torus.vtk", mass = 3, useNeoHookean = 1, NeoHookeanMu = 180, NeoHookeanLambda = 600, NeoHookeanDamping = 0.01, collisionMargin = 0.006, useSelfCollision = 1, frictionCoeff = 0.5, repulsionStiffness = 800) -# p.changeVisualShape(bunnyId, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0) - -bunny2 = p.loadURDF("reduced_cube/reduced_cube.urdf", [0,1,0.5], flags=p.URDF_USE_SELF_COLLISION) -p.changeVisualShape(bunny2, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0) +p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "reduced_cube.mp4") +cube = p.loadURDF("reduced_cube/reduced_cube.urdf", [1,1,1]) +p.changeVisualShape(cube, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0) p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25) p.setRealTimeSimulation(0) diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index 1220c4cb4..eb1b04202 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -461,6 +461,11 @@ void btReducedSoftBody::scale(const btVector3& scl) internalInitialization(); } +void btReducedSoftBody::setTotalMass(btScalar mass, bool fromfaces) +{ + // +} + void btReducedSoftBody::updateRestNodalPositions() { // update reset nodal position diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index 8a2aef15a..9640fc63c 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -124,6 +124,8 @@ class btReducedSoftBody : public btSoftBody void setDamping(const btScalar alpha, const btScalar beta); + virtual void setTotalMass(btScalar mass, bool fromfaces = false); + // // various internal updates //