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

reduced motor grasp example is working

Browse Source
This commit is contained in:
jingyuc
2021-09-24 13:16:33 -04:00
parent 5e0a805d73
commit 107c5526eb
7 changed files with 53 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/MultiBody/MultiDofDemo.cpp
View File

@@ -165,7 +165,7 @@ void MultiDofDemo::initPhysics()
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(world, numLinks, btVector3(-0.4f, 3.f, 0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(world, numLinks, btVector3(-0.4f, 3.f, 0.f), baseHalfExtents, linkHalfExtents, spherical, g_floatingBase);
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
mbC->setCanSleep(canSleep);

2
examples/ReducedDeformableDemo/ModeVisualizer.cpp
View File

@@ -119,7 +119,7 @@ void ModeVisualizer::initPhysics()
// create volumetric soft body
{
btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedBeam(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes);
btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedCube(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);

56
examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp
View File

@@ -41,7 +41,7 @@ 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 int num_modes = 20;
static float friction = 1.;
struct TetraCube
{
@@ -126,14 +126,14 @@ public:
if (dofIndex == 6)
{
motor->setVelocityTarget(-fingerTargetVelocities[1], 1);
motor->setMaxAppliedImpulse(20);
motor->setMaxAppliedImpulse(70);
}
if (dofIndex == 7)
{
motor->setVelocityTarget(fingerTargetVelocities[1], 1);
motor->setMaxAppliedImpulse(20);
motor->setMaxAppliedImpulse(70);
}
motor->setMaxAppliedImpulse(1);
motor->setMaxAppliedImpulse(50);
}
}
dofIndex += mb->getLink(link).m_dofCount;
@@ -141,10 +141,10 @@ public:
}
//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()
@@ -217,8 +217,8 @@ void ReducedMotorGrasp::initPhysics()
m_broadphase = new btDbvtBroadphase();
btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver();
btVector3 gravity = btVector3(0, 0, 0);
// btVector3 gravity = btVector3(0, -9.81, 0);
// btVector3 gravity = btVector3(0, 0, 0);
btVector3 gravity = btVector3(0, -9.81, 0);
reducedSoftBodySolver->setGravity(gravity);
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
@@ -242,10 +242,10 @@ void ReducedMotorGrasp::initPhysics()
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);
btVector3 linkHalfExtents(1, 1, 0.15);
btVector3 baseHalfExtents(0.5, 0.35, 0.35);
// btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), btVector3(0.f, 0.05f,0.f), baseHalfExtents, linkHalfExtents, false);
btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), btVector3(0.f, 5.f,0.f), baseHalfExtents, linkHalfExtents, false);
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
@@ -317,21 +317,21 @@ void ReducedMotorGrasp::initPhysics()
// create volumetric reduced deformable body
{
btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedBeam(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes);
btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedCube(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
rsb->getCollisionShape()->setMargin(0.05);
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));
init_transform.setOrigin(btVector3(0, 0.5, 0));
init_transform.setRotation(btQuaternion(SIMD_PI / 2.0, 0, SIMD_PI / 2.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(100);
rsb->setStiffnessScale(20);
rsb->setDamping(damping_alpha, damping_beta);
rsb->setRigidVelocity(btVector3(0, 1, 0));
// rsb->setRigidVelocity(btVector3(0, 1, 0));
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
@@ -348,11 +348,11 @@ void ReducedMotorGrasp::initPhysics()
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.3;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-6;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 200;
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
{
@@ -421,8 +421,8 @@ btMultiBody* ReducedMotorGrasp::createFeatherstoneMultiBody(btMultiBodyDynamicsW
{
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 0.1;
float linkMass = 0.1;
float baseMass = 55;
float linkMass = 55;
int numLinks = 2;
if (baseMass)
@@ -450,11 +450,11 @@ btMultiBody* ReducedMotorGrasp::createFeatherstoneMultiBody(btMultiBodyDynamicsW
//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
parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 2.f, -baseHalfExtents[2] * 2.f));
parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 2.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
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1]*2.f, 0); //cur body's COM to cur body's PIV offset
btAlignedObjectArray<btVector3> parentComToCurrentPivot;
parentComToCurrentPivot.push_back(btVector3(parentComToCurrentCom[0] - currentPivotToCurrentCom));

8
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;
@@ -364,6 +364,12 @@ void btReducedSoftBody::transform(const btTransform& trs)
internalInitialization();
}
void btReducedSoftBody::scale(const btVector3& scl)
{
// scale the mesh
btSoftBody::scale(scl);
}
void btReducedSoftBody::updateRestNodalPositions()
{
// update reset nodal position

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

@@ -122,6 +122,7 @@ class btReducedSoftBody : public btSoftBody
//
// various internal updates
//
virtual void scale(const btVector3& scl);
virtual void transform(const btTransform& trs);
virtual void translate(const btVector3& trs)
{
@@ -131,10 +132,6 @@ class btReducedSoftBody : public btSoftBody
{
btAssert(false); // use transform().
}
virtual void scale(const btVector3& scl)
{
btAssert(false); // scale is NOT supported in the reduced deformable body
}
private:
void updateRestNodalPositions();

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

@@ -30,6 +30,19 @@ btReducedSoftBody* btReducedSoftBodyHelpers::createReducedCube(btSoftBodyWorldIn
return rsb;
}
btReducedSoftBody* btReducedSoftBodyHelpers::createReducedSponge(btSoftBodyWorldInfo& worldInfo, const int start_mode, const int num_modes)
{
std::string filepath("../../../examples/SoftDemo/sponge/");
std::string filename = filepath + "mesh.vtk";
btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createFromVtkFile(worldInfo, filename.c_str());
rsb->setReducedModes(start_mode, num_modes, rsb->m_nodes.size());
btVector3 half_extents(0.025, 0.025, 0.025); //TODO: fix
btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(rsb, filepath.c_str(), half_extents);
return rsb;
}
btReducedSoftBody* btReducedSoftBodyHelpers::createFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file)
{
std::ifstream fs;

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

@@ -9,6 +9,8 @@ struct btReducedSoftBodyHelpers
static btReducedSoftBody* createReducedBeam(btSoftBodyWorldInfo& worldInfo, const int start_mode, const int num_modes);
// create a cube
static btReducedSoftBody* createReducedCube(btSoftBodyWorldInfo& worldInfo, const int start_mode, const int num_modes);
// create a sponge (small rectangular cuboid)
static btReducedSoftBody* createReducedSponge(btSoftBodyWorldInfo& worldInfo, const int start_mode, const int num_modes);
// read in geometry info from Vtk file
static btReducedSoftBody* createFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file);