Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-06-08 08:13:55 +00:00
Code Issues Packages Projects Releases Wiki Activity

rotate the modes when the mesh has a initial transformation. new grasping example with motor WIP

Browse Source
This commit is contained in:
jingyuc
2021-09-09 18:35:07 -04:00
parent ce765a8a88
commit e62c42f54c
24 changed files with 1459 additions and 1218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -400,6 +400,8 @@ SET(BulletExampleBrowser_SRCS
../ReducedDeformableDemo/ReducedCollide.h
../ReducedDeformableDemo/ReducedGrasp.cpp
../ReducedDeformableDemo/ReducedGrasp.h
../ReducedDeformableDemo/ReducedMotorGrasp.cpp
../ReducedDeformableDemo/ReducedMotorGrasp.h
../Constraints/TestHingeTorque.cpp
../Constraints/TestHingeTorque.h
../Constraints/ConstraintDemo.cpp

2
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -78,6 +78,7 @@
#include "../ReducedDeformableDemo/FrictionSlope.h"
#include "../ReducedDeformableDemo/ReducedCollide.h"
#include "../ReducedDeformableDemo/ReducedGrasp.h"
#include "../ReducedDeformableDemo/ReducedMotorGrasp.h"
#include "../InverseKinematics/InverseKinematicsExample.h"
#ifdef B3_ENABLE_TINY_AUDIO
@@ -227,6 +228,7 @@ static ExampleEntry gDefaultExamples[] =
ExampleEntry(1, "Reduced Free Fall", "Free fall ground contact test for the reduced deformable model", ReducedFreeFallCreateFunc),
ExampleEntry(1, "Reduced Collision Test", "Collision between a reduced block and the a rigid block", ReducedCollideCreateFunc),
ExampleEntry(1, "Reduced Grasp", "Grasp a reduced deformable block", ReducedGraspCreateFunc),
ExampleEntry(1, "Reduced Motor Grasp", "Grasp a reduced deformable block with motor", ReducedMotorGraspCreateFunc),
ExampleEntry(1, "Reduced Friction Slope", "Grasp a reduced deformable block", FrictionSlopeCreateFunc),
// ExampleEntry(1, "Simple Reduced Deformable Test", "Simple dynamics test for the reduced deformable objects", ReducedBasicTestCreateFunc),

33
examples/ReducedDeformableDemo/FreeFall.cpp
View File

@@ -56,14 +56,14 @@ public:
void resetCamera()
{
float dist = 10;
float pitch = -20;
float yaw = 90;
float targetPos[3] = {0, 0, 0.5};
// float dist = 20;
// float pitch = -30;
// float yaw = 125;
// float targetPos[3] = {-2, 0, 2};
// float dist = 10;
// float pitch = -20;
// float yaw = 90;
// float targetPos[3] = {0, 0, 0.5};
float dist = 20;
float pitch = -30;
float yaw = 125;
float targetPos[3] = {-2, 0, 2};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
@@ -108,7 +108,6 @@ public:
{
deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_fixedNodes[p]].m_x, 0.2, btVector3(1, 0, 0));
}
// static int num = 0;
for (int p = 0; p < rsb->m_nodeRigidContacts.size(); ++p)
{
deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.2, btVector3(0, 1, 0));
@@ -158,13 +157,13 @@ void FreeFall::initPhysics()
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 2.5, 0));
// init_transform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 4.0));
// init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
init_transform.setOrigin(btVector3(0, 10, 0));
init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
// init_transform.setRotation(btQuaternion(SIMD_PI / 2.0, 0, 0));
rsb->transform(init_transform);
// rsb->setTotalMass(0.5);
rsb->setStiffnessScale(10);
rsb->setStiffnessScale(200);
rsb->setDamping(damping_alpha, damping_beta);
// rsb->setFriction(200);
@@ -272,15 +271,15 @@ void FreeFall::initPhysics()
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(true);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.3;
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
// add a few rigid bodies
Ctor_RbUpStack();
// Ctor_RbUpStack();
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
m_dynamicsWorld->setGravity(gravity);
}

8
examples/ReducedDeformableDemo/FrictionSlope.cpp
View File

@@ -82,9 +82,9 @@ public:
btTransform groundTransform;
groundTransform.setIdentity();
// groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0));
groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0));
groundTransform.setOrigin(btVector3(0, 0, 0));
btScalar mass(1e6);
btScalar mass(0);
btRigidBody* ground = createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
// ground->setFriction(1);
}
@@ -222,12 +222,12 @@ void FrictionSlope::initPhysics()
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(true);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.3;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.6;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
getDeformableDynamicsWorld()->setSolverCallback(FrictionSlopeHelper::groundMotion);
// getDeformableDynamicsWorld()->setSolverCallback(FrictionSlopeHelper::groundMotion);
m_dynamicsWorld->setGravity(gravity);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}

8
examples/ReducedDeformableDemo/ModeVisualizer.cpp
View File

@@ -119,7 +119,7 @@ void ModeVisualizer::initPhysics()
// create volumetric soft body
{
std::string filepath("../../../examples/SoftDemo/");
std::string filepath("../../../examples/SoftDemo/beam/");
std::string filename = filepath + "mesh.vtk";
btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str());
@@ -128,6 +128,12 @@ void ModeVisualizer::initPhysics()
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 2, 0));
init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
rsb->transform(init_transform);
btSoftBodyHelpers::generateBoundaryFaces(rsb);
}
getDeformableDynamicsWorld()->setImplicit(false);

173
examples/ReducedDeformableDemo/ReducedGrasp.cpp
View File

@@ -51,9 +51,13 @@ public:
void resetCamera()
{
float dist = 25;
float pitch = -30;
float yaw = 100;
float dist = 15;
float pitch = -10;
float yaw = 90;
// float dist = 25;
// float pitch = -30;
// float yaw = 100;
float targetPos[3] = {0, -0, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
@@ -61,26 +65,32 @@ public:
void stepSimulation(float deltaTime)
{
//use a smaller internal timestep, there are stability issues
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
// float internalTimeStep = 1. / 240.f;
// m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
float internalTimeStep = 1. / 60.f;
m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
}
void createGrip()
{
int count = 2;
float mass = 1e6;
btCollisionShape* shape[] = {
new btBoxShape(btVector3(3, 4, 0.5)),
};
static const int nshapes = sizeof(shape) / sizeof(shape[0]);
for (int i = 0; i < count; ++i)
btCollisionShape* shape = new btBoxShape(btVector3(3, 4, 0.5));
{
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(10, 0, 0));
startTransform.setOrigin(btVector3(0,4,3));
startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
createRigidBody(mass, startTransform, shape[i % nshapes]);
createRigidBody(mass, startTransform, shape);
}
{
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,4,-3));
startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
createRigidBody(mass, startTransform, shape);
}
}
void Ctor_RbUpStack()
@@ -90,7 +100,7 @@ public:
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,4,0));
startTransform.setOrigin(btVector3(0,9.5,0));
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
rb1->setLinearVelocity(btVector3(0, 0, 0));
rb1->setFriction(0.7);
@@ -103,10 +113,15 @@ public:
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(deformableWorld->getSoftBodyArray()[i]);
{
btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
}
for (int p = 0; p < rsb->m_nodeRigidContacts.size(); ++p)
{
deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.2, btVector3(0, 1, 0));
}
}
}
@@ -121,6 +136,7 @@ void ReducedGrasp::GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsW
return;
btRigidBody* rb0 = rbs[0];
// btScalar pressTime = 0.9;
// btScalar pressTime = 0.96;
btScalar pressTime = 1;
btScalar liftTime = 2.5;
btScalar shiftTime = 3.5;
@@ -146,37 +162,37 @@ void ReducedGrasp::GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsW
velocity = pinchVelocityLeft;
translation = initialTranslationLeft + pinchVelocityLeft * time;
}
else
{
velocity = pinchVelocityRight;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime;
}
// else if (time < liftTime)
// {
// velocity = liftVelocity;
// translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime);
// }
// else if (time < shiftTime)
// {
// velocity = shiftVelocity;
// translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
// }
// else if (time < holdTime)
// {
// velocity = btVector3(0,0,0);
// translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
// }
// else if (time < dropTime)
// {
// velocity = openVelocityLeft;
// translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime);
// }
// else
// {
// velocity = holdVelocity;
// translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime);
// velocity = pinchVelocityRight;
// translation = initialTranslationLeft + pinchVelocityLeft * pressTime;
// }
else if (time < liftTime)
{
velocity = liftVelocity;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime);
}
else if (time < shiftTime)
{
velocity = shiftVelocity;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
}
else if (time < holdTime)
{
velocity = btVector3(0,0,0);
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
}
else if (time < dropTime)
{
velocity = openVelocityLeft;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime);
}
else
{
velocity = holdVelocity;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime);
}
rbTransform.setOrigin(translation);
rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
rb0->setCenterOfMassTransform(rbTransform);
@@ -189,37 +205,37 @@ void ReducedGrasp::GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsW
velocity = pinchVelocityRight;
translation = initialTranslationRight + pinchVelocityRight * time;
}
else
{
velocity = pinchVelocityRight;
translation = initialTranslationRight + pinchVelocityRight * pressTime;
}
// else if (time < liftTime)
// {
// velocity = liftVelocity;
// translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (time - pressTime);
// }
// else if (time < shiftTime)
// {
// velocity = shiftVelocity;
// translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
// }
// else if (time < holdTime)
// {
// velocity = btVector3(0,0,0);
// translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
// }
// else if (time < dropTime)
// {
// velocity = openVelocityRight;
// translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime);
// }
// else
// {
// velocity = holdVelocity;
// translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime);
// velocity = pinchVelocityRight;
// translation = initialTranslationRight + pinchVelocityRight * pressTime;
// }
else if (time < liftTime)
{
velocity = liftVelocity;
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (time - pressTime);
}
else if (time < shiftTime)
{
velocity = shiftVelocity;
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
}
else if (time < holdTime)
{
velocity = btVector3(0,0,0);
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
}
else if (time < dropTime)
{
velocity = openVelocityRight;
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime);
}
else
{
velocity = holdVelocity;
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime);
}
rbTransform.setOrigin(translation);
rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
rb1->setCenterOfMassTransform(rbTransform);
@@ -242,7 +258,7 @@ void ReducedGrasp::initPhysics()
m_broadphase = new btDbvtBroadphase();
btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver();
btVector3 gravity = btVector3(0, 0, 0);
btVector3 gravity = btVector3(0, -10, 0);
reducedSoftBodySolver->setGravity(gravity);
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
@@ -266,15 +282,16 @@ void ReducedGrasp::initPhysics()
btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(rsb, filepath.c_str());
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
rsb->getCollisionShape()->setMargin(0.015);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 4, 0));
init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
// init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(10);
rsb->setStiffnessScale(200);
rsb->setDamping(damping_alpha, damping_beta);
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
@@ -297,10 +314,10 @@ void ReducedGrasp::initPhysics()
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(true);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
// grippers

536
examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp Normal file
View File

@@ -0,0 +1,536 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "ReducedMotorGrasp.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h"
#include "BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.h"
#include "BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonDeformableBodyBase.h"
#include "../Utils/b3ResourcePath.h"
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../Utils/b3BulletDefaultFileIO.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../CommonInterfaces/CommonFileIOInterface.h"
#include "Bullet3Common/b3FileUtils.h"
///The ReducedMotorGrasp shows grasping a volumetric deformable objects with multibody gripper with moter constraints.
static btScalar sGripperVerticalVelocity = 0.f;
static btScalar sGripperClosingTargetVelocity = 0.f;
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0;
static int start_mode = 6;
static int num_modes = 1;
static float friction = 1.;
struct TetraCube
{
#include "../SoftDemo/cube.inl"
};
struct TetraBunny
{
#include "../SoftDemo/bunny.inl"
};
static bool supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
{
bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute
|| mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic);
return canHaveMotor;
}
class ReducedMotorGrasp : public CommonDeformableBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
ReducedMotorGrasp(struct GUIHelperInterface* helper)
:CommonDeformableBodyBase(helper)
{
}
virtual ~ReducedMotorGrasp()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
// float dist = 0.3;
// float pitch = -45;
// float yaw = 100;
// float targetPos[3] = {0, -0.1, 0};
float dist = 8;
float pitch = -20;
float yaw = 100;
float targetPos[3] = {0, 0, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
btMultiBody* createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld,const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool floating);
void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
btMultiBody* createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating);
void stepSimulation(float deltaTime)
{
double fingerTargetVelocities[2] = {sGripperVerticalVelocity, sGripperClosingTargetVelocity};
int num_multiBody = getDeformableDynamicsWorld()->getNumMultibodies();
for (int i = 0; i < num_multiBody; ++i)
{
btMultiBody* mb = getDeformableDynamicsWorld()->btMultiBodyDynamicsWorld::getMultiBody(i);
mb->setBaseVel(btVector3(0,sGripperVerticalVelocity, 0));
int dofIndex = 6; //skip the 3 linear + 3 angular degree of freedom entries of the base
for (int link = 0; link < mb->getNumLinks(); link++)
{
if (supportsJointMotor(mb, link))
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
if (motor)
{
if (dofIndex == 6)
{
motor->setVelocityTarget(-fingerTargetVelocities[1], 1);
motor->setMaxAppliedImpulse(20);
}
if (dofIndex == 7)
{
motor->setVelocityTarget(fingerTargetVelocities[1], 1);
motor->setMaxAppliedImpulse(20);
}
motor->setMaxAppliedImpulse(1);
}
}
dofIndex += mb->getLink(link).m_dofCount;
}
}
//use a smaller internal timestep, there are stability issues
// float internalTimeStep = 1. / 240.f;
// m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
float internalTimeStep = 1. / 60.f;
m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
}
void createGrip()
{
int count = 2;
float mass = 2;
btCollisionShape* shape[] = {
new btBoxShape(btVector3(3, 3, 0.5)),
};
static const int nshapes = sizeof(shape) / sizeof(shape[0]);
for (int i = 0; i < count; ++i)
{
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(10, 0, 0));
startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
createRigidBody(mass, startTransform, shape[i % nshapes]);
}
}
virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
{
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
{
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(deformableWorld->getSoftBodyArray()[i]);
{
btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), fDrawFlags::Faces);// deformableWorld->getDrawFlags());
}
for (int p = 0; p < rsb->m_contactNodesList.size(); ++p)
{
int index = rsb->m_contactNodesList[p];
deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[index].m_x, 0.2, btVector3(0, 1, 0));
}
}
}
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{
return false;
}
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{
return false;
}
virtual void removePickingConstraint(){}
};
void ReducedMotorGrasp::initPhysics()
{
m_guiHelper->setUpAxis(1);
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver();
// btVector3 gravity = btVector3(0, 0, 0);
btVector3 gravity = btVector3(0, -9.81, 0);
reducedSoftBodySolver->setGravity(gravity);
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(reducedSoftBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
// getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.1;
// getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0;
// getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 150;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_maxPickingForce = 0.001;
// build a gripper
{
bool damping = true;
bool gyro = false;
bool canSleep = false;
bool selfCollide = true;
int numLinks = 2;
// btVector3 linkHalfExtents(0.02, 0.018, .003);
// btVector3 baseHalfExtents(0.02, 0.002, .002);
btVector3 linkHalfExtents(0.5, 0.5, 0.1);
btVector3 baseHalfExtents(0.5, 0.1, 0.1);
// btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), btVector3(0.f, 0.05f,0.f), linkHalfExtents, baseHalfExtents, false);
btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), btVector3(0.f, 3.f,0.f), linkHalfExtents, baseHalfExtents, false);
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
for (int i = 0; i < numLinks; i++)
{
int mbLinkIndex = i;
double maxMotorImpulse = 1;
if (supportsJointMotor(mbC, mbLinkIndex))
{
int dof = 0;
btScalar desiredVelocity = 0.f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mbC, mbLinkIndex, dof, desiredVelocity, maxMotorImpulse);
motor->setPositionTarget(0, 0);
motor->setVelocityTarget(0, 1);
mbC->getLink(mbLinkIndex).m_userPtr = motor;
getDeformableDynamicsWorld()->addMultiBodyConstraint(motor);
motor->finalizeMultiDof();
}
}
if (!damping)
{
mbC->setLinearDamping(0.0f);
mbC->setAngularDamping(0.0f);
}
else
{
mbC->setLinearDamping(0.04f);
mbC->setAngularDamping(0.04f);
}
btScalar q0 = 0.f * SIMD_PI / 180.f;
if (numLinks > 0)
mbC->setJointPosMultiDof(0, &q0);
addColliders(mbC, getDeformableDynamicsWorld(), baseHalfExtents, linkHalfExtents);
}
//create a ground
{
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(10.)));
groundShape->setMargin(0.001);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -5.1, 0));
groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
groundShape->calculateLocalInertia(mass, localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setFriction(0.5);
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body,1,1+2);
}
// create volumetric reduced deformable body
{
std::string filepath("../../../examples/SoftDemo/beam/");
std::string filename = filepath + "mesh.vtk";
btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str());
rsb->setReducedModes(start_mode, num_modes, rsb->m_nodes.size());
btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(rsb, filepath.c_str());
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 0.25, 0));
init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(100);
rsb->setDamping(damping_alpha, damping_beta);
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
rsb->m_cfg.kDF = 0;
rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
rsb->m_sleepingThreshold = 0;
btSoftBodyHelpers::generateBoundaryFaces(rsb);
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.3;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
{
SliderParams slider("Moving velocity", &sGripperVerticalVelocity);
// slider.m_minVal = -.02;
// slider.m_maxVal = .02;
slider.m_minVal = -.2;
slider.m_maxVal = .2;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity);
slider.m_minVal = -1;
slider.m_maxVal = 1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
}
void ReducedMotorGrasp::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
removePickingConstraint();
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete forces
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
btMultiBody* ReducedMotorGrasp::createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool floating)
{
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 0.1;
float linkMass = 0.1;
int numLinks = 2;
if (baseMass)
{
btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
delete pTempBox;
}
bool canSleep = false;
btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
delete pTempBox;
//y-axis assumed up
btAlignedObjectArray<btVector3> parentComToCurrentCom;
parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 8.f, -baseHalfExtents[2] * 2.f));
parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 8.f, +baseHalfExtents[2] * 2.f));//par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1]*8.f, 0); //cur body's COM to cur body's PIV offset
btAlignedObjectArray<btVector3> parentComToCurrentPivot;
parentComToCurrentPivot.push_back(btVector3(parentComToCurrentCom[0] - currentPivotToCurrentCom));
parentComToCurrentPivot.push_back(btVector3(parentComToCurrentCom[1] - currentPivotToCurrentCom));//par body's COM to cur body's PIV offset
//////
btScalar q0 = 0.f * SIMD_PI / 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
for (int i = 0; i < numLinks; ++i)
{
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot[i], currentPivotToCurrentCom, true);
}
pMultiBody->finalizeMultiDof();
///
pWorld->addMultiBody(pMultiBody);
///
return pMultiBody;
}
void ReducedMotorGrasp::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
{
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
{
btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
if (1)
{
btCollisionShape* box = new btBoxShape(baseHalfExtents);
box->setMargin(0.001);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(local_origin[0]);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
pWorld->addCollisionObject(col, 2, 1 + 2);
col->setFriction(friction);
pMultiBody->setBaseCollider(col);
}
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i + 1];
btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()};
btCollisionShape* box = new btBoxShape(linkHalfExtents);
box->setMargin(0.001);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
col->setFriction(friction);
pWorld->addCollisionObject(col, 2, 1 + 2);
pMultiBody->getLink(i).m_collider = col;
}
}
class CommonExampleInterface* ReducedMotorGraspCreateFunc(struct CommonExampleOptions& options)
{
return new ReducedMotorGrasp(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/ReducedMotorGrasp.h Normal file
View File

@@ -0,0 +1,19 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef _REDUCED_MOTOR_GRASP
#define _REDUCED_MOTOR_GRASP
class CommonExampleInterface* ReducedMotorGraspCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_MOTOR_GRASP

BIN
examples/SoftDemo/K_r_diag_mat.bin
View File

Binary file not shown.

BIN
examples/SoftDemo/M_diag_mat.bin
View File

Binary file not shown.

BIN
examples/SoftDemo/M_r_diag_mat.bin
View File

Binary file not shown.

BIN
examples/SoftDemo/eigenvalues.bin
View File

Binary file not shown.

83
examples/SoftDemo/mesh.vtk
View File

@@ -1,83 +0,0 @@
#vtk DataFile Version 2.0
I don't think this matters
ASCII
DATASET UNSTRUCTURED_GRID
POINTS 28 double
-0.5000000000000000 -0.2500000000000000 -2.0000000000000000
-0.5000000000000000 0.2500000000000000 -2.0000000000000000
0.5000000000000000 -0.2500000000000000 -2.0000000000000000
0.5000000000000000 0.2500000000000000 -2.0000000000000000
-0.5000000000000000 -0.2500000000000000 2.0000000000000000
-0.5000000000000000 0.2500000000000000 2.0000000000000000
0.5000000000000000 -0.2500000000000000 2.0000000000000000
0.5000000000000000 0.2500000000000000 2.0000000000000000
-0.5000000000000000 -0.2500000000000000 1.0000000000000000
-0.5000000000000000 -0.2500000000000000 0.0000000000000000
-0.5000000000000000 -0.2500000000000000 -1.0000000000000000
0.5000000000000000 -0.2500000000000000 -1.0000000000000000
0.5000000000000000 -0.2500000000000000 0.0000000000000000
0.5000000000000000 -0.2500000000000000 1.0000000000000000
-0.5000000000000000 0.2500000000000000 1.0000000000000000
-0.5000000000000000 0.2500000000000000 0.0000000000000000
-0.5000000000000000 0.2500000000000000 -1.0000000000000000
0.5000000000000000 0.2500000000000000 1.0000000000000000
0.5000000000000000 0.2500000000000000 0.0000000000000000
0.5000000000000000 0.2500000000000000 -1.0000000000000000
0.0000000000000000 -0.2500000000000000 -1.5000000000000000
0.0000000000000000 -0.2500000000000000 -0.5000000000000000
0.0000000000000000 -0.2500000000000000 0.5000000000000000
0.0000000000000000 -0.2500000000000000 1.5000000000000000
0.0000000000000000 0.2500000000000000 1.5000000000000000
0.0000000000000000 0.2500000000000000 0.5000000000000000
0.0000000000000000 0.2500000000000000 -0.5000000000000000
0.0000000000000000 0.2500000000000000 -1.5000000000000000
CELLS 48 192
4 22 12 17 13
4 20 26 21 11
4 23 8 5 24
4 11 27 26 19
4 27 11 26 20
4 17 22 24 25
4 7 23 13 6
4 22 17 24 13
4 9 25 21 15
4 24 22 8 14
4 24 8 22 23
4 25 9 21 22
4 23 7 13 24
4 15 10 16 26
4 14 25 9 15
4 25 14 9 22
4 24 23 22 13
4 15 10 21 9
4 15 21 10 26
4 24 25 22 14
4 2 11 27 20
4 7 5 23 6
4 11 21 12 26
4 20 1 0 16
4 18 25 12 17
4 18 12 25 26
4 5 7 23 24
4 26 21 25 15
4 20 16 27 1
4 21 25 22 12
4 20 16 10 26
4 20 27 16 26
4 20 10 16 0
4 11 12 19 26
4 14 22 8 9
4 8 23 5 4
4 24 14 8 5
4 11 2 27 3
4 2 27 1 20
4 12 18 19 26
4 27 2 1 3
4 24 13 17 7
4 20 2 0 1
4 5 23 6 4
4 20 10 21 26
4 12 22 17 25
4 3 27 11 19
4 21 26 25 12
CELL_TYPES 10

BIN
examples/SoftDemo/modes.bin
View File

Binary file not shown.

BIN
examples/SoftDemo/sponge/K_r_diag_mat.bin
View File

Binary file not shown.

BIN
examples/SoftDemo/sponge/M_diag_mat.bin
View File

Binary file not shown.

BIN
examples/SoftDemo/sponge/M_r_diag_mat.bin
View File

Binary file not shown.

BIN
examples/SoftDemo/sponge/eigenvalues.bin
View File

Binary file not shown.

1717
examples/SoftDemo/sponge/mesh.vtk
View File

File diff suppressed because it is too large Load Diff

BIN
examples/SoftDemo/sponge/modes.bin
View File

Binary file not shown.

43
src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp
View File

@@ -56,15 +56,7 @@ btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstr
m_erp = infoGlobal.m_deformable_erp;
m_friction = infoGlobal.m_friction;
btRigidBody* rb = m_contact->m_cti.m_colObj ? (btRigidBody*)btRigidBody::upcast(m_contact->m_cti.m_colObj) : nullptr;
if (!rb)
{
btAssert(false);
}
else
{
m_collideStatic = rb->isStaticObject();
}
m_collideStatic = m_contact->m_cti.m_colObj->isStaticObject();
}
void btReducedDeformableRigidContactConstraint::setSolverBody(btSolverBody& solver_body)
@@ -230,23 +222,22 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btAssert(false); //TODO: unsupported yet
// btMultiBodyLinkCollider* multibodyLinkCol = 0;
// multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
// if (multibodyLinkCol)
// {
// const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
// // apply normal component of the impulse
// multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
// if (impulse_tangent.norm() > SIMD_EPSILON)
// {
// // apply tangential component of the impulse
// const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
// multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1));
// const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
// multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2));
// }
// }
btMultiBodyLinkCollider* multibodyLinkCol = 0;
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
// apply normal component of the impulse
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
if (impulse_tangent.norm() > SIMD_EPSILON)
{
// apply tangential component of the impulse
const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1));
const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2));
}
}
}
}
return residualSquare;

45
src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp
View File

@@ -8,7 +8,7 @@
btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
: btSoftBody(worldInfo, node_count, x, m)
{
m_rigidOnly = true; //! only use rigid frame to debug
m_rigidOnly = false; //! only use rigid frame to debug
// reduced deformable
m_reducedModel = true;
@@ -27,7 +27,6 @@ btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_co
m_angularFactor.setValue(1, 1, 1);
m_linearFactor.setValue(1, 1, 1);
m_invInertiaLocal.setValue(1, 1, 1);
// m_invInertiaLocal.setZero();
m_mass = 0.0;
m_inverseMass = 0.0;
@@ -88,6 +87,7 @@ void btReducedSoftBody::setInertiaProps(const btVector3& inertia)
btMatrix3x3 rotation;
rotation.setIdentity();
updateInitialInertiaTensor(rotation);
// updateInitialInertiaTensorFromNodes();
updateInertiaTensor();
m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
}
@@ -358,7 +358,10 @@ void btReducedSoftBody::transform(const btTransform& trs)
// update inertia tensor
updateInitialInertiaTensor(trs.getBasis());
// updateInitialInertiaTensorFromNodes();
updateInertiaTensor();
// update modes
updateModesByRotation(trs.getBasis());
}
void btReducedSoftBody::updateRestNodalPositions()
@@ -371,11 +374,47 @@ void btReducedSoftBody::updateRestNodalPositions()
}
}
void btReducedSoftBody::updateInitialInertiaTensorFromNodes()
{
btMatrix3x3 inertia_tensor;
inertia_tensor.setZero();
for (int p = 0; p < m_nFull; ++p)
{
btVector3 r = m_nodes[p].m_x - m_initialOrigin;
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
inertia_tensor[i][j] += m_nodalMass[p] * r[i] * r[j];
}
}
}
m_invInertiaTensorWorldInitial = inertia_tensor.inverse();
}
void btReducedSoftBody::updateInitialInertiaTensor(const btMatrix3x3& rotation)
{
m_invInertiaTensorWorldInitial = rotation.scaled(m_invInertiaLocal) * rotation.transpose();
}
void btReducedSoftBody::updateModesByRotation(const btMatrix3x3& rotation)
{
for (int r = 0; r < m_nReduced; ++r)
{
for (int i = 0; i < m_nFull; ++i)
{
btVector3 nodal_disp(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
nodal_disp = rotation * nodal_disp;
for (int k = 0; k < 3; ++k)
{
m_modes[r][3 * i + k] = nodal_disp[k];
}
}
}
}
void btReducedSoftBody::updateInertiaTensor()
{
m_invInertiaTensorWorld = m_rigidTransformWorld.getBasis() * m_invInertiaTensorWorldInitial * m_rigidTransformWorld.getBasis().transpose();
@@ -533,7 +572,7 @@ void btReducedSoftBody::internalApplyFullSpaceImpulse(const btVector3& impulse,
applyFullSpaceNodalForce(impulse / dt, n_node);
// update reduced internal force
applyReducedDampingForce(m_reducedVelocity);
applyReducedDampingForce(m_reducedVelocity); //TODO: this needs to be the current velocity
// delta reduced velocity
for (int r = 0; r < m_nReduced; ++r)

6
src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h
View File

@@ -136,12 +136,18 @@ class btReducedSoftBody : public btSoftBody
btAssert(false); // scale is NOT supported in the reduced deformable body
}
private:
void updateRestNodalPositions();
void updateInitialInertiaTensor(const btMatrix3x3& rotation);
void updateInitialInertiaTensorFromNodes();
void updateInertiaTensor();
void updateModesByRotation(const btMatrix3x3& rotation);
public:
void updateLocalMomentArm();
void predictIntegratedTransform(btScalar dt, btTransform& predictedTransform);

2
src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp
View File

@@ -135,7 +135,7 @@ void btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(btReducedSoftB
// calculate the inertia tensor in the local frame
btVector3 inertia(0, 0, 0);
calculateLocalInertia(inertia, rsb->getTotalMass(), btVector3(1, 0.5, 4), btVector3(0, 0, 0));
calculateLocalInertia(inertia, rsb->getTotalMass(), btVector3(0.5, 0.25, 2), btVector3(0, 0, 0));
// calculateLocalInertia(inertia, rsb->getTotalMass(), btVector3(0.5, 0.5, 0.5), btVector3(0, 0, 0));
rsb->setInertiaProps(inertia);