Merge remote-tracking branch 'origin/master' into KKT

This commit is contained in:
Xuchen Han 2020-04-20 19:35:46 -07:00
commit 78aebd44e2
31 changed files with 1270 additions and 395 deletions

View File

@ -437,7 +437,7 @@ const char **InPlaceParser::GetArglist(char *line, int &count) // convert sourc
{
const char **ret = 0;
const char *argv[MAXARGS];
static const char *argv[MAXARGS];
int argc = 0;
char *foo = line;

13
data/torus_deform.urdf Normal file
View File

@ -0,0 +1,13 @@
<?xml version="1.0"?>
<robot name="torus">
<deformable name="torus">
<inertial>
<mass value="1" />
<inertia ixx="0.0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
</inertial>
<collision_margin value="0.006"/>
<friction value= "0.5"/>
<neohookean mu= "60" lambda= "200" damping= "0.01" />
<visual filename="torus.vtk"/>
</deformable>
</robot>

View File

@ -51,7 +51,7 @@ struct MySliderEventHandler : public Gwen::Event::Handler
m_showValue(true)
{
memcpy(m_variableName, varName, strlen(varName) + 1);
strncpy(m_variableName, varName, sizeof(m_variableName));
}
void SliderMoved(Gwen::Controls::Base* pControl)
@ -85,7 +85,7 @@ struct MySliderEventHandler : public Gwen::Event::Handler
if (m_showValue)
{
char txt[1024];
sprintf(txt, "%s : %.3f", m_variableName, val);
snprintf(txt, sizeof(txt), "%s : %.3f", m_variableName, val);
m_label->SetText(txt);
}
}
@ -231,7 +231,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params)
}
pSlider->SetValue(*params.m_paramValuePointer); //dimensions[i] );
char labelName[1024];
sprintf(labelName, "%s", params.m_name); //axisNames[0]);
snprintf(labelName, sizeof(labelName), "%s", params.m_name); //axisNames[0]);
MySliderEventHandler<btScalar>* handler = new MySliderEventHandler<btScalar>(labelName, label, pSlider, params.m_paramValuePointer, params.m_callback, params.m_userPointer);
handler->m_showValue = params.m_showValues;
m_paramInternalData->m_sliderEventHandlers.push_back(handler);

View File

@ -796,7 +796,7 @@ struct BulletMJCFImporterInternalData
bool lastThree = false;
parseVector3(size, sizeStr, logger, lastThree);
}
geom.m_boxSize = size;
geom.m_boxSize = 2*size;
handledGeomType = true;
}
if (geomType == "box")
@ -809,7 +809,7 @@ struct BulletMJCFImporterInternalData
parseVector3(size, sizeStr, logger, lastThree);
}
geom.m_type = URDF_GEOM_BOX;
geom.m_boxSize = size;
geom.m_boxSize = 2*size;
handledGeomType = true;
}
@ -1012,7 +1012,7 @@ struct BulletMJCFImporterInternalData
}
case URDF_GEOM_BOX:
{
totalVolume += 8. * col->m_geometry.m_boxSize[0] *
totalVolume += col->m_geometry.m_boxSize[0] *
col->m_geometry.m_boxSize[1] *
col->m_geometry.m_boxSize[2];
break;
@ -1890,7 +1890,7 @@ void BulletMJCFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
case URDF_GEOM_BOX:
{
btVector3 extents = visual->m_geometry.m_boxSize;
btVector3 extents = 0.5*visual->m_geometry.m_boxSize;
btBoxShape* boxShape = new btBoxShape(extents * 0.5f);
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
convexColShape = boxShape;
@ -2336,7 +2336,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
}
case URDF_GEOM_BOX:
{
childShape = new btBoxShape(col->m_geometry.m_boxSize);
childShape = new btBoxShape(0.5*col->m_geometry.m_boxSize);
break;
}
case URDF_GEOM_CYLINDER:

View File

@ -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();
}

View File

@ -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

View File

@ -1098,6 +1098,155 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, XMLElement* config,
return true;
}
bool UrdfParser::parseLameCoefficients(LameCoefficients& lameCoefficients, tinyxml2::XMLElement* config, ErrorLogger* logger)
{
const char* mu = config->Attribute("mu");
const char* lambda = config->Attribute("lambda");
const char* damping = config->Attribute("damping");
if (!mu || !lambda)
{
logger->reportError("expected mu lambda for LameCoefficients.");
return false;
}
lameCoefficients.mu = urdfLexicalCast<double>(mu);
lameCoefficients.lambda = urdfLexicalCast<double>(lambda);
if (damping)
lameCoefficients.damping = urdfLexicalCast<double>(damping);
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)
{
if (!spring_xml->Attribute("elastic_stiffness") || !spring_xml->Attribute("damping_stiffness"))
{
logger->reportError("spring element expect elastic and damping stiffness");
return false;
}
deformable.m_springCoefficients.elastic_stiffness = urdfLexicalCast<double>(spring_xml->Attribute("elastic_stiffness"));
deformable.m_springCoefficients.damping_stiffness = urdfLexicalCast<double>(spring_xml->Attribute("damping_stiffness"));
if (spring_xml->Attribute("bending_stiffness"))
deformable.m_springCoefficients.bending_stiffness = urdfLexicalCast<double>(spring_xml->Attribute("bending_stiffness"));
}
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;
}
std::string fn = vis_xml->Attribute("filename");
deformable.m_visualFileName = fn;
int out_type(0);
bool success = UrdfFindMeshFile(m_fileIO,
model.m_sourceFile, fn, sourceFileLocation(vis_xml),
&deformable.m_visualFileName, &out_type);
if (!success)
{
// warning already printed
return false;
}
XMLElement* col_xml = config->FirstChildElement("collision");
if (col_xml)
{
if (!col_xml->Attribute("filename"))
{
logger->reportError("expected a filename for collision geoemtry");
return false;
}
fn = vis_xml->Attribute("filename");
success = UrdfFindMeshFile(m_fileIO,
model.m_sourceFile, fn, sourceFileLocation(vis_xml),
&deformable.m_simFileName, &out_type);
if (!success)
{
// warning already printed
return false;
}
}
ParseUserData(config, deformable.m_userData, logger);
return true;
}
bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLogger* logger)
{
joint.m_lowerLimit = 0.f;
@ -1898,6 +2047,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 +2072,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 +2196,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");

View 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,11 +400,14 @@ 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);
bool recreateModel(UrdfModel& model, UrdfLink* link, ErrorLogger* logger);
std::string sourceFileLocation(tinyxml2::XMLElement* e);

View File

@ -141,8 +141,13 @@ void EGLOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
if (eglGetError() == EGL_SUCCESS && initialized == EGL_TRUE)
{
m_data->egl_display = display;
break;
}
}
else
{
fprintf(stderr, "GetDisplay %d failed with error: %x\n", i, eglGetError());
}
}
}
else
@ -166,6 +171,10 @@ void EGLOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
m_data->egl_display = display;
}
}
else
{
fprintf(stderr, "GetDisplay %d failed with error: %x\n", m_data->m_renderDevice, eglGetError());
}
}
if (!eglInitialize(m_data->egl_display, NULL, NULL))

View File

@ -654,8 +654,8 @@ void PhysicsClientExample::createButtons()
{
if (m_numMotors < MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName, "%s q", info.m_jointName);
char motorName[1026];
snprintf(motorName, sizeof(motorName), "%s q", info.m_jointName);
// MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
MyMotorInfo2* motorInfo = &m_motorTargetPositions[m_numMotors];
motorInfo->m_velTarget = 0.f;

View File

@ -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,276 @@ 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;
}
bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SIM_MESH)
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;
}
#endif
}
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())
{
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)
{
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)
{
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld)
psb = btSoftBodyHelpers::CreateFromTriMesh(softWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
if (!psb)
{
psb = btSoftBodyHelpers::CreateFromTriMesh(softWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
printf("Load deformable failed\n");
return false;
}
}
}
{
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
psb = btSoftBodyHelpers::CreateFromTriMesh(deformWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
if (!psb)
{
psb = btSoftBodyHelpers::CreateFromTriMesh(deformWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
printf("Load deformable failed\n");
return false;
}
}
}
}
#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)
{
#ifndef SKIP_DEFORMABLE_BODY
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
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)
{
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
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);
}
if (psb != NULL)
{
#endif
}
else if (out_sim_type == UrdfGeometry::FILE_VTK)
{
#ifndef SKIP_DEFORMABLE_BODY
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
psb = btSoftBodyHelpers::CreateFromVtkFile(deformWorld->getWorldInfo(), out_found_sim_filename.c_str());
if (!psb)
{
printf("Load deformable failed\n");
return false;
}
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 +8458,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 +8481,123 @@ 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;
}
b3Notification notification;
notification.m_notificationType = BODY_ADDED;
notification.m_bodyArgs.m_bodyUniqueId = *bodyUniqueId;
m_data->m_pluginManager.addNotification(notification);
}
return true;
}
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
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());
b3Notification notification;
notification.m_notificationType = BODY_ADDED;
notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId;
m_data->m_pluginManager.addNotification(notification);
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;
#endif
}
return hasStatus;
}

View File

@ -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);

View File

@ -1350,9 +1350,6 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL(
m_data->m_instancingRenderer->updateCamera(m_data->m_upAxis);
m_data->m_instancingRenderer->renderScene();
m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(1, 0, 0), b3MakeVector3(1, 0, 0), 3);
m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(0, 1, 0), b3MakeVector3(0, 1, 0), 3);
m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(0, 0, 1), b3MakeVector3(0, 0, 1), 3);
int numBytesPerPixel = 4; //RGBA

View File

@ -72,7 +72,6 @@ struct InMemoryFileIO : public CommonFileIOInterface
{
for (int i=0;i<m_fileCache.size();i++)
{
b3HashString name = m_fileCache.getKeyAtIndex(i);
InMemoryFile** memPtr = m_fileCache.getAtIndex(i);
if (memPtr && *memPtr)
{
@ -81,9 +80,9 @@ struct InMemoryFileIO : public CommonFileIOInterface
m_numFrees++;
delete (mem);
m_numFrees++;
m_fileCache.remove(name);
}
}
m_fileCache.clear();
}
char* allocateBuffer(int len)

View File

@ -12,6 +12,9 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2])
boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
bunnyId = p.loadSoftBody("torus.vtk", useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, useSelfCollision = 1, frictionCoeff = 0.5)
bunny2 = p.loadURDF("torus_deform.urdf", [0,1,0], flags=p.URDF_USE_SELF_COLLISION)
p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
p.setRealTimeSimulation(1)

View File

@ -0,0 +1,596 @@
<?xml version="1.0" ?>
<robot name="plane">
<link name="chassis">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0.03 0.043794"/>
<mass value="13.715"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.043794"/>
<geometry>
<mesh filename="chassis.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="-1.57 0 0" xyz="0 0 0.043794"/>
<geometry>
<mesh filename="chassis_vhacd_mod.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
<link name="FR_hip_motor">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0.02 0 0"/>
<mass value="1.095"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="hip_motor_mirror.obj" scale="1 1 1"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="hip_motor_mirror.stl" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="FR_hip_motor_2_chassis_joint" type="revolute">
<axis xyz="0 0 -1"/>
<parent link="chassis"/>
<child link="FR_hip_motor"/>
<origin rpy="0 0 0" xyz="-0.0817145 0 0.242889"/>
<limit effort="100" velocity="100" lower="-0.873" upper="1.0472"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="FR_upper_leg">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.04 -0.04"/>
<mass value="1.527"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="upper_leg_mirror.obj" scale="1 1 1"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="upper_leg_right_vhacd.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="FR_upper_leg_2_hip_motor_joint" type="revolute">
<axis xyz="1 0 0"/>
<parent link="FR_hip_motor"/>
<child link="FR_upper_leg"/>
<origin rpy="0 0 0" xyz="-0.053565 0 0"/>
<limit effort="100" velocity="100" lower="-1.3" upper="3.4"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="FR_lower_leg">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.16 -0.02"/>
<mass value="0.241"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 1.57079 0" xyz="0 0.0 0"/>
<geometry>
<mesh filename="lower_leg3.obj" scale="1 1 1"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 1.57079 0" xyz="0 0.0 0"/>
<geometry>
<mesh filename="lower_leg_3.stl" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="FR_lower_leg_2_upper_leg_joint" type="revolute">
<axis xyz="1 0 0"/>
<parent link="FR_upper_leg"/>
<child link="FR_lower_leg"/>
<origin rpy="0 0 0" xyz="0.02069 -0.20833 -0.1422"/>
<limit effort="100" velocity="100" lower="-2.164" upper="0"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="FL_hip_motor">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="-.02 0 0"/>
<mass value="1.095"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="hip_motor.obj" scale="1 1 1"/>
</geometry>
<material name="white">
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="hip_motor.stl" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="FL_hip_motor_2_chassis_joint" type="revolute">
<axis xyz="0 0 1"/>
<parent link="chassis"/>
<child link="FL_hip_motor"/>
<origin rpy="0 0 0" xyz="0.0817145 0 0.242889"/>
<limit effort="100" velocity="100" lower="-0.873" upper="1.0472"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="FL_upper_leg">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.04 -0.04"/>
<mass value="1.527"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="upper_leg_left.obj" scale="1 1 1"/>
</geometry>
<material name="white">
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="upper_leg_left_vhacd.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="FL_upper_leg_2_hip_motor_joint" type="revolute">
<axis xyz="1 0 0"/>
<parent link="FL_hip_motor"/>
<child link="FL_upper_leg"/>
<origin rpy="0 0 0" xyz="0.055855 0 0"/>
<limit effort="100" velocity="100" lower="-1.3" upper="3.4"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="FL_lower_leg">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.16 -0.02"/>
<mass value="0.241"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
<geometry>
<mesh filename="lower_leg3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
</material>
</visual>
<collision>
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
<geometry>
<mesh filename="lower_leg_3.stl" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="FL_lower_leg_2_upper_leg_joint" type="revolute">
<axis xyz="1 0 0"/>
<parent link="FL_upper_leg"/>
<child link="FL_lower_leg"/>
<origin rpy="0 0 0" xyz="-0.02069 -0.20833 -0.1422"/>
<limit effort="100" velocity="100" lower="-2.164" upper="0"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="RR_hip_motor">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0.02 0 0"/>
<mass value="1.095"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="hip_motor_mirror.obj" scale="1 1 1"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="hip_motor_mirror.stl" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="RR_hip_motor_2_chassis_joint" type="revolute">
<axis xyz="0 0 -1"/>
<parent link="chassis"/>
<child link="RR_hip_motor"/>
<origin rpy="0 0 0" xyz="-0.0817145 0 -0.194401"/>
<limit effort="100" velocity="100" lower="-0.873" upper="1.0472"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="RR_upper_leg">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.04 -0.04"/>
<mass value="1.527"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="upper_leg_mirror2.obj" scale="1 1 1"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="upper_leg_right_vhacd.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="RR_upper_leg_2_hip_motor_joint" type="revolute">
<axis xyz="1 0 0"/>
<parent link="RR_hip_motor"/>
<child link="RR_upper_leg"/>
<origin rpy="0 0 0" xyz="-0.053565 0 0"/>
<limit effort="100" velocity="100" lower="-1.3" upper="3.4"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="RR_lower_leg">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.16 -0.02"/>
<mass value="0.241"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
<geometry>
<mesh filename="lower_leg3.obj" scale="1 1 1"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
<geometry>
<mesh filename="lower_leg_3.stl" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="RR_lower_leg_2_upper_leg_joint" type="revolute">
<axis xyz="1 0 0"/>
<parent link="RR_upper_leg"/>
<child link="RR_lower_leg"/>
<origin rpy="0 0 0" xyz="0.02069 -0.20833 -0.1422"/>
<limit effort="100" velocity="100" lower="-2.164" upper="0"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="RL_hip_motor">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="-.02 0 0"/>
<mass value="1.095"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="hip_motor.obj" scale="1 1 1"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="hip_motor.stl" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="RL_hip_motor_2_chassis_joint" type="revolute">
<axis xyz="0 0 1"/>
<parent link="chassis"/>
<child link="RL_hip_motor"/>
<origin rpy="0 0 0" xyz="0.0817145 0 -0.194401"/>
<limit effort="100" velocity="100" lower="-0.873" upper="1.0472"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="RL_upper_leg">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.04 -0.04"/>
<mass value="1.527"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="upper_leg_left2.obj" scale="1 1 1"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="upper_leg_left_vhacd.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="RL_upper_leg_2_hip_motor_joint" type="revolute">
<axis xyz="1 0 0"/>
<parent link="RL_hip_motor"/>
<child link="RL_upper_leg"/>
<origin rpy="0 0 0" xyz="0.055855 0 0"/>
<limit effort="100" velocity="100" lower="-1.3" upper="3.4"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="RL_lower_leg">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.16 -0.02"/>
<mass value="0.241"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
<geometry>
<mesh filename="lower_leg3.obj" scale="1 1 1"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
<geometry>
<mesh filename="lower_leg_3.stl" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="RL_lower_leg_2_upper_leg_joint" type="revolute">
<axis xyz="1 0 0"/>
<parent link="RL_upper_leg"/>
<child link="RL_lower_leg"/>
<origin rpy="0 0 0" xyz="-0.02069 -0.20833 -0.1422"/>
<limit effort="100" velocity="100" lower="-2.164" upper="0"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="toeRL">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.1"/>
<lateral_friction value="1.0"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.03"/>
</geometry>
</collision>
<inertial>
<mass value="0.1"/>
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
</link>
<joint name="jtoeRL" type="fixed">
<parent link="RL_lower_leg"/>
<child link="toeRL"/>
<origin xyz="0 -0.25 -0.022"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<link name="toeRR">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.1"/>
<lateral_friction value="1.0"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.03"/>
</geometry>
</collision>
<inertial>
<mass value="0.1"/>
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
</link>
<joint name="jtoeRR" type="fixed">
<parent link="RR_lower_leg"/>
<child link="toeRR"/>
<origin xyz="0 -0.25 -0.022"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<link name="toeFL">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.1"/>
<lateral_friction value="1.0"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.03"/>
</geometry>
</collision>
<inertial>
<mass value="0.1"/>
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
</link>
<joint name="jtoeFL" type="fixed">
<parent link="FL_lower_leg"/>
<child link="toeFL"/>
<origin xyz="0 -0.25 -0.022"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<link name="toeFR">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.1"/>
<lateral_friction value="1.0"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.03"/>
</geometry>
</collision>
<inertial>
<mass value="0.1"/>
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
</link>
<joint name="jtoeFR" type="fixed">
<parent link="FR_lower_leg"/>
<child link="toeFR"/>
<origin xyz="0 -0.25 -0.022"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
</robot>

View File

@ -30,9 +30,18 @@
#if defined(__APPLE__) && (!defined(B3_NO_PYTHON_FRAMEWORK))
#include <Python/Python.h>
#else
#ifdef _WIN32
#ifdef _DEBUG
#define BT_REMOVED_DEBUG
//always use the release build of Python
#undef _DEBUG
#endif //_DEBUG
#endif
#include <Python.h>
#endif
#ifdef BT_REMOVED_DEBUG
#define _DEBUG
#endif
#include "../Importers/ImportURDFDemo/urdfStringSplit.h"
#ifdef B3_DUMP_PYTHON_VERSION

View File

@ -501,7 +501,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:
setup(
name='pybullet',
version='2.7.1',
version='2.7.3',
description=
'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
long_description=

View File

@ -46,8 +46,6 @@ protected:
btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr;
btManifoldResult m_defaultManifoldResult;
btNearCallback m_nearCallback;
btPoolAllocator* m_collisionAlgorithmPoolAllocator;
@ -95,11 +93,15 @@ public:
btPersistentManifold* getManifoldByIndexInternal(int index)
{
btAssert(index>=0);
btAssert(index<m_manifoldsPtr.size());
return m_manifoldsPtr[index];
}
const btPersistentManifold* getManifoldByIndexInternal(int index) const
{
btAssert(index>=0);
btAssert(index<m_manifoldsPtr.size());
return m_manifoldsPtr[index];
}

View File

@ -876,7 +876,10 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
// will we not request a velocity with the wrong direction ?
// and the answer is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
// so the sign of the force that is really matters
info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
if (m_flags & BT_6DOF_FLAGS_USE_INFINITE_ERROR)
info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
else
info->m_constraintError[srow] = vel + f / m * (rotational ? -1 : 1);
btScalar minf = f < fd ? f : fd;
btScalar maxf = f < fd ? fd : f;

View File

@ -265,6 +265,7 @@ enum bt6DofFlags2
BT_6DOF_FLAGS_ERP_STOP2 = 2,
BT_6DOF_FLAGS_CFM_MOTO2 = 4,
BT_6DOF_FLAGS_ERP_MOTO2 = 8,
BT_6DOF_FLAGS_USE_INFINITE_ERROR = (1<<16)
};
#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis

View File

@ -14,7 +14,9 @@ subject to the following restrictions:
*/
//#define COMPUTE_IMPULSE_DENOM 1
//#define BT_ADDITIONAL_DEBUG
#ifdef BT_DEBUG
# define BT_ADDITIONAL_DEBUG
#endif
//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
@ -690,8 +692,10 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
{
#if BT_THREADSAFE
int solverBodyId = -1;
bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
if (isRigidBodyType && !body.isStaticOrKinematicObject())
const bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
const bool isStaticOrKinematic = body.isStaticOrKinematicObject();
const bool isKinematic = body.isKinematicObject();
if (isRigidBodyType && !isStaticOrKinematic)
{
// dynamic body
// Dynamic bodies can only be in one island, so it's safe to write to the companionId
@ -704,7 +708,7 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
body.setCompanionId(solverBodyId);
}
}
else if (isRigidBodyType && body.isKinematicObject())
else if (isRigidBodyType && isKinematic)
{
//
// NOTE: must test for kinematic before static because some kinematic objects also

View File

@ -136,8 +136,13 @@ void btRigidBody::setGravity(const btVector3& acceleration)
void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
{
m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
#ifdef BT_USE_OLD_DAMPING_METHOD
m_linearDamping = btMax(lin_damping, btScalar(0.0));
m_angularDamping = btMax(ang_damping, btScalar(0.0));
#else
m_linearDamping = btClamped(lin_damping, btScalar(0.0), btScalar(1.0));
m_angularDamping = btClamped(ang_damping, btScalar(0.0), btScalar(1.0));
#endif
}
///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
@ -146,10 +151,9 @@ void btRigidBody::applyDamping(btScalar timeStep)
//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
//#define USE_OLD_DAMPING_METHOD 1
#ifdef USE_OLD_DAMPING_METHOD
m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
#ifdef BT_USE_OLD_DAMPING_METHOD
m_linearVelocity *= btMax((btScalar(1.0) - timeStep * m_linearDamping), btScalar(0.0));
m_angularVelocity *= btMax((btScalar(1.0) - timeStep * m_angularDamping), btScalar(0.0));
#else
m_linearVelocity *= btPow(btScalar(1) - m_linearDamping, timeStep);
m_angularVelocity *= btPow(btScalar(1) - m_angularDamping, timeStep);

View File

@ -171,6 +171,8 @@ void btSimulationIslandManagerMt::initIslandPools()
btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland(int id)
{
btAssert(id >= 0);
btAssert(id < m_lookupIslandFromId.size());
Island* island = m_lookupIslandFromId[id];
if (island == NULL)
{

View File

@ -583,52 +583,6 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
}
}
btScalar btMultiBody::getKineticEnergy() const
{
int num_links = getNumLinks();
// TODO: would be better not to allocate memory here
btAlignedObjectArray<btVector3> omega;
omega.resize(num_links + 1);
btAlignedObjectArray<btVector3> vel;
vel.resize(num_links + 1);
compTreeLinkVelocities(&omega[0], &vel[0]);
// we will do the factor of 0.5 at the end
btScalar result = m_baseMass * vel[0].dot(vel[0]);
result += omega[0].dot(m_baseInertia * omega[0]);
for (int i = 0; i < num_links; ++i)
{
result += m_links[i].m_mass * vel[i + 1].dot(vel[i + 1]);
result += omega[i + 1].dot(m_links[i].m_inertiaLocal * omega[i + 1]);
}
return 0.5f * result;
}
btVector3 btMultiBody::getAngularMomentum() const
{
int num_links = getNumLinks();
// TODO: would be better not to allocate memory here
btAlignedObjectArray<btVector3> omega;
omega.resize(num_links + 1);
btAlignedObjectArray<btVector3> vel;
vel.resize(num_links + 1);
btAlignedObjectArray<btQuaternion> rot_from_world;
rot_from_world.resize(num_links + 1);
compTreeLinkVelocities(&omega[0], &vel[0]);
rot_from_world[0] = m_baseQuat;
btVector3 result = quatRotate(rot_from_world[0].inverse(), (m_baseInertia * omega[0]));
for (int i = 0; i < num_links; ++i)
{
rot_from_world[i + 1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent + 1];
result += (quatRotate(rot_from_world[i + 1].inverse(), (m_links[i].m_inertiaLocal * omega[i + 1])));
}
return result;
}
void btMultiBody::clearConstraintForces()
{

View File

@ -307,13 +307,6 @@ public:
//
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const;
//
// calculate kinetic energy and angular momentum
// useful for debugging.
//
btScalar getKineticEnergy() const;
btVector3 getAngularMomentum() const;
//
// set external forces and torques. Note all external forces/torques are given in the WORLD frame.

View File

@ -30,10 +30,12 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
//solve featherstone non-contact constraints
btScalar nonContactResidual = 0;
//printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
for (int i = 0; i < infoGlobal.m_numNonContactInnerIterations; ++i)
{
// reset the nonContactResdual to 0 at start of each inner iteration
nonContactResidual = 0;
for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
{
int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
@ -41,7 +43,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
btScalar residual = resolveSingleConstraintRowGeneric(constraint);
leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
nonContactResidual = btMax(nonContactResidual, residual * residual);
if (constraint.m_multiBodyA)
constraint.m_multiBodyA->setPosUpdated(false);
@ -49,6 +51,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
constraint.m_multiBodyB->setPosUpdated(false);
}
}
leastSquaredResidual = btMax(leastSquaredResidual, nonContactResidual);
//solve featherstone normal contact
for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++)

View File

@ -222,6 +222,9 @@ void btSoftBody::initDefaults()
m_useSelfCollision = false;
m_collisionFlags = 0;
m_softSoftCollision = false;
m_maxSpeedSquared = 0;
m_repulsionStiffness = 0.5;
m_fdbvnt = 0;
}
//
@ -2602,7 +2605,7 @@ void btSoftBody::initializeFaceTree()
for (int i = 0; i < m_faces.size(); ++i)
{
Face& f = m_faces[i];
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = VolumeOf(f, m_sst.radmrg);
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = VolumeOf(f, 0);
btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
node->parent = NULL;
node->data = &f;
@ -2638,10 +2641,10 @@ void btSoftBody::initializeFaceTree()
}
}
m_fdbvt.m_root = buildTreeBottomUp(leafNodes, adj);
updateFaceTree(false, true);
if (m_fdbvnt)
delete m_fdbvnt;
m_fdbvnt = copyToDbvnt(m_fdbvt.m_root);
updateFaceTree(false, false);
rebuildNodeTree();
}
@ -2654,7 +2657,7 @@ void btSoftBody::rebuildNodeTree()
for (int i = 0; i < m_nodes.size(); ++i)
{
Node& n = m_nodes[i];
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg);
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = btDbvtVolume::FromCR(n.m_x, 0);
btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
node->parent = NULL;
node->data = &n;
@ -3451,7 +3454,7 @@ void btSoftBody::setSpringStiffness(btScalar k)
{
m_links[i].Feature::m_material->m_kLST = k;
}
repulsionStiffness = k;
m_repulsionStiffness = k;
}
void btSoftBody::initializeDmInverse()

View File

@ -724,6 +724,15 @@ public:
/* SolverState */
struct SolverState
{
//if you add new variables, always initialize them!
SolverState()
:sdt(0),
isdt(0),
velmrg(0),
radmrg(0),
updmrg(0)
{
}
btScalar sdt; // dt*timescale
btScalar isdt; // 1/sdt
btScalar velmrg; // velocity margin
@ -810,7 +819,7 @@ public:
btScalar m_sleepingThreshold;
btScalar m_maxSpeedSquared;
btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection
btScalar repulsionStiffness;
btScalar m_repulsionStiffness;
btAlignedObjectArray<btVector3> m_X; // initial positions
btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
@ -1300,7 +1309,7 @@ public:
btScalar I = 0;
btScalar mass = node->m_im == 0 ? 0 : btScalar(1)/node->m_im;
if (applySpringForce)
I = -btMin(repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn));
I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn));
if (vn < 0)
I += 0.5 * mass * vn;
btScalar face_penetration = 0, node_penetration = node->m_penetration;

View File

@ -1300,13 +1300,15 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo,
}
else if (reading_tets)
{
int d;
ss >> d;
if (d != 4)
{
printf("Only Tetrahedra are supported in vtk file\n");
return 0;
}
int d;
ss >> d;
if (d != 4)
{
printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n");
fs.close();
return 0;
}
ss.ignore(128, ' '); // ignore "4"
Index tet;
tet.resize(4);
for (size_t i = 0; i < 4; i++)
@ -1597,7 +1599,13 @@ void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb)
{
new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
}
if (new_min_bary_weight > min_bary_weight)
// p is out of the current best triangle, we found a traingle that's better
bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight<0.);
// p is inside of the current best triangle, we found a triangle that's better
bool better_than_best_inside = (new_min_bary_weight>=0 && min_bary_weight>=0 && btFabs(dist)<btFabs(optimal_dist));
if (better_than_closest_outisde || better_than_best_inside)
{
btAlignedObjectArray<const btSoftBody::Node*> parents;
parents.push_back(f.m_n[0]);
@ -1607,11 +1615,6 @@ void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb)
optimal_bary = bary;
optimal_dist = dist;
min_bary_weight = new_min_bary_weight;
// stop searching if the projected p is inside the triangle at hand
if (bary[0]>=0. && bary[1]>=0. && bary[2]>=0.)
{
break;
}
}
}
psb->m_renderNodesInterpolationWeights[i] = optimal_bary;

View File

@ -346,10 +346,9 @@ struct btMatrixX
T dotProd = 0;
{
{
int r = rows();
int c = cols();
for (int k = 0; k < cols(); k++)
for (int k = 0; k < c; k++)
{
T w = (*this)(i, k);
if (other(k, j) != 0.f)