Merge 64eed19 into a01ff7c

gazebosim · Dec 6, 2022 · bba9874 · bba9874
2 parents a01ff7c + 64eed19
commit bba9874
Show file tree

Hide file tree

Showing 4 changed files with 28 additions and 1 deletion.
diff --git a/bullet-featherstone/src/SimulationFeatures.hh b/bullet-featherstone/src/SimulationFeatures.hh
@@ -21,6 +21,7 @@
 #include <unordered_map>
 #include <vector>
 
+#include <gz/physics/CanWriteData.hh>
 #include <gz/physics/ForwardStep.hh>
 #include <gz/physics/GetContacts.hh>
 
@@ -36,6 +37,8 @@ struct SimulationFeatureList : gz::physics::FeatureList<
 > { };
 
 class SimulationFeatures :
+    public CanWriteExpectedData<SimulationFeatures,
+      ExpectData<ChangedWorldPoses>>,
     public virtual Base,
     public virtual Implements3d<SimulationFeatureList>
 {

diff --git a/bullet/src/Base.hh b/bullet/src/Base.hh
@@ -143,6 +143,25 @@ inline Eigen::Vector3d convert(btVector3 vec)
   return val;
 }
 
+inline math::Vector3d convertToGz(const btVector3 &_vec)
+{
+  return math::Vector3d(_vec[0], _vec[1], _vec[2]);
+}
+
+inline math::Quaterniond convertToGz(const btMatrix3x3 &_mat)
+{
+  return math::Quaterniond(math::Matrix3d(_mat[0][0], _mat[0][1], _mat[0][2],
+                                          _mat[1][0], _mat[1][1], _mat[1][2],
+                                          _mat[2][0], _mat[2][1], _mat[2][2]));
+}
+
+inline math::Pose3d convertToGz(const btTransform &_pose)
+{
+  return math::Pose3d(convertToGz(_pose.getOrigin()),
+                      convertToGz(_pose.getBasis()));
+}
+
+
 class Base : public Implements3d<FeatureList<Feature>>
 {
   public: std::size_t entityCount = 0;
@@ -156,6 +175,7 @@ class Base : public Implements3d<FeatureList<Feature>>
   {
     const auto id = this->GetNextEntity();
     assert(id == 0);
+    (void)id;
 
     return this->GenerateIdentity(0);
   }

diff --git a/bullet/src/SimulationFeatures.cc b/bullet/src/SimulationFeatures.cc
@@ -60,7 +60,8 @@ void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const
     if (info)
     {
       WorldPose wp;
-      wp.pose = info->pose;
+      wp.pose = convertToGz(info->link->getCenterOfMassTransform()) *
+                info->inertialPose.Inverse();
       wp.body = id;
 
       auto iter = this->prevLinkPoses.find(id);

diff --git a/bullet/src/SimulationFeatures.hh b/bullet/src/SimulationFeatures.hh
@@ -21,6 +21,7 @@
 #include <unordered_map>
 #include <vector>
 
+#include <gz/physics/CanWriteData.hh>
 #include <gz/physics/ForwardStep.hh>
 
 #include "Base.hh"
@@ -34,6 +35,8 @@ struct SimulationFeatureList : gz::physics::FeatureList<
 > { };
 
 class SimulationFeatures :
+    public CanWriteExpectedData<SimulationFeatures,
+      ExpectData<ChangedWorldPoses>>,
     public virtual Base,
     public virtual Implements3d<SimulationFeatureList>
 {