mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 05:10:08 +00:00
fixed a sign issue in the friction of the reduced deformable vs moving rigid body contact
This commit is contained in:
jingyuc
parent
3a70641c65
commit
eef3b73cd2
@ -56,21 +56,20 @@ 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]);
|
||||
}
|
||||
|
||||
void Ctor_RbUpStack()
|
||||
{
|
||||
float mass = 10;
|
||||
// btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
|
||||
btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
|
||||
btTransform startTransform;
|
||||
startTransform.setIdentity();
|
||||
@ -78,7 +77,8 @@ public:
|
||||
// btRigidBody* rb0 = createRigidBody(mass, startTransform, shape);
|
||||
// rb0->setLinearVelocity(btVector3(0, 0, 0));
|
||||
|
||||
startTransform.setOrigin(btVector3(0,10,0));
|
||||
startTransform.setOrigin(btVector3(0,4,0));
|
||||
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 4.0));
|
||||
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
|
||||
rb1->setLinearVelocity(btVector3(0, 0, 0));
|
||||
}
|
||||
@ -157,7 +157,7 @@ void FreeFall::initPhysics()
|
||||
|
||||
btTransform init_transform;
|
||||
init_transform.setIdentity();
|
||||
init_transform.setOrigin(btVector3(0, 4, 0));
|
||||
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));
|
||||
rsb->transform(init_transform);
|
||||
@ -194,7 +194,7 @@ void FreeFall::initPhysics()
|
||||
|
||||
btTransform groundTransform;
|
||||
groundTransform.setIdentity();
|
||||
groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
|
||||
// groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
|
||||
// groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0));
|
||||
groundTransform.setOrigin(btVector3(0, 0, 0));
|
||||
// groundTransform.setOrigin(btVector3(0, 0, 6));
|
||||
@ -204,11 +204,76 @@ void FreeFall::initPhysics()
|
||||
createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
|
||||
}
|
||||
}
|
||||
// {
|
||||
// // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
|
||||
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(2), btScalar(0.5), btScalar(1)));
|
||||
// m_collisionShapes.push_back(groundShape);
|
||||
|
||||
// btTransform groundTransform;
|
||||
// groundTransform.setIdentity();
|
||||
// // groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
|
||||
// groundTransform.setOrigin(btVector3(0, 2, 3.1));
|
||||
// // groundTransform.setOrigin(btVector3(0, 0, 6));
|
||||
// // groundTransform.setOrigin(btVector3(0, -50, 0));
|
||||
// {
|
||||
// btScalar mass(0.);
|
||||
// createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
|
||||
// }
|
||||
// }
|
||||
// {
|
||||
// // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
|
||||
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(2), btScalar(0.5), btScalar(1)));
|
||||
// m_collisionShapes.push_back(groundShape);
|
||||
|
||||
// btTransform groundTransform;
|
||||
// groundTransform.setIdentity();
|
||||
// // groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
|
||||
// groundTransform.setOrigin(btVector3(0, 2, -3.1));
|
||||
// // groundTransform.setOrigin(btVector3(0, 0, 6));
|
||||
// // groundTransform.setOrigin(btVector3(0, -50, 0));
|
||||
// {
|
||||
// btScalar mass(0.);
|
||||
// createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
|
||||
// }
|
||||
// }
|
||||
// {
|
||||
// // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
|
||||
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(1), btScalar(0.5), btScalar(2)));
|
||||
// m_collisionShapes.push_back(groundShape);
|
||||
|
||||
// btTransform groundTransform;
|
||||
// groundTransform.setIdentity();
|
||||
// // groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
|
||||
// groundTransform.setOrigin(btVector3(2, 2, 0));
|
||||
// // groundTransform.setOrigin(btVector3(0, 0, 6));
|
||||
// // groundTransform.setOrigin(btVector3(0, -50, 0));
|
||||
// {
|
||||
// btScalar mass(0.);
|
||||
// createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
|
||||
// }
|
||||
// }
|
||||
// {
|
||||
// // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
|
||||
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(1), btScalar(0.5), btScalar(2)));
|
||||
// m_collisionShapes.push_back(groundShape);
|
||||
|
||||
// btTransform groundTransform;
|
||||
// groundTransform.setIdentity();
|
||||
// // groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
|
||||
// groundTransform.setOrigin(btVector3(-2, 2, 0));
|
||||
// // groundTransform.setOrigin(btVector3(0, 0, 6));
|
||||
// // groundTransform.setOrigin(btVector3(0, -50, 0));
|
||||
// {
|
||||
// btScalar mass(0.);
|
||||
// createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
|
||||
// }
|
||||
// }
|
||||
|
||||
getDeformableDynamicsWorld()->setImplicit(false);
|
||||
getDeformableDynamicsWorld()->setLineSearch(false);
|
||||
getDeformableDynamicsWorld()->setUseProjection(true);
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.0;
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true;
|
||||
|
||||
@ -70,7 +70,7 @@ public:
|
||||
int count = 2;
|
||||
float mass = 1e6;
|
||||
btCollisionShape* shape[] = {
|
||||
new btBoxShape(btVector3(3, 3, 0.5)),
|
||||
new btBoxShape(btVector3(3, 4, 0.5)),
|
||||
};
|
||||
static const int nshapes = sizeof(shape) / sizeof(shape[0]);
|
||||
for (int i = 0; i < count; ++i)
|
||||
@ -82,6 +82,19 @@ public:
|
||||
createRigidBody(mass, startTransform, shape[i % nshapes]);
|
||||
}
|
||||
}
|
||||
|
||||
void Ctor_RbUpStack()
|
||||
{
|
||||
float mass = 8;
|
||||
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));
|
||||
rb1->setFriction(0.7);
|
||||
}
|
||||
|
||||
virtual void renderScene()
|
||||
{
|
||||
@ -111,15 +124,15 @@ void ReducedGrasp::GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsW
|
||||
btScalar pressTime = 1;
|
||||
btScalar liftTime = 2.5;
|
||||
btScalar shiftTime = 3.5;
|
||||
btScalar holdTime = 4.5*1000;
|
||||
btScalar dropTime = 5.3*1000;
|
||||
btScalar holdTime = 4.5;
|
||||
btScalar dropTime = 5.3;
|
||||
btTransform rbTransform;
|
||||
rbTransform.setIdentity();
|
||||
btVector3 translation;
|
||||
btVector3 velocity;
|
||||
|
||||
btVector3 initialTranslationLeft = btVector3(0,3,3); // inner face has z=2
|
||||
btVector3 initialTranslationRight = btVector3(0,3,-3); // inner face has z=-2
|
||||
btVector3 initialTranslationLeft = btVector3(0,4,3); // inner face has z=2
|
||||
btVector3 initialTranslationRight = btVector3(0,4,-3); // inner face has z=-2
|
||||
btVector3 pinchVelocityLeft = btVector3(0,0,-2);
|
||||
btVector3 pinchVelocityRight = btVector3(0,0,2);
|
||||
btVector3 liftVelocity = btVector3(0,5,0);
|
||||
@ -133,33 +146,37 @@ void ReducedGrasp::GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsW
|
||||
velocity = pinchVelocityLeft;
|
||||
translation = initialTranslationLeft + pinchVelocityLeft * time;
|
||||
}
|
||||
else if (time < liftTime)
|
||||
{
|
||||
exit(1);
|
||||
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);
|
||||
@ -172,32 +189,37 @@ void ReducedGrasp::GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsW
|
||||
velocity = pinchVelocityRight;
|
||||
translation = initialTranslationRight + pinchVelocityRight * time;
|
||||
}
|
||||
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);
|
||||
@ -220,7 +242,7 @@ void ReducedGrasp::initPhysics()
|
||||
|
||||
m_broadphase = new btDbvtBroadphase();
|
||||
btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver();
|
||||
btVector3 gravity = btVector3(0, -10, 0);
|
||||
btVector3 gravity = btVector3(0, 0, 0);
|
||||
reducedSoftBodySolver->setGravity(gravity);
|
||||
|
||||
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
|
||||
@ -275,13 +297,18 @@ void ReducedGrasp::initPhysics()
|
||||
getDeformableDynamicsWorld()->setLineSearch(false);
|
||||
getDeformableDynamicsWorld()->setUseProjection(true);
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0;
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true;
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
|
||||
|
||||
// grippers
|
||||
createGrip();
|
||||
|
||||
// rigid block
|
||||
// Ctor_RbUpStack();
|
||||
|
||||
// {
|
||||
// float mass = 10;
|
||||
// btCollisionShape* shape = new btBoxShape(btVector3(0.25, 2, 0.5));
|
||||
|
||||
@ -73,8 +73,8 @@ void btReducedDeformableRigidContactConstraint::setSolverBody(btSolverBody& solv
|
||||
btVector3 torqueAxis = -m_relPosA.cross(m_contactNormalA);
|
||||
m_angularComponentNormal = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxis;
|
||||
|
||||
m_linearComponentTangent = -m_contactTangent * m_solverBody->internalGetInvMass();
|
||||
btVector3 torqueAxisTangent = -m_relPosA.cross(m_contactNormalA);
|
||||
m_linearComponentTangent = m_contactTangent * m_solverBody->internalGetInvMass();
|
||||
btVector3 torqueAxisTangent = m_relPosA.cross(m_contactTangent);
|
||||
m_angularComponentTangent = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxisTangent;
|
||||
}
|
||||
|
||||
@ -166,6 +166,7 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
|
||||
|
||||
// initial relative tangential veloity magnitude
|
||||
btScalar v_rel_tangent = abs((Va - getVb()).dot(m_contactTangent));
|
||||
std::cout << "v_rel_tangent: " << v_rel_tangent << '\n';
|
||||
btScalar deltaV_rel_tangent = deltaV_rel.dot(m_contactTangent);
|
||||
std::cout << "deltaV_rel_tangent: " << deltaV_rel_tangent << "\n";
|
||||
btScalar v_rel_tangent_budget = v_rel_tangent - deltaV_rel_tangent;
|
||||
@ -234,7 +235,7 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
|
||||
<< m_solverBody->getDeltaAngularVelocity()[2] << '\n';
|
||||
|
||||
m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, deltaImpulse);
|
||||
// m_solverBody->internalApplyImpulse(m_linearComponentTangent, m_angularComponentTangent, deltaImpulse_tangent);
|
||||
m_solverBody->internalApplyImpulse(m_linearComponentTangent, m_angularComponentTangent, deltaImpulse_tangent);
|
||||
|
||||
std::cout << "after\n";
|
||||
std::cout << "rigid impulse applied!!\n";
|
||||
|
||||
Loading…
Reference in New Issue
Block a user