Commit 659ef190 authored by Jordan Lack's avatar Jordan Lack

Working on shared_ptr stuff

parent 9aed64b3
Pipeline #56227957 (#924) failed with stages
in 3 minutes and 40 seconds
......@@ -16,7 +16,6 @@
*/
#include "rdl_dynamics/rdl_eigenmath.h"
#include "rdl_dynamics/ReferenceFrameHolder.hpp"
namespace RobotDynamics
{
......@@ -25,7 +24,7 @@ namespace RobotDynamics
* @ingroup reference_frame
* @brief An interface that objects with a ReferenceFrame extend to inherit the FrameObject::changeFrame method
*/
class FrameObject : public ReferenceFrameHolder
class FrameObject
{
public:
explicit FrameObject(ReferenceFramePtr referenceFrame) : referenceFrame(referenceFrame)
......@@ -63,6 +62,20 @@ class FrameObject : public ReferenceFrameHolder
referenceFrame = frame;
}
/**
* @brief Check if two ReferenceFrameHolders hold the same ReferenceFrame
* @param referenceFrameHolder
*/
void checkReferenceFramesMatch(const FrameObject* frameObject) const
{
getReferenceFrame()->checkReferenceFramesMatch(frameObject->getReferenceFrame());
}
void checkReferenceFramesMatch(FrameObject* frameObject) const
{
getReferenceFrame()->checkReferenceFramesMatch(frameObject->getReferenceFrame());
}
protected:
ReferenceFramePtr referenceFrame /**< Pointer to a ReferenceFrame*/;
......
......@@ -336,7 +336,7 @@ class FramePoint : public FrameObject, public Math::Point3d
*/
inline bool operator==(const FramePoint& lhs, const FramePoint& rhs)
{
lhs.checkReferenceFramesMatch(rhs.getReferenceFrame());
lhs.checkReferenceFramesMatch(&rhs);
if (lhs.x() != rhs.x())
{
......
......@@ -58,7 +58,6 @@ class FrameVector : public FrameObject, public Math::Vector3d
FrameVector(ReferenceFramePtr referenceFrame, const double& x, const double& y, const double& z) : FrameObject(referenceFrame), Math::Vector3d(x, y, z)
{
}
}
/**
* @brief Constructor
......@@ -247,6 +246,8 @@ class FrameVector : public FrameObject, public Math::Vector3d
}
};
typedef std::shared_ptr<FrameVector> FrameVectorPtr;
inline FrameVector operator+(FrameVector v1, const Vector3d v2)
{
v1 += v2;
......
......@@ -58,14 +58,6 @@ class FrameVectorPair
{
}
/*
* @brief constructor
* @param referenceFrame pointer to a reference frame
*/
explicit FrameVectorPair(ReferenceFrame* referenceFrame) : lin(referenceFrame), ang(referenceFrame)
{
}
/**
* @brief Constructor
* @param v Spatial motion
......@@ -76,16 +68,6 @@ class FrameVectorPair
ang.setIncludingFrame(v.getAngularPart(), v.getReferenceFrame());
}
/**
* @brief Constructor
* @param referenceFrame A pointer to a ReferenceFrame
* @param linear linear component
* @param angular angular component
*/
FrameVectorPair(ReferenceFrame* referenceFrame, const Vector3d& linear, const Vector3d& angular) : lin(referenceFrame, linear), ang(referenceFrame, angular)
{
}
/**
* @brief Constructor
* @param referenceFrame A pointer to a ReferenceFrame
......@@ -105,15 +87,6 @@ class FrameVectorPair
{
}
/**
* @brief Constructor
* @param referenceFrame
* @param v
*/
FrameVectorPair(ReferenceFrame* referenceFrame, const SpatialVector& v) : lin(referenceFrame, v.getLinearPart()), ang(referenceFrame, v.getAngularPart())
{
}
/**
* @brief Destructor
*/
......@@ -143,71 +116,18 @@ class FrameVectorPair
ang.changeFrame(referenceFrame);
}
/**
* @brief Change the frame of the two 3d vectors. Equivalent to the following math expression
* \f[
* \begin{bmatrix}
* E & 0 \\
* 0 & E
* \end{bmatrix}
* \begin{bmatrix}
* angular \\
* linear
* \end{bmatrix}
* \f]
*
* @param referenceFrame
*/
void changeFrame(ReferenceFrame* referenceFrame)
{
assert(referenceFrame);
lin.changeFrame(referenceFrame);
ang.changeFrame(referenceFrame);
}
/**
* @brief copy into new frame vector and change the frame of that
* @param referenceFrame
* @return Copy of frame vector pair expressed in new frame
*/
FrameVectorPair changeFrameAndCopy(ReferenceFrame* referenceFrame) const
FrameVectorPair changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
{
FrameVectorPair p = *this;
p.changeFrame(referenceFrame);
return p;
}
/**
* @brief copy into new frame vector and change the frame of that
* @param referenceFrame
* @return Copy of frame vector pair expressed in new frame
*/
FrameVectorPair changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
{
return changeFrameAndCopy(referenceFrame.get());
}
/**
* @brief Copy *this into p and change its frame
* @param referenceFrame
* @param p Modified
*/
void changeFrameAndCopy(ReferenceFrame* referenceFrame, FrameVectorPair& p) const
{
p = *this;
p.changeFrame(referenceFrame);
}
/**
* @brief Copy *this into p and change its frame
* @param referenceFrame
* @param p Modified
*/
void changeFrameAndCopy(std::shared_ptr<ReferenceFrame> referenceFrame, FrameVectorPair& p) const
{
changeFrameAndCopy(referenceFrame.get(), p);
}
/**
* @brief Set x, y, and z components of linear and angular to 0
*/
......@@ -227,23 +147,13 @@ class FrameVectorPair
ang.setReferenceFrame(referenceFrame);
}
/**
* @brief Set the reference frame
* @param referenceFrame
*/
void setReferenceFrame(ReferenceFrame* referenceFrame)
{
lin.setReferenceFrame(referenceFrame);
ang.setReferenceFrame(referenceFrame);
}
/**
* @brief Set the components and the ReferenceFrame these components are expressed in
* @param linear Linear component
* @param angular Angular component
* @param referenceFrame Pointer to a ReferenceFrame this point is expressed in
*/
inline void setIncludingFrame(ReferenceFrame* referenceFrame, const Vector3d& linear, const Vector3d& angular)
inline void setIncludingFrame(ReferenceFramePtr referenceFrame, const Vector3d& linear, const Vector3d& angular)
{
assert(referenceFrame);
......@@ -251,19 +161,6 @@ class FrameVectorPair
ang.setIncludingFrame(angular, referenceFrame);
}
/**
* @brief Set the components and the ReferenceFrame these components are expressed in
* @param v spatial vector
* @param referenceFrame Pointer to a ReferenceFrame this point is expressed in
*/
inline void setIncludingFrame(ReferenceFrame* referenceFrame, const SpatialVector& v)
{
assert(referenceFrame);
lin.setIncludingFrame(v.getLinearPart(), referenceFrame);
ang.setIncludingFrame(v.getAngularPart(), referenceFrame);
}
/**
* @brief Set the components and the ReferenceFrame these components are expressed in
* @param v spatial vector
......@@ -282,19 +179,6 @@ class FrameVectorPair
* @param v Motion vector
* @param referenceFrame pointer to a reference frame
*/
inline void setIncludingFrame(ReferenceFrame* referenceFrame, const MotionVector& v)
{
assert(referenceFrame);
lin.setIncludingFrame(v.getLinearPart(), referenceFrame);
ang.setIncludingFrame(v.getAngularPart(), referenceFrame);
}
/**
* @brief Set the components and the ReferenceFrame these components are expressed in
* @param referenceFrame Pointer to a reference frame
* @param v Motion vector
*/
inline void setIncludingFrame(ReferenceFramePtr referenceFrame, const MotionVector& v)
{
assert(referenceFrame);
......@@ -313,20 +197,6 @@ class FrameVectorPair
ang.setIncludingFrame(v.getAngularPart(), v.getReferenceFrame());
}
/**
* @brief Set the components and the ReferenceFrame these components are expressed in
* @param linear Linear component
* @param angular Angular component
* @param referenceFrame Pointer to a ReferenceFrame this point is expressed in
*/
inline void setIncludingFrame(ReferenceFramePtr referenceFrame, const Vector3d& linear, const Vector3d& angular)
{
assert(referenceFrame);
lin.setIncludingFrame(linear, referenceFrame);
ang.setIncludingFrame(angular, referenceFrame);
}
/**
* @brief Set the linear vector
* @param v
......
......@@ -383,12 +383,12 @@ struct Model
* @return Pointer to the frame, or nullptr if it doesn't exist
*
*/
ReferenceFramePtr getReferenceFrame(const std::string& bodyName) const
ReferenceFramePtr getReferenceFrame(const std::string& frameName) const
{
ReferenceFramePtr frame;
try
{
frame = referenceNameMap.at(bodyName)
frame = referenceFrameMap.at(frameName);
}
catch(const std::out_of_range& e)
{
......
......@@ -126,7 +126,7 @@ class ReferenceFrame
"parentFrame=nullptr");
}
framesStartingWithRootEndingWithThis = constructVectorOfFramesStartingWithRootEndingWithThis(this);
framesStartingWithRootEndingWithThis = constructVectorOfFramesStartingWithRootEndingWithThis(ReferenceFramePtr(this));
update();
}
......@@ -194,6 +194,8 @@ class ReferenceFrame
*/
void checkReferenceFramesMatch(ReferenceFramePtr referenceFrame) const;
void checkReferenceFramesMatch(ReferenceFrame* referenceFrame) const;
/**
* @brief Get this frames ReferenceFrame::transformToRoot
* @return ReferenceFrame::transformToRoot
......@@ -341,7 +343,7 @@ class ReferenceFrame
ReferenceFrame(const std::string& frameName, bool isWorldFrame, unsigned int movableBodyId, bool isBodyFrame)
: frameName(frameName), parentFrame(nullptr), isWorldFrame(isWorldFrame), isBodyFrame(isBodyFrame), movableBodyId(movableBodyId)
{
framesStartingWithRootEndingWithThis = constructVectorOfFramesStartingWithRootEndingWithThis(this);
framesStartingWithRootEndingWithThis = constructVectorOfFramesStartingWithRootEndingWithThis(ReferenceFramePtr(this));
update();
}
......
/*
* RDL - Robot Dynamics Library
* Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
*
* Licensed under the zlib license. See LICENSE for more details.
*/
#ifndef __RDL_REFERENCE_FRAME_HOLDER_HPP__
#define __RDL_REFERENCE_FRAME_HOLDER_HPP__
/**
* @page reference_frame_holder Reference Frame Holder
*
* A RobotDynamics::ReferenceFrameHolder is a pure abstract class that requires implementing classes to
* implement a function allowing access to their RobotDynamics::ReferenceFrame
*/
#include "rdl_dynamics/ReferenceFrame.hpp"
namespace RobotDynamics
{
/**
* @class ReferenceFrameHolder
* @ingroup reference_frame
* @brief This class and its
* implementation are an adaptation of ReferenceFrameHolder.java by <a href="http://robots.ihmc.us/">Jerry Pratt and the
**IHMC Robotics Group</a>.
*/
class ReferenceFrameHolder
{
public:
/**
* @brief Objects inheriting from this class must implement this method. This makes it possible to
* access any ReferenceFrameHolders referenceFrame to do runtime frame checks on reference frames
* @return A pointer to a ReferenceFrameHolder's ReferenceFrame
*/
virtual ReferenceFramePtr getReferenceFrame() const = 0;
/**
* @brief Check if two ReferenceFrames are the same
* @param referenceFrame
*/
void checkReferenceFramesMatch(ReferenceFramePtr referenceFrame) const
{
getReferenceFrame()->checkReferenceFramesMatch(referenceFrame);
}
/**
* @brief Check if two ReferenceFrameHolders hold the same ReferenceFrame
* @param referenceFrameHolder
*/
void checkReferenceFramesMatch(const ReferenceFrameHolderPtr referenceFrameHolder) const
{
getReferenceFrame()->checkReferenceFramesMatch(referenceFrameHolder->getReferenceFrame());
}
/**
* @brief Check if two ReferenceFrameHolders hold the same ReferenceFrame
* @param referenceFrameHolder
*/
void checkReferenceFramesMatch(ReferenceFrameHolderPtr referenceFrameHolder) const
{
getReferenceFrame()->checkReferenceFramesMatch(referenceFrameHolder->getReferenceFrame());
}
};
} // namespace RobotDynamics
#endif // ifndef __RDL_REFERENCE_FRAME_HOLDER_HPP__
......@@ -473,7 +473,7 @@ namespace RobotDynamics
for(i = model.mBodies.size() - 1; i > 0; i--)
{
unsigned int q_index = model.mJoints[i].q_index;
ReferenceFramePtr bodyFrame = model.bodyFrames[i].get();
ReferenceFramePtr bodyFrame = model.bodyFrames[i];
if(model.mJoints[i].mDoFCount == 3 && model.mJoints[i].mJointType != JointTypeCustom)
{
......@@ -741,14 +741,14 @@ namespace RobotDynamics
{
unsigned int fbody_id = body_id - model.fixed_body_discriminator;
movable_body_id = model.mFixedBodies[fbody_id].mMovableParent;
p.setIncludingFrame(point,model.fixedBodyFrames[fbody_id].get());
p.changeFrame(model.worldFrame.get());
p.setIncludingFrame(point,model.fixedBodyFrames[fbody_id]);
p.changeFrame(model.worldFrame);
point_global = p.vec();
}
else
{
p.setIncludingFrame(point,model.bodyFrames[body_id].get());
p.changeFrame(model.worldFrame.get());
p.setIncludingFrame(point,model.bodyFrames[body_id]);
p.changeFrame(model.worldFrame);
point_global = p.vec();
}
......
......@@ -30,7 +30,7 @@ namespace RobotDynamics
{
unsigned int q_index = model.mJoints[i].q_index;
unsigned int lambda = model.lambda[i];
ReferenceFramePtr bodyFrame = model.bodyFrames[i].get();
ReferenceFramePtr bodyFrame = model.bodyFrames[i];
if(update_kinematics)
{
......@@ -107,7 +107,7 @@ namespace RobotDynamics
{
if(!model.mBodies[i].mIsVirtual)
{
model.f_b[i].set(-(model.I[i] *model.gravity.transform_copy(model.worldFrame->getTransformToDesiredFrame(model.bodyFrames[i].get()))));
model.f_b[i].set(-(model.I[i] *model.gravity.transform_copy(model.worldFrame->getTransformToDesiredFrame(model.bodyFrames[i]))));
}
else
{
......@@ -150,7 +150,7 @@ namespace RobotDynamics
{
if(!model.mBodies[i].mIsVirtual)
{
model.f_b[i].set((model.I[i] *model.gravity.transform_copy(model.worldFrame->getTransformToDesiredFrame(model.bodyFrames[i].get()))));
model.f_b[i].set((model.I[i] *model.gravity.transform_copy(model.worldFrame->getTransformToDesiredFrame(model.bodyFrames[i]))));
}
else
{
......@@ -165,7 +165,7 @@ namespace RobotDynamics
i--;
}
gravity_wrench.setIncludingFrame(model.bodyFrames[body_id].get(),model.f_b[body_id]);
gravity_wrench.setIncludingFrame(model.bodyFrames[body_id],model.f_b[body_id]);
}
void nonlinearEffects(Model &model, const VectorNd &Q, const VectorNd &QDot, VectorNd &Tau, bool update_kinematics)
......@@ -395,7 +395,7 @@ namespace RobotDynamics
for(unsigned int i = 1; i < model.mBodies.size(); i++)
{
unsigned int lambda = model.lambda[i];
ReferenceFrame *bodyFrame = model.bodyFrames[i].get();
ReferenceFramePtr bodyFrame = model.bodyFrames[i];
if(update_kinematics)
{
......@@ -416,7 +416,7 @@ namespace RobotDynamics
for(unsigned int i = model.mBodies.size() - 1; i > 0; i--)
{
unsigned int q_index = model.mJoints[i].q_index;
ReferenceFrame *bodyFrame = model.bodyFrames[i].get();
ReferenceFramePtr bodyFrame = model.bodyFrames[i];
if(model.mJoints[i].mDoFCount == 1 && model.mJoints[i].mJointType != JointTypeCustom)
{
......
This diff is collapsed.
......@@ -40,7 +40,7 @@ std::vector<ReferenceFramePtr> ReferenceFrame::constructVectorOfFramesStartingWi
RobotDynamics::Math::SpatialTransform ReferenceFrame::getTransformToDesiredFrame(ReferenceFramePtr desiredFrame)
{
if (this == desiredFrame)
if (this == desiredFrame.get())
{
return RobotDynamics::Math::SpatialTransform();
}
......@@ -62,6 +62,11 @@ void ReferenceFrame::update()
}
void ReferenceFrame::checkReferenceFramesMatch(ReferenceFramePtr referenceFrame) const
{
checkReferenceFramesMatch(referenceFrame.get());
}
void ReferenceFrame::checkReferenceFramesMatch(ReferenceFrame* referenceFrame) const
{
if (referenceFrame == nullptr)
{
......
......@@ -19,8 +19,8 @@ void SpatialAcceleration::changeFrameWithRelativeMotion(ReferenceFramePtr newFra
return;
}
checkReferenceFramesMatch(twistOfCurrentFrameWithRespectToNewFrame.getReferenceFrame());
checkReferenceFramesMatch(twistOfCurrentFrameWithRespectToNewFrame.getBodyFrame());
referenceFrame->checkReferenceFramesMatch(twistOfCurrentFrameWithRespectToNewFrame.getReferenceFrame());
referenceFrame->checkReferenceFramesMatch(twistOfCurrentFrameWithRespectToNewFrame.getBodyFrame());
newFrame->checkReferenceFramesMatch(twistOfCurrentFrameWithRespectToNewFrame.getBaseFrame());
checkReferenceFramesMatch(&twistOfBodyWrtBaseExpressedInCurrent);
......
......@@ -241,7 +241,7 @@ namespace RobotDynamics
{
Vector3d com_v;
calcCenterOfMass(model,q,com_v,update_kinematics);
com.setIncludingFrame(com_v,model.worldFrame.get());
com.setIncludingFrame(com_v,model.worldFrame);
}
void calcCenterOfMass(Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot,
......@@ -252,7 +252,7 @@ namespace RobotDynamics
calcCenterOfMass(model, q, qdot, mass, comPosition, &comVelocity, &angularMomentum, update_kinematics);
ReferenceFrame *worldFrame = model.worldFrame.get();
ReferenceFramePtr worldFrame = model.worldFrame;
com.setIncludingFrame(comPosition, worldFrame);
......@@ -345,11 +345,11 @@ namespace RobotDynamics
}
}
com.setIncludingFrame(p_com / mass, model.worldFrame.get());
com.setIncludingFrame(p_com / mass, model.worldFrame);
if (com_velocity)
{
com_velocity->setIncludingFrame(v_com / mass, model.worldFrame.get());
com_velocity->setIncludingFrame(v_com / mass, model.worldFrame);
}
}
......@@ -398,7 +398,7 @@ namespace RobotDynamics
std::vector<unsigned int> childBodyIds = model.mu[bodyId];
Math::FramePointd comScaledByMass(model.worldFrame.get(), (model.bodyCenteredFrames[bodyId]->getInverseTransformToRoot().r * model.mBodies[bodyId].mMass));
Math::FramePointd comScaledByMass(model.worldFrame, (model.bodyCenteredFrames[bodyId]->getInverseTransformToRoot().r * model.mBodies[bodyId].mMass));
for (unsigned int j = 0; j < childBodyIds.size(); j++)
{
......@@ -469,7 +469,7 @@ namespace RobotDynamics
for(unsigned int i = 1; i < model.mBodies.size(); i++)
{
j = i;
ReferenceFrame* bodyFrame = model.bodyFrames[i].get();
ReferenceFramePtr bodyFrame = model.bodyFrames[i];
while(j != 0)
{
if(model.mJoints[j].mJointType != JointTypeCustom)
......@@ -520,7 +520,7 @@ namespace RobotDynamics
for(unsigned int i = 1; i < model.mBodies.size(); i++)
{
j = i;
ReferenceFrame* bodyFrame = model.bodyFrames[i].get();
ReferenceFramePtr bodyFrame = model.bodyFrames[i];
v_tmp = -MotionVector(com_twist-model.v[i].transform_copy(bodyFrame->getTransformToRoot())).transform_copy(X_com);
......
This diff is collapsed.
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