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

4
data/MPL/mpl2.xml
View File

@@ -25,7 +25,7 @@
<default>
<default class="MPL">
<geom material="MatMesh" contype="1" conaffinity="1" condim="4" margin="0.001"/>
<geom material="MatMesh" contype="1" conaffinity="1" condim="4" friction="1 .5 0.5" margin="0.001"/>
<joint limited="true" damping="0.2" armature=".01"/>
<site material="MatTouch" type="ellipsoid" group="3"/>
<position ctrllimited="true" kp="10"/>
@@ -104,7 +104,7 @@
<!-- ======= ROBOT ======= -->
<body name="wristy" pos="0 0 0">
<body childclass="MPL" name="wristy" pos="0 0 0">
<inertial pos="-7.08369e-005 -0.0217787 -0.000286168" quat="0.707488 0.00581744 -0.0107421 0.70662" mass="0.0272932" diaginertia="2.46813e-005 1.77029e-005 1.71079e-005" />
<geom type="mesh" mesh="wristy"/>
<joint type="free"/>

226
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
View File

@@ -1,6 +1,8 @@
#include "BulletMJCFImporter.h"
#include "../../ThirdPartyLibs/tinyxml/tinyxml.h"
#include "Bullet3Common/b3FileUtils.h"
#include "Bullet3Common/b3HashMap.h"
#include <string>
#include "../../Utils/b3ResourcePath.h"
#include <iostream>
@@ -129,6 +131,35 @@ struct MyMJCFAsset
std::string m_fileName;
};
struct MyMJCFDefaults
{
int m_defaultCollisionGroup;
int m_defaultCollisionMask;
btScalar m_defaultCollisionMargin;
// joint defaults
std::string m_defaultJointLimited;
// geom defaults
std::string m_defaultGeomRgba;
int m_defaultConDim;
double m_defaultLateralFriction;
double m_defaultSpinningFriction;
double m_defaultRollingFriction;
MyMJCFDefaults()
:m_defaultCollisionGroup(1),
m_defaultCollisionMask(1),
m_defaultCollisionMargin(0.001),//assume unit meters, margin is 1mm
m_defaultConDim(3),
m_defaultLateralFriction(0.5),
m_defaultSpinningFriction(0),
m_defaultRollingFriction(0)
{
}
};
struct BulletMJCFImporterInternalData
{
GUIHelperInterface* m_guiHelper;
@@ -149,26 +180,17 @@ struct BulletMJCFImporterInternalData
int m_activeModel;
int m_activeBodyUniqueId;
//todo: for full MJCF compatibility, we would need a stack of default values
int m_defaultCollisionGroup;
int m_defaultCollisionMask;
btScalar m_defaultCollisionMargin;
// joint defaults
std::string m_defaultJointLimited;
// geom defaults
std::string m_defaultGeomRgba;
//todo: for better MJCF compatibility, we would need a stack of default values
MyMJCFDefaults m_globalDefaults;
b3HashMap<b3HashString, MyMJCFDefaults> m_classDefaults;
//those collision shapes are deleted by caller (todo: make sure this happens!)
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
BulletMJCFImporterInternalData()
:m_activeModel(-1),
m_activeBodyUniqueId(-1),
m_defaultCollisionGroup(1),
m_defaultCollisionMask(1),
m_defaultCollisionMargin(0.001)//assume unit meters, margin is 1mm
m_activeBodyUniqueId(-1)
{
m_pathPrefix[0] = 0;
}
@@ -246,7 +268,9 @@ struct BulletMJCFImporterInternalData
}
}
bool parseDefaults(TiXmlElement* root_xml, MJCFErrorLogger* logger)
bool parseDefaults(MyMJCFDefaults& defaults, TiXmlElement* root_xml, MJCFErrorLogger* logger)
{
bool handled= false;
//rudimentary 'default' support, would need more work for better feature coverage
@@ -254,6 +278,27 @@ struct BulletMJCFImporterInternalData
{
std::string n = child_xml->Value();
if (n.find("default")!=std::string::npos)
{
const char* className = child_xml->Attribute("class");
if (className)
{
MyMJCFDefaults* curDefaultsPtr = m_classDefaults[className];
if (!curDefaultsPtr)
{
MyMJCFDefaults def;
m_classDefaults.insert(className,def);
curDefaultsPtr = m_classDefaults[className];
}
if (curDefaultsPtr)
{
MyMJCFDefaults& curDefaults = *curDefaultsPtr;
parseDefaults(curDefaults, child_xml, logger);
}
}
}
if (n=="inertial")
{
}
@@ -269,7 +314,7 @@ struct BulletMJCFImporterInternalData
// limited="true"
if (const char* conTypeStr = child_xml->Attribute("limited"))
{
m_defaultJointLimited = child_xml->Attribute("limited");
defaults.m_defaultJointLimited = child_xml->Attribute("limited");
}
}
if (n=="geom")
@@ -278,24 +323,60 @@ struct BulletMJCFImporterInternalData
const char* conTypeStr = child_xml->Attribute("contype");
if (conTypeStr)
{
m_defaultCollisionGroup = urdfLexicalCast<int>(conTypeStr);
defaults.m_defaultCollisionGroup = urdfLexicalCast<int>(conTypeStr);
}
const char* conAffinityStr = child_xml->Attribute("conaffinity");
if (conAffinityStr)
{
m_defaultCollisionMask = urdfLexicalCast<int>(conAffinityStr);
defaults.m_defaultCollisionMask = urdfLexicalCast<int>(conAffinityStr);
}
const char* rgba = child_xml->Attribute("rgba");
if (rgba)
{
m_defaultGeomRgba = rgba;
defaults.m_defaultGeomRgba = rgba;
}
const char* conDimS = child_xml->Attribute("condim");
if (conDimS)
{
defaults.m_defaultConDim=urdfLexicalCast<int>(conDimS);
}
int conDim = defaults.m_defaultConDim;
const char* frictionS = child_xml->Attribute("friction");
if (frictionS)
{
btArray<std::string> pieces;
btArray<float> frictions;
btAlignedObjectArray<std::string> strArray;
urdfIsAnyOf(" ", strArray);
urdfStringSplit(pieces, frictionS, strArray);
for (int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
{
frictions.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
}
}
if (frictions.size()>0)
{
defaults.m_defaultLateralFriction = frictions[0];
}
if (frictions.size()>1)
{
defaults.m_defaultSpinningFriction = frictions[1];
}
if (frictions.size()>2)
{
defaults.m_defaultRollingFriction = frictions[2];
}
}
}
}
handled=true;
return handled;
}
bool parseRootLevel(TiXmlElement* root_xml,MJCFErrorLogger* logger)
bool parseRootLevel(MyMJCFDefaults& defaults, TiXmlElement* root_xml,MJCFErrorLogger* logger)
{
for (TiXmlElement* rootxml = root_xml->FirstChildElement() ; rootxml ; rootxml = rootxml->NextSiblingElement())
{
@@ -308,7 +389,7 @@ struct BulletMJCFImporterInternalData
int modelIndex = m_models.size();
UrdfModel* model = new UrdfModel();
m_models.push_back(model);
parseBody(rootxml,modelIndex, INVALID_LINK_INDEX,logger);
parseBody(defaults, rootxml,modelIndex, INVALID_LINK_INDEX,logger);
initTreeAndRoot(*model,logger);
handled = true;
}
@@ -336,7 +417,7 @@ struct BulletMJCFImporterInternalData
// modelPtr->m_rootLinks.push_back(linkPtr);
btVector3 inertialShift(0,0,0);
parseGeom(rootxml,modelIndex, linkIndex,logger,inertialShift);
parseGeom(defaults, rootxml,modelIndex, linkIndex,logger,inertialShift);
initTreeAndRoot(*modelPtr,logger);
handled = true;
@@ -355,7 +436,7 @@ struct BulletMJCFImporterInternalData
return true;
}
bool parseJoint(TiXmlElement* link_xml, int modelIndex, int parentLinkIndex, int linkIndex, MJCFErrorLogger* logger, const btTransform& parentToLinkTrans, btTransform& jointTransOut)
bool parseJoint(MyMJCFDefaults& defaults, TiXmlElement* link_xml, int modelIndex, int parentLinkIndex, int linkIndex, MJCFErrorLogger* logger, const btTransform& parentToLinkTrans, btTransform& jointTransOut)
{
bool jointHandled = false;
const char* jType = link_xml->Attribute("type");
@@ -399,7 +480,7 @@ struct BulletMJCFImporterInternalData
}
double range[2] = {1,0};
std::string lim = m_defaultJointLimited;
std::string lim = m_globalDefaults.m_defaultJointLimited;
if (limitedStr)
{
lim = limitedStr;
@@ -531,7 +612,7 @@ struct BulletMJCFImporterInternalData
*/
return false;
}
bool parseGeom(TiXmlElement* link_xml, int modelIndex, int linkIndex, MJCFErrorLogger* logger, btVector3& inertialShift)
bool parseGeom(MyMJCFDefaults& defaults, TiXmlElement* link_xml, int modelIndex, int linkIndex, MJCFErrorLogger* logger, btVector3& inertialShift)
{
UrdfLink** linkPtrPtr = m_models[modelIndex]->m_links.getAtIndex(linkIndex);
if (linkPtrPtr==0)
@@ -554,8 +635,66 @@ struct BulletMJCFImporterInternalData
const char* gType = link_xml->Attribute("type");
const char* sz = link_xml->Attribute("size");
const char* posS = link_xml->Attribute("pos");
int conDim = defaults.m_defaultConDim;
std::string rgba = m_defaultGeomRgba;
const char* conDimS = link_xml->Attribute("condim");
{
if (conDimS)
{
conDim = urdfLexicalCast<int>(conDimS);
}
}
double lateralFriction = defaults.m_defaultLateralFriction;
double spinningFriction = defaults.m_defaultSpinningFriction;
double rollingFriction = defaults.m_defaultRollingFriction;
const char* frictionS = link_xml->Attribute("friction");
if (frictionS)
{
btArray<std::string> pieces;
btArray<float> frictions;
btAlignedObjectArray<std::string> strArray;
urdfIsAnyOf(" ", strArray);
urdfStringSplit(pieces, sz, strArray);
for (int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
{
frictions.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
}
}
if (frictions.size()>0)
{
lateralFriction = frictions[0];
}
if (frictions.size()>1 && conDim>3)
{
spinningFriction = frictions[1];
}
if (frictions.size()>2 && conDim>4)
{
rollingFriction = frictions[2];
}
}
linkPtr->m_contactInfo.m_lateralFriction=lateralFriction;
linkPtr->m_contactInfo.m_spinningFriction=spinningFriction;
linkPtr->m_contactInfo.m_rollingFriction=rollingFriction;
if (conDim>3)
{
linkPtr->m_contactInfo.m_spinningFriction=defaults.m_defaultSpinningFriction;
linkPtr->m_contactInfo.m_flags |= URDF_CONTACT_HAS_SPINNING_FRICTION;
}
if (conDim>4)
{
linkPtr->m_contactInfo.m_rollingFriction=defaults.m_defaultRollingFriction;
linkPtr->m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
}
std::string rgba = defaults.m_defaultGeomRgba;
if (const char* rgbattr = link_xml->Attribute("rgba"))
{
rgba = rgbattr;
@@ -717,10 +856,10 @@ struct BulletMJCFImporterInternalData
UrdfCollision col;
col.m_flags |= URDF_HAS_COLLISION_GROUP;
col.m_collisionGroup = m_defaultCollisionGroup;
col.m_collisionGroup = defaults.m_defaultCollisionGroup;
col.m_flags |= URDF_HAS_COLLISION_MASK;
col.m_collisionMask = m_defaultCollisionMask;
col.m_collisionMask = defaults.m_defaultCollisionMask;
//contype, conaffinity
const char* conTypeStr = link_xml->Attribute("contype");
@@ -891,10 +1030,21 @@ struct BulletMJCFImporterInternalData
return orgChildLinkIndex;
}
bool parseBody(TiXmlElement* link_xml, int modelIndex, int orgParentLinkIndex, MJCFErrorLogger* logger)
bool parseBody(MyMJCFDefaults& defaults, TiXmlElement* link_xml, int modelIndex, int orgParentLinkIndex, MJCFErrorLogger* logger)
{
MyMJCFDefaults curDefaults = defaults;
int newParentLinkIndex = orgParentLinkIndex;
const char* childClassName = link_xml->Attribute("childclass");
if (childClassName)
{
MyMJCFDefaults* classDefaults = m_classDefaults[childClassName];
if (classDefaults)
{
curDefaults = *classDefaults;
}
}
const char* bodyName = link_xml->Attribute("name");
int orgChildLinkIndex = createBody(modelIndex,bodyName);
btTransform localInertialFrame;
@@ -998,7 +1148,7 @@ struct BulletMJCFImporterInternalData
}
int newLinkIndex = createBody(modelIndex,0);
parseJoint(xml,modelIndex,newParentLinkIndex, newLinkIndex,logger,linkTransform,jointTrans);
parseJoint(curDefaults,xml,modelIndex,newParentLinkIndex, newLinkIndex,logger,linkTransform,jointTrans);
//getLink(modelIndex,newLinkIndex)->m_linkTransformInWorld = jointTrans*linkTransform;
@@ -1013,7 +1163,7 @@ struct BulletMJCFImporterInternalData
int newLinkIndex = createBody(modelIndex,0);
btTransform joint2nextjoint = jointTrans.inverse();
btTransform unused;
parseJoint(xml,modelIndex,newParentLinkIndex, newLinkIndex,logger,joint2nextjoint,unused);
parseJoint(curDefaults, xml,modelIndex,newParentLinkIndex, newLinkIndex,logger,joint2nextjoint,unused);
newParentLinkIndex = newLinkIndex;
//todo: compute relative joint transforms (if any) and append to linkTransform
hasJoint = true;
@@ -1024,7 +1174,7 @@ struct BulletMJCFImporterInternalData
if (n == "geom")
{
btVector3 inertialShift(0,0,0);
parseGeom(xml,modelIndex, orgChildLinkIndex , logger,inertialShift);
parseGeom(curDefaults, xml,modelIndex, orgChildLinkIndex , logger,inertialShift);
if (!massDefined)
{
localInertialFrame.setOrigin(inertialShift);
@@ -1035,7 +1185,7 @@ struct BulletMJCFImporterInternalData
//recursive
if (n=="body")
{
parseBody(xml,modelIndex,orgChildLinkIndex,logger);
parseBody(curDefaults, xml,modelIndex,orgChildLinkIndex,logger);
handled = true;
}
@@ -1288,7 +1438,7 @@ bool BulletMJCFImporter::parseMJCFString(const char* xmlText, MJCFErrorLogger* l
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("default"); link_xml; link_xml = link_xml->NextSiblingElement("default"))
{
m_data->parseDefaults(link_xml,logger);
m_data->parseDefaults(m_data->m_globalDefaults,link_xml,logger);
}
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("compiler"); link_xml; link_xml = link_xml->NextSiblingElement("compiler"))
@@ -1304,12 +1454,12 @@ bool BulletMJCFImporter::parseMJCFString(const char* xmlText, MJCFErrorLogger* l
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("body"); link_xml; link_xml = link_xml->NextSiblingElement("body"))
{
m_data->parseRootLevel(link_xml,logger);
m_data->parseRootLevel(m_data->m_globalDefaults, link_xml,logger);
}
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("worldbody"); link_xml; link_xml = link_xml->NextSiblingElement("worldbody"))
{
m_data->parseRootLevel(link_xml,logger);
m_data->parseRootLevel(m_data->m_globalDefaults, link_xml,logger);
}
@@ -1570,7 +1720,7 @@ static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector<tinyobj::sha
}
class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes( int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
{
btCompoundShape* compound = new btCompoundShape();
m_data->m_allocatedCollisionShapes.push_back(compound);
@@ -1655,7 +1805,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str());
//create a convex hull for each shape, and store it in a btCompoundShape
childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin);
childShape = MjcfCreateConvexHullFromShapes( shapes, col->m_geometry.m_meshScale, m_data->m_globalDefaults.m_defaultCollisionMargin);
}
break;
@@ -1713,7 +1863,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
convexHull->optimizeConvexHull();
//convexHull->initializePolyhedralFeatures();
convexHull->setMargin(m_data->m_defaultCollisionMargin);
convexHull->setMargin(m_data->m_globalDefaults.m_defaultCollisionMargin);
childShape = convexHull;
}
}

110
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,17 +104,6 @@ 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;
@@ -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);
b3JointInfo jointInfo;
jointInfo.m_jointType = eFixedType;
#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
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)
{