support configuring gravity factor from client

This commit is contained in:
Xuchen Han
2020-05-26 16:51:45 -07:00
parent e89903a8ab
commit 8a189763df
5 changed files with 30 additions and 3 deletions
@@ -1174,6 +1174,17 @@ bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config,
}
deformable.m_repulsionStiffness = urdfLexicalCast<double>(repulsion_xml->Attribute("value"));
}
XMLElement* grav_xml = config->FirstChildElement("gravity_factor");
if (grav_xml)
{
if (!grav_xml->Attribute("value"))
{
logger->reportError("gravity_factor element must have value attribute");
return false;
}
deformable.m_gravFactor = urdfLexicalCast<double>(grav_xml->Attribute("value"));
}
XMLElement* spring_xml = config->FirstChildElement("spring");
if (spring_xml)
@@ -228,6 +228,7 @@ struct UrdfDeformable
double m_collisionMargin;
double m_friction;
double m_repulsionStiffness;
double m_gravFactor;
SpringCoeffcients m_springCoefficients;
LameCoefficients m_corotatedCoefficients;
@@ -237,7 +238,7 @@ struct UrdfDeformable
std::string m_simFileName;
btHashMap<btHashString, std::string> m_userData;
UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_repulsionStiffness(0.5), m_visualFileName(""), m_simFileName("")
UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_repulsionStiffness(0.5), m_gravFactor(1.), m_visualFileName(""), m_simFileName("")
{
}
};
@@ -413,6 +413,15 @@ B3_SHARED_API int b3LoadSoftBodySetRepulsionStiffness(b3SharedMemoryCommandHandl
return 0;
}
B3_SHARED_API int b3LoadSoftBodySetGravityFactor(b3SharedMemoryCommandHandle commandHandle, double gravFactor)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_gravFactor = gravFactor;
command->m_updateFlags |= LOAD_SOFT_BODY_SET_GRAVITY_FACTOR;
return 0;
}
B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
@@ -8421,7 +8421,10 @@ void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDe
{
deformable.m_repulsionStiffness = loadSoftBodyArgs.m_repulsionStiffness;
}
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_GRAVITY_FACTOR)
{
deformable.m_gravFactor = loadSoftBodyArgs.m_gravFactor;
}
#endif
}
@@ -8634,7 +8637,7 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
// 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
// turn on face contact 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;
@@ -8642,6 +8645,7 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
psb->setTotalMass(deformable.m_mass);
psb->setSelfCollision(useSelfCollision);
psb->setSpringStiffness(deformable.m_repulsionStiffness);
psb->setGravityFactor(deformable.m_gravFactor);
psb->initializeFaceTree();
}
#endif //SKIP_DEFORMABLE_BODY
@@ -515,6 +515,7 @@ enum EnumLoadSoftBodyUpdateFlags
LOAD_SOFT_BODY_SIM_MESH = 1<<15,
LOAD_SOFT_BODY_SET_REPULSION_STIFFNESS = 1<<16,
LOAD_SOFT_BODY_SET_DAMPING_SPRING_MODE = 1<<17,
LOAD_SOFT_BODY_SET_GRAVITY_FACTOR = 1<<18,
};
enum EnumSimParamInternalSimFlags
@@ -549,6 +550,7 @@ struct LoadSoftBodyArgs
int m_useFaceContact;
char m_simFileName[MAX_FILENAME_LENGTH];
double m_repulsionStiffness;
double m_gravFactor;
};
struct b3LoadSoftBodyResultArgs