AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-12 21:00:11 +00:00
Code Issues Releases Wiki Activity

add cfm and penetration correction to the velocity error for the contact constraint. clean up btReducedSoftBody class. momenumtum conservation WIP

Browse Source
This commit is contained in:
jingyuc 2021-10-07 15:10:57 -04:00
parent 1949710131
commit 07cfa9a433
20 changed files with 1585 additions and 3324 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/ExampleBrowser/CMakeLists.txt
View File

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

2
examples/ExampleBrowser/ExampleEntries.cpp
View File

@ -79,6 +79,7 @@
#include "../ReducedDeformableDemo/FrictionSlope.h"
#include "../ReducedDeformableDemo/ReducedCollide.h"
#include "../ReducedDeformableDemo/ReducedGrasp.h"
#include "../ReducedDeformableDemo/ReducedPress.h"
#include "../ReducedDeformableDemo/ReducedMotorGrasp.h"
#include "../InverseKinematics/InverseKinematicsExample.h"
@ -230,6 +231,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 Press", "Press a reduced deformable block against the ground", ReducedPressCreateFunc),
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),

3
examples/ReducedDeformableDemo/ConservationTest.cpp
View File

@ -29,7 +29,7 @@
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0;
static int start_mode = 6;
static int num_modes = 20;
static int num_modes = 1;
class ConservationTest : public CommonDeformableBodyBase
{
@ -217,6 +217,7 @@ void ConservationTest::initPhysics()
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;

9
examples/ReducedDeformableDemo/FreeFall.cpp
View File

@ -151,15 +151,15 @@ void FreeFall::initPhysics()
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.01);
rsb->scale(btVector3(1, 1, 0.5));
// rsb->scale(btVector3(1, 1, 0.5));
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 10, 0));
init_transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 4.0));
// init_transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 4.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(20);
rsb->setStiffnessScale(100);
rsb->setDamping(damping_alpha, damping_beta);
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
@ -175,7 +175,7 @@ void FreeFall::initPhysics()
// rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
}
// add a few rigid bodies
// Ctor_RbUpStack();
Ctor_RbUpStack();
// create a static rigid box as the ground
{
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
@ -263,6 +263,7 @@ void FreeFall::initPhysics()
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
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;

21
examples/ReducedDeformableDemo/ModeVisualizer.cpp
View File

@ -28,7 +28,7 @@
static int start_mode = 6;
static int num_modes = 20;
static int num_modes = 40;
static btScalar visualize_mode = 0;
static btScalar frequency_scale = 1;
@ -44,6 +44,19 @@ class ModeVisualizer : public CommonDeformableBodyBase
rsb->m_nodes[i].m_x[k] = rsb->m_x0[i][k] + rsb->m_modes[mode_n][3 * i + k] * time_term;
}
btVector3 computeMassWeightedColumnSum(btReducedSoftBody* rsb, const int mode_n)
{
btVector3 sum(0, 0, 0);
for (int i = 0; i < rsb->m_nodes.size(); ++i)
{
for (int k = 0; k < 3; ++k)
{
sum[k] += rsb->m_nodalMass[i] * rsb->m_modes[mode_n][3 * i + k];
}
}
return sum;
}
public:
ModeVisualizer(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
@ -79,6 +92,10 @@ public:
int n_mode = floor(visualize_mode);
btScalar scale = sin(sqrt(rsb->m_eigenvalues[n_mode]) * sim_time / frequency_scale);
getDeformedShape(rsb, n_mode, scale);
btVector3 mass_weighted_column_sum = computeMassWeightedColumnSum(rsb, visualize_mode);
std::cout << "mode=" << int(visualize_mode) << "\t" << mass_weighted_column_sum[0] << "\t"
<< mass_weighted_column_sum[1] << "\t"
<< mass_weighted_column_sum[2] << "\n";
}
virtual void renderScene()
@ -127,7 +144,7 @@ void ModeVisualizer::initPhysics()
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));
// init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
rsb->transform(init_transform);
btSoftBodyHelpers::generateBoundaryFaces(rsb);
}

8
examples/ReducedDeformableDemo/ReducedCollide.cpp
View File

@ -75,7 +75,7 @@ public:
void Ctor_RbUpStack()
{
float mass = 55;
float mass = 28;
// float mass = 8;
btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
@ -209,7 +209,7 @@ void ReducedCollide::initPhysics()
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
rsb->scale(btVector3(1, 1, 1));
// rsb->scale(btVector3(0.5, 0.5, 0.5));
btTransform init_transform;
init_transform.setIdentity();
@ -217,7 +217,7 @@ void ReducedCollide::initPhysics()
// init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(20);
rsb->setStiffnessScale(100);
rsb->setDamping(damping_alpha, damping_beta);
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
@ -230,6 +230,7 @@ void ReducedCollide::initPhysics()
rsb->setRigidVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
// rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
b3Printf("total mass: %e", rsb->getTotalMass());
}
// rigidBar();
@ -298,6 +299,7 @@ void ReducedCollide::initPhysics()
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;

55
examples/ReducedDeformableDemo/ReducedGrasp.cpp
View File

@ -30,8 +30,7 @@
// static btScalar E = 50;
// static btScalar nu = 0.3;
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.001;
static btScalar COLLIDING_VELOCITY = 4;
static btScalar damping_beta = 0.0;
static int start_mode = 6;
static int num_modes = 20;
@ -58,17 +57,17 @@ public:
// float dist = 25;
// float pitch = -30;
// float yaw = 100;
float targetPos[3] = {0, 0, 1};
float targetPos[3] = {0, 0, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
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. / 60.f;
m_dynamicsWorld->stepSimulation(deltaTime, 1, 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()
@ -79,14 +78,14 @@ public:
{
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,1,2));
startTransform.setOrigin(btVector3(0,1,0));
startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
createRigidBody(mass, startTransform, shape);
}
{
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,1,-2));
startTransform.setOrigin(btVector3(0,1,-4));
startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
createRigidBody(mass, startTransform, shape);
}
@ -119,10 +118,10 @@ public:
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));
// }
for (int p = 0; p < rsb->m_nodeRigidContacts.size(); ++p)
{
deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.1, btVector3(0, 1, 0));
}
}
}
@ -137,18 +136,20 @@ void ReducedGrasp::GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsW
btRigidBody* rb0 = rbs[0];
// btScalar pressTime = 0.9;
// btScalar pressTime = 0.96;
btScalar pressTime = 1.25;
btScalar pressTime = 1.26;
btScalar liftTime = 2.5;
btScalar shiftTime = 3.5;
btScalar holdTime = 4.5;
btScalar dropTime = 5.3;
btScalar shiftTime = 6;
btScalar holdTime = 7;
btScalar dropTime = 10;
// btScalar holdTime = 500;
// btScalar dropTime = 1000;
btTransform rbTransform;
rbTransform.setIdentity();
btVector3 translation;
btVector3 velocity;
btVector3 initialTranslationLeft = btVector3(0,1,2); // inner face has z=2
btVector3 initialTranslationRight = btVector3(0,1,-2); // inner face has z=-2
btVector3 initialTranslationLeft = btVector3(0,1,0); // inner face has z=2
btVector3 initialTranslationRight = btVector3(0,1,-4); // inner face has z=-2
btVector3 pinchVelocityLeft = btVector3(0,0,-1);
btVector3 pinchVelocityRight = btVector3(0,0,1);
btVector3 liftVelocity = btVector3(0,4,0);
@ -164,14 +165,13 @@ void ReducedGrasp::GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsW
}
// else
// {
// velocity = pinchVelocityRight;
// velocity = btVector3(0, 0, 0);
// translation = initialTranslationLeft + pinchVelocityLeft * pressTime;
// }
else if (time < liftTime)
{
velocity = liftVelocity;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime);
}
else if (time < shiftTime)
{
@ -207,14 +207,13 @@ void ReducedGrasp::GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsW
}
// else
// {
// velocity = pinchVelocityRight;
// velocity = btVector3(0, 0, 0);
// translation = initialTranslationRight + pinchVelocityRight * pressTime;
// }
else if (time < liftTime)
{
velocity = liftVelocity;
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (time - pressTime);
}
else if (time < shiftTime)
{
@ -281,12 +280,12 @@ void ReducedGrasp::initPhysics()
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.setOrigin(btVector3(0, 1, -2));
// 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(25);
rsb->setStiffnessScale(100);
rsb->setDamping(damping_alpha, damping_beta);
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
@ -297,14 +296,14 @@ void ReducedGrasp::initPhysics()
rsb->m_sleepingThreshold = 0;
btSoftBodyHelpers::generateBoundaryFaces(rsb);
// rsb->setRigidVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
// rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(true);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;

5
examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp
View File

@ -39,7 +39,7 @@
static btScalar sGripperVerticalVelocity = 0.f;
static btScalar sGripperClosingTargetVelocity = 0.f;
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.001;
static btScalar damping_beta = 0.0;
static int start_mode = 6;
static int num_modes = 20;
static float friction = 1.;
@ -328,7 +328,7 @@ void ReducedMotorGrasp::initPhysics()
// init_transform.setRotation(btQuaternion(SIMD_PI / 2.0, 0, SIMD_PI / 2.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(25);
rsb->setStiffnessScale(100);
rsb->setDamping(damping_alpha, damping_beta);
// rsb->setRigidVelocity(btVector3(0, 1, 0));
@ -348,6 +348,7 @@ void ReducedMotorGrasp::initPhysics()
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-6;

328
examples/ReducedDeformableDemo/ReducedPress.cpp Normal file
View File

@ -0,0 +1,328 @@
/*
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 "ReducedPress.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 "../CommonInterfaces/CommonParameterInterface.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonDeformableBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The BasicTest shows the contact between volumetric deformable objects and rigid objects.
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0;
static int start_mode = 6;
static int num_modes = 20;
class ReducedPress : public CommonDeformableBodyBase
{
public:
ReducedPress(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{
}
virtual ~ReducedPress()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
float dist = 5;
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]);
}
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. / 60.f;
// m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
}
void createGrip()
{
int count = 2;
float mass = 1e6;
btCollisionShape* shape = new btBoxShape(btVector3(1, 0.25, 1));
{
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,2,0));
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
createRigidBody(mass, startTransform, shape);
}
// {
// btTransform startTransform;
// startTransform.setIdentity();
// startTransform.setOrigin(btVector3(0,1,-2));
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
// createRigidBody(mass, startTransform, shape);
// }
}
void Ctor_RbUpStack()
{
float mass = 8;
btCollisionShape* shape = new btBoxShape(btVector3(0.25, 2, 0.5));
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,9.5,0));
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
rb1->setLinearVelocity(btVector3(0, 0, 0));
rb1->setFriction(0.7);
}
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(), 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.1, btVector3(0, 1, 0));
}
}
}
static void GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
};
void ReducedPress::GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsWorld* world)
{
btAlignedObjectArray<btRigidBody*>& rbs = world->getNonStaticRigidBodies();
if (rbs.size()>1)
return;
btRigidBody* rb0 = rbs[0];
btScalar pressTime = 0.75;
btTransform rbTransform;
rbTransform.setIdentity();
btVector3 translation;
btVector3 velocity;
btVector3 initialTranslationLeft = btVector3(0,2,0); // inner face has z=2
btVector3 pinchVelocityLeft = btVector3(0,-1,0);
if (time < pressTime)
{
velocity = pinchVelocityLeft;
translation = initialTranslationLeft + pinchVelocityLeft * time;
}
else
{
velocity = btVector3(0, 0, 0);
translation = initialTranslationLeft + pinchVelocityLeft * pressTime;
}
rbTransform.setOrigin(translation);
// rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
rb0->setCenterOfMassTransform(rbTransform);
rb0->setAngularVelocity(btVector3(0,0,0));
rb0->setLinearVelocity(velocity);
rb0->setFriction(20);
}
void ReducedPress::initPhysics()
{
m_guiHelper->setUpAxis(1);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver();
btVector3 gravity = btVector3(0, 0, 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()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
getDeformableDynamicsWorld()->setSolverCallback(GripperDynamics);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// create volumetric reduced deformable body
{
btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedCube(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.015);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 1, 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(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);
// rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(true);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
// grippers
createGrip();
// rigid block
// Ctor_RbUpStack();
// {
// float mass = 10;
// btCollisionShape* shape = new btBoxShape(btVector3(0.25, 2, 0.5));
// btTransform startTransform;
// startTransform.setIdentity();
// startTransform.setOrigin(btVector3(0,4,0));
// btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
// rb1->setLinearVelocity(btVector3(0, 0, 0));
// }
//create a ground
{
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -25, 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);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void ReducedPress::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;
}
class CommonExampleInterface* ReducedPressCreateFunc(struct CommonExampleOptions& options)
{
return new ReducedPress(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/ReducedPress.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_PRESS_H
#define _REDUCED_PRESS_H
class CommonExampleInterface* ReducedPressCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_PRESS_H

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

Binary file not shown.

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

Binary file not shown.

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

Binary file not shown.

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

Binary file not shown.

4305
examples/SoftDemo/cube/mesh.vtk
View File

File diff suppressed because it is too large Load Diff

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

Binary file not shown.

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

@ -57,8 +57,10 @@ btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstr
m_appliedTangentImpulse = 0;
m_rhs = 0;
m_rhs_tangent = 0;
m_cfm = 0;
m_cfm = infoGlobal.m_deformable_cfm;
m_cfm_friction = 0;
m_erp = infoGlobal.m_deformable_erp;
m_erp_friction = infoGlobal.m_deformable_erp;
m_friction = infoGlobal.m_friction;
m_collideStatic = m_contact->m_cti.m_colObj->isStaticObject();
@ -123,7 +125,7 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
// }
// get the normal impulse to be applied
btScalar deltaImpulse = m_rhs - deltaV_rel_normal / m_normalImpulseFactor;
btScalar deltaImpulse = m_rhs - m_appliedNormalImpulse * m_cfm - deltaV_rel_normal / m_normalImpulseFactor;
// if (!m_collideStatic)
// {
// std::cout << "m_rhs: " << m_rhs << '\t' << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << "\n";
@ -284,7 +286,7 @@ void btReducedDeformableRigidContactConstraint::calculateTangentialImpulse(btSca
{
btScalar deltaV_rel_tangent = btDot(deltaV_rel, tangent);
btScalar impulse_changed = deltaV_rel_tangent * tangentImpulseFactorInv;
deltaImpulse_tangent = rhs_tangent - impulse_changed;
deltaImpulse_tangent = rhs_tangent - m_cfm_friction * appliedImpulse - impulse_changed;
btScalar sum = appliedImpulse + deltaImpulse_tangent;
if (sum > upper_limit)
@ -387,7 +389,7 @@ void btReducedDeformableNodeRigidContactConstraint::warmStarting()
btScalar position_error = 0;
if (m_penetration > 0)
{
// velocity_error += m_penetration / m_dt; // TODO: why?
velocity_error += m_penetration / m_dt;
}
else
{

2
src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h
View File

@ -53,7 +53,9 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac
btScalar m_rhs_tangent2;
btScalar m_cfm;
btScalar m_cfm_friction;
btScalar m_erp;
btScalar m_erp_friction;
btScalar m_friction;
btVector3 m_contactNormalA; // surface normal for rigid body (opposite direction as impulse)

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

@ -24,6 +24,8 @@ btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_co
m_angularVelocity.setZero();
m_internalDeltaLinearVelocity.setZero();
m_internalDeltaAngularVelocity.setZero();
m_angularVelocityFromReduced.setZero();
m_internalDeltaAngularVelocityFromReduced.setZero();
m_angularFactor.setValue(1, 1, 1);
m_linearFactor.setValue(1, 1, 1);
// m_invInertiaLocal.setValue(1, 1, 1);
@ -290,6 +292,38 @@ void btReducedSoftBody::updateReducedVelocity(btScalar solverdt, bool explicit_f
void btReducedSoftBody::mapToFullVelocity(const btTransform& ref_trans)
{
// compute the reduced contribution to the angular velocity
btVector3 sum_linear(0, 0, 0);
btVector3 sum_angular(0, 0, 0);
m_linearVelocityFromReduced.setZero();
m_angularVelocityFromReduced.setZero();
// for (int i = 0; i < m_nFull; ++i)
// {
// btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[i];
// btMatrix3x3 r_star = Cross(r_com);
// btVector3 v_from_reduced(0, 0, 0);
// for (int k = 0; k < 3; ++k)
// {
// for (int r = 0; r < m_nReduced; ++r)
// {
// v_from_reduced[k] += m_modes[r][3 * i + k] * m_reducedVelocity[r];
// }
// }
// btVector3 delta_linear = m_nodalMass[i] * v_from_reduced;
// btVector3 delta_angular = m_nodalMass[i] * (r_star * ref_trans.getBasis() * v_from_reduced);
// sum_linear += delta_linear;
// sum_angular += delta_angular;
// std::cout << "delta_linear: " << delta_linear[0] << "\t" << delta_linear[1] << "\t" << delta_linear[2] << "\n";
// std::cout << "delta_angular: " << delta_angular[0] << "\t" << delta_angular[1] << "\t" << delta_angular[2] << "\n";
// std::cout << "sum_linear: " << sum_linear[0] << "\t" << sum_linear[1] << "\t" << sum_linear[2] << "\n";
// std::cout << "sum_angular: " << sum_angular[0] << "\t" << sum_angular[1] << "\t" << sum_angular[2] << "\n";
// }
// m_linearVelocityFromReduced = 1.0 / m_mass * (ref_trans.getBasis() * sum_linear);
// m_angularVelocityFromReduced = m_interpolateInvInertiaTensorWorld * sum_angular;
// std::cout << "-----end here------\n";
for (int i = 0; i < m_nFull; ++i)
{
m_nodes[i].m_v = computeNodeFullVelocity(ref_trans, i);
@ -309,9 +343,9 @@ const btVector3 btReducedSoftBody::computeNodeFullVelocity(const btTransform& re
}
}
// get new velocity
btVector3 vel = m_angularVelocity.cross(r_com) +
btVector3 vel = (m_angularVelocity - m_angularVelocityFromReduced).cross(r_com) +
ref_trans.getBasis() * v_from_reduced +
m_linearVelocity;
m_linearVelocity - m_linearVelocityFromReduced;
return vel;
}
@ -506,18 +540,6 @@ void btReducedSoftBody::internalApplyRigidImpulse(const btVector3& impulse, cons
m_internalDeltaAngularVelocity += m_interpolateInvInertiaTensorWorld * torque * m_angularFactor;
}
void btReducedSoftBody::applyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos)
{
if (m_inverseMass != btScalar(0.))
{
applyCentralImpulse(impulse);
if (m_angularFactor)
{
applyTorqueImpulse(rel_pos.cross(impulse * m_linearFactor));
}
}
}
btVector3 btReducedSoftBody::getRelativePos(int n_node)
{
btMatrix3x3 rotation = m_interpolationWorldTransform.getBasis();
@ -585,35 +607,6 @@ btMatrix3x3 btReducedSoftBody::getImpulseFactor(int n_node)
return m_rigidOnly ? K1 : K1 + K2;
}
void btReducedSoftBody::applyVelocityConstraint(const btVector3& target_vel, int n_node, btScalar dt)
{
// get impulse
btMatrix3x3 impulse_factor = getImpulseFactor(n_node);
btVector3 impulse = impulse_factor.inverse() * (target_vel - m_nodes[n_node].m_v);
// btScalar impulse_magnitude = impulse.norm();
// if (impulse_magnitude < 5e-7)
// {
// impulse.setZero();
// impulse_magnitude = 0;
// }
// relative position
btMatrix3x3 rotation = m_interpolationWorldTransform.getBasis();
btVector3 ri = rotation * m_localMomentArm[n_node];
// apply full space impulse
applyFullSpaceImpulse(impulse, ri, n_node, dt);
}
void btReducedSoftBody::applyPositionConstraint(const btVector3& target_pos, int n_node, btScalar dt)
{
btVector3 impulse(0, 0, 0);
// apply full space impulse
// applyFullSpaceImpulse(impulse, n_node, dt);
}
void btReducedSoftBody::internalApplyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt)
{
if (!m_rigidOnly)
@ -635,34 +628,6 @@ void btReducedSoftBody::internalApplyFullSpaceImpulse(const btVector3& impulse,
internalApplyRigidImpulse(impulse, rel_pos);
}
void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt)
{
if (!m_rigidOnly)
{
// apply impulse force
applyFullSpaceNodalForce(impulse / dt, n_node);
// update reduced internal force
applyReducedDampingForce(m_reducedVelocity);
// update reduced velocity
updateReducedVelocity(dt); // TODO: add back
}
// // update reduced dofs
// updateReducedDofs(dt);
// // internal force
// // applyReducedInternalForce(0, 0.01); // TODO: this should be necessary, but no obvious effects. Check again.
// // update local moment arm
// updateLocalMomentArm();
// updateExternalForceProjectMatrix(true);
// impulse causes rigid motion
applyRigidImpulse(impulse, rel_pos);
}
void btReducedSoftBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_node)
{
// f_local = R^-1 * f_ext //TODO: interpoalted transfrom
@ -706,24 +671,6 @@ void btReducedSoftBody::applyReducedDampingForce(const tDenseArray& reduce_vel)
}
}
void btReducedSoftBody::applyFixedContraints(btScalar dt)
{
for (int n = 0; n < m_fixedNodes.size(); ++n)
{
// apply impulse, velocity constraint
applyVelocityConstraint(btVector3(0, 0, 0), m_fixedNodes[n], dt);
// update predicted basis
proceedToTransform(dt, false); // TODO: maybe don't need?
// update full space velocity
mapToFullVelocity(getInterpolationWorldTransform());
// apply impulse again, position constraint
// applyPositionConstraint(m_x0[m_fixedNodes[n]], m_fixedNodes[n], dt);
}
}
btScalar btReducedSoftBody::getTotalMass() const
{
return m_mass;

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

@ -35,6 +35,10 @@ class btReducedSoftBody : public btSoftBody
btVector3 m_internalDeltaLinearVelocity;
btVector3 m_internalDeltaAngularVelocity;
tDenseArray m_internalDeltaReducedVelocity;
btVector3 m_linearVelocityFromReduced; // contribution to the linear velocity from reduced velocity
btVector3 m_angularVelocityFromReduced; // contribution to the angular velocity from reduced velocity
btVector3 m_internalDeltaAngularVelocityFromReduced;
protected:
// rigid frame
@ -203,18 +207,13 @@ class btReducedSoftBody : public btSoftBody
// apply impulse to the rigid frame
void internalApplyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos);
void applyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos);
// apply impulse to nodes in the full space
void internalApplyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt);
void applyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt);
// apply nodal external force in the full space
void applyFullSpaceNodalForce(const btVector3& f_ext, int n_node);
// apply fixed contraints to the nodes
void applyFixedContraints(btScalar dt);
// apply gravity to the rigid frame
void applyRigidGravity(const btVector3& gravity, btScalar dt);
@ -230,12 +229,6 @@ class btReducedSoftBody : public btSoftBody
// get relative position from a node to the CoM of the rigid frame
btVector3 getRelativePos(int n_node);
// apply velocity constraint
void applyVelocityConstraint(const btVector3& target_vel, int n_node, btScalar dt);
// apply position constraint
void applyPositionConstraint(const btVector3& target_pos, int n_node, btScalar dt);
//
// accessors
//