mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-18 21:10:05 +00:00
Add rolling friction, set rolling friction coefficient from urdf, and set up two point contact experiment.
This commit is contained in:
yunfeibai
parent
c741b17da8
commit
d784c61b61
45
data/cube_gripper_left.urdf
Normal file
45
data/cube_gripper_left.urdf
Normal file
@ -0,0 +1,45 @@
|
||||
<?xml version="0.0" ?>
|
||||
<robot name="cube_gripper_left.urdf">
|
||||
<link name="world">
|
||||
</link>
|
||||
|
||||
<link name="baseLink">
|
||||
<contact>
|
||||
<lateral_friction value="1.0"/>
|
||||
<rolling_friction value="1.0"/>
|
||||
<inertia_scaling value="3.0"/>
|
||||
<contact_cfm value="0.0"/>
|
||||
<contact_erp value="1.0"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value=".1"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0.785398 0.785398" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="cube.obj" scale=".05 .05 .05"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0.785398 0.785398" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size=".05 .05 .05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="cube_joint" type="prismatic">
|
||||
<parent link="world"/>
|
||||
<child link="baseLink"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
|
||||
<dynamics damping="0.0"/>
|
||||
</joint>
|
||||
</robot>
|
||||
|
||||
45
data/cube_gripper_right.urdf
Normal file
45
data/cube_gripper_right.urdf
Normal file
@ -0,0 +1,45 @@
|
||||
<?xml version="0.0" ?>
|
||||
<robot name="cube_gripper_right.urdf">
|
||||
<link name="world">
|
||||
</link>
|
||||
|
||||
<link name="baseLink">
|
||||
<contact>
|
||||
<lateral_friction value="1.0"/>
|
||||
<rolling_friction value="1.0"/>
|
||||
<inertia_scaling value="3.0"/>
|
||||
<contact_cfm value="0.0"/>
|
||||
<contact_erp value="1.0"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value=".1"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 -0.785398 -0.785398" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="cube.obj" scale=".05 .05 .05"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 -0.785398 -0.785398" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size=".05 .05 .05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="cube_joint" type="prismatic">
|
||||
<parent link="world"/>
|
||||
<child link="baseLink"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
|
||||
<dynamics damping="0.0"/>
|
||||
</joint>
|
||||
</robot>
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
<link name="baseLink">
|
||||
<contact>
|
||||
<lateral_friction value="1.0"/>
|
||||
<rolling_friction value="0.0"/>
|
||||
<rolling_friction value="1.0"/>
|
||||
<inertia_scaling value="3.0"/>
|
||||
<contact_cfm value="0.0"/>
|
||||
<contact_erp value="1.0"/>
|
||||
|
||||
@ -261,7 +261,8 @@ static ExampleEntry gDefaultExamples[]=
|
||||
|
||||
ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP),
|
||||
ExampleEntry(1,"URDF Compliant Contact","Work-in-progress, experiment/improve compliant rigid contact using parameters from URDF file (contact_cfm, contact_erp, lateral_friction, rolling_friction)", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_COMPLIANT_CONTACT),
|
||||
ExampleEntry(1,"Contact for Grasp","Grasp experiment to improve contact model", GripperGraspExampleCreateFunc),
|
||||
ExampleEntry(1,"Gripper Grasp","Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc,eGRIPPER_GRASP),
|
||||
ExampleEntry(1,"Two Point Grasp","Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP),
|
||||
|
||||
|
||||
|
||||
|
||||
@ -646,6 +646,27 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TiXmlElement *rolling_xml = ci->FirstChildElement("rolling_friction");
|
||||
if (rolling_xml)
|
||||
{
|
||||
if (m_parseSDF)
|
||||
{
|
||||
link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->GetText());
|
||||
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
|
||||
} else
|
||||
{
|
||||
if (!rolling_xml->Attribute("value"))
|
||||
{
|
||||
logger->reportError("Link/contact: rolling friction element must have value attribute");
|
||||
return false;
|
||||
}
|
||||
|
||||
link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->Attribute("value"));
|
||||
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -65,6 +65,7 @@ public:
|
||||
bool connected = m_robotSim.connect(m_guiHelper);
|
||||
b3Printf("robotSim connected = %d",connected);
|
||||
|
||||
if ((m_options & eGRIPPER_GRASP)!=0)
|
||||
{
|
||||
|
||||
{
|
||||
@ -87,7 +88,7 @@ public:
|
||||
args.m_fileName = "cube_small.urdf";
|
||||
args.m_startPosition.setValue(0, 0, .107);
|
||||
args.m_startOrientation.setEulerZYX(0, 0, 0);
|
||||
args.m_useMultiBody = false;
|
||||
args.m_useMultiBody = true;
|
||||
m_robotSim.loadFile(args, results);
|
||||
}
|
||||
|
||||
@ -152,10 +153,70 @@ public:
|
||||
|
||||
}
|
||||
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
|
||||
m_robotSim.setNumSimulationSubSteps(4);
|
||||
//m_robotSim.setNumSimulationSubSteps(4);
|
||||
}
|
||||
|
||||
|
||||
if ((m_options & eTWO_POINT_GRASP)!=0)
|
||||
{
|
||||
{
|
||||
b3RobotSimLoadFileArgs args("");
|
||||
b3RobotSimLoadFileResults results;
|
||||
args.m_fileName = "cube_small.urdf";
|
||||
args.m_startPosition.setValue(0, 0, .107);
|
||||
args.m_startOrientation.setEulerZYX(0, 0, 0);
|
||||
args.m_useMultiBody = true;
|
||||
m_robotSim.loadFile(args, results);
|
||||
}
|
||||
|
||||
{
|
||||
b3RobotSimLoadFileArgs args("");
|
||||
b3RobotSimLoadFileResults results;
|
||||
args.m_fileName = "cube_gripper_left.urdf";
|
||||
//args.m_startPosition.setValue(0.0675, 0.02, 0.13);
|
||||
args.m_startPosition.setValue(0.068, 0.02, 0.11);
|
||||
//args.m_startOrientation.setEulerZYX(0, 0.785398, 0.785398);
|
||||
args.m_useMultiBody = true;
|
||||
m_robotSim.loadFile(args, results);
|
||||
|
||||
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
|
||||
controlArgs.m_targetVelocity = -0.5;
|
||||
controlArgs.m_maxTorqueValue = 50.0;
|
||||
controlArgs.m_kd = 1.;
|
||||
m_robotSim.setJointMotorControl(1,0,controlArgs);
|
||||
}
|
||||
|
||||
{
|
||||
b3RobotSimLoadFileArgs args("");
|
||||
b3RobotSimLoadFileResults results;
|
||||
args.m_fileName = "cube_gripper_right.urdf";
|
||||
//args.m_startPosition.setValue(-0.0675, 0.02, 0.13);
|
||||
args.m_startPosition.setValue(-0.068, 0.02, 0.11);
|
||||
//args.m_startOrientation.setEulerZYX(0, -0.785398, -0.785398);
|
||||
args.m_useMultiBody = true;
|
||||
m_robotSim.loadFile(args, results);
|
||||
|
||||
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
|
||||
controlArgs.m_targetVelocity = 0.5;
|
||||
controlArgs.m_maxTorqueValue = 50.0;
|
||||
controlArgs.m_kd = 1.;
|
||||
m_robotSim.setJointMotorControl(2,0,controlArgs);
|
||||
}
|
||||
|
||||
if (1)
|
||||
{
|
||||
b3RobotSimLoadFileArgs args("");
|
||||
args.m_fileName = "plane.urdf";
|
||||
args.m_startPosition.setValue(0,0,0);
|
||||
args.m_startOrientation.setEulerZYX(0,0,0);
|
||||
args.m_forceOverrideFixedBase = true;
|
||||
args.m_useMultiBody = true;
|
||||
b3RobotSimLoadFileResults results;
|
||||
m_robotSim.loadFile(args,results);
|
||||
|
||||
}
|
||||
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
|
||||
m_robotSim.setNumSimulationSubSteps(4);
|
||||
}
|
||||
|
||||
}
|
||||
virtual void exitPhysics()
|
||||
@ -164,22 +225,26 @@ public:
|
||||
}
|
||||
virtual void stepSimulation(float deltaTime)
|
||||
{
|
||||
if ((m_gripperIndex>=0))
|
||||
if ((m_options & eGRIPPER_GRASP)!=0)
|
||||
{
|
||||
int fingerJointIndices[3]={0,1,3};
|
||||
double fingerTargetVelocities[3]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
|
||||
,-sGripperClosingTargetVelocity
|
||||
};
|
||||
double maxTorqueValues[3]={40.0,50.0,50.0};
|
||||
for (int i=0;i<3;i++)
|
||||
if ((m_gripperIndex>=0))
|
||||
{
|
||||
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
|
||||
controlArgs.m_targetVelocity = fingerTargetVelocities[i];
|
||||
controlArgs.m_maxTorqueValue = maxTorqueValues[i];
|
||||
controlArgs.m_kd = 1.;
|
||||
m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
|
||||
int fingerJointIndices[3]={0,1,3};
|
||||
double fingerTargetVelocities[3]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
|
||||
,-sGripperClosingTargetVelocity
|
||||
};
|
||||
double maxTorqueValues[3]={40.0,50.0,50.0};
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
|
||||
controlArgs.m_targetVelocity = fingerTargetVelocities[i];
|
||||
controlArgs.m_maxTorqueValue = maxTorqueValues[i];
|
||||
controlArgs.m_kd = 1.;
|
||||
m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
m_robotSim.stepSimulation();
|
||||
}
|
||||
virtual void renderScene()
|
||||
|
||||
@ -16,6 +16,11 @@ subject to the following restrictions:
|
||||
#ifndef GRIPPER_GRASP_EXAMPLE_H
|
||||
#define GRIPPER_GRASP_EXAMPLE_H
|
||||
|
||||
enum GripperGraspExampleOptions
|
||||
{
|
||||
eGRIPPER_GRASP=1,
|
||||
eTWO_POINT_GRASP=2,
|
||||
};
|
||||
|
||||
class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
|
||||
@ -530,8 +530,285 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
|
||||
const btVector3& constraintNormal,
|
||||
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
|
||||
btScalar& relaxation,
|
||||
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
|
||||
{
|
||||
|
||||
BT_PROFILE("setupMultiBodyRollingFrictionConstraint");
|
||||
btVector3 rel_pos1;
|
||||
btVector3 rel_pos2;
|
||||
|
||||
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
|
||||
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
|
||||
|
||||
const btVector3& pos1 = cp.getPositionWorldOnA();
|
||||
const btVector3& pos2 = cp.getPositionWorldOnB();
|
||||
|
||||
btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
|
||||
btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
|
||||
|
||||
btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
|
||||
btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
|
||||
|
||||
if (bodyA)
|
||||
rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
|
||||
if (bodyB)
|
||||
rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
|
||||
|
||||
relaxation = infoGlobal.m_sor;
|
||||
|
||||
btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
|
||||
btScalar cfm = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)?cp.m_contactCFM:infoGlobal.m_globalCfm;
|
||||
cfm *= invTimeStep;
|
||||
|
||||
btScalar erp = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)?cp.m_contactERP:infoGlobal.m_erp2;
|
||||
|
||||
|
||||
|
||||
|
||||
if (multiBodyA)
|
||||
{
|
||||
if (solverConstraint.m_linkA<0)
|
||||
{
|
||||
rel_pos1 = pos1 - multiBodyA->getBasePos();
|
||||
} else
|
||||
{
|
||||
rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
|
||||
}
|
||||
const int ndofA = multiBodyA->getNumDofs() + 6;
|
||||
|
||||
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
|
||||
|
||||
if (solverConstraint.m_deltaVelAindex <0)
|
||||
{
|
||||
solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
|
||||
multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
|
||||
m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA);
|
||||
} else
|
||||
{
|
||||
btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
|
||||
}
|
||||
|
||||
solverConstraint.m_jacAindex = m_data.m_jacobians.size();
|
||||
m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA);
|
||||
m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
|
||||
btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
|
||||
|
||||
btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
|
||||
multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
|
||||
btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
|
||||
|
||||
btVector3 torqueAxis0 = constraintNormal;
|
||||
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
|
||||
solverConstraint.m_contactNormal1 = constraintNormal;
|
||||
} else
|
||||
{
|
||||
btVector3 torqueAxis0 = constraintNormal;
|
||||
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
|
||||
solverConstraint.m_contactNormal1 = constraintNormal;
|
||||
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (multiBodyB)
|
||||
{
|
||||
if (solverConstraint.m_linkB<0)
|
||||
{
|
||||
rel_pos2 = pos2 - multiBodyB->getBasePos();
|
||||
} else
|
||||
{
|
||||
rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
|
||||
}
|
||||
|
||||
const int ndofB = multiBodyB->getNumDofs() + 6;
|
||||
|
||||
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
|
||||
if (solverConstraint.m_deltaVelBindex <0)
|
||||
{
|
||||
solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
|
||||
multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
|
||||
m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB);
|
||||
}
|
||||
|
||||
solverConstraint.m_jacBindex = m_data.m_jacobians.size();
|
||||
|
||||
m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB);
|
||||
m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
|
||||
btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
|
||||
|
||||
multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
|
||||
multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
|
||||
|
||||
btVector3 torqueAxis1 = constraintNormal;
|
||||
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
|
||||
solverConstraint.m_contactNormal2 = -constraintNormal;
|
||||
|
||||
} else
|
||||
{
|
||||
btVector3 torqueAxis1 = constraintNormal;
|
||||
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
|
||||
solverConstraint.m_contactNormal2 = -constraintNormal;
|
||||
|
||||
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
|
||||
}
|
||||
|
||||
{
|
||||
|
||||
btVector3 vec;
|
||||
btScalar denom0 = 0.f;
|
||||
btScalar denom1 = 0.f;
|
||||
btScalar* jacB = 0;
|
||||
btScalar* jacA = 0;
|
||||
btScalar* lambdaA =0;
|
||||
btScalar* lambdaB =0;
|
||||
int ndofA = 0;
|
||||
if (multiBodyA)
|
||||
{
|
||||
ndofA = multiBodyA->getNumDofs() + 6;
|
||||
jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
|
||||
lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
{
|
||||
btScalar j = jacA[i] ;
|
||||
btScalar l =lambdaA[i];
|
||||
denom0 += j*l;
|
||||
}
|
||||
} else
|
||||
{
|
||||
if (rb0)
|
||||
{
|
||||
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
|
||||
denom0 = rb0->getInvMass() + constraintNormal.dot(vec);
|
||||
}
|
||||
}
|
||||
if (multiBodyB)
|
||||
{
|
||||
const int ndofB = multiBodyB->getNumDofs() + 6;
|
||||
jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
|
||||
lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
|
||||
for (int i = 0; i < ndofB; ++i)
|
||||
{
|
||||
btScalar j = jacB[i] ;
|
||||
btScalar l =lambdaB[i];
|
||||
denom1 += j*l;
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
if (rb1)
|
||||
{
|
||||
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
|
||||
denom1 = rb1->getInvMass() + constraintNormal.dot(vec);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
btScalar d = denom0+denom1+cfm;
|
||||
if (d>SIMD_EPSILON)
|
||||
{
|
||||
solverConstraint.m_jacDiagABInv = relaxation/(d);
|
||||
} else
|
||||
{
|
||||
//disable the constraint row to handle singularity/redundant constraint
|
||||
solverConstraint.m_jacDiagABInv = 0.f;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
//compute rhs and remaining solverConstraint fields
|
||||
|
||||
|
||||
|
||||
btScalar restitution = 0.f;
|
||||
btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
|
||||
|
||||
btScalar rel_vel = 0.f;
|
||||
int ndofA = 0;
|
||||
int ndofB = 0;
|
||||
{
|
||||
|
||||
btVector3 vel1,vel2;
|
||||
if (multiBodyA)
|
||||
{
|
||||
ndofA = multiBodyA->getNumDofs() + 6;
|
||||
btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
|
||||
for (int i = 0; i < ndofA ; ++i)
|
||||
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
|
||||
} else
|
||||
{
|
||||
if (rb0)
|
||||
{
|
||||
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
|
||||
}
|
||||
}
|
||||
if (multiBodyB)
|
||||
{
|
||||
ndofB = multiBodyB->getNumDofs() + 6;
|
||||
btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
|
||||
for (int i = 0; i < ndofB ; ++i)
|
||||
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
|
||||
|
||||
} else
|
||||
{
|
||||
if (rb1)
|
||||
{
|
||||
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
|
||||
}
|
||||
}
|
||||
|
||||
solverConstraint.m_friction = cp.m_combinedRollingFriction;
|
||||
|
||||
if(!isFriction)
|
||||
{
|
||||
restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
|
||||
if (restitution <= btScalar(0.))
|
||||
{
|
||||
restitution = 0.f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
solverConstraint.m_appliedPushImpulse = 0.f;
|
||||
|
||||
{
|
||||
|
||||
btScalar positionalError = 0.f;
|
||||
btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
|
||||
|
||||
if (penetration>0)
|
||||
{
|
||||
positionalError = 0;
|
||||
velocityError -= penetration / infoGlobal.m_timeStep;
|
||||
|
||||
} else
|
||||
{
|
||||
positionalError = -penetration * erp/infoGlobal.m_timeStep;
|
||||
}
|
||||
|
||||
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv;
|
||||
|
||||
solverConstraint.m_rhs = velocityImpulse;
|
||||
solverConstraint.m_rhsPenetration = 0.f;
|
||||
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
|
||||
solverConstraint.m_upperLimit = solverConstraint.m_friction;
|
||||
|
||||
solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
|
||||
{
|
||||
@ -568,6 +845,41 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo
|
||||
return solverConstraint;
|
||||
}
|
||||
|
||||
btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyRollingFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
|
||||
{
|
||||
BT_PROFILE("addMultiBodyRollingFrictionConstraint");
|
||||
btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
|
||||
solverConstraint.m_orgConstraint = 0;
|
||||
solverConstraint.m_orgDofIndex = -1;
|
||||
|
||||
solverConstraint.m_frictionIndex = frictionIndex;
|
||||
bool isFriction = true;
|
||||
|
||||
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
|
||||
const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
|
||||
|
||||
btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
|
||||
btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
|
||||
|
||||
int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
|
||||
int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
|
||||
|
||||
solverConstraint.m_solverBodyIdA = solverBodyIdA;
|
||||
solverConstraint.m_solverBodyIdB = solverBodyIdB;
|
||||
solverConstraint.m_multiBodyA = mbA;
|
||||
if (mbA)
|
||||
solverConstraint.m_linkA = fcA->m_link;
|
||||
|
||||
solverConstraint.m_multiBodyB = mbB;
|
||||
if (mbB)
|
||||
solverConstraint.m_linkB = fcB->m_link;
|
||||
|
||||
solverConstraint.m_originalContactPoint = &cp;
|
||||
|
||||
setupMultiBodyRollingFrictionConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
|
||||
return solverConstraint;
|
||||
}
|
||||
|
||||
void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
|
||||
@ -592,8 +904,9 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
|
||||
// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
|
||||
// return;
|
||||
|
||||
|
||||
|
||||
//only a single rollingFriction per manifold
|
||||
int rollingFriction=1;
|
||||
|
||||
for (int j=0;j<manifold->getNumContacts();j++)
|
||||
{
|
||||
|
||||
@ -675,9 +988,9 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
|
||||
#endif //ROLLING_FRICTION
|
||||
|
||||
///Bullet has several options to set the friction directions
|
||||
///By default, each contact has only a single friction direction that is recomputed automatically very frame
|
||||
///By default, each contact has only a single friction direction that is recomputed automatically every frame
|
||||
///based on the relative linear velocity.
|
||||
///If the relative velocity it zero, it will automatically compute a friction direction.
|
||||
///If the relative velocity is zero, it will automatically compute a friction direction.
|
||||
|
||||
///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
|
||||
///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
|
||||
@ -718,6 +1031,15 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
|
||||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
|
||||
|
||||
if (rollingFriction > 0)
|
||||
{
|
||||
addMultiBodyRollingFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
|
||||
addMultiBodyRollingFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
|
||||
addMultiBodyRollingFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
|
||||
|
||||
rollingFriction--;
|
||||
}
|
||||
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
{
|
||||
|
||||
@ -47,8 +47,10 @@ protected:
|
||||
|
||||
|
||||
void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
|
||||
btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
|
||||
|
||||
btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
|
||||
|
||||
btMultiBodySolverConstraint& addMultiBodyRollingFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
|
||||
|
||||
void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow,
|
||||
btScalar* jacA,btScalar* jacB,
|
||||
@ -60,6 +62,12 @@ protected:
|
||||
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
|
||||
btScalar& relaxation,
|
||||
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
|
||||
|
||||
void setupMultiBodyRollingFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
|
||||
const btVector3& contactNormal,
|
||||
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
|
||||
btScalar& relaxation,
|
||||
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
|
||||
|
||||
void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
|
||||
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user