Commit 9f318d83 authored by Jordan Lack's avatar Jordan Lack

Minor rework to ReferenceFrame class to fix some memory issues

parent 6ee85f98
Pipeline #57698424 failed with stages
in 15 minutes
......@@ -15,8 +15,9 @@
#include <memory>
#include "rdl_dynamics/SpatialAlgebraOperators.h"
#include <string>
#include <vector>
#include <climits>
#include <utility>
#include <vector>
#include "rdl_dynamics/FrameExceptions.hpp"
namespace RobotDynamics
......@@ -81,7 +82,7 @@ typedef std::shared_ptr<ReferenceFrame> ReferenceFramePtr;
* This class and its implementation are an adaptation of ReferenceFrame.java by <a href="http://robots.ihmc.us/">Jerry
**Pratt and the IHMC Robotics Group</a>.
*/
class ReferenceFrame
class ReferenceFrame : public std::enable_shared_from_this<ReferenceFrame>
{
public:
/**
......@@ -89,12 +90,12 @@ class ReferenceFrame
* @param referenceFrameToCopy
*/
ReferenceFrame(const ReferenceFrame& referenceFrameToCopy)
: framesStartingWithRootEndingWithThis(referenceFrameToCopy.framesStartingWithRootEndingWithThis)
, frameName(referenceFrameToCopy.frameName)
: frameName(referenceFrameToCopy.frameName)
, parentFrame(referenceFrameToCopy.parentFrame)
, transformFromParent(referenceFrameToCopy.transformFromParent)
, transformToRoot(referenceFrameToCopy.transformToRoot)
, inverseTransformToRoot(referenceFrameToCopy.inverseTransformToRoot)
, rootFrame(referenceFrameToCopy.rootFrame)
, isWorldFrame(referenceFrameToCopy.isWorldFrame)
, isBodyFrame(referenceFrameToCopy.isBodyFrame)
, movableBodyId(referenceFrameToCopy.movableBodyId)
......@@ -126,11 +127,23 @@ class ReferenceFrame
"parentFrame=nullptr");
}
framesStartingWithRootEndingWithThis = constructVectorOfFramesStartingWithRootEndingWithThis(ReferenceFramePtr(this));
ReferenceFramePtr root = parentFrame;
while(root->getParentFrame()!=nullptr)
{
root = root->getParentFrame();
}
rootFrame = root.get();
update();
}
ReferenceFramePtr getPtr()
{
return shared_from_this();
}
/**
* @brief Empty constructor. All contained ptrs will be initialize to nullptr
*/
......@@ -138,12 +151,17 @@ class ReferenceFrame
{
}
ReferenceFrame* getRootFrame()
{
return rootFrame;
}
/**
* @brief Destructor
*/
virtual ~ReferenceFrame()
{
std::cout << "DELETING " << frameName << std::endl;
}
/**
......@@ -215,25 +233,6 @@ class ReferenceFrame
return this->inverseTransformToRoot;
}
/**
* @brief Get a pointer to this frames root frame
* @return Pointer to this frames root ReferenceFrame
*/
inline ReferenceFramePtr getRootFrame()
{
return this->framesStartingWithRootEndingWithThis[0];
}
/**
* @brief Get a vector containing all frames in the chain from this frames root to this frame
* @return A vector of framse with element 0 correspordind to this frames root, and the final element coresponding
* to this frame
*/
inline std::vector<ReferenceFramePtr> getFramesStartingWithRootEndingWithThis()
{
return framesStartingWithRootEndingWithThis;
}
/**
* @brief get a pointer to this frames parent
* @return ReferenceFrame::parentFrame
......@@ -320,7 +319,6 @@ class ReferenceFrame
ReferenceFrame& operator=(const ReferenceFrame& other)
{
worldFrame = other.worldFrame;
framesStartingWithRootEndingWithThis = other.framesStartingWithRootEndingWithThis;
frameName = other.frameName;
parentFrame = other.parentFrame;
transformFromParent = other.transformFromParent;
......@@ -329,6 +327,7 @@ class ReferenceFrame
isWorldFrame = other.isWorldFrame;
isBodyFrame = other.isBodyFrame;
movableBodyId = other.movableBodyId;
rootFrame = other.rootFrame;
return *this;
}
......@@ -341,11 +340,9 @@ class ReferenceFrame
* @param movableBodyId
* @param isBodyFrame
*/
ReferenceFrame(const std::string& frameName, bool isWorldFrame, unsigned int movableBodyId, bool isBodyFrame)
: frameName(frameName), parentFrame(nullptr), isWorldFrame(isWorldFrame), isBodyFrame(isBodyFrame), movableBodyId(movableBodyId)
ReferenceFrame(const std::string& frameName, bool isWorldFrame, unsigned int movableBodyId, bool isBodyFrame) : frameName(frameName), parentFrame(nullptr), isWorldFrame(isWorldFrame), isBodyFrame(isBodyFrame), movableBodyId(movableBodyId)
{
framesStartingWithRootEndingWithThis = constructVectorOfFramesStartingWithRootEndingWithThis(ReferenceFramePtr(this));
rootFrame = this;
update();
}
......@@ -354,22 +351,12 @@ class ReferenceFrame
* @param frameName
* @return Pointer to the created world frame
*/
static ReferenceFramePtr createAWorldFrame(const std::string& frameName)
static ReferenceFramePtr createAWorldFrame(const std::string& frameName = "World")
{
return ReferenceFramePtr(new ReferenceFrame(frameName, true, 0, true));
}
/**
* @brief Upon creation of a ReferenceFrame, this method is called to create the vector of frames each ReferenceFrame
**holds
* @param thisFrame
* @return ReferenceFrame::framesStartingWithRootEndingWithThis
*/
static std::vector<ReferenceFramePtr> constructVectorOfFramesStartingWithRootEndingWithThis(ReferenceFramePtr thisFrame);
static ReferenceFramePtr worldFrame; /**< Static world frame pointer */
std::vector<ReferenceFramePtr> framesStartingWithRootEndingWithThis; /**< A vector of frames holding pointers to all
frames in the chain from root to this frame */
std::string frameName; /**< A frames name */
ReferenceFramePtr parentFrame; /**< Pointer to a frames parent frames */
RobotDynamics::Math::SpatialTransform transformFromParent; /**< SpatialTransform to a frame from its parent*/
......@@ -378,6 +365,7 @@ class ReferenceFrame
RobotDynamics::Math::SpatialTransform inverseTransformToRoot; /**< SpatialTransform to a frame from the root frame
*/
ReferenceFrame* rootFrame;
bool isWorldFrame; /**< True if a frame is the world frame, false
otherwise */
bool isBodyFrame; /**< True if a frame is a body frame, false
......
......@@ -9,34 +9,7 @@
namespace RobotDynamics
{
ReferenceFramePtr ReferenceFrame::worldFrame = ReferenceFrame::createAWorldFrame("World");
std::vector<ReferenceFramePtr> ReferenceFrame::constructVectorOfFramesStartingWithRootEndingWithThis(ReferenceFramePtr thisFrame)
{
ReferenceFramePtr parentFrame = thisFrame->getParentFrame();
if (parentFrame == nullptr)
{
// referenceFrame is the root frame.
std::vector<ReferenceFramePtr> vector(1);
vector[0] = thisFrame;
return vector;
}
// Need to add referenceFrame to the chain.
int nElements = parentFrame->framesStartingWithRootEndingWithThis.size() + 1;
std::vector<ReferenceFramePtr> vector(nElements);
for (int i = 0; i < (nElements - 1); i++)
{
vector[i] = parentFrame->framesStartingWithRootEndingWithThis[i];
}
vector[nElements - 1] = thisFrame;
return vector;
}
ReferenceFramePtr ReferenceFrame::worldFrame = ReferenceFrame::createAWorldFrame();
RobotDynamics::Math::SpatialTransform ReferenceFrame::getTransformToDesiredFrame(ReferenceFramePtr desiredFrame)
{
......@@ -81,7 +54,7 @@ void ReferenceFrame::checkReferenceFramesMatch(ReferenceFrame* referenceFrame) c
void ReferenceFrame::verifyFramesHaveSameRoot(ReferenceFramePtr frame)
{
if (!(frame->getRootFrame() == this->getRootFrame()))
if (rootFrame != frame->getRootFrame())
{
std::string msg = "Frames " + frame->getName() + " and " + this->getName() + " have mismatched roots!";
throw ReferenceFrameException(msg);
......
This diff is collapsed.
......@@ -358,41 +358,30 @@ static inline RobotDynamics::Math::SpatialTransform createRandomSpatialTransform
static inline RobotDynamics::Math::SpatialTransform getTransformToRootByClimbingTree(ReferenceFramePtr frame)
{
const std::vector<ReferenceFramePtr> framesStartingWithRootEndingWithFrame = frame->getFramesStartingWithRootEndingWithThis();
if(frame->getParentFrame()==nullptr)
{
return RobotDynamics::Math::SpatialTransform();
}
RobotDynamics::Math::SpatialTransform transform;
if (frame->getParentFrame() != nullptr)
ReferenceFramePtr parent = frame;
while(parent != nullptr)
{
for (int i = 0; i < framesStartingWithRootEndingWithFrame.size(); i++)
{
transform *= framesStartingWithRootEndingWithFrame[i]->getTransformToParent();
}
transform *= parent->getTransformToParent();
parent = parent->getParentFrame();
}
// for (int i = 0; i < framesStartingWithRootEndingWithFrame.size(); i++)
// {
// transform *= framesStartingWithRootEndingWithFrame[i]->getTransformToParent();
// }
return transform;
}
//class RandomUnchangingFrame : public ReferenceFrame
//{
// public:
// RandomUnchangingFrame()
// : ReferenceFrame(frameName, parentFrame, createRandomSpatialTransform(), false, movableBodyId)
// {
// }
//};
static inline ReferenceFramePtr createRandomUnchangingFrame(const std::string& frameName, ReferenceFramePtr parentFrame, const unsigned int movableBodyId)
{
return ReferenceFramePtr(new ReferenceFrame(frameName, parentFrame, createRandomSpatialTransform(), false, movableBodyId));
}
// class RandomUnchangingFrame
// {
// public:
// RandomUnchangingFrame(const std::string& frameName, ReferenceFramePtr parentFrame, const unsigned int movableBodyId)
// {
// }
// };
class RandomlyChangingFrame : public ReferenceFrame
{
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment