From 4db6fa9e29aa463dd8830f2fafb57d89f689d517 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 15 Mar 2017 15:38:50 -0700 Subject: [PATCH] update minitaur.py to use minitaur.urdf (instead of quadruped.urdf), also sort the legs in the same order as real hardware added test urdf files for minitaur with all fixed joints, or fixed knees. added some stiffness/damping to minitaur legs (testing) tiny_obj_loader, don't crash on invalid texture coordinates btMultiBodyConstraintSolver: sweep back and forward to reduce asymmetry --- data/quadruped/minitaur.urdf | 34 +- data/quadruped/minitaur_fixed_all.urdf | 913 +++++++++++++++++ data/quadruped/minitaur_fixed_knees.urdf | 929 ++++++++++++++++++ .../b3RobotSimulatorClientAPI.h | 2 +- .../Wavefront/tiny_obj_loader.cpp | 14 +- examples/pybullet/minitaur.py | 89 +- examples/pybullet/minitaur_evaluate.py | 2 +- examples/pybullet/minitaur_test.py | 4 + examples/pybullet/mylittleminitaur.py | 21 + examples/pybullet/quadruped.py | 141 +-- .../btMultiBodyConstraintSolver.cpp | 12 +- 11 files changed, 2043 insertions(+), 118 deletions(-) create mode 100644 data/quadruped/minitaur_fixed_all.urdf create mode 100644 data/quadruped/minitaur_fixed_knees.urdf create mode 100644 examples/pybullet/mylittleminitaur.py diff --git a/data/quadruped/minitaur.urdf b/data/quadruped/minitaur.urdf index c483bd4b4..663ff2a84 100644 --- a/data/quadruped/minitaur.urdf +++ b/data/quadruped/minitaur.urdf @@ -427,7 +427,9 @@ - + + + @@ -493,7 +495,9 @@ - + + + @@ -557,7 +561,9 @@ - + + + @@ -622,7 +628,9 @@ - + + + @@ -684,7 +692,9 @@ - + + + @@ -750,9 +760,11 @@ - + + + - + @@ -814,7 +826,9 @@ - + + + @@ -877,7 +891,9 @@ - + + + diff --git a/data/quadruped/minitaur_fixed_all.urdf b/data/quadruped/minitaur_fixed_all.urdf new file mode 100644 index 000000000..68815043a --- /dev/null +++ b/data/quadruped/minitaur_fixed_all.urdf @@ -0,0 +1,913 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/quadruped/minitaur_fixed_knees.urdf b/data/quadruped/minitaur_fixed_knees.urdf new file mode 100644 index 000000000..2c5903e1f --- /dev/null +++ b/data/quadruped/minitaur_fixed_knees.urdf @@ -0,0 +1,929 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h index f1ca5fe98..e6f658cc5 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -146,7 +146,7 @@ public: b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat); int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args=b3RobotSimulatorLoadUrdfFileArgs()); - bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args=b3RobotSimulatorLoadSdfFileArgs()); + bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results=b3RobotSimulatorLoadFileResults(), const struct b3RobotSimulatorLoadSdfFileArgs& args=b3RobotSimulatorLoadSdfFileArgs()); bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp index f69fa3dda..f56de333a 100644 --- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp +++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp @@ -218,8 +218,18 @@ updateVertex( } if (i.vt_idx >= 0) { - texcoords.push_back(in_texcoords[2*i.vt_idx+0]); - texcoords.push_back(in_texcoords[2*i.vt_idx+1]); + int numTexCoords = in_texcoords.size(); + int index0 = 2*i.vt_idx+0; + int index1 = 2*i.vt_idx+1; + + if (index0>=0 && (index0)=0 && (index1)m_multiBodyFrictionContactConstraints.size();j++) + for (int j1=0;j1m_multiBodyFrictionContactConstraints.size();j1++) { if (iteration < infoGlobal.m_numIterations) { - btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j]; + int index = iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index]; btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; //adjust friction limits here if (totalImpulse>btScalar(0))