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

Replacing more raw pointers with smart pointers

parent e3470f14
Pipeline #55810640 failed with stages
in 4 minutes and 8 seconds
......@@ -36,7 +36,7 @@ namespace Math
* implementation are an adaptation of FramePoint.java by <a href="http://robots.ihmc.us/">Jerry Pratt and the IHMC
**Robotics Group</a>.
*/
class RDL_DLLAPI FramePoint : public FrameObject, public Math::Point3d
class FramePoint : public FrameObject, public Math::Point3d
{
public:
/**
......@@ -46,7 +46,7 @@ class RDL_DLLAPI FramePoint : public FrameObject, public Math::Point3d
* @param y The y-component of the point
* @param z The z-component of the point
*/
FramePoint(ReferenceFrame* referenceFrame, const double x, const double y, const double z) : FrameObject(referenceFrame), Math::Point3d(x, y, z)
FramePoint(ReferenceFramePtr referenceFrame, const double x, const double y, const double z) : FrameObject(referenceFrame), Math::Point3d(x, y, z)
{
}
......@@ -55,7 +55,7 @@ class RDL_DLLAPI FramePoint : public FrameObject, public Math::Point3d
* @param referenceFrame A pointer to the ReferenceFrame the point will be expressed in
* @param v A Vector3d that will be used to set the components of this FramePoint
*/
FramePoint(ReferenceFrame* referenceFrame, Math::Vector3d v) : FrameObject(referenceFrame), Math::Point3d(v[0], v[1], v[2])
FramePoint(ReferenceFramePtr referenceFrame, Math::Vector3d v) : FrameObject(referenceFrame), Math::Point3d(v[0], v[1], v[2])
{
}
......@@ -64,7 +64,7 @@ class RDL_DLLAPI FramePoint : public FrameObject, public Math::Point3d
* @param referenceFrame A pointer to the ReferenceFrame the point will be expressed in
* @param point A Math::Point3 that will be used to set the components of this FramePoint
*/
FramePoint(ReferenceFrame* referenceFrame, const Math::Point3d& point) : FrameObject(referenceFrame), Math::Point3d(point)
FramePoint(ReferenceFramePtr referenceFrame, const Math::Point3d& point) : FrameObject(referenceFrame), Math::Point3d(point)
{
}
......@@ -80,7 +80,7 @@ class RDL_DLLAPI FramePoint : public FrameObject, public Math::Point3d
* @brief Constructor that initializes to (x,y,z) = (0,0,0)
* @param referenceFrame A pointer to the ReferenceFrame the point will be expressed in
*/
explicit FramePoint(ReferenceFrame* referenceFrame) : FrameObject(referenceFrame), Math::Point3d()
explicit FramePoint(ReferenceFramePtr referenceFrame) : FrameObject(referenceFrame), Math::Point3d()
{
}
......@@ -122,52 +122,19 @@ class RDL_DLLAPI FramePoint : public FrameObject, public Math::Point3d
* @param referenceFrame
* @return
*/
FramePoint changeFrameAndCopy(ReferenceFrame* referenceFrame) const
FramePoint changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
{
FramePoint p = *this;
p.changeFrame(referenceFrame);
return p;
}
/**
* @brief copy into new frame point and change the frame of that
* @param referenceFrame
* @return
*/
FramePoint changeFrameAndCopy(std::shared_ptr<ReferenceFrame> referenceFrame) const
{
return changeFrameAndCopy(referenceFrame.get());
}
/**
* @brief Copy *this into p and change its frame
* @param referenceFrame
* @param p Modified
* @return
*/
void changeFrameAndCopy(ReferenceFrame* referenceFrame, FramePoint& p) const
{
p = *this;
p.changeFrame(referenceFrame);
}
/**
* @brief Copy *this into p and change its frame
* @param referenceFrame
* @param p Modified
* @return
*/
void changeFrameAndCopy(std::shared_ptr<ReferenceFrame> referenceFrame, FramePoint& p) const
{
changeFrameAndCopy(referenceFrame.get(), p);
}
/**
* @brief Set both the ReferenceFrame this object is expressed in as well as the (x,y,z) coordinates of the point
* @param v Vector3d that this point will be set to
* @param referenceFrame Pointer to the ReferenceFrame this object will be expressed in
*/
EIGEN_STRONG_INLINE void setIncludingFrame(const Math::Vector3d& v, ReferenceFrame* referenceFrame)
EIGEN_STRONG_INLINE void setIncludingFrame(const Math::Vector3d& v, ReferenceFramePtr referenceFrame)
{
setIncludingFrame(v(0), v(1), v(2), referenceFrame);
}
......@@ -179,7 +146,7 @@ class RDL_DLLAPI FramePoint : public FrameObject, public Math::Point3d
* @param z The z coordinate
* @param referenceFrame The ReferenceFrame this point is to be expressed in
*/
void setIncludingFrame(const double x, const double y, const double z, ReferenceFrame* referenceFrame)
void setIncludingFrame(const double x, const double y, const double z, ReferenceFramePtr referenceFrame)
{
if (!referenceFrame)
{
......@@ -195,7 +162,7 @@ class RDL_DLLAPI FramePoint : public FrameObject, public Math::Point3d
* @param point Math::Point3d to set this point to
* @param referenceFrame Pointer to ReferenceFrame this point will be expressed in
*/
void setIncludingFrame(const Math::Point3d& point, ReferenceFrame* referenceFrame)
void setIncludingFrame(const Math::Point3d& point, ReferenceFramePtr referenceFrame)
{
if (!referenceFrame)
{
......@@ -269,32 +236,6 @@ class RDL_DLLAPI FramePoint : public FrameObject, public Math::Point3d
return tmp > fabs(dz) ? tmp : fabs(dz);
}
/**
* @brief Perform addition of FramePoint argument
* @param point The FramePoint to be added
* @throws ReferenceFrameException If both points are not expressed in the same ReferenceFrame
*/
void add(const FramePoint& point)
{
checkReferenceFramesMatch(&point);
this->x() += point.x();
this->y() += point.y();
this->z() += point.z();
}
/**
* @brief Perform subtraction of FramePoint argument
* @param point The FramePoint to be subtracted
* @throws ReferenceFrameException If both points are not expressed in the same ReferenceFrame
*/
void subtract(const FramePoint& point)
{
checkReferenceFramesMatch(&point);
this->x() -= point.x();
this->y() -= point.y();
this->z() -= point.z();
}
/**
* @brief Return true FramePoint argument is within epsilon of this, false otherwise
* @param point The FramePoint to be compared
......
......@@ -40,14 +40,6 @@ class FrameVector : public FrameObject, public Math::Vector3d
{
}
/**
* @brief Constructor
* @param referenceFrame A pointer to a ReferenceFrame
*/
explicit FrameVector(ReferenceFrame* referenceFrame) : FrameObject(referenceFrame), Math::Vector3d()
{
}
/**
* @brief Constructor
* @param referenceFrame A pointer to a ReferenceFrame
......@@ -63,28 +55,9 @@ class FrameVector : public FrameObject, public Math::Vector3d
* @param y Value of the y-coordinate
* @param z Value of the z-coordinate
*/
FrameVector(ReferenceFrame* referenceFrame, const double& x, const double& y, const double& z) : FrameObject(referenceFrame), Math::Vector3d(x, y, z)
{
}
/**
* @brief Constructor
* @param referenceFrame A pointer to a ReferenceFrame
* @param x Value of the x-coordinate
* @param y Value of the y-coordinate
* @param z Value of the z-coordinate
*/
FrameVector(ReferenceFramePtr referenceFrame, const double& x, const double& y, const double& z) : FrameObject(referenceFrame.get()), Math::Vector3d(x, y, z)
FrameVector(ReferenceFramePtr referenceFrame, const double& x, const double& y, const double& z) : FrameObject(referenceFrame), Math::Vector3d(x, y, z)
{
}
/**
* @brief Constructor
* @param referenceFrame Pointer to a ReferenceFrame
* @param vector A Vector3d used to set the x,y, and z coordinates
*/
FrameVector(ReferenceFrame* referenceFrame, const Eigen::Vector3d& vector) : FrameObject(referenceFrame), Math::Vector3d(vector[0], vector[1], vector[2])
{
}
/**
......@@ -92,7 +65,7 @@ class FrameVector : public FrameObject, public Math::Vector3d
* @param referenceFrame Pointer to a ReferenceFrame
* @param vector A Vector3d used to set the x,y, and z coordinates
*/
FrameVector(ReferenceFramePtr referenceFrame, const Eigen::Vector3d& vector) : FrameObject(referenceFrame.get()), Math::Vector3d(vector[0], vector[1], vector[2])
FrameVector(ReferenceFramePtr referenceFrame, const Eigen::Vector3d& vector) : FrameObject(referenceFrame), Math::Vector3d(vector[0], vector[1], vector[2])
{
}
......@@ -118,46 +91,13 @@ class FrameVector : public FrameObject, public Math::Vector3d
* @param referenceFrame
* @return
*/
FrameVector changeFrameAndCopy(ReferenceFrame* referenceFrame) const
FrameVector changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
{
FrameVector p = *this;
p.changeFrame(referenceFrame);
return p;
}
/**
* @brief copy into new frame vector and change the frame of that
* @param referenceFrame
* @return
*/
FrameVector changeFrameAndCopy(std::shared_ptr<ReferenceFrame> referenceFrame) const
{
return changeFrameAndCopy(referenceFrame.get());
}
/**
* @brief Copy *this into p and change its frame
* @param referenceFrame
* @param p Modified
* @return
*/
void changeFrameAndCopy(ReferenceFrame* referenceFrame, FrameVector& p) const
{
p = *this;
p.changeFrame(referenceFrame);
}
/**
* @brief Copy *this into p and change its frame
* @param referenceFrame
* @param p Modified
* @return
*/
void changeFrameAndCopy(std::shared_ptr<ReferenceFrame> referenceFrame, FrameVector& p) const
{
changeFrameAndCopy(referenceFrame.get(), p);
}
/**
* @brief Set x, y, and z components to 0
*/
......@@ -174,7 +114,7 @@ class FrameVector : public FrameObject, public Math::Vector3d
* @throws ReferenceFrameException If *referenceFrame=nullptr
* @param referenceFrame Pointer to a ReferenceFrame this point is expressed in
*/
inline void setIncludingFrame(const double x, const double y, const double z, ReferenceFrame* referenceFrame)
inline void setIncludingFrame(const double x, const double y, const double z, ReferenceFramePtr referenceFrame)
{
if (!referenceFrame)
{
......@@ -185,23 +125,6 @@ class FrameVector : public FrameObject, public Math::Vector3d
this->referenceFrame = referenceFrame;
}
/**
* @brief Set the x, y, and z components and the ReferenceFrame these components are expressed in
* @param vector Used to set the x,y, and z components of this point
* @throws ReferenceFrameException If *referenceFrame=nullptr
* @param referenceFrame Pointer to a ReferenceFrame this point is expressed in
*/
inline void setIncludingFrame(const Eigen::Vector3d& vector, ReferenceFrame* referenceFrame)
{
if (!referenceFrame)
{
throw ReferenceFrameException("Reference frame cannot be nullptr!");
}
set(vector[0], vector[1], vector[2]);
this->referenceFrame = referenceFrame;
}
/**
* @brief Set the x, y, and z components and the ReferenceFrame these components are expressed in
* @param vector Used to set the x,y, and z components of this point
......@@ -216,7 +139,7 @@ class FrameVector : public FrameObject, public Math::Vector3d
}
set(vector[0], vector[1], vector[2]);
this->referenceFrame = referenceFrame.get();
this->referenceFrame = referenceFrame;
}
/**
......
......@@ -383,14 +383,19 @@ struct Model
* @return Pointer to the frame, or nullptr if it doesn't exist
*
*/
ReferenceFrame* getBodyFrame(const std::string& bodyName) const
ReferenceFramePtr getReferenceFrame(const std::string& bodyName) const
{
if (bodyFrameMap.count(bodyName) == 0)
ReferenceFramePtr frame;
try
{
return nullptr;
frame = referenceNameMap.at(bodyName)
}
catch(const std::out_of_range& e)
{
frame = nullptr;
}
return bodyFrameMap.find(bodyName)->second;
return frame;
}
/** @brief Returns the name of a body for a given body id */
......
......@@ -111,7 +111,7 @@ class ReferenceFrame
* @param movableBodyId The ID of the movable body this frame is attached to. For frames attached to fixed bodies, this
**should be FixedBody::mMovableParent
*/
ReferenceFrame(const std::string& frameName, ReferenceFrame* parentFrame, const RobotDynamics::Math::SpatialTransform& transformFromParent, bool isBodyFrame,
ReferenceFrame(const std::string& frameName, ReferenceFramePtr parentFrame, const RobotDynamics::Math::SpatialTransform& transformFromParent, bool isBodyFrame,
unsigned int movableBodyId)
: frameName(frameName)
, parentFrame(parentFrame)
......@@ -159,7 +159,7 @@ class ReferenceFrame
* @param transformToPack Resulting transform to the desired frame will be stored here
* @param desiredFrame The resulting transform will transform vectors into desiredFrame
*/
inline void getTransformToDesiredFrame(RobotDynamics::Math::SpatialTransform& transformToPack, ReferenceFrame* desiredFrame)
inline void getTransformToDesiredFrame(RobotDynamics::Math::SpatialTransform& transformToPack, ReferenceFramePtr desiredFrame)
{
transformToPack = getTransformToDesiredFrame(desiredFrame);
}
......@@ -169,14 +169,14 @@ class ReferenceFrame
* @param desiredFrame The resulting transform will transform vectors into desiredFrame
* @return Spatial transform that will transform vectors into the ReferenceFrame desiredFrame
*/
virtual RobotDynamics::Math::SpatialTransform getTransformToDesiredFrame(ReferenceFrame* desiredFrame);
virtual RobotDynamics::Math::SpatialTransform getTransformToDesiredFrame(ReferenceFramePtr desiredFrame);
/**
* @brief Check if two frames have the same roots
* @throws ReferenceFrameException if the frames do not have the same roots
* @param frame
*/
void verifyFramesHaveSameRoot(ReferenceFrame* frame);
void verifyFramesHaveSameRoot(ReferenceFramePtr frame);
/**
* @brief Set a frames ReferenceFrame::transformToParent. For frames connected by a joint, this needs to be
......@@ -192,7 +192,7 @@ class ReferenceFrame
* @brief Check if the argument ReferenceFrame equals this
* @param referenceFrame
*/
void checkReferenceFramesMatch(ReferenceFrame* referenceFrame) const;
void checkReferenceFramesMatch(ReferenceFramePtr referenceFrame) const;
/**
* @brief Get this frames ReferenceFrame::transformToRoot
......@@ -216,7 +216,7 @@ class ReferenceFrame
* @brief Get a pointer to this frames root frame
* @return Pointer to this frames root ReferenceFrame
*/
inline ReferenceFrame* getRootFrame()
inline ReferenceFramePtr getRootFrame()
{
return this->framesStartingWithRootEndingWithThis[0];
}
......@@ -226,7 +226,7 @@ class ReferenceFrame
* @return A vector of framse with element 0 correspordind to this frames root, and the final element coresponding
* to this frame
*/
inline std::vector<ReferenceFrame*> getFramesStartingWithRootEndingWithThis()
inline std::vector<ReferenceFramePtr> getFramesStartingWithRootEndingWithThis()
{
return framesStartingWithRootEndingWithThis;
}
......@@ -235,7 +235,7 @@ class ReferenceFrame
* @brief get a pointer to this frames parent
* @return ReferenceFrame::parentFrame
*/
inline ReferenceFrame* getParentFrame()
inline ReferenceFramePtr getParentFrame()
{
return this->parentFrame;
}
......@@ -362,13 +362,13 @@ class ReferenceFrame
* @param thisFrame
* @return ReferenceFrame::framesStartingWithRootEndingWithThis
*/
static std::vector<ReferenceFrame*> constructVectorOfFramesStartingWithRootEndingWithThis(ReferenceFrame* thisFrame);
static std::vector<ReferenceFramePtr> constructVectorOfFramesStartingWithRootEndingWithThis(ReferenceFramePtr thisFrame);
static ReferenceFramePtr worldFrame; /**< Static world frame pointer */
std::vector<ReferenceFrame*> framesStartingWithRootEndingWithThis; /**< A vector of frames holding pointers to all
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 */
ReferenceFrame* parentFrame; /**< Pointer to a frames parent frames */
ReferenceFramePtr parentFrame; /**< Pointer to a frames parent frames */
RobotDynamics::Math::SpatialTransform transformFromParent; /**< SpatialTransform to a frame from its parent*/
RobotDynamics::Math::SpatialTransform transformToRoot; /**< SpatialTransform from a frame to the root frame
*/
......@@ -387,7 +387,7 @@ class ReferenceFrame
class FixedReferenceFrame : public ReferenceFrame
{
public:
FixedReferenceFrame(const std::string& frameName, ReferenceFrame* parentFrame, const RobotDynamics::Math::SpatialTransform& transformFromParent,
FixedReferenceFrame(const std::string& frameName, ReferenceFramePtr parentFrame, const RobotDynamics::Math::SpatialTransform& transformFromParent,
unsigned int movableBodyId)
: ReferenceFrame(frameName, parentFrame, transformFromParent, false, movableBodyId)
{
......
......@@ -34,13 +34,13 @@ class ReferenceFrameHolder
* access any ReferenceFrameHolders referenceFrame to do runtime frame checks on reference frames
* @return A pointer to a ReferenceFrameHolder's ReferenceFrame
*/
virtual ReferenceFrame* getReferenceFrame() const = 0;
virtual ReferenceFramePtr getReferenceFrame() const = 0;
/**
* @brief Check if two ReferenceFrames are the same
* @param referenceFrame
*/
void checkReferenceFramesMatch(ReferenceFrame* referenceFrame) const
void checkReferenceFramesMatch(ReferenceFramePtr referenceFrame) const
{
getReferenceFrame()->checkReferenceFramesMatch(referenceFrame);
}
......@@ -49,7 +49,7 @@ class ReferenceFrameHolder
* @brief Check if two ReferenceFrameHolders hold the same ReferenceFrame
* @param referenceFrameHolder
*/
void checkReferenceFramesMatch(const ReferenceFrameHolder* referenceFrameHolder) const
void checkReferenceFramesMatch(const ReferenceFrameHolderPtr referenceFrameHolder) const
{
getReferenceFrame()->checkReferenceFramesMatch(referenceFrameHolder->getReferenceFrame());
}
......@@ -58,7 +58,7 @@ class ReferenceFrameHolder
* @brief Check if two ReferenceFrameHolders hold the same ReferenceFrame
* @param referenceFrameHolder
*/
void checkReferenceFramesMatch(ReferenceFrameHolder* referenceFrameHolder) const
void checkReferenceFramesMatch(ReferenceFrameHolderPtr referenceFrameHolder) const
{
getReferenceFrame()->checkReferenceFramesMatch(referenceFrameHolder->getReferenceFrame());
}
......
......@@ -56,7 +56,7 @@ class SpatialAcceleration : public SpatialMotion
* @param vy Linear y-coordinate
* @param vz Linear z-coordinate
*/
SpatialAcceleration(ReferenceFrame* bodyFrame, ReferenceFrame* baseFrame, ReferenceFrame* expressedInFrame, const double wx, const double wy, const double wz,
SpatialAcceleration(ReferenceFramePtr bodyFrame, ReferenceFramePtr baseFrame, ReferenceFramePtr expressedInFrame, const double wx, const double wy, const double wz,
const double vx, const double vy, const double vz)
: SpatialMotion(bodyFrame, baseFrame, expressedInFrame, wx, wy, wz, vx, vy, vz)
{
......@@ -70,7 +70,7 @@ class SpatialAcceleration : public SpatialMotion
* @param w Vector containig the angular component of the acceleration
* @param v Vector containing the linear component of the acceleration
*/
SpatialAcceleration(ReferenceFrame* bodyFrame, ReferenceFrame* baseFrame, ReferenceFrame* expressedInFrame, const Vector3d& w, const Vector3d v)
SpatialAcceleration(ReferenceFramePtr bodyFrame, ReferenceFramePtr baseFrame, ReferenceFramePtr expressedInFrame, const Vector3d& w, const Vector3d v)
: SpatialMotion(bodyFrame, baseFrame, expressedInFrame, w.x(), w.y(), w.z(), v.x(), v.y(), v.z())
{
}
......@@ -82,7 +82,7 @@ class SpatialAcceleration : public SpatialMotion
* @param expressedInFrame The acceleration is expressed in this frame
* @param v
*/
SpatialAcceleration(ReferenceFrame* bodyFrame, ReferenceFrame* baseFrame, ReferenceFrame* expressedInFrame, const SpatialVector& v)
SpatialAcceleration(ReferenceFramePtr bodyFrame, ReferenceFramePtr baseFrame, ReferenceFramePtr expressedInFrame, const SpatialVector& v)
: SpatialMotion(bodyFrame, baseFrame, expressedInFrame, v)
{
}
......@@ -104,7 +104,7 @@ class SpatialAcceleration : public SpatialMotion
* @param twistOfCurrentFrameWithRespectToNewFrame
* @param twistOfBodyWrtBaseExpressedInCurrent
*/
void changeFrameWithRelativeMotion(ReferenceFrame* newFrame, SpatialMotion twistOfCurrentFrameWithRespectToNewFrame,
void changeFrameWithRelativeMotion(ReferenceFramePtr newFrame, SpatialMotion twistOfCurrentFrameWithRespectToNewFrame,
const SpatialMotion& twistOfBodyWrtBaseExpressedInCurrent);
/**
......
......@@ -39,14 +39,6 @@ class SpatialForce : public ForceVector, public FrameObject
{
}
/**
* @brief Constructor. Force vector elements will be zero
* @param referenceFrame Frame spatial force will be expressed in
*/
explicit SpatialForce(ReferenceFrame* referenceFrame) : ForceVector(), FrameObject(referenceFrame)
{
}
/**
* @brief Constructor. Force vector elements will be zero
* @param referenceFrame Frame spatial force will be expressed in
......@@ -65,7 +57,7 @@ class SpatialForce : public ForceVector, public FrameObject
* @param fy y-Force
* @param fz z-Force
*/
SpatialForce(ReferenceFrame* referenceFrame, const double mx, const double my, const double mz, const double fx, const double fy, const double fz)
SpatialForce(ReferenceFramePtr referenceFrame, const double mx, const double my, const double mz, const double fx, const double fy, const double fz)
: ForceVector(mx, my, mz, fx, fy, fz), FrameObject(referenceFrame)
{
}
......@@ -76,7 +68,7 @@ class SpatialForce : public ForceVector, public FrameObject
* @param m Vector containing the angular component
* @param f Vector containing the linear component
*/
SpatialForce(ReferenceFrame* referenceFrame, const Vector3d& m, const Vector3d f) : ForceVector(m.x(), m.y(), m.z(), f.x(), f.y(), f.z()), FrameObject(referenceFrame)
SpatialForce(ReferenceFramePtr referenceFrame, const Vector3d& m, const Vector3d f) : ForceVector(m.x(), m.y(), m.z(), f.x(), f.y(), f.z()), FrameObject(referenceFrame)
{
}
......@@ -90,44 +82,13 @@ class SpatialForce : public ForceVector, public FrameObject
* @param referenceFrame
* @return Copied spatial transform with frame changed
*/
SpatialForce changeFrameAndCopy(std::shared_ptr<ReferenceFrame> referenceFrame) const
{
return changeFrameAndCopy(referenceFrame.get());
}
/**
* @brief Copy and change frame
* @param referenceFrame
* @return Copied spatial transform with frame changed
*/
SpatialForce changeFrameAndCopy(ReferenceFrame* referenceFrame) const
SpatialForce changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
{
SpatialForce ret = *this;
ret.changeFrame(referenceFrame);
return ret;
}
/**
* @brief Copy this into spatial motion and change frame
* @param referenceFrame
* @param spatialForce Storage for copied result
*/
void changeFrameAndCopy(ReferenceFrame* referenceFrame, SpatialForce& spatialForce) const
{
spatialForce = *this;
spatialForce.changeFrame(referenceFrame);
}
/**
* @brief Copy this into spatial motion and change frame
* @param referenceFrame
* @param spatialForce Storage for copied result
*/
void changeFrameAndCopy(std::shared_ptr<ReferenceFrame> referenceFrame, SpatialForce& spatialForce) const
{
this->changeFrameAndCopy(referenceFrame.get(), spatialForce);
}
/**
* @brief Get linear part of spatial force as a frame vector
* @return FrameVector consisting of the reference frame and the linear portion
......@@ -159,7 +120,7 @@ class SpatialForce : public ForceVector, public FrameObject
* @param referenceFrame ReferenceFrame this force vector is expressed in
* @param spatialVector
*/
SpatialForce(ReferenceFrame* referenceFrame, const SpatialVector& spatialVector) : ForceVector(spatialVector), FrameObject(referenceFrame)
SpatialForce(ReferenceFramePtr referenceFrame, const SpatialVector& spatialVector) : ForceVector(spatialVector), FrameObject(referenceFrame)
{
}
......@@ -172,13 +133,13 @@ class SpatialForce : public ForceVector, public FrameObject
return ForceVector(this->mx(), this->my(), this->mz(), this->fx(), this->fy(), this->fz());
}
void setIncludingFrame(ReferenceFrame* referenceFrame, const SpatialVector& v)
void setIncludingFrame(ReferenceFramePtr referenceFrame, const SpatialVector& v)
{
this->set(v);
this->referenceFrame = referenceFrame;
}
void setIncludingFrame(ReferenceFrame* referenceFrame, double wx, double wy, double wz, double vx, double vy, double vz)
void setIncludingFrame(ReferenceFramePtr referenceFrame, double wx, double wy, double wz, double vx, double vy, double vz)
{
this->SpatialVector::set(wx, wy, wz, vx, vy, vz);
this->referenceFrame = referenceFrame;
......
......@@ -49,7 +49,7 @@ class SpatialMomentum : public Momentum, public FrameObject
* @param ly y-Linear
* @param lz z-Linear
*/
SpatialMomentum(ReferenceFrame* referenceFrame, const double kx, const double ky, const double kz, const double lx, const double ly, const double lz)
SpatialMomentum(ReferenceFramePtr referenceFrame, const double kx, const double ky, const double kz, const double lx, const double ly, const double lz)
: Momentum(kx, ky, kz, lx, ly, lz), FrameObject(referenceFrame)
{
}
......@@ -60,7 +60,7 @@ class SpatialMomentum : public Momentum, public FrameObject
* @param k Angular part
* @param l Linear part
*/
SpatialMomentum(ReferenceFrame* referenceFrame, const Vector3d& k, const Vector3d l) : Momentum(k.x(), k.y(), k.z(), l.x(), l.y(), l.z()), FrameObject(referenceFrame)
SpatialMomentum(ReferenceFramePtr referenceFrame, const Vector3d& k, const Vector3d l) : Momentum(k.x(), k.y(), k.z(), l.x(), l.y(), l.z()), FrameObject(referenceFrame)
{
}
......@@ -77,15 +77,7 @@ class SpatialMomentum : public Momentum, public FrameObject
* @param referenceFrame RobotDynamics::ReferenceFrame the momentum is expressed in
* @param forceVector
*/
SpatialMomentum(ReferenceFrame* referenceFrame, const ForceVector& forceVector) : Momentum(forceVector), FrameObject(referenceFrame)
{
}
/**
* @brief Constructor
* @param referenceFrame RobotDynamics::ReferenceFrame the momentum is expressed in
*/
explicit SpatialMomentum(ReferenceFrame* referenceFrame) : Momentum(), FrameObject(referenceFrame)
SpatialMomentum(ReferenceFramePtr referenceFrame, const ForceVector& forceVector) : Momentum(forceVector), FrameObject(referenceFrame)
{
}
......@@ -129,7 +121,7 @@ class SpatialMomentum : public Momentum, public FrameObject
* @param inertia
* @param vector
*/
void set(ReferenceFrame* referenceFrame, const RigidBodyInertia& inertia, const MotionVector& vector)
void setIncludingFrame(ReferenceFramePtr referenceFrame, const RigidBodyInertia& inertia, const MotionVector& vector)
{
this->referenceFrame = referenceFrame;
computeMomentum(inertia, vector);
......@@ -140,7 +132,7 @@ class SpatialMomentum : public Momentum, public FrameObject
* @param referenceFrame
* @param f
*/
void set(ReferenceFrame* referenceFrame, const ForceVector& f)
void setIncludingFrame(ReferenceFramePtr referenceFrame, const ForceVector& f)
{
this->referenceFrame = referenceFrame;
ForceVector::set(f);
......
......@@ -57,7 +57,7 @@ class SpatialMotion : public MotionVector, public FrameObject
* @param vy y-Linear part
* @param vz z-Linear part
*/
SpatialMotion(ReferenceFrame* bodyFrame, ReferenceFrame* baseFrame, ReferenceFrame* expressedInFrame, const double wx, const double wy, const double wz,
SpatialMotion(ReferenceFramePtr bodyFrame, ReferenceFramePtr baseFrame, ReferenceFramePtr expressedInFrame, const double wx, const double wy, const double wz,
const double vx, const double vy, const double vz)
: MotionVector(wx, wy, wz, vx, vy, vz), FrameObject(expressedInFrame)
{
......@@ -73,7 +73,7 @@ class SpatialMotion : public MotionVector, public FrameObject
* @param w Angular part
* @param v Linear part
*/
SpatialMotion(ReferenceFrame* bodyFrame, ReferenceFrame* baseFrame, ReferenceFrame* expressedInFrame, const Vector3d& w, const Vector3d v)
SpatialMotion(ReferenceFramePtr bodyFrame, ReferenceFramePtr baseFrame, ReferenceFramePtr expressedInFrame, const Vector3d& w, const Vector3d v)
: MotionVector(w.x(), w.y(), w.z(), v.x(), v.y(), v.z()), FrameObject(expressedInFrame)
{
this->bodyFrame = bodyFrame;
......@@ -87,7 +87,7 @@ class SpatialMotion : public MotionVector, public FrameObject
* @param expressedInFrame RobotDynamics::ReferenceFrame the motion vector is expressed in
* @param v
*/
SpatialMotion(ReferenceFrame* bodyFrame, ReferenceFrame* baseFrame, ReferenceFrame* expressedInFrame, const SpatialVector& v)
SpatialMotion(ReferenceFramePtr bodyFrame, ReferenceFramePtr baseFrame, ReferenceFramePtr expressedInFrame, const SpatialVector& v)
: MotionVector(v), FrameObject(expressedInFrame)
{
this->bodyFrame = bodyFrame;
......@@ -132,49 +132,18 @@ class SpatialMotion : public MotionVector, public FrameObject
* @param referenceFrame
* @return Copied spatial transform with frame changed
*/
SpatialMotion changeFrameAndCopy(std::shared_ptr<ReferenceFrame> referenceFrame) const
{
return changeFrameAndCopy(referenceFrame.get());
}
/**
* @brief Copy and change frame
* @param referenceFrame
* @return Copied spatial transform with frame changed
*/
SpatialMotion changeFrameAndCopy(ReferenceFrame* referenceFrame) const
SpatialMotion changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
{
SpatialMotion ret = *this;
ret.changeFrame(referenceFrame);
return ret;
}
/**
* @brief Copy this into spatial motion and change frame
* @param referenceFrame
* @param spatialMotion Storage for copied result
*/
void changeFrameAndCopy(ReferenceFrame* referenceFrame, SpatialMotion& spatialMotion) const
{
spatialMotion = *this;
spatialMotion.changeFrame(referenceFrame);
}
/**
* @brief Copy this into spatial motion and change frame
* @param referenceFrame
* @param spatialMotion Storage for copied result
*/
void changeFrameAndCopy(std::shared_ptr<ReferenceFrame> referenceFrame, SpatialMotion& spatialMotion) const
{
this->changeFra