Add offset to link and collision pose

tpe/lib/CMakeLists.txt

@@ -9,6 +9,7 @@ target_link_libraries(${tpelib_target}
   PUBLIC
   PRIVATE
     ignition-common${IGN_COMMON_VER}::requested
+    ignition-math${IGN_MATH_VER}::eigen3
 )
  ign_build_tests(
   TYPE UNIT_tpelib

tpe/lib/src/Collision.cc

@@ -25,6 +25,10 @@ class ignition::physics::tpelib::CollisionPrivate
   /// \brief Collision's geometry shape
   public: std::shared_ptr<Shape> shape = nullptr;
 
+  /// \brief Relative transform to its parent entity
+  public: Eigen::Isometry3d tf = math::eigen3::convert(
+    math::Pose3d(0, 0, 0, 0, 0, 0));
+
   /// \brief Collide bitmask
   public: uint16_t collideBitmask = 0xFF;
 };
@@ -124,3 +128,23 @@ uint16_t Collision::GetCollideBitmask() const
 {
   return this->dataPtr->collideBitmask;
 }
+
+//////////////////////////////////////////////////
+void Collision::UpdatePose(const math::Pose3d _linkPose)
+{
+  math::Pose3d nextPose = math::eigen3::convert(
+    math::eigen3::convert(_linkPose) * this->dataPtr->tf);
+  this->SetPose(nextPose);
+}
+
+//////////////////////////////////////////////////
+const Eigen::Isometry3d Collision::GetTf()
+{
+  return this->dataPtr->tf;
+}
+
+//////////////////////////////////////////////////
+void Collision::SetTf(const Eigen::Isometry3d _tf)
+{
+  this->dataPtr->tf = _tf;
+}

tpe/lib/src/Collision.hh

@@ -18,7 +18,7 @@
 #ifndef IGNITION_PHYSICS_TPE_LIB_SRC_COLLISION_HH_
 #define IGNITION_PHYSICS_TPE_LIB_SRC_COLLISION_HH_
 
-#include <ignition/math/AxisAlignedBox.hh>
+#include <ignition/math/eigen3/Conversions.hh>
 
 #include "ignition/physics/tpelib/Export.hh"
 
@@ -72,6 +72,18 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Collision : public Entity
   // Documentation inherited
   public: math::AxisAlignedBox GetBoundingBox(bool _force) override;
 
+  /// \brief Update the pose of the entity
+  /// \param[in] _linkPose parent link pose
+  public: void UpdatePose(const math::Pose3d _linkPose);
+
+  /// \brief Get the relative transform to its parent link
+  /// \return relative transform
+  public: const Eigen::Isometry3d GetTf();
+
+  /// \brief Set the relative transform to its parent link
+  /// \param[in] _tf relative transform
+  public: void SetTf(const Eigen::Isometry3d _tf);
+
   /// \brief Private data pointer class
   private: CollisionPrivate *dataPtr = nullptr;
 };

tpe/lib/src/Collision_TEST.cc

@@ -37,6 +37,15 @@ TEST(Collision, BasicAPI)
   collision.SetPose(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3));
   EXPECT_EQ(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3), collision.GetPose());
 
+  collision.SetTf(
+    math::eigen3::convert(math::Pose3d(0, 1.5, 0, 0, 0, 0)));
+  EXPECT_EQ(math::Pose3d(0, 1.5, 0, 0, 0, 0),
+            math::eigen3::convert(collision.GetTf()));
+
+  auto linkPose = math::Pose3d(1, 0, 0, 0, 0, 0);
+  collision.UpdatePose(linkPose);
+  EXPECT_EQ(math::Pose3d(1, 1.5, 0, 0, 0, 0), collision.GetPose());
+
   EXPECT_EQ(0xFF, collision.GetCollideBitmask());
   collision.SetCollideBitmask(0x03);
   EXPECT_EQ(0x03, collision.GetCollideBitmask());

tpe/lib/src/Link.cc

@@ -42,3 +42,23 @@ Entity &Link::AddCollision()
   this->ChildrenChanged();
   return *it->second.get();
 }
+
+//////////////////////////////////////////////////
+void Link::UpdatePose(const math::Pose3d _modelPose)
+{
+  math::Pose3d nextPose = math::eigen3::convert(
+    math::eigen3::convert(_modelPose) * this->tf);
+  this->SetPose(nextPose);
+}
+
+//////////////////////////////////////////////////
+const Eigen::Isometry3d Link::GetTf()
+{
+  return this->tf;
+}
+
+//////////////////////////////////////////////////
+void Link::SetTf(const Eigen::Isometry3d _tf)
+{
+  this->tf = _tf;
+}

tpe/lib/src/Link.hh

@@ -18,6 +18,8 @@
 #ifndef IGNITION_PHYSICS_TPE_LIB_SRC_LINK_HH_
 #define IGNITION_PHYSICS_TPE_LIB_SRC_LINK_HH_
 
+#include <ignition/math/eigen3/Conversions.hh>
+
 #include "ignition/physics/tpelib/Export.hh"
 
 #include "Entity.hh"
@@ -42,6 +44,22 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Link : public Entity
   /// \brief Add a collision
   /// \return Newly created Collision
   public: Entity &AddCollision();
+
+  /// \brief Update the pose of the entity
+  /// \param[in] _modelPose parent model pose
+  public: void UpdatePose(const math::Pose3d _modelPose);
+
+  /// \brief Get the relative transform to its parent model
+  /// \return relative transform
+  public: const Eigen::Isometry3d GetTf();
+
+  /// \brief Set the relative transform to its parent model
+  /// \param[in] _tf relative transform
+  public: void SetTf(const Eigen::Isometry3d _tf);
+
+  /// \brief Private relative transform to its parent entity
+  public: Eigen::Isometry3d tf =
+    math::eigen3::convert(math::Pose3d(0, 0, 0, 0, 0, 0));
 };
 
 }

tpe/lib/src/Link_TEST.cc

@@ -37,6 +37,16 @@ TEST(Link, BasicAPI)
   link.SetPose(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3));
   EXPECT_EQ(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3), link.GetPose());
 
+  link.SetTf(math::eigen3::convert(
+      math::Pose3d(0, 0.2, 0.5, 0, 1, 0)));
+  EXPECT_EQ(math::Pose3d(0, 0.2, 0.5, 0, 1, 0),
+            math::eigen3::convert(link.GetTf()));
+
+  auto modelPose = math::Pose3d(10, 0, 2, 1, 0, 0);
+  link.UpdatePose(modelPose);
+  EXPECT_EQ(math::Pose3d(10, -0.312675, 2.43845, 1.23686, 0.471978, 0.918989),
+            link.GetPose());
+
   Link link2;
   EXPECT_NE(link.GetId(), link2.GetId());
 }

tpe/lib/src/Model.cc

@@ -92,4 +92,13 @@ void Model::UpdatePose(
     currentPose.Pos() + _linearVelocity * _timeStep,
     currentPose.Rot().Integrate(_angularVelocity, _timeStep));
   this->SetPose(nextPose);
+
+  // update link poses
+  auto &children = this->GetChildren();
+  for (auto it = children.begin(); it != children.end(); ++it)
+  {
+    std::shared_ptr<Link> link =
+      std::dynamic_pointer_cast<Link>(it->second);
+    link->UpdatePose(this->GetPose());
+  }
 }