Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Rolling friction demo for sphere and torsional friction demo for two point contact.

Browse Source
This commit is contained in:
yunfeibai
2016-08-30 17:50:37 -07:00
parent d784c61b61
commit 9c124b5896
10 changed files with 79 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
data/cube_gripper_left.urdf
View File

@@ -6,7 +6,7 @@
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="1.0"/>
<rolling_friction value="0.3"/>
<inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>

2
data/cube_gripper_right.urdf
View File

@@ -6,7 +6,7 @@
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="1.0"/>
<rolling_friction value="0.3"/>
<inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>

2
data/cube_small.urdf
View File

@@ -3,7 +3,7 @@
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="1.0"/>
<rolling_friction value="0.3"/>
<inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>

3
data/plane.urdf
View File

@@ -1,6 +1,9 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<rolling_friction value="0.3"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".0"/>

29
data/sphere2_rolling_friction.urdf Normal file
View File

@@ -0,0 +1,29 @@
<?xml version="0.0" ?>
<robot name="urdf_robot">
<link name="base_link">
<contact>
<rolling_friction value="0.3"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="10.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="textured_sphere_smooth.obj" scale="0.5 0.5 0.5"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.5"/>
</geometry>
</collision>
</link>
</robot>

1
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -261,6 +261,7 @@ 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,"Rolling friction","Experiment on multibody rolling friction", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_ROLLING_FRICTION),
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),

12
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -172,15 +172,13 @@ public:
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_targetVelocity = -0.1;
controlArgs.m_maxTorqueValue = 10.0;
controlArgs.m_kd = 1.;
m_robotSim.setJointMotorControl(1,0,controlArgs);
}
@@ -189,15 +187,13 @@ public:
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_targetVelocity = 0.1;
controlArgs.m_maxTorqueValue = 10.0;
controlArgs.m_kd = 1.;
m_robotSim.setJointMotorControl(2,0,controlArgs);
}

31
examples/RoboticsLearning/R2D2GraspExample.cpp
View File

@@ -142,6 +142,37 @@ public:
}
}
if ((m_options & eROBOTIC_LEARN_ROLLING_FRICTION)!=0)
{
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
{
args.m_fileName = "sphere2_rolling_friction.urdf";
args.m_startPosition.setValue(0,0,2.5);
args.m_startOrientation.setEulerZYX(0,0,0);
args.m_useMultiBody = true;
m_robotSim.loadFile(args,results);
}
{
args.m_fileName = "sphere2.urdf";
args.m_startPosition.setValue(0,2,2.5);
args.m_startOrientation.setEulerZYX(0,0,0);
args.m_useMultiBody = true;
m_robotSim.loadFile(args,results);
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_startOrientation.setEulerZYX(0,0.2,0);
args.m_useMultiBody = true;
args.m_forceOverrideFixedBase = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
}
}
}
virtual void exitPhysics()

1
examples/RoboticsLearning/R2D2GraspExample.h
View File

@@ -20,6 +20,7 @@ enum RobotLearningExampleOptions
{
eROBOTIC_LEARN_GRASP=1,
eROBOTIC_LEARN_COMPLIANT_CONTACT=2,
eROBOTIC_LEARN_ROLLING_FRICTION=4,
};
class CommonExampleInterface* R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options);

14
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -598,18 +598,18 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
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);
multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, btVector3(0,0,0), 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;
solverConstraint.m_contactNormal1 = btVector3(0,0,0);
} else
{
btVector3 torqueAxis0 = constraintNormal;
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = constraintNormal;
solverConstraint.m_contactNormal1 = btVector3(0,0,0);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
}
@@ -641,18 +641,18 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
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->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0,0,0), &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;
solverConstraint.m_contactNormal2 = -btVector3(0,0,0);
} else
{
btVector3 torqueAxis1 = constraintNormal;
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -constraintNormal;
solverConstraint.m_contactNormal2 = -btVector3(0,0,0);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
}
@@ -762,7 +762,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
}
}
solverConstraint.m_friction = cp.m_combinedRollingFriction;
if(!isFriction)