Rolling friction demo for sphere and torsional friction demo for two point contact.
This commit is contained in:
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user