Merge pull request #849 from lrapetti/viz-improve-transform-get

Add get frame transform in visualizer
robotology · Apr 9, 2021 · faf9eb2 · faf9eb2
2 parents 5d9d7a2 + 284f53b
commit faf9eb2
Show file tree

Hide file tree

Showing 5 changed files with 63 additions and 1 deletion.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -9,6 +9,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 - Add the possibility to use `MatrixView` and `Span` as input/output objects for `InverseKinematics` class (https://github.com/robotology/idyntree/pull/822).
+- Add the possibility to get frame trasform from the visualizer, and to use frame/link name in place of indices (https://github.com/robotology/idyntree/pull/849).
+
 ### Changed
 - The wheels uploaded to PyPI are now `manylinux_2_24` compliant (https://github.com/robotology/idyntree/pull/844)
 
@@ -23,7 +25,6 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Deprecated
 - The method `ModelVisualization::getWorldModelTransform()` is deprecated, and will be removed in iDynTree 4.0.
->>>>>>> master
 
 ## [3.0.0] - 2020-02-03
 

diff --git a/src/visualization/include/iDynTree/Visualizer.h b/src/visualization/include/iDynTree/Visualizer.h
@@ -602,6 +602,21 @@ class IModelVisualization
      * Get the transformation of given link with respect to visualizer world \f$ w_H_{link}\f$
      */
     virtual Transform getWorldLinkTransform(const LinkIndex& linkIndex) = 0;
+
+    /**
+     * Get the transformation of given link with respect to visualizer world \f$ w_H_{link}\f$
+     */
+    virtual Transform getWorldLinkTransform(const std::string& linkName) = 0;
+
+    /**
+     * Get the transformation of given frame with respect to visualizer world \f$ w_H_{frame}\f$
+     */
+    virtual Transform getWorldFrameTransform(const FrameIndex& frameIndex) = 0;
+
+    /**
+     * Get the transformation of given frame with respect to visualizer world \f$ w_H_{frame}\f$
+     */
+    virtual Transform getWorldFrameTransform(const std::string& frameName) = 0;
 };
 
 /**

diff --git a/src/visualization/src/DummyImplementations.h b/src/visualization/src/DummyImplementations.h
@@ -148,6 +148,9 @@ class DummyModelVisualization : public IModelVisualization
     virtual IJetsVisualization& jets() { return m_dummyJets;  }
     virtual Transform getWorldModelTransform() { return iDynTree::Transform::Identity(); }
     virtual Transform getWorldLinkTransform(const LinkIndex &) { return iDynTree::Transform::Identity(); }
+    virtual Transform getWorldFrameTransform(const FrameIndex &) { return iDynTree::Transform::Identity(); }
+    virtual Transform getWorldLinkTransform(const std::string &) { return iDynTree::Transform::Identity(); }
+    virtual Transform getWorldFrameTransform(const std::string &) { return iDynTree::Transform::Identity(); }
 };
 
 class DummyTexturesHandler : public ITexturesHandler

diff --git a/src/visualization/src/ModelVisualization.cpp b/src/visualization/src/ModelVisualization.cpp
@@ -234,6 +234,46 @@ Transform ModelVisualization::getWorldLinkTransform(const LinkIndex& linkIndex)
     return w_H_link;
 }
 
+Transform ModelVisualization::getWorldFrameTransform(const FrameIndex& frameIndex)
+{
+    if (!pimpl->m_model.isValidFrameIndex(frameIndex))
+    {
+        reportError("ModelVisualization","getWorldFrameTransform", "invalid frame index. returning identity transform");
+        return Transform::Identity();
+    }
+
+    LinkIndex linkIndex = pimpl->m_model.getFrameLink(frameIndex);
+
+    Transform w_H_frame;
+    w_H_frame = getWorldLinkTransform(linkIndex) * pimpl->m_model.getFrameTransform(frameIndex);
+
+    return w_H_frame;
+}
+
+Transform ModelVisualization::getWorldLinkTransform(const std::string& linkName)
+{
+    LinkIndex linkIndex = pimpl->m_model.getLinkIndex(linkName);
+    if (linkIndex == LINK_INVALID_INDEX)
+    {
+        reportError("ModelVisualization","getWorldLinkTransform", "invalid link name. returning identity transform");
+        return Transform::Identity();
+    }
+
+    return getWorldLinkTransform(linkIndex);
+}
+
+Transform ModelVisualization::getWorldFrameTransform(const std::string& frameName)
+{
+    FrameIndex frameIndex = pimpl->m_model.getFrameIndex(frameName);
+    if (frameIndex == LINK_INVALID_INDEX)
+    {
+        reportError("ModelVisualization","getWorldFrameTransform", "invalid frame name. returning identity transform");
+        return Transform::Identity();
+    }
+
+    return getWorldFrameTransform(frameIndex);
+}
+
 
 bool ModelVisualization::setPositions(const Transform& world_H_base, const VectorDynSize& jointPos)
 {

diff --git a/src/visualization/src/ModelVisualization.h b/src/visualization/src/ModelVisualization.h
@@ -51,6 +51,9 @@ class ModelVisualization: public IModelVisualization
     virtual IJetsVisualization& jets();
     virtual Transform getWorldModelTransform();
     virtual Transform getWorldLinkTransform(const LinkIndex& linkIndex);
+    virtual Transform getWorldFrameTransform(const FrameIndex& frameIndex);
+    virtual Transform getWorldLinkTransform(const std::string& linkName);
+    virtual Transform getWorldFrameTransform(const std::string& frameName);
 };
 
 }