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

Improve MJCF import, to allow reading of friction parameters, including spinning and rolling friction, with some rudimentary 'default class' support.

Browse Source 
Tweak VRGloveSimulatorMain to work better with MPL/mpl2.xml hand.
This commit is contained in:
Erwin Coumans
2017-05-02 18:05:36 -07:00
parent 74ce6af26c
commit 07992fe61d
3 changed files with 272 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

112
examples/RobotSimulator/VRGloveSimulatorMain.cpp
View File

@@ -30,7 +30,7 @@ void setJointMotorPositionControl(b3RobotSimulatorClientAPI* sim, int obUid, int
int main(int argc, char* argv[])
{
b3CommandLineArgs args(argc,argv);
std::string port="COM10";
std::string port="COM9";
args.GetCmdLineArgument("port",port);
int baud = 115200;
args.GetCmdLineArgument("speed",baud);
@@ -41,15 +41,15 @@ int main(int argc, char* argv[])
int disableShadows = 0;
args.GetCmdLineArgument("disableShadows",disableShadows);
int deviceTypeFilter = VR_DEVICE_GENERIC_TRACKER;
args.GetCmdLineArgument("deviceTypeFilter",deviceTypeFilter);
int wireFrame = 0;
args.GetCmdLineArgument("wireFrame",wireFrame);
printf("port=%s, speed=%d, connection mode=%s\n", port.c_str(), baud,mode.c_str());
// port, baudrate, timeout in milliseconds
serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1));
if (!my_serial.isOpen())
{
printf("Cannot open serial port\n");
return -1;
}
b3RobotSimulatorClientAPI* sim = new b3RobotSimulatorClientAPI();
@@ -82,6 +82,12 @@ int main(int argc, char* argv[])
sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);
}
if (wireFrame)
{
sim->configureDebugVisualizer( COV_ENABLE_WIREFRAME, 1);
}
sim->setTimeOut(12345);
//syncBodies is only needed when connecting to an existing physics server that has already some bodies
sim->syncBodies();
@@ -98,18 +104,7 @@ int main(int argc, char* argv[])
sim->loadURDF("plane_with_collision_audio.urdf");
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.300000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.200000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.100000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.000000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.900000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.800000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.700000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.600000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("table/table.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.000000,-0.200000,0.000000),b3Quaternion(0.000000,0.000000,0.707107,0.707107)));
sim->loadURDF("cube_small.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3( 0.950000,-0.100000,0.700000),b3Quaternion(0.000000,0.000000,0.707107,0.707107)));
sim->loadURDF("sphere_small.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.850000,-0.400000,0.700000),b3Quaternion(0.000000,0.000000,0.707107,0.707107)));
int handUid = -1;
b3RobotSimulatorLoadFileResults mjcfResults;
@@ -127,14 +122,24 @@ int main(int argc, char* argv[])
printf("Cannot load MJCF file %s\n", mjcfFileName);
}
b3Quaternion handOrn = sim->getQuaternionFromEuler(b3MakeVector3(3.14,-3.14/2,0));
b3Vector3 handPos = b3MakeVector3(-0.05,0,0.02);
#ifdef TOUCH
b3Vector3 handPos = b3MakeVector3(-0.10,-0.03,0.02);
b3Vector3 rollPitchYaw = b3MakeVector3(0.5*B3_PI,0,1.25*B3_PI);//-B3_PI/2,0,B3_PI/2);
handOrn = sim->getQuaternionFromEuler(rollPitchYaw);
#else
b3Quaternion handOrn = sim->getQuaternionFromEuler(b3MakeVector3(3.14,-3.14/2,0));
b3Vector3 handPos = b3MakeVector3(-0.05,0,0.02);
#endif
b3JointInfo jointInfo;
jointInfo.m_jointType = eFixedType;
b3Vector3 handStartPosWorld = b3MakeVector3(0.500000,0.300006,0.900000);
b3Quaternion handStartOrnWorld = b3Quaternion ::getIdentity();
b3JointInfo jointInfo;
jointInfo.m_jointType = eFixedType;
printf("handStartOrnWorld=%f,%f,%f,%f\n",handStartOrnWorld[0],handStartOrnWorld[1],handStartOrnWorld[2],handStartOrnWorld[3]);
jointInfo.m_childFrame[0] = handStartPosWorld[0];
jointInfo.m_childFrame[1] = handStartPosWorld[1];
jointInfo.m_childFrame[2] = handStartPosWorld[2];
@@ -152,17 +157,32 @@ int main(int argc, char* argv[])
jointInfo.m_parentFrame[6] = handOrn[3];
int handConstraintId = sim->createConstraint(handUid,-1,-1,-1,&jointInfo);
double maxFingerForce = 50;
double maxFingerForce = 10;
double maxArmForce = 1000;
for (int j=0; j<sim->getNumJoints(handUid);j++)
{
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
controlArgs.m_maxTorqueValue = maxFingerForce ;
controlArgs.m_kp = 0.1;
controlArgs.m_kd = 1;
controlArgs.m_targetPosition = 0;
controlArgs.m_targetVelocity = 0;
sim->setJointMotorControl(handUid,j,controlArgs);
}
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.300000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.200000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.100000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.000000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.900000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.800000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.700000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.600000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("table/table.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.000000,-0.200000,0.000000),b3Quaternion(0.000000,0.000000,0.707107,0.707107)));
sim->loadURDF("cube_small.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3( 0.950000,-0.100000,0.700000),b3Quaternion(0.000000,0.000000,0.707107,0.707107)));
sim->loadURDF("sphere_small.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.850000,-0.400000,0.700000),b3Quaternion(0.000000,0.000000,0.707107,0.707107)));
b3Clock clock;
double startTime = clock.getTimeInSeconds();
double simWallClockSeconds = 20.;
@@ -170,11 +190,36 @@ int main(int argc, char* argv[])
int vidLogId = -1;
int minitaurLogId = -1;
int rotateCamera = 0;
serial::Serial my_serial;
try
{
// port, baudrate, timeout in milliseconds
my_serial.setBaudrate(baud);
my_serial.setPort(port);
my_serial.setBytesize(serial::sevenbits);
my_serial.setParity(serial::parity_odd);
my_serial.setStopbits(serial::stopbits_two);
my_serial.setTimeout(serial::Timeout::simpleTimeout(10));
my_serial.open();
} catch(...)
{
printf("Cannot open port, use --port=PORTNAME\n");
exit(0);
}
if (!my_serial.isOpen())
{
printf("Cannot open serial port\n");
return -1;
}
my_serial.flush();
while (sim->canSubmitCommand())
{
b3VREventsData vrEvents;
int deviceTypeFilter = VR_DEVICE_GENERIC_TRACKER;
sim->getVREvents(&vrEvents, deviceTypeFilter);
//instead of iterating over all vr events, we just take the most up-to-date one
@@ -185,7 +230,7 @@ int main(int argc, char* argv[])
// printf("e.pos=%f,%f,%f\n",e.m_pos[0],e.m_pos[1],e.m_pos[2]);
b3JointInfo changeConstraintInfo;
changeConstraintInfo.m_flags = 0;
changeConstraintInfo.m_jointMaxForce = 50;
changeConstraintInfo.m_jointMaxForce = maxArmForce ;
changeConstraintInfo.m_flags |= eJointChangeMaxForce;
changeConstraintInfo.m_childFrame[0] = e.m_pos[0];
@@ -203,10 +248,17 @@ int main(int argc, char* argv[])
}
//read the serial output from the hand, extract into parts
std::string result = my_serial.readline();
//my_serial.flush();
std::string result;
try
{
result = my_serial.readline();
} catch(...)
{
}
if (result.length())
{
//my_serial.flush();
int res = result.find("\n");
while (res<0)
{