Commit 353f1338 authored by Jordan Lack's avatar Jordan Lack

Merge branch 'feature/RDL-117' into 'develop'

Feature/rdl 117

See merge request !63
parents 9f649c90 632c3668
Pipeline #60857709 (#949) canceled with stages
in 3 minutes and 44 seconds
<?xml version="1.0"?>
<package format="2">
<name>rdl</name>
<version>3.1.0</version>
<version>3.2.0</version>
<description>The rdl meta-package</description>
<maintainer email="jlack1987@gmail.com">jordan</maintainer>
......
<?xml version="1.0"?>
<package format="2">
<name>rdl_benchmark</name>
<version>3.1.0</version>
<version>3.2.0</version>
<description>The rdl_benchmark package</description>
<maintainer email="jlack1987@gmail.com">jordan</maintainer>
......
<?xml version="1.0"?>
<package format="2">
<name>rdl_cmake</name>
<version>3.1.0</version>
<version>3.2.0</version>
<description>The rdl_cmake package</description>
<maintainer email="jlack1987@gmail.com">jordan</maintainer>
......
......@@ -14,6 +14,7 @@
#define __RDL_FRAME_POINT_HPP__
#include "rdl_dynamics/FrameObject.hpp"
#include "rdl_dynamics/FrameVector.hpp"
#include "rdl_dynamics/Point3.hpp"
#include "rdl_dynamics/FrameExceptions.hpp"
......@@ -248,58 +249,6 @@ class FramePoint : public FrameObject, public Math::Point3d
return fabs(this->x() - point.x()) < epsilon && fabs(this->y() - point.y()) < epsilon && fabs(this->z() - point.z()) < epsilon;
}
/**
* @brief Overloaded += operator, performs this = this + point
* @param point FramePoint to be added
* @return FramePoint representing this = this + point
*/
void operator+=(const FramePoint& point)
{
checkReferenceFramesMatch(&point);
this->x() += point.x();
this->y() += point.y();
this->z() += point.z();
}
/**
* @brief Overloaded += operator, performs this = this + vec
* @param vec Vector3d to be added
* @return FramePoint representing this = this + vec
* @note No frame checking/changing occers
*/
void operator+=(const Vector3d& vec)
{
this->x() += vec.x();
this->y() += vec.y();
this->z() += vec.z();
}
/**
* @brief Overloaded -= operator, performs this = this - point
* @param point FramePoint to be subtracted
* @return FramePoint representing this = this - point
*/
void operator-=(const FramePoint& point)
{
checkReferenceFramesMatch(&point);
this->x() -= point.x();
this->y() -= point.y();
this->z() -= point.z();
}
/**
* @brief Overloaded -= operator, performs this = this - vec
* @param vec Vector3d to be subtracted
* @return FramePoint representing this = this - vec
* @note No frame checking/changing occers
*/
void operator-=(const Vector3d& vec)
{
this->x() -= vec.x();
this->y() -= vec.y();
this->z() -= vec.z();
}
/**
* @brief Overloaded *= operator, performs this = this*scala
* @param scale Scalar to scale each element of this FramePoint by
......@@ -356,19 +305,6 @@ inline bool operator==(const FramePoint& lhs, const FramePoint& rhs)
return true;
}
/**
* @brief Add two FramePoints and return result in newly created FramePoint
* @param p1
* @param p2
* @throws ReferenceFrameException If the two FramePoint arguments are not expressed in the same ReferenceFrame
* @return A FramePoint that is the addition of the two argument FramePoints, i.e. p1+=p2
*/
inline FramePoint operator+(FramePoint p1, const FramePoint& p2)
{
p1 += p2;
return p1;
}
/**
* @brief Subtract two FramePoints and return result in newly created FramePoint
* @param p1
......@@ -376,10 +312,10 @@ inline FramePoint operator+(FramePoint p1, const FramePoint& p2)
* @throws ReferenceFrameException If the two FramePoint arguments are not expressed in the same ReferenceFrame
* @return A FramePoint that is the difference of the two argument FramePoints, i.e. p1-=p2
*/
inline FramePoint operator-(FramePoint p1, const FramePoint& p2)
inline FrameVector operator-(FramePoint p1, const FramePoint& p2)
{
p1 -= p2;
return p1;
p1.getReferenceFrame()->checkReferenceFramesMatch(p2.getReferenceFrame());
return FrameVector(p1.getReferenceFrame(), p1.x() - p2.x(), p1.y() - p2.y(), p1.z() - p2.z());
}
/**
......
......@@ -269,20 +269,6 @@ class Point3d : public Math::TransformableGeometricObject
return *this;
}
void operator+=(const Point3d& point)
{
this->x() += point.x();
this->y() += point.y();
this->z() += point.z();
}
void operator-=(const Point3d& point)
{
this->x() -= point.x();
this->y() -= point.y();
this->z() -= point.z();
}
void operator*=(const double scale)
{
this->x() *= scale;
......@@ -316,16 +302,9 @@ class Point3d : public Math::TransformableGeometricObject
double point[3];
};
inline Point3d operator+(Point3d leftHandSide, const Point3d& point)
inline Vector3d operator-(Point3d p1, const Point3d& p2)
{
leftHandSide += point;
return leftHandSide;
}
inline Point3d operator-(Point3d leftHandSide, const Point3d& point)
{
leftHandSide -= point;
return leftHandSide;
return Vector3d(p1.x() - p2.x(), p1.y() - p2.y(), p1.z() - p2.z());
}
inline Point3d operator*(Point3d leftHandSide, const double scale)
......
......@@ -165,7 +165,7 @@ void calcCenterOfMassJacobian(Model& model, const Math::VectorNd& q, Math::Matri
* @param q The current joint positions
* @param updateKinematics If true, kinematic variables will be computed. Default = true
*/
Math::FramePoint calcSubtreeCenterOfMassScaledByMass(Model& model, const unsigned int bodyId, const Math::VectorNd& q, bool updateKinematics = true);
Math::Vector3d calcSubtreeCenterOfMassScaledByMass(Model& model, const unsigned int bodyId, const Math::VectorNd& q, bool updateKinematics = true);
/**
* @brief Calculates the total mass of the subtree beginning with body bodyId and traversing outwards from there
......
<?xml version="1.0"?>
<package format="2">
<name>rdl_dynamics</name>
<version>3.1.0</version>
<version>3.2.0</version>
<description>The rdl_dynamics package</description>
<maintainer email="jlack1987@gmail.com">jordan</maintainer>
......
......@@ -388,7 +388,7 @@ namespace RobotDynamics
jCom /= calcSubtreeMass(model, 0);
}
Math::FramePoint
Math::Vector3d
calcSubtreeCenterOfMassScaledByMass(Model &model, const unsigned int bodyId, const VectorNd &q, bool updateKinematics)
{
if (updateKinematics)
......@@ -398,7 +398,7 @@ namespace RobotDynamics
std::vector<unsigned int> childBodyIds = model.mu[bodyId];
Math::FramePoint comScaledByMass(model.worldFrame, (model.bodyCenteredFrames[bodyId]->getInverseTransformToRoot().r * model.mBodies[bodyId].mMass));
Math::Vector3d comScaledByMass(model.bodyCenteredFrames[bodyId]->getInverseTransformToRoot().r * model.mBodies[bodyId].mMass);
for (unsigned int j = 0; j < childBodyIds.size(); j++)
{
......
......@@ -319,32 +319,6 @@ TEST_F(FramePointTest, testDistanceLinfMixingTypes)
}
}
TEST_F(FramePointTest, testAdd)
{
FramePoint framePoint(root1, 1., 2., 3.);
framePoint *= 3.;
EXPECT_EQ(framePoint.x(), 3.);
EXPECT_EQ(framePoint.y(), 6.);
EXPECT_EQ(framePoint.z(), 9.);
FramePoint p2(root1, RobotDynamics::Math::Vector3d(-1., -2., -3.));
FramePoint p3 = framePoint - p2;
EXPECT_EQ(p3.x(), 4.);
EXPECT_EQ(p3.y(), 8.);
EXPECT_EQ(p3.z(), 12.);
FramePoint p4 = p3;
p4 += p2;
EXPECT_EQ(framePoint.x(), p4.x());
EXPECT_EQ(framePoint.y(), p4.y());
EXPECT_EQ(framePoint.z(), p4.z());
}
TEST_F(FramePointTest, testOperatorOverloads)
{
FramePoint framePoint(root1, 1., 2., 3.);
......@@ -357,31 +331,11 @@ TEST_F(FramePointTest, testOperatorOverloads)
FramePoint p2(root1, RobotDynamics::Math::Vector3d(-1., -2., -3.));
FramePoint p3 = framePoint - p2;
EXPECT_EQ(p3.x(), 4.);
EXPECT_EQ(p3.y(), 8.);
EXPECT_EQ(p3.z(), 12.);
FramePoint p4 = p3 + p2;
EXPECT_EQ(framePoint.x(), p4.x());
EXPECT_EQ(framePoint.y(), p4.y());
EXPECT_EQ(framePoint.z(), p4.z());
Vector3d v3d(13., 14., 15.);
p4.set(1., 2., 3.);
p4 += v3d;
EXPECT_EQ(p4.x(), 14.);
EXPECT_EQ(p4.y(), 16.);
EXPECT_EQ(p4.z(), 18.);
p4 -= v3d;
FrameVector v = framePoint - p2;
EXPECT_EQ(p4.x(), 1.);
EXPECT_EQ(p4.y(), 2.);
EXPECT_EQ(p4.z(), 3.);
EXPECT_EQ(v.x(), 4.);
EXPECT_EQ(v.y(), 8.);
EXPECT_EQ(v.z(), 12.);
}
int main(int argc, char** argv)
......
......@@ -147,7 +147,7 @@ TEST_F(FixedBase3DoFPlanar, testPlanar3LinkPendulum)
try
{
p += p2;
FrameVector v = p2 - p;
}
catch (RobotDynamics::ReferenceFrameException& e)
{
......
......@@ -238,60 +238,14 @@ TEST_F(Point3Test, testDistanceLInf)
}
}
TEST_F(Point3Test, testAdd1)
{
RobotDynamics::Math::Vector3d v1(unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>());
RobotDynamics::Math::Vector3d v2(unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>());
Point3d point1(v1);
Point3d point2(v2);
Point3d point3(v1 + v2);
point1 += point2;
EXPECT_TRUE(point1.epsilonEquals(point3, unit_test_utils::TEST_PREC));
}
TEST_F(Point3Test, testAdd2)
{
Point3d point1, point2;
Point3d point3, point4;
point1.set(unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>());
RobotDynamics::Math::Vector3d v(unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>());
point2.set(v);
Point3d tmpPoint;
tmpPoint.set(v[0] + point1.x(), v[1] + point1.y(), v[2] + point1.z());
point2 = point2 + point1;
EXPECT_TRUE(point2.epsilonEquals(tmpPoint, unit_test_utils::TEST_PREC));
}
TEST_F(Point3Test, testSubtract1)
{
for (int i = 0; i < 1000; i++)
{
Point3d point1 = unit_test_utils::getRandomPoint3();
Point3d point2 = unit_test_utils::getRandomPoint3();
Point3d point3(point1.x() - point2.x(), point1.y() - point2.y(), point1.z() - point2.z());
point1 -= point2;
EXPECT_TRUE(point3.epsilonEquals(point1, 1e-12));
}
for (int i = 0; i < 1000; i++)
{
Point3d point1 = unit_test_utils::getRandomPoint3();
Point3d point2 = unit_test_utils::getRandomPoint3();
Point3d point3(point1.x() - point2.x(), point1.y() - point2.y(), point1.z() - point2.z());
point1 -= point2;
Point3d point1 = unit_test_utils::getRandomPoint3();
Point3d point2 = unit_test_utils::getRandomPoint3();
Vector3d v(point2.x() - point1.x(), point2.y() - point1.y(), point2.z() - point1.z());
Vector3d v2 = point2 - point1;
EXPECT_TRUE(point3.epsilonEquals(point1, unit_test_utils::TEST_PREC));
}
EXPECT_TRUE(v.isApprox(v2, 1e-12));
}
TEST_F(Point3Test, testScale1)
......
......@@ -76,7 +76,7 @@ TEST_F(FixedBase3DoFPlanar, changeFrameAndCopy)
v2 = v1.changeFrameAndCopy(ReferenceFrame::getWorldFrame());
EXPECT_FALSE(v2.wx() == v1.wx());
// EXPECT_FALSE(v2.wy()==v1.wy()); // its a rot about y axis, so this one isn't supposed to change
EXPECT_TRUE(v2.wy() == v1.wy()); // its a rot about y axis, so this one isn't supposed to change
EXPECT_FALSE(v2.wz() == v1.wz());
EXPECT_FALSE(v2.vx() == v1.vx());
EXPECT_FALSE(v2.vy() == v1.vy());
......@@ -97,7 +97,7 @@ TEST_F(FixedBase3DoFPlanar, changeFrameAndCopy)
v4 = v3.changeFrameAndCopy(ReferenceFrame::getWorldFrame());
EXPECT_FALSE(v3.wx() == v4.wx());
// EXPECT_FALSE(v3.wy()==v4.wy()); // its a rot about y axis, so this one isn't supposed to change
EXPECT_TRUE(v3.wy() == v4.wy()); // its a rot about y axis, so this one isn't supposed to change
EXPECT_FALSE(v3.wz() == v4.wz());
EXPECT_FALSE(v3.vx() == v4.vx());
EXPECT_FALSE(v3.vy() == v4.vy());
......
<?xml version="1.0"?>
<package format="2">
<name>rdl_msgs</name>
<version>3.1.0</version>
<version>3.2.0</version>
<description>Custom msgs for rdl types</description>
<maintainer email="jlack1987@gmail.com">jordan</maintainer>
......
<?xml version="1.0"?>
<package format="2">
<name>rdl_ros_tools</name>
<version>3.1.0</version>
<version>3.2.0</version>
<description>ROS interface into rdl tools</description>
<maintainer email="jlack1987@gmail.com">jordan</maintainer>
......
<?xml version="1.0"?>
<package format="2">
<name>rdl_urdfreader</name>
<version>3.1.0</version>
<version>3.2.0</version>
<description>The rdl_urdfreader package</description>
<maintainer email="jlack1987@gmail.com">jordan</maintainer>
......
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