mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 13:20:07 +00:00
load deformable object from URDF files
This commit is contained in:
Chuyuan Fu
parent
0c6c7976ba
commit
3c46e6a584
@ -1533,3 +1533,8 @@ class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIn
|
||||
const struct UrdfModel* BulletURDFImporter::getUrdfModel() const {
|
||||
return &m_data->m_urdfParser.getModel();
|
||||
};
|
||||
|
||||
const struct UrdfDeformable& BulletURDFImporter::getDeformableModel() const
|
||||
{
|
||||
return m_data->m_urdfParser.getDeformable();
|
||||
}
|
||||
|
||||
@ -2,8 +2,8 @@
|
||||
#define BULLET_URDF_IMPORTER_H
|
||||
|
||||
#include "URDFImporterInterface.h"
|
||||
|
||||
#include "UrdfRenderingInterface.h"
|
||||
#include "UrdfParser.h"
|
||||
|
||||
struct BulletURDFTexture
|
||||
{
|
||||
@ -91,6 +91,7 @@ public:
|
||||
|
||||
virtual void setEnableTinyRenderer(bool enable);
|
||||
void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<struct BulletURDFTexture>& texturesOut, struct b3ImportMeshData& meshData) const;
|
||||
const struct UrdfDeformable& getDeformableModel() const;
|
||||
};
|
||||
|
||||
#endif //BULLET_URDF_IMPORTER_H
|
||||
|
||||
@ -1098,6 +1098,137 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, XMLElement* config,
|
||||
return true;
|
||||
}
|
||||
|
||||
bool UrdfParser::parseLameCoefficients(LameCoefficients& lameCoefficients, tinyxml2::XMLElement* config, ErrorLogger* logger)
|
||||
{
|
||||
XMLElement* mu = config->FirstChildElement("mu");
|
||||
XMLElement* lambda = config->FirstChildElement("lambda");
|
||||
XMLElement* damping = config->FirstChildElement("damping");
|
||||
if (!mu || !lambda)
|
||||
{
|
||||
logger->reportError("expected mu lambda for LameCoefficients.");
|
||||
return false;
|
||||
}
|
||||
lameCoefficients.mu = urdfLexicalCast<double>(mu->GetText());
|
||||
lameCoefficients.lambda = urdfLexicalCast<double>(lambda->GetText());
|
||||
if (damping)
|
||||
lameCoefficients.damping = urdfLexicalCast<double>(damping->GetText());
|
||||
else
|
||||
lameCoefficients.damping = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger)
|
||||
{
|
||||
UrdfDeformable& deformable = model.m_deformable;
|
||||
const char* deformableName = config->Attribute("name");
|
||||
if (!deformableName)
|
||||
{
|
||||
logger->reportError("Deformable with no name");
|
||||
return false;
|
||||
}
|
||||
deformable.m_name = deformableName;
|
||||
|
||||
XMLElement* i = config->FirstChildElement("inertial");
|
||||
if (!i)
|
||||
{
|
||||
logger->reportError("expected an inertial element");
|
||||
}
|
||||
UrdfInertia inertia;
|
||||
if (!parseInertia(inertia, i, logger))
|
||||
{
|
||||
logger->reportError("Could not parse inertial element for deformable:");
|
||||
logger->reportError(deformable.m_name.c_str());
|
||||
return false;
|
||||
}
|
||||
deformable.m_mass = inertia.m_mass;
|
||||
|
||||
XMLElement* collisionMargin_xml = config->FirstChildElement("collision_margin");
|
||||
if (collisionMargin_xml)
|
||||
{
|
||||
if (!collisionMargin_xml->Attribute("value"))
|
||||
{
|
||||
logger->reportError("collision_margin element must have value attribute");
|
||||
return false;
|
||||
}
|
||||
deformable.m_collisionMargin = urdfLexicalCast<double>(collisionMargin_xml->Attribute("value"));
|
||||
}
|
||||
|
||||
XMLElement* friction_xml = config->FirstChildElement("friction");
|
||||
if (friction_xml)
|
||||
{
|
||||
if (!friction_xml->Attribute("value"))
|
||||
{
|
||||
logger->reportError("friction element must have value attribute");
|
||||
return false;
|
||||
}
|
||||
deformable.m_friction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
|
||||
}
|
||||
|
||||
XMLElement* spring_xml = config->FirstChildElement("spring");
|
||||
if (spring_xml)
|
||||
{
|
||||
XMLElement* elastic_stiffness = spring_xml->FirstChildElement("elastic_stiffness");
|
||||
XMLElement* damping_stiffness = spring_xml->FirstChildElement("damping_stiffness");
|
||||
XMLElement* bending_stiffness = spring_xml->FirstChildElement("bending_stiffness");
|
||||
if (!elastic_stiffness || !damping_stiffness)
|
||||
{
|
||||
logger->reportError("srping element expect elastic and damping stiffness");
|
||||
return false;
|
||||
}
|
||||
|
||||
deformable.m_springCoefficients.elastic_stiffness = urdfLexicalCast<double>(elastic_stiffness->GetText());
|
||||
deformable.m_springCoefficients.damping_stiffness = urdfLexicalCast<double>(damping_stiffness->GetText());
|
||||
|
||||
if (bending_stiffness)
|
||||
deformable.m_springCoefficients.bending_stiffness = urdfLexicalCast<double>(bending_stiffness->GetText());
|
||||
}
|
||||
|
||||
XMLElement* corotated_xml = config->FirstChildElement("corotated");
|
||||
if (corotated_xml)
|
||||
{
|
||||
if (!parseLameCoefficients(deformable.m_corotatedCoefficients, corotated_xml, logger))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
XMLElement* neohookean_xml = config->FirstChildElement("neohookean");
|
||||
if (neohookean_xml)
|
||||
{
|
||||
if (!parseLameCoefficients(deformable.m_neohookeanCoefficients, neohookean_xml, logger))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
XMLElement* vis_xml = config->FirstChildElement("visual");
|
||||
if (!vis_xml)
|
||||
{
|
||||
logger->reportError("expected an visual element");
|
||||
return false;
|
||||
}
|
||||
if (!vis_xml->Attribute("filename"))
|
||||
{
|
||||
logger->reportError("expected a filename for visual geometry");
|
||||
return false;
|
||||
}
|
||||
deformable.m_visualFileName = vis_xml->Attribute("filename");
|
||||
|
||||
XMLElement* col_xml = config->FirstChildElement("collision");
|
||||
if (col_xml)
|
||||
{
|
||||
if (!col_xml->Attribute("filename"))
|
||||
{
|
||||
logger->reportError("expected a filename for collision geoemtry");
|
||||
return false;
|
||||
}
|
||||
deformable.m_simFileName = col_xml->Attribute("filename");
|
||||
}
|
||||
|
||||
ParseUserData(config, deformable.m_userData, logger);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLogger* logger)
|
||||
{
|
||||
joint.m_lowerLimit = 0.f;
|
||||
@ -1898,6 +2029,8 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
|
||||
}
|
||||
m_urdf2Model.m_name = name;
|
||||
|
||||
ParseUserData(robot_xml, m_urdf2Model.m_userData, logger);
|
||||
|
||||
// Get all Material elements
|
||||
for (XMLElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
|
||||
{
|
||||
@ -1921,6 +2054,11 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
|
||||
// sprintf(msg,"Num materials=%d", m_model.m_materials.size());
|
||||
// logger->printMessage(msg);
|
||||
|
||||
|
||||
XMLElement* deformable_xml = robot_xml->FirstChildElement("deformable");
|
||||
if(deformable_xml)
|
||||
return parseDeformable(m_urdf2Model, deformable_xml, logger);
|
||||
|
||||
for (XMLElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
|
||||
{
|
||||
UrdfLink* link = new UrdfLink;
|
||||
@ -2040,8 +2178,6 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
|
||||
}
|
||||
}
|
||||
|
||||
ParseUserData(robot_xml, m_urdf2Model.m_userData, logger);
|
||||
|
||||
if (m_urdf2Model.m_links.size() == 0)
|
||||
{
|
||||
logger->reportWarning("No links found in URDF file");
|
||||
|
||||
@ -200,6 +200,45 @@ struct UrdfJoint
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct SpringCoeffcients{
|
||||
double elastic_stiffness;
|
||||
double damping_stiffness;
|
||||
double bending_stiffness;
|
||||
SpringCoeffcients():
|
||||
elastic_stiffness(0.),
|
||||
damping_stiffness(0.),
|
||||
bending_stiffness(0.){}
|
||||
};
|
||||
|
||||
struct LameCoefficients
|
||||
{
|
||||
double mu;
|
||||
double lambda;
|
||||
double damping;
|
||||
LameCoefficients() : mu(0.), lambda(0.), damping(0.) {}
|
||||
};
|
||||
|
||||
struct UrdfDeformable
|
||||
{
|
||||
std::string m_name;
|
||||
double m_mass;
|
||||
double m_collisionMargin;
|
||||
double m_friction;
|
||||
|
||||
SpringCoeffcients m_springCoefficients;
|
||||
LameCoefficients m_corotatedCoefficients;
|
||||
LameCoefficients m_neohookeanCoefficients;
|
||||
|
||||
std::string m_visualFileName;
|
||||
std::string m_simFileName;
|
||||
btHashMap<btHashString, std::string> m_userData;
|
||||
|
||||
UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_visualFileName(""), m_simFileName("")
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
struct UrdfModel
|
||||
{
|
||||
std::string m_name;
|
||||
@ -208,6 +247,7 @@ struct UrdfModel
|
||||
btHashMap<btHashString, UrdfMaterial*> m_materials;
|
||||
btHashMap<btHashString, UrdfLink*> m_links;
|
||||
btHashMap<btHashString, UrdfJoint*> m_joints;
|
||||
UrdfDeformable m_deformable;
|
||||
// Maps user data keys to user data values.
|
||||
btHashMap<btHashString, std::string> m_userData;
|
||||
|
||||
@ -283,6 +323,9 @@ protected:
|
||||
bool parseJoint(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
bool parseLink(UrdfModel& model, UrdfLink& link, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
bool parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
bool parseLameCoefficients(LameCoefficients& lameCoefficients, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
bool parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
|
||||
|
||||
public:
|
||||
UrdfParser(struct CommonFileIOInterface* fileIO);
|
||||
@ -357,6 +400,9 @@ public:
|
||||
return m_urdf2Model;
|
||||
}
|
||||
|
||||
const UrdfDeformable& getDeformable() const{
|
||||
return m_urdf2Model.m_deformable;
|
||||
}
|
||||
|
||||
bool mergeFixedLinks(UrdfModel& model, UrdfLink* link, ErrorLogger* logger, bool forceFixedBase, int level);
|
||||
bool printTree(UrdfLink* link, ErrorLogger* logger, int level);
|
||||
|
||||
@ -3489,6 +3489,12 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
|
||||
rootTrans.setOrigin(pos);
|
||||
rootTrans.setRotation(orn);
|
||||
u2b.setRootTransformInWorld(rootTrans);
|
||||
if (!(u2b.getDeformableModel().m_visualFileName.empty()))
|
||||
{
|
||||
bool use_self_collision = false;
|
||||
use_self_collision = (flags & CUF_USE_SELF_COLLISION);
|
||||
return processDeformable(u2b.getDeformableModel(), pos, orn, bodyUniqueIdPtr, bufferServerToClient, bufferSizeInBytes, globalScaling, use_self_collision);
|
||||
}
|
||||
bool ok = processImportedObjects(fileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
|
||||
if (ok)
|
||||
{
|
||||
@ -8169,274 +8175,261 @@ bool PhysicsServerCommandProcessor::processLoadURDFCommand(const struct SharedMe
|
||||
return hasStatus;
|
||||
}
|
||||
|
||||
bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
|
||||
void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDeformable& deformable, bool verbose)
|
||||
{
|
||||
serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_FAILED;
|
||||
bool hasStatus = true;
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
double scale = 1;
|
||||
double mass = 1;
|
||||
double collisionMargin = 0.02;
|
||||
const LoadSoftBodyArgs& loadSoftBodyArgs = clientCmd.m_loadSoftBodyArguments;
|
||||
if (m_data->m_verboseOutput)
|
||||
if (verbose)
|
||||
{
|
||||
b3Printf("Processed CMD_LOAD_SOFT_BODY:%s", loadSoftBodyArgs.m_fileName);
|
||||
}
|
||||
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;
|
||||
}
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_MASS)
|
||||
{
|
||||
mass = clientCmd.m_loadSoftBodyArguments.m_mass;
|
||||
deformable.m_mass = loadSoftBodyArgs.m_mass;
|
||||
}
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN)
|
||||
{
|
||||
collisionMargin = clientCmd.m_loadSoftBodyArguments.m_collisionMargin;
|
||||
deformable.m_collisionMargin = loadSoftBodyArgs.m_collisionMargin;
|
||||
}
|
||||
btScalar spring_bending_stiffness = 0;
|
||||
deformable.m_visualFileName = loadSoftBodyArgs.m_fileName;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SIM_MESH)
|
||||
{
|
||||
btSoftBody* psb = NULL;
|
||||
CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
|
||||
char relativeFileName[1024];
|
||||
char pathPrefix[1024];
|
||||
pathPrefix[0] = 0;
|
||||
if (fileIO->findResourcePath(loadSoftBodyArgs.m_fileName, relativeFileName, 1024))
|
||||
{
|
||||
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
|
||||
}
|
||||
const std::string& error_message_prefix = "";
|
||||
std::string out_found_filename, out_found_sim_filename;
|
||||
int out_type(0), out_sim_type(0);
|
||||
deformable.m_simFileName = loadSoftBodyArgs.m_simFileName;
|
||||
}
|
||||
else
|
||||
{
|
||||
deformable.m_simFileName = "";
|
||||
}
|
||||
#ifndef SKIP_DEFORMABLE_BODY
|
||||
deformable.m_springCoefficients.elastic_stiffness = loadSoftBodyArgs.m_springElasticStiffness;
|
||||
deformable.m_springCoefficients.damping_stiffness = loadSoftBodyArgs.m_springDampingStiffness;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS)
|
||||
{
|
||||
deformable.m_springCoefficients.bending_stiffness = loadSoftBodyArgs.m_springBendingStiffness;
|
||||
}
|
||||
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE)
|
||||
{
|
||||
deformable.m_corotatedCoefficients.mu = loadSoftBodyArgs.m_corotatedMu;
|
||||
deformable.m_corotatedCoefficients.lambda = loadSoftBodyArgs.m_corotatedLambda;
|
||||
}
|
||||
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE)
|
||||
{
|
||||
deformable.m_neohookeanCoefficients.mu = loadSoftBodyArgs.m_NeoHookeanMu;
|
||||
deformable.m_neohookeanCoefficients.lambda = loadSoftBodyArgs.m_NeoHookeanLambda;
|
||||
deformable.m_neohookeanCoefficients.damping = loadSoftBodyArgs.m_NeoHookeanDamping;
|
||||
}
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT)
|
||||
{
|
||||
deformable.m_friction = loadSoftBodyArgs.m_frictionCoeff;
|
||||
}
|
||||
|
||||
bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SIM_MESH)
|
||||
{
|
||||
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<tinyobj::shape_t> 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<btScalar> vertices;
|
||||
btAlignedObjectArray<int> 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);
|
||||
}
|
||||
}
|
||||
{
|
||||
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
|
||||
if (deformWorld)
|
||||
{
|
||||
psb = btSoftBodyHelpers::CreateFromTriMesh(deformWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#ifndef SKIP_DEFORMABLE_BODY
|
||||
btScalar spring_elastic_stiffness, spring_damping_stiffness;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE)
|
||||
{
|
||||
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
|
||||
if (deformWorld)
|
||||
{
|
||||
spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
|
||||
spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS)
|
||||
{
|
||||
spring_bending_stiffness = clientCmd.m_loadSoftBodyArguments.m_springBendingStiffness;
|
||||
}
|
||||
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
|
||||
}
|
||||
else if (out_sim_type == UrdfGeometry::FILE_VTK)
|
||||
}
|
||||
|
||||
bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision)
|
||||
{
|
||||
btSoftBody* psb = NULL;
|
||||
CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
|
||||
char relativeFileName[1024];
|
||||
char pathPrefix[1024];
|
||||
pathPrefix[0] = 0;
|
||||
if (fileIO->findResourcePath(deformable.m_visualFileName.c_str(), relativeFileName, 1024))
|
||||
{
|
||||
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
|
||||
}
|
||||
const std::string& error_message_prefix = "";
|
||||
std::string out_found_filename, out_found_sim_filename;
|
||||
int out_type(0), out_sim_type(0);
|
||||
|
||||
bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
|
||||
if (!deformable.m_simFileName.empty())
|
||||
{
|
||||
bool foundSimMesh = UrdfFindMeshFile(fileIO, pathPrefix, deformable.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<tinyobj::shape_t> shapes;
|
||||
tinyobj::attrib_t attribute;
|
||||
std::string err = tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO);
|
||||
if (!shapes.empty())
|
||||
{
|
||||
#ifndef SKIP_DEFORMABLE_BODY
|
||||
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
|
||||
if (deformWorld)
|
||||
const tinyobj::shape_t& shape = shapes[0];
|
||||
btAlignedObjectArray<btScalar> vertices;
|
||||
btAlignedObjectArray<int> 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)
|
||||
{
|
||||
psb = btSoftBodyHelpers::CreateFromVtkFile(deformWorld->getWorldInfo(), out_found_sim_filename.c_str());
|
||||
btScalar corotated_mu, corotated_lambda;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE)
|
||||
{
|
||||
corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu;
|
||||
corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda;
|
||||
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;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE)
|
||||
{
|
||||
neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu;
|
||||
neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda;
|
||||
neohookean_damping = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanDamping;
|
||||
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;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE)
|
||||
{
|
||||
spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
|
||||
spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS)
|
||||
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
|
||||
if (softWorld)
|
||||
{
|
||||
spring_bending_stiffness = clientCmd.m_loadSoftBodyArguments.m_springBendingStiffness;
|
||||
psb = btSoftBodyHelpers::CreateFromTriMesh(softWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
|
||||
}
|
||||
}
|
||||
{
|
||||
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
|
||||
if (deformWorld)
|
||||
{
|
||||
psb = btSoftBodyHelpers::CreateFromTriMesh(deformWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
|
||||
}
|
||||
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
|
||||
}
|
||||
if (psb != NULL)
|
||||
{
|
||||
#ifndef SKIP_DEFORMABLE_BODY
|
||||
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
|
||||
if (deformWorld)
|
||||
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,
|
||||
true, deformable.m_springCoefficients.bending_stiffness);
|
||||
deformWorld->addForce(psb, springForce);
|
||||
m_data->m_lf.push_back(springForce);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
else if (out_sim_type == UrdfGeometry::FILE_VTK)
|
||||
{
|
||||
#ifndef SKIP_DEFORMABLE_BODY
|
||||
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
|
||||
if (deformWorld)
|
||||
{
|
||||
psb = btSoftBodyHelpers::CreateFromVtkFile(deformWorld->getWorldInfo(), out_found_sim_filename.c_str());
|
||||
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
|
||||
}
|
||||
if (psb != NULL)
|
||||
{
|
||||
#ifndef SKIP_DEFORMABLE_BODY
|
||||
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
|
||||
if (deformWorld)
|
||||
{
|
||||
// load render mesh
|
||||
if (out_found_sim_filename != out_found_filename)
|
||||
{
|
||||
// load render mesh
|
||||
if (out_found_sim_filename != out_found_filename)
|
||||
{
|
||||
// load render mesh
|
||||
tinyobj::attrib_t attribute;
|
||||
std::vector<tinyobj::shape_t> 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::attrib_t attribute;
|
||||
std::vector<tinyobj::shape_t> 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; v < vertexCount; v++)
|
||||
{
|
||||
tinyobj::shape_t& shape = shapes[s];
|
||||
int faceCount = shape.mesh.indices.size();
|
||||
int vertexCount = attribute.vertices.size()/3;
|
||||
for (int v=0;v<vertexCount;v++)
|
||||
btSoftBody::Node n;
|
||||
n.m_x = btVector3(attribute.vertices[3 * v], attribute.vertices[3 * v + 1], attribute.vertices[3 * v + 2]);
|
||||
psb->m_renderNodes.push_back(n);
|
||||
}
|
||||
for (int f = 0; f < faceCount; f += 3)
|
||||
{
|
||||
if (f < 0 && f >= int(shape.mesh.indices.size()))
|
||||
{
|
||||
btSoftBody::Node n;
|
||||
n.m_x = btVector3(attribute.vertices[3*v],attribute.vertices[3*v+1],attribute.vertices[3*v+2]);
|
||||
psb->m_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);
|
||||
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);
|
||||
}
|
||||
}
|
||||
if (out_sim_type == UrdfGeometry::FILE_VTK)
|
||||
{
|
||||
btSoftBodyHelpers::interpolateBarycentricWeights(psb);
|
||||
}
|
||||
else if (out_sim_type == UrdfGeometry::FILE_OBJ)
|
||||
{
|
||||
btSoftBodyHelpers::extrapolateBarycentricWeights(psb);
|
||||
}
|
||||
}
|
||||
else
|
||||
if (out_sim_type == UrdfGeometry::FILE_VTK)
|
||||
{
|
||||
psb->m_renderNodes.resize(0);
|
||||
btSoftBodyHelpers::interpolateBarycentricWeights(psb);
|
||||
}
|
||||
btVector3 gravity = m_data->m_dynamicsWorld->getGravity();
|
||||
btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity);
|
||||
deformWorld->addForce(psb, gravityForce);
|
||||
m_data->m_lf.push_back(gravityForce);
|
||||
btScalar collision_hardness = 1;
|
||||
psb->m_cfg.kKHR = collision_hardness;
|
||||
psb->m_cfg.kCHR = collision_hardness;
|
||||
|
||||
btScalar friction_coeff = 0;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT)
|
||||
else if (out_sim_type == UrdfGeometry::FILE_OBJ)
|
||||
{
|
||||
friction_coeff = loadSoftBodyArgs.m_frictionCoeff;
|
||||
btSoftBodyHelpers::extrapolateBarycentricWeights(psb);
|
||||
}
|
||||
psb->m_cfg.kDF = friction_coeff;
|
||||
bool use_bending_spring = false;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS)
|
||||
{
|
||||
use_bending_spring = loadSoftBodyArgs.m_useBendingSprings;
|
||||
if (use_bending_spring)
|
||||
{
|
||||
psb->generateBendingConstraints(2);
|
||||
}
|
||||
}
|
||||
btSoftBody::Material* pm = psb->appendMaterial();
|
||||
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
|
||||
|
||||
// turn on the collision flag for deformable
|
||||
// collision between deformable and rigid
|
||||
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
|
||||
// turn on face contact only for multibodies
|
||||
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF;
|
||||
// collion between deformable and deformable and self-collision
|
||||
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
|
||||
psb->setCollisionFlags(0);
|
||||
psb->setTotalMass(mass);
|
||||
bool use_self_collision = false;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_USE_SELF_COLLISION)
|
||||
{
|
||||
use_self_collision = loadSoftBodyArgs.m_useSelfCollision;
|
||||
}
|
||||
psb->setSelfCollision(use_self_collision);
|
||||
}
|
||||
#endif//SKIP_DEFORMABLE_BODY
|
||||
else
|
||||
{
|
||||
psb->m_renderNodes.resize(0);
|
||||
}
|
||||
btVector3 gravity = m_data->m_dynamicsWorld->getGravity();
|
||||
btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity);
|
||||
deformWorld->addForce(psb, gravityForce);
|
||||
m_data->m_lf.push_back(gravityForce);
|
||||
btScalar collision_hardness = 1;
|
||||
psb->m_cfg.kKHR = collision_hardness;
|
||||
psb->m_cfg.kCHR = collision_hardness;
|
||||
|
||||
psb->m_cfg.kDF = deformable.m_friction;
|
||||
if (deformable.m_springCoefficients.bending_stiffness)
|
||||
{
|
||||
psb->generateBendingConstraints(2);
|
||||
}
|
||||
btSoftBody::Material* pm = psb->appendMaterial();
|
||||
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
|
||||
|
||||
// turn on the collision flag for deformable
|
||||
// collision between deformable and rigid
|
||||
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
|
||||
// turn on face contact only for multibodies
|
||||
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF;
|
||||
// collion between deformable and deformable and self-collision
|
||||
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
|
||||
psb->setCollisionFlags(0);
|
||||
psb->setTotalMass(deformable.m_mass);
|
||||
psb->setSelfCollision(useSelfCollision);
|
||||
}
|
||||
#endif //SKIP_DEFORMABLE_BODY
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
|
||||
if (softWorld)
|
||||
@ -8450,21 +8443,22 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
|
||||
//turn on softbody vs softbody collision
|
||||
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
|
||||
psb->randomizeConstraints();
|
||||
psb->setTotalMass(mass, true);
|
||||
psb->setTotalMass(deformable.m_mass, true);
|
||||
}
|
||||
#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
psb->scale(btVector3(scale, scale, scale));
|
||||
psb->rotate(initialOrn);
|
||||
psb->translate(initialPos);
|
||||
#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
psb->scale(btVector3(scale, scale, scale));
|
||||
psb->rotate(orn);
|
||||
psb->translate(pos);
|
||||
|
||||
psb->getCollisionShape()->setMargin(collisionMargin);
|
||||
psb->getCollisionShape()->setUserPointer(psb);
|
||||
psb->getCollisionShape()->setMargin(deformable.m_collisionMargin);
|
||||
psb->getCollisionShape()->setUserPointer(psb);
|
||||
#ifndef SKIP_DEFORMABLE_BODY
|
||||
if (deformWorld)
|
||||
{
|
||||
deformWorld->addSoftBody(psb);
|
||||
} else
|
||||
#endif//SKIP_DEFORMABLE_BODY
|
||||
}
|
||||
else
|
||||
#endif //SKIP_DEFORMABLE_BODY
|
||||
{
|
||||
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
|
||||
if (softWorld)
|
||||
@ -8472,71 +8466,124 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
|
||||
softWorld->addSoftBody(psb);
|
||||
}
|
||||
}
|
||||
m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape());
|
||||
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
|
||||
int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
|
||||
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
|
||||
bodyHandle->m_softBody = psb;
|
||||
m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape());
|
||||
*bodyUniqueId = m_data->m_bodyHandles.allocHandle();
|
||||
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(*bodyUniqueId);
|
||||
bodyHandle->m_softBody = psb;
|
||||
|
||||
b3VisualShapeData visualShape;
|
||||
b3VisualShapeData visualShape;
|
||||
|
||||
visualShape.m_objectUniqueId = bodyUniqueId;
|
||||
visualShape.m_linkIndex = -1;
|
||||
visualShape.m_visualGeometryType = URDF_GEOM_MESH;
|
||||
//dimensions just contains the scale
|
||||
visualShape.m_dimensions[0] = scale;
|
||||
visualShape.m_dimensions[1] = scale;
|
||||
visualShape.m_dimensions[2] = scale;
|
||||
//filename
|
||||
strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN);
|
||||
visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0;
|
||||
//position and orientation
|
||||
visualShape.m_localVisualFrame[0] = initialPos[0];
|
||||
visualShape.m_localVisualFrame[1] = initialPos[1];
|
||||
visualShape.m_localVisualFrame[2] = initialPos[2];
|
||||
visualShape.m_localVisualFrame[3] = initialOrn[0];
|
||||
visualShape.m_localVisualFrame[4] = initialOrn[1];
|
||||
visualShape.m_localVisualFrame[5] = initialOrn[2];
|
||||
visualShape.m_localVisualFrame[6] = initialOrn[3];
|
||||
//color and ids to be set by the renderer
|
||||
visualShape.m_rgbaColor[0] = 0;
|
||||
visualShape.m_rgbaColor[1] = 0;
|
||||
visualShape.m_rgbaColor[2] = 0;
|
||||
visualShape.m_rgbaColor[3] = 1;
|
||||
visualShape.m_tinyRendererTextureId = -1;
|
||||
visualShape.m_textureUniqueId =-1;
|
||||
visualShape.m_openglTextureId = -1;
|
||||
visualShape.m_objectUniqueId = *bodyUniqueId;
|
||||
visualShape.m_linkIndex = -1;
|
||||
visualShape.m_visualGeometryType = URDF_GEOM_MESH;
|
||||
//dimensions just contains the scale
|
||||
visualShape.m_dimensions[0] = scale;
|
||||
visualShape.m_dimensions[1] = scale;
|
||||
visualShape.m_dimensions[2] = scale;
|
||||
//filename
|
||||
strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN);
|
||||
visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0;
|
||||
//position and orientation
|
||||
visualShape.m_localVisualFrame[0] = pos[0];
|
||||
visualShape.m_localVisualFrame[1] = pos[1];
|
||||
visualShape.m_localVisualFrame[2] = pos[2];
|
||||
visualShape.m_localVisualFrame[3] = orn[0];
|
||||
visualShape.m_localVisualFrame[4] = orn[1];
|
||||
visualShape.m_localVisualFrame[5] = orn[2];
|
||||
visualShape.m_localVisualFrame[6] = orn[3];
|
||||
//color and ids to be set by the renderer
|
||||
visualShape.m_rgbaColor[0] = 0;
|
||||
visualShape.m_rgbaColor[1] = 0;
|
||||
visualShape.m_rgbaColor[2] = 0;
|
||||
visualShape.m_rgbaColor[3] = 1;
|
||||
visualShape.m_tinyRendererTextureId = -1;
|
||||
visualShape.m_textureUniqueId = -1;
|
||||
visualShape.m_openglTextureId = -1;
|
||||
|
||||
m_data->m_pluginManager.getRenderInterface()->addVisualShape(&visualShape, fileIO);
|
||||
m_data->m_pluginManager.getRenderInterface()->addVisualShape(&visualShape, fileIO);
|
||||
|
||||
serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
|
||||
serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED;
|
||||
int pos = strlen(relativeFileName)-1;
|
||||
while(pos>=0 && relativeFileName[pos]!='/') { pos--;}
|
||||
btAssert(strlen(relativeFileName)-pos-5>0);
|
||||
std::string object_name (std::string(relativeFileName).substr(pos+1, strlen(relativeFileName)- 5 - pos));
|
||||
if (!deformable.m_name.empty())
|
||||
{
|
||||
bodyHandle->m_bodyName = deformable.m_name;
|
||||
}
|
||||
else
|
||||
{
|
||||
int pos = strlen(relativeFileName) - 1;
|
||||
while (pos >= 0 && relativeFileName[pos] != '/')
|
||||
{
|
||||
pos--;
|
||||
}
|
||||
btAssert(strlen(relativeFileName) - pos - 5 > 0);
|
||||
std::string object_name(std::string(relativeFileName).substr(pos + 1, strlen(relativeFileName) - 5 - pos));
|
||||
bodyHandle->m_bodyName = object_name;
|
||||
|
||||
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;
|
||||
m_data->m_pluginManager.addNotification(notification);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
|
||||
{
|
||||
serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_FAILED;
|
||||
bool hasStatus = true;
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
UrdfDeformable deformable;
|
||||
|
||||
constructUrdfDeformable(clientCmd, deformable, m_data->m_verboseOutput);
|
||||
// const LoadSoftBodyArgs& loadSoftBodyArgs = clientCmd.m_loadSoftBodyArguments;
|
||||
btVector3 initialPos(0, 0, 0);
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_POSITION)
|
||||
{
|
||||
initialPos[0] = clientCmd.m_loadSoftBodyArguments.m_initialPosition[0];
|
||||
initialPos[1] = clientCmd.m_loadSoftBodyArguments.m_initialPosition[1];
|
||||
initialPos[2] = clientCmd.m_loadSoftBodyArguments.m_initialPosition[2];
|
||||
}
|
||||
btQuaternion initialOrn(0, 0, 0, 1);
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_ORIENTATION)
|
||||
{
|
||||
initialOrn[0] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[0];
|
||||
initialOrn[1] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[1];
|
||||
initialOrn[2] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[2];
|
||||
initialOrn[3] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[3];
|
||||
}
|
||||
|
||||
double scale = 1;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_SCALE)
|
||||
{
|
||||
scale = clientCmd.m_loadSoftBodyArguments.m_scale;
|
||||
}
|
||||
bool use_self_collision = false;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_USE_SELF_COLLISION)
|
||||
{
|
||||
use_self_collision = clientCmd.m_loadSoftBodyArguments.m_useSelfCollision;
|
||||
}
|
||||
|
||||
int bodyUniqueId = -1;
|
||||
bool completedOk = processDeformable(deformable, initialPos, initialOrn, &bodyUniqueId, bufferServerToClient, bufferSizeInBytes, scale, use_self_collision);
|
||||
if (completedOk && bodyUniqueId > 0)
|
||||
{
|
||||
m_data->m_guiHelper->autogenerateGraphicsObjects(m_data->m_dynamicsWorld);
|
||||
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());
|
||||
serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
|
||||
|
||||
b3Notification notification;
|
||||
notification.m_notificationType = BODY_ADDED;
|
||||
notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId;
|
||||
m_data->m_pluginManager.addNotification(notification);
|
||||
#endif
|
||||
}
|
||||
return hasStatus;
|
||||
}
|
||||
|
||||
|
||||
@ -5,6 +5,7 @@
|
||||
#include "LinearMath/btVector3.h"
|
||||
|
||||
#include "PhysicsCommandProcessorInterface.h"
|
||||
#include "../Importers/ImportURDFDemo/UrdfParser.h"
|
||||
|
||||
struct SharedMemLines
|
||||
{
|
||||
@ -104,6 +105,7 @@ protected:
|
||||
bool loadMjcf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags);
|
||||
|
||||
bool processImportedObjects(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, class URDFImporterInterface& u2b);
|
||||
bool processDeformable(const UrdfDeformable& deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision);
|
||||
|
||||
bool supportsJointMotor(class btMultiBody* body, int linkIndex);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user