Merge branch 'gz-physics7' into bullet_convex_manifold

CMakeLists.txt

@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR)
 #============================================================================
 # Initialize the project
 #============================================================================
-project(gz-physics7 VERSION 7.3.0)
+project(gz-physics7 VERSION 7.4.0)
 
 #============================================================================
 # Find gz-cmake

Changelog.md

@@ -1,5 +1,35 @@
 ## Gazebo Physics 7.x
 
+### Gazebo Physics 7.4.0 (2024-11-08)
+
+1. bullet-featherstone: Reset joint motor constraint's velocity target after each step
+    * [Pull request #699](https://github.com/gazebosim/gz-physics/pull/699)
+
+1. GzOdeCollisionDetector: Use static mutex in create
+    * [Pull request #675](https://github.com/gazebosim/gz-physics/pull/675)
+
+1. bullet-featherstone: add applied constraint to joint transmitted wrench
+    * [Pull request #668](https://github.com/gazebosim/gz-physics/pull/668)
+
+1. bullet-featherstone: Support empty links
+    * [Pull request #665](https://github.com/gazebosim/gz-physics/pull/665)
+
+1. Fix compile warnings
+    * [Pull request #663](https://github.com/gazebosim/gz-physics/pull/663)
+    * [Pull request #629](https://github.com/gazebosim/gz-physics/pull/629)
+
+1. bullet-featherstone: Enforce joint velocity and effort limits for velocity control commands
+    * [Pull request #658](https://github.com/gazebosim/gz-physics/pull/658)
+
+1. dartsim: optimize picking contact points with ODE collision detector
+    * [Pull request #584](https://github.com/gazebosim/gz-physics/pull/584)
+
+1. Add no gravity link support
+    * [Pull request #633](https://github.com/gazebosim/gz-physics/pull/633)
+
+1. Disable test failing due to ODE/libccd (backport #621)
+    * [Pull request #621](https://github.com/gazebosim/gz-physics/pull/621)
+
 ### Gazebo Physics 7.3.0 (2024-06-25)
 
 1. Backport: Add Cone as a collision shape

bullet-featherstone/src/SimulationFeatures.cc

@@ -51,6 +51,16 @@ void SimulationFeatures::WorldForwardStep(
   worldInfo->world->stepSimulation(static_cast<btScalar>(stepSize), 1,
                                    static_cast<btScalar>(stepSize));
 
+  // Reset joint velocity target after each step to be consistent with dart's
+  // joint velocity command behavior
+  for (auto & joint : this->joints)
+  {
+    if (joint.second->motor)
+    {
+      joint.second->motor->setVelocityTarget(btScalar(0));
+    }
+  }
+
   this->WriteRequiredData(_h);
   this->Write(_h.Get<ChangedWorldPoses>());
 }

dartsim/src/Base.hh

@@ -97,6 +97,7 @@ struct ModelInfo
   std::vector<std::shared_ptr<LinkInfo>> links {};
   std::vector<std::shared_ptr<JointInfo>> joints {};
   std::vector<std::size_t> nestedModels = {};
+  std::optional<Eigen::VectorXd> lastGoodPositions = {};
 };
 
 struct ShapeInfo

dartsim/src/GzOdeCollisionDetector.cc

@@ -16,6 +16,7 @@
 */
 
 #include <memory>
+#include <mutex>
 #include <unordered_map>
 #include <utility>
 
@@ -43,6 +44,13 @@ GzOdeCollisionDetector::Registrar<GzOdeCollisionDetector>
 /////////////////////////////////////////////////
 std::shared_ptr<GzOdeCollisionDetector> GzOdeCollisionDetector::create()
 {
+  // GzOdeCollisionDetector constructor calls the OdeCollisionDetector
+  // constructor, that calls the non-thread safe dInitODE2(0).
+  // To mitigate this problem, we use a static mutex to ensure that
+  // each GzOdeCollisionDetector constructor is called not at the same time.
+  // See https://github.com/gazebosim/gz-sim/issues/18 for more info.
+  static std::mutex odeInitMutex;
+  std::unique_lock<std::mutex> lock(odeInitMutex);
   return std::shared_ptr<GzOdeCollisionDetector>(new GzOdeCollisionDetector());
 }
 

dartsim/src/SimulationFeatures.cc

@@ -15,8 +15,10 @@
  *
 */
 
+#include <Eigen/Geometry>
 #include <limits>
 #include <memory>
+#include <sstream>
 #include <string>
 #include <unordered_map>
 #include <utility>
@@ -30,6 +32,7 @@
 #include <dart/constraint/ContactSurface.hpp>
 #endif
 
+#include <gz/common/Console.hh>
 #include <gz/common/Profiler.hh>
 
 #include <gz/math/Pose3.hh>
@@ -106,6 +109,36 @@ void SimulationFeatures::WorldForwardStep(
 
   // TODO(MXG): Parse input
   world->step();
+
+  for (auto &[ignore, modelInfo] : this->models.idToObject)
+  {
+    const Eigen::VectorXd &positions = modelInfo->model->getPositions();
+    if (positions.hasNaN())
+    {
+      std::stringstream ss;
+      ss << "Some links in model '" << modelInfo->localName
+         << "' have invalid poses. ";
+      if (modelInfo->lastGoodPositions)
+      {
+        ss << "Resetting to last known poses.";
+        modelInfo->model->setPositions(*modelInfo->lastGoodPositions);
+      }
+      else
+      {
+        ss << "Resetting to zero poses.";
+        modelInfo->model->resetPositions();
+      }
+      gzerr << ss.str()
+            << " Also resetting velocities and accelerations to zero."
+            << std::endl;
+      modelInfo->model->resetVelocities();
+      modelInfo->model->resetAccelerations();
+    }
+    else
+    {
+      modelInfo->lastGoodPositions = positions;
+    }
+  }
   this->WriteRequiredData(_h);
   this->Write(_h.Get<ChangedWorldPoses>());
   // TODO(MXG): Fill in state

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>gz-physics7</name>
-  <version>7.3.0</version>
+  <version>7.4.0</version>
   <description>Gazebo Physics : Physics classes and functions for robot applications</description>
   <maintainer email="[email protected]">Steve Peters</maintainer>
   <license>Apache License 2.0</license>

test/common_test/joint_features.cc

@@ -174,14 +174,11 @@ TYPED_TEST(JointFeaturesTest, JointSetCommand)
       EXPECT_NEAR(1.0, joint->GetVelocity(0), 1e-2);
     }
 
-    if(this->PhysicsEngineName(name) == "dartsim")
+    for (std::size_t i = 0; i < numSteps; ++i)
     {
-      for (std::size_t i = 0; i < numSteps; ++i)
-      {
-        // expect joint to freeze in subsequent steps without SetVelocityCommand
-        world->Step(output, state, input);
-        EXPECT_NEAR(0.0, joint->GetVelocity(0), 1e-1);
-      }
+      // expect joint to freeze in subsequent steps without SetVelocityCommand
+      world->Step(output, state, input);
+      EXPECT_NEAR(0.0, joint->GetVelocity(0), 1e-1);
     }
 
     // Check that invalid velocity commands don't cause collisions to fail