Fix Transform Tree to include tilted frame

Author: Jonathan Marino

core_sim/include/core_sim/actor/payload_actor.hpp

@@ -66,6 +66,9 @@ class PayloadActor : public Actor {
   const float GetMass() const;
   void SetMass(const float mass);
 
+  const Kinematics ComputeAttachedKinematics(
+      const Kinematics& kinematics) const;
+
   void BeginUpdate();
   void EndUpdate();
 

core_sim/src/actor/payload_actor.cpp

@@ -59,7 +59,7 @@ class PayloadActor::Impl : public ActorImpl {
   const Kinematics& GetKinematics() const;
   void UpdateKinematics(const Kinematics& kinematics);
   void SetCallbackKinematicsUpdated(const KinematicsCallback& callback);
-  const Kinematics& ComputeAttachedKinematics(
+  const Kinematics ComputeAttachedKinematics(
       const Kinematics& kinematics) const;
 
   const bool InAttachedState() const;
@@ -161,7 +161,7 @@ void PayloadActor::UpdateKinematics(const Kinematics& kinematics) {
   static_cast<PayloadActor::Impl*>(pimpl_.get())->UpdateKinematics(kinematics);
 }
 
-const Kinematics& PayloadActor::ComputeAttachedKinematics(
+const Kinematics PayloadActor::ComputeAttachedKinematics(
     const Kinematics& kinematics) const {
   return static_cast<PayloadActor::Impl*>(pimpl_.get())
       ->ComputeAttachedKinematics(kinematics);
@@ -311,7 +311,7 @@ void PayloadActor::Impl::UpdateKinematics(const Kinematics& kinematics) {
   topic_manager_.PublishTopic(payload_actor_pose_topic_, pose_msg);
 }
 
-const Kinematics& PayloadActor::Impl::ComputeAttachedKinematics(
+const Kinematics PayloadActor::Impl::ComputeAttachedKinematics(
     const Kinematics& kinematics) const {
   Vector3 new_pos(kinematics.pose.position.x() + payload_offset_.position.x(),
                   kinematics.pose.position.y() + payload_offset_.position.y(),

core_sim/src/transforms/transform_tree.cpp

@@ -7,6 +7,9 @@
 #include "Eigen/Dense"
 #include "Eigen/Geometry"
 
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+
 #ifdef _WIN32
 extern "C" void OutputDebugStringA(const char*);
 #else
@@ -41,7 +44,7 @@ TransformTree::~TransformTree() {
 void TransformTree::CombinePose(Pose* ppose_inout, const Pose& pose2) {
   // Rotate the position vector by the orientation of the frame
   auto rotated_position = pose2.orientation * ppose_inout->position;
-  // Translate the position to the position of frame to get the broader position
+  // translate the position to the position of frame to get the broader position
   ppose_inout->position = pose2.position + rotated_position;
   ppose_inout->orientation = ppose_inout->orientation * pose2.orientation;
 }

core_sim/test/gtest_transform_tree.cpp

@@ -2,6 +2,7 @@
 
 #include <cmath>
 
+
 #include "core_sim/transforms/transform_tree.hpp"
 #include "core_sim/transforms/transform_utils.hpp"
 #include "gtest/gtest.h"
@@ -184,11 +185,11 @@ TEST(TransformTree, ConvertCross2) {
   projectairsim::Quaternion quatA =
       projectairsim::TransformUtils::ToQuaternion(0.0f, 0.0f, 0.0f);
   projectairsim::Quaternion quatB =
-      projectairsim::TransformUtils::ToQuaternion(0.0f, M_PI, 0.0f);
+      projectairsim::TransformUtils::ToQuaternion(M_PI, 0.0f, 0.0f);
   projectairsim::Quaternion quatA2 =
       projectairsim::TransformUtils::ToQuaternion(0.0f, 0.0f, 0.0f);
   projectairsim::Quaternion quatB2 =
-      projectairsim::TransformUtils::ToQuaternion(0.0f, M_PI, 0.0f);
+      projectairsim::TransformUtils::ToQuaternion(M_PI, 0.0f, 0.0f);
   projectairsim::Quaternion quatAToGlobal = quatA * quatA2;
   projectairsim::Quaternion quatGlobalToB = quatB2.inverse() * quatB.inverse();
   projectairsim::Quaternion quatAInB =
@@ -200,7 +201,7 @@ TEST(TransformTree, ConvertCross2) {
   projectairsim::TransformTree transformtree;
   projectairsim::Vector3 vec3A = projectairsim::Vector3(1.0f, 2.0f, 3.0f);
   projectairsim::Vector3 vec3A2 = projectairsim::Vector3(4.0f, 5.0f, 6.0f);
-  projectairsim::Vector3 vec3B = projectairsim::Vector3(-10.0f, 20.0f, -30.0f);
+  projectairsim::Vector3 vec3B = projectairsim::Vector3(10.0f, -20.0f, -30.0f);
   projectairsim::Vector3 vec3B2 = projectairsim::Vector3(40.0f, 50.0f, 60.0f);
   projectairsim::Vector3 vec3AToGlobal = vec3A + vec3A2;
   projectairsim::Vector3 vec3BToB2Global = quatB2 * vec3B;
@@ -309,37 +310,4 @@ TEST(TransformTree, ConvertSemiCross) {
   EXPECT_NEAR(poseA.orientation.y(), poseAStart.orientation.y(), 1.0e-6);
   EXPECT_NEAR(poseA.orientation.z(), poseAStart.orientation.z(), 1.0e-6);
   EXPECT_NEAR(poseA.orientation.w(), poseAStart.orientation.w(), 1.0e-6);
-}
-
-TEST(TransformTree, NonzeroRelativePositionAndTiltedFrame) {
-  constexpr float ap_x = 0.0f;
-  constexpr float ap_y = 0.0f;
-  constexpr float ap_z = 0.0f;
-  projectairsim::Quaternion quatA =
-      projectairsim::TransformUtils::ToQuaternion(M_PI / 4, 0.0f, 0.0f);
-  projectairsim::TransformTree::StaticRefFrame staticrefframeA("A");
-  projectairsim::TransformTree transformtree;
-  projectairsim::Pose poseA ;
-  poseA.position.z() = -1.0f;
-  projectairsim::Pose poseGlobal;
-
-  staticrefframeA.SetLocalPose(
-      projectairsim::Pose(projectairsim::Vector3(ap_x, ap_y, ap_z), quatA));
-
-  transformtree.Register(&staticrefframeA,
-                         projectairsim::TransformTree::kRefFrameGlobal);
-  transformtree.Convert(poseA, staticrefframeA,
-                        projectairsim::TransformTree::kRefFrameGlobal,
-                        &poseGlobal);
-
-  // Verify the identity pose in A's reference frame is transformed into A's
-  // pose in the global reference frame
-  EXPECT_FLOAT_EQ(poseGlobal.position.x(), ap_x);
-  EXPECT_NEAR(poseGlobal.position.y(), -(std::sin(M_PI / 4)), 1.0e-6);
-  EXPECT_NEAR(poseGlobal.position.z(), -(std::cos(M_PI / 4)), 1.0e-6);
-
-  EXPECT_FLOAT_EQ(poseGlobal.orientation.x(), quatA.x());
-  EXPECT_FLOAT_EQ(poseGlobal.orientation.y(), quatA.y());
-  EXPECT_FLOAT_EQ(poseGlobal.orientation.z(), quatA.z());
-  EXPECT_FLOAT_EQ(poseGlobal.orientation.w(), quatA.w());
-}
+}