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>
<default class="MPL"> <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"/> <joint limited="true" damping="0.2" armature=".01"/>
<site material="MatTouch" type="ellipsoid" group="3"/> <site material="MatTouch" type="ellipsoid" group="3"/>
<position ctrllimited="true" kp="10"/> <position ctrllimited="true" kp="10"/>
@@ -104,7 +104,7 @@
<!-- ======= ROBOT ======= --> <!-- ======= 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" /> <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"/> <geom type="mesh" mesh="wristy"/>
<joint type="free"/> <joint type="free"/>

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

@@ -1,6 +1,8 @@
#include "BulletMJCFImporter.h" #include "BulletMJCFImporter.h"
#include "../../ThirdPartyLibs/tinyxml/tinyxml.h" #include "../../ThirdPartyLibs/tinyxml/tinyxml.h"
#include "Bullet3Common/b3FileUtils.h" #include "Bullet3Common/b3FileUtils.h"
#include "Bullet3Common/b3HashMap.h"
#include <string> #include <string>
#include "../../Utils/b3ResourcePath.h" #include "../../Utils/b3ResourcePath.h"
#include <iostream> #include <iostream>
@@ -129,6 +131,35 @@ struct MyMJCFAsset
std::string m_fileName; 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 struct BulletMJCFImporterInternalData
{ {
GUIHelperInterface* m_guiHelper; GUIHelperInterface* m_guiHelper;
@@ -149,26 +180,17 @@ struct BulletMJCFImporterInternalData
int m_activeModel; int m_activeModel;
int m_activeBodyUniqueId; 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 //todo: for better MJCF compatibility, we would need a stack of default values
std::string m_defaultJointLimited; MyMJCFDefaults m_globalDefaults;
b3HashMap<b3HashString, MyMJCFDefaults> m_classDefaults;
// geom defaults
std::string m_defaultGeomRgba;
//those collision shapes are deleted by caller (todo: make sure this happens!) //those collision shapes are deleted by caller (todo: make sure this happens!)
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes; btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
BulletMJCFImporterInternalData() BulletMJCFImporterInternalData()
:m_activeModel(-1), :m_activeModel(-1),
m_activeBodyUniqueId(-1), m_activeBodyUniqueId(-1)
m_defaultCollisionGroup(1),
m_defaultCollisionMask(1),
m_defaultCollisionMargin(0.001)//assume unit meters, margin is 1mm
{ {
m_pathPrefix[0] = 0; 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; bool handled= false;
//rudimentary 'default' support, would need more work for better feature coverage //rudimentary 'default' support, would need more work for better feature coverage
@@ -254,6 +278,27 @@ struct BulletMJCFImporterInternalData
{ {
std::string n = child_xml->Value(); 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") if (n=="inertial")
{ {
} }
@@ -269,7 +314,7 @@ struct BulletMJCFImporterInternalData
// limited="true" // limited="true"
if (const char* conTypeStr = child_xml->Attribute("limited")) if (const char* conTypeStr = child_xml->Attribute("limited"))
{ {
m_defaultJointLimited = child_xml->Attribute("limited"); defaults.m_defaultJointLimited = child_xml->Attribute("limited");
} }
} }
if (n=="geom") if (n=="geom")
@@ -278,24 +323,60 @@ struct BulletMJCFImporterInternalData
const char* conTypeStr = child_xml->Attribute("contype"); const char* conTypeStr = child_xml->Attribute("contype");
if (conTypeStr) if (conTypeStr)
{ {
m_defaultCollisionGroup = urdfLexicalCast<int>(conTypeStr); defaults.m_defaultCollisionGroup = urdfLexicalCast<int>(conTypeStr);
} }
const char* conAffinityStr = child_xml->Attribute("conaffinity"); const char* conAffinityStr = child_xml->Attribute("conaffinity");
if (conAffinityStr) if (conAffinityStr)
{ {
m_defaultCollisionMask = urdfLexicalCast<int>(conAffinityStr); defaults.m_defaultCollisionMask = urdfLexicalCast<int>(conAffinityStr);
} }
const char* rgba = child_xml->Attribute("rgba"); const char* rgba = child_xml->Attribute("rgba");
if (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; handled=true;
return handled; 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()) for (TiXmlElement* rootxml = root_xml->FirstChildElement() ; rootxml ; rootxml = rootxml->NextSiblingElement())
{ {
@@ -308,7 +389,7 @@ struct BulletMJCFImporterInternalData
int modelIndex = m_models.size(); int modelIndex = m_models.size();
UrdfModel* model = new UrdfModel(); UrdfModel* model = new UrdfModel();
m_models.push_back(model); m_models.push_back(model);
parseBody(rootxml,modelIndex, INVALID_LINK_INDEX,logger); parseBody(defaults, rootxml,modelIndex, INVALID_LINK_INDEX,logger);
initTreeAndRoot(*model,logger); initTreeAndRoot(*model,logger);
handled = true; handled = true;
} }
@@ -336,7 +417,7 @@ struct BulletMJCFImporterInternalData
// modelPtr->m_rootLinks.push_back(linkPtr); // modelPtr->m_rootLinks.push_back(linkPtr);
btVector3 inertialShift(0,0,0); btVector3 inertialShift(0,0,0);
parseGeom(rootxml,modelIndex, linkIndex,logger,inertialShift); parseGeom(defaults, rootxml,modelIndex, linkIndex,logger,inertialShift);
initTreeAndRoot(*modelPtr,logger); initTreeAndRoot(*modelPtr,logger);
handled = true; handled = true;
@@ -355,7 +436,7 @@ struct BulletMJCFImporterInternalData
return true; 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; bool jointHandled = false;
const char* jType = link_xml->Attribute("type"); const char* jType = link_xml->Attribute("type");
@@ -399,7 +480,7 @@ struct BulletMJCFImporterInternalData
} }
double range[2] = {1,0}; double range[2] = {1,0};
std::string lim = m_defaultJointLimited; std::string lim = m_globalDefaults.m_defaultJointLimited;
if (limitedStr) if (limitedStr)
{ {
lim = limitedStr; lim = limitedStr;
@@ -531,7 +612,7 @@ struct BulletMJCFImporterInternalData
*/ */
return false; 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); UrdfLink** linkPtrPtr = m_models[modelIndex]->m_links.getAtIndex(linkIndex);
if (linkPtrPtr==0) if (linkPtrPtr==0)
@@ -554,8 +635,66 @@ struct BulletMJCFImporterInternalData
const char* gType = link_xml->Attribute("type"); const char* gType = link_xml->Attribute("type");
const char* sz = link_xml->Attribute("size"); const char* sz = link_xml->Attribute("size");
const char* posS = link_xml->Attribute("pos"); 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")) if (const char* rgbattr = link_xml->Attribute("rgba"))
{ {
rgba = rgbattr; rgba = rgbattr;
@@ -717,10 +856,10 @@ struct BulletMJCFImporterInternalData
UrdfCollision col; UrdfCollision col;
col.m_flags |= URDF_HAS_COLLISION_GROUP; 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_flags |= URDF_HAS_COLLISION_MASK;
col.m_collisionMask = m_defaultCollisionMask; col.m_collisionMask = defaults.m_defaultCollisionMask;
//contype, conaffinity //contype, conaffinity
const char* conTypeStr = link_xml->Attribute("contype"); const char* conTypeStr = link_xml->Attribute("contype");
@@ -891,10 +1030,21 @@ struct BulletMJCFImporterInternalData
return orgChildLinkIndex; 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; 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"); const char* bodyName = link_xml->Attribute("name");
int orgChildLinkIndex = createBody(modelIndex,bodyName); int orgChildLinkIndex = createBody(modelIndex,bodyName);
btTransform localInertialFrame; btTransform localInertialFrame;
@@ -998,7 +1148,7 @@ struct BulletMJCFImporterInternalData
} }
int newLinkIndex = createBody(modelIndex,0); 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; //getLink(modelIndex,newLinkIndex)->m_linkTransformInWorld = jointTrans*linkTransform;
@@ -1013,7 +1163,7 @@ struct BulletMJCFImporterInternalData
int newLinkIndex = createBody(modelIndex,0); int newLinkIndex = createBody(modelIndex,0);
btTransform joint2nextjoint = jointTrans.inverse(); btTransform joint2nextjoint = jointTrans.inverse();
btTransform unused; btTransform unused;
parseJoint(xml,modelIndex,newParentLinkIndex, newLinkIndex,logger,joint2nextjoint,unused); parseJoint(curDefaults, xml,modelIndex,newParentLinkIndex, newLinkIndex,logger,joint2nextjoint,unused);
newParentLinkIndex = newLinkIndex; newParentLinkIndex = newLinkIndex;
//todo: compute relative joint transforms (if any) and append to linkTransform //todo: compute relative joint transforms (if any) and append to linkTransform
hasJoint = true; hasJoint = true;
@@ -1024,7 +1174,7 @@ struct BulletMJCFImporterInternalData
if (n == "geom") if (n == "geom")
{ {
btVector3 inertialShift(0,0,0); btVector3 inertialShift(0,0,0);
parseGeom(xml,modelIndex, orgChildLinkIndex , logger,inertialShift); parseGeom(curDefaults, xml,modelIndex, orgChildLinkIndex , logger,inertialShift);
if (!massDefined) if (!massDefined)
{ {
localInertialFrame.setOrigin(inertialShift); localInertialFrame.setOrigin(inertialShift);
@@ -1035,7 +1185,7 @@ struct BulletMJCFImporterInternalData
//recursive //recursive
if (n=="body") if (n=="body")
{ {
parseBody(xml,modelIndex,orgChildLinkIndex,logger); parseBody(curDefaults, xml,modelIndex,orgChildLinkIndex,logger);
handled = true; 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")) 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")) 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")) 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")) 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);
} }
@@ -1655,7 +1805,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str()); 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 //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; break;
@@ -1713,7 +1863,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
convexHull->optimizeConvexHull(); convexHull->optimizeConvexHull();
//convexHull->initializePolyhedralFeatures(); //convexHull->initializePolyhedralFeatures();
convexHull->setMargin(m_data->m_defaultCollisionMargin); convexHull->setMargin(m_data->m_globalDefaults.m_defaultCollisionMargin);
childShape = convexHull; childShape = convexHull;
} }
} }

106
examples/RobotSimulator/VRGloveSimulatorMain.cpp
View File

@@ -30,7 +30,7 @@ void setJointMotorPositionControl(b3RobotSimulatorClientAPI* sim, int obUid, int
int main(int argc, char* argv[]) int main(int argc, char* argv[])
{ {
b3CommandLineArgs args(argc,argv); b3CommandLineArgs args(argc,argv);
std::string port="COM10"; std::string port="COM9";
args.GetCmdLineArgument("port",port); args.GetCmdLineArgument("port",port);
int baud = 115200; int baud = 115200;
args.GetCmdLineArgument("speed",baud); args.GetCmdLineArgument("speed",baud);
@@ -41,15 +41,15 @@ int main(int argc, char* argv[])
int disableShadows = 0; int disableShadows = 0;
args.GetCmdLineArgument("disableShadows",disableShadows); 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()); 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(); b3RobotSimulatorClientAPI* sim = new b3RobotSimulatorClientAPI();
@@ -82,6 +82,12 @@ int main(int argc, char* argv[])
sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0); sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);
} }
if (wireFrame)
{
sim->configureDebugVisualizer( COV_ENABLE_WIREFRAME, 1);
}
sim->setTimeOut(12345); sim->setTimeOut(12345);
//syncBodies is only needed when connecting to an existing physics server that has already some bodies //syncBodies is only needed when connecting to an existing physics server that has already some bodies
sim->syncBodies(); sim->syncBodies();
@@ -98,17 +104,6 @@ int main(int argc, char* argv[])
sim->loadURDF("plane_with_collision_audio.urdf"); 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; int handUid = -1;
@@ -127,14 +122,24 @@ int main(int argc, char* argv[])
printf("Cannot load MJCF file %s\n", mjcfFileName); printf("Cannot load MJCF file %s\n", mjcfFileName);
} }
#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)); b3Quaternion handOrn = sim->getQuaternionFromEuler(b3MakeVector3(3.14,-3.14/2,0));
b3Vector3 handPos = b3MakeVector3(-0.05,0,0.02); b3Vector3 handPos = b3MakeVector3(-0.05,0,0.02);
#endif
b3JointInfo jointInfo;
jointInfo.m_jointType = eFixedType;
b3Vector3 handStartPosWorld = b3MakeVector3(0.500000,0.300006,0.900000); b3Vector3 handStartPosWorld = b3MakeVector3(0.500000,0.300006,0.900000);
b3Quaternion handStartOrnWorld = b3Quaternion ::getIdentity(); 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[0] = handStartPosWorld[0];
jointInfo.m_childFrame[1] = handStartPosWorld[1]; jointInfo.m_childFrame[1] = handStartPosWorld[1];
jointInfo.m_childFrame[2] = handStartPosWorld[2]; jointInfo.m_childFrame[2] = handStartPosWorld[2];
@@ -152,17 +157,32 @@ int main(int argc, char* argv[])
jointInfo.m_parentFrame[6] = handOrn[3]; jointInfo.m_parentFrame[6] = handOrn[3];
int handConstraintId = sim->createConstraint(handUid,-1,-1,-1,&jointInfo); 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++) for (int j=0; j<sim->getNumJoints(handUid);j++)
{ {
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
controlArgs.m_maxTorqueValue = maxFingerForce ; controlArgs.m_maxTorqueValue = maxFingerForce ;
controlArgs.m_kp = 0.1;
controlArgs.m_kd = 1;
controlArgs.m_targetPosition = 0; controlArgs.m_targetPosition = 0;
controlArgs.m_targetVelocity = 0; controlArgs.m_targetVelocity = 0;
sim->setJointMotorControl(handUid,j,controlArgs); 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; b3Clock clock;
double startTime = clock.getTimeInSeconds(); double startTime = clock.getTimeInSeconds();
double simWallClockSeconds = 20.; double simWallClockSeconds = 20.;
@@ -170,11 +190,36 @@ int main(int argc, char* argv[])
int vidLogId = -1; int vidLogId = -1;
int minitaurLogId = -1; int minitaurLogId = -1;
int rotateCamera = 0; 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()) while (sim->canSubmitCommand())
{ {
b3VREventsData vrEvents; b3VREventsData vrEvents;
int deviceTypeFilter = VR_DEVICE_GENERIC_TRACKER;
sim->getVREvents(&vrEvents, deviceTypeFilter); sim->getVREvents(&vrEvents, deviceTypeFilter);
//instead of iterating over all vr events, we just take the most up-to-date one //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]); // printf("e.pos=%f,%f,%f\n",e.m_pos[0],e.m_pos[1],e.m_pos[2]);
b3JointInfo changeConstraintInfo; b3JointInfo changeConstraintInfo;
changeConstraintInfo.m_flags = 0; changeConstraintInfo.m_flags = 0;
changeConstraintInfo.m_jointMaxForce = 50; changeConstraintInfo.m_jointMaxForce = maxArmForce ;
changeConstraintInfo.m_flags |= eJointChangeMaxForce; changeConstraintInfo.m_flags |= eJointChangeMaxForce;
changeConstraintInfo.m_childFrame[0] = e.m_pos[0]; 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 //read the serial output from the hand, extract into parts
std::string result = my_serial.readline(); std::string result;
//my_serial.flush(); try
{
result = my_serial.readline();
} catch(...)
{
}
if (result.length()) if (result.length())
{ {
//my_serial.flush();
int res = result.find("\n"); int res = result.find("\n");
while (res<0) while (res<0)
{ {