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

Add rolling friction, set rolling friction coefficient from urdf, and set up two point contact experiment.

Browse Source
This commit is contained in:
yunfeibai
2016-08-30 11:19:23 -07:00
parent c741b17da8
commit d784c61b61
9 changed files with 536 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -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),

21
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -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;
}
}
}
}

95
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -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()

5
examples/RoboticsLearning/GripperGraspExample.h
View File

@@ -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);