Commit 320bba1c authored by Jordan Lack's avatar Jordan Lack

Run auto formatter

parent 5d5d9f6c
Pipeline #57746748 failed with stages
in 3 minutes and 31 seconds
......@@ -24,7 +24,7 @@ SET ( RDL_SOURCES
src/MotionVector.cpp
src/FrameObject.cpp
src/RigidBodyInertia.cpp
)
include/rdl_dynamics/types.hpp)
include_directories( ${INCLUDES_DIR} ${EIGEN3_INCLUDE_DIR})
......
......@@ -664,8 +664,9 @@ Math::FrameVectorPair calcPointAcceleration6D(Model& model, const Math::VectorNd
* an update of the kinematic state one has to add the gravity
* acceleration has to be added to the result.
*/
Math::FrameVectorPair calcPointAcceleration6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& QDDot, ReferenceFramePtr body_frame,
ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame = nullptr, const bool update_kinematics = true);
Math::FrameVectorPair calcPointAcceleration6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& QDDot,
ReferenceFramePtr body_frame, ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame = nullptr,
const bool update_kinematics = true);
/** \brief Computes the inverse kinematics iteratively using a damped Levenberg-Marquardt method (also known as Damped Least
Squares method)
......
......@@ -390,7 +390,7 @@ struct Model
{
frame = referenceFrameMap.at(frameName);
}
catch(const std::out_of_range& e)
catch (const std::out_of_range& e)
{
frame = nullptr;
}
......
......@@ -129,7 +129,7 @@ class ReferenceFrame : public std::enable_shared_from_this<ReferenceFrame>
ReferenceFramePtr root = parentFrame;
while(root->getParentFrame()!=nullptr)
while (root->getParentFrame() != nullptr)
{
root = root->getParentFrame();
}
......@@ -161,7 +161,6 @@ class ReferenceFrame : public std::enable_shared_from_this<ReferenceFrame>
*/
virtual ~ReferenceFrame()
{
}
/**
......@@ -340,7 +339,8 @@ class ReferenceFrame : public std::enable_shared_from_this<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)
{
rootFrame = this;
update();
......@@ -356,14 +356,14 @@ class ReferenceFrame : public std::enable_shared_from_this<ReferenceFrame>
return ReferenceFramePtr(new ReferenceFrame(frameName, true, 0, true));
}
static ReferenceFramePtr worldFrame; /**< Static world frame pointer */
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*/
RobotDynamics::Math::SpatialTransform transformToRoot; /**< SpatialTransform from a frame to the root frame
*/
RobotDynamics::Math::SpatialTransform inverseTransformToRoot; /**< SpatialTransform to a frame from the root frame
*/
static ReferenceFramePtr worldFrame; /**< Static world frame pointer */
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*/
RobotDynamics::Math::SpatialTransform transformToRoot; /**< SpatialTransform from a frame to the root frame
*/
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
......
......@@ -68,7 +68,8 @@ class SpatialForce : public ForceVector, public FrameObject
* @param m Vector containing the angular component
* @param f Vector containing the linear component
*/
SpatialForce(ReferenceFramePtr 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)
{
}
......
......@@ -60,7 +60,8 @@ class SpatialMomentum : public Momentum, public FrameObject
* @param k Angular part
* @param l Linear part
*/
SpatialMomentum(ReferenceFramePtr 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)
{
}
......
......@@ -71,8 +71,8 @@ class SpatialInertia : public RigidBodyInertia, public FrameObject
* @param Izy
* @param Izz
*/
SpatialInertia(ReferenceFramePtr referenceFrame, double m, const Vector3d& h, const double Ixx, const double Iyx, const double Iyy, const double Izx, const double Izy,
const double Izz)
SpatialInertia(ReferenceFramePtr referenceFrame, double m, const Vector3d& h, const double Ixx, const double Iyx, const double Iyy, const double Izx,
const double Izy, const double Izz)
: RigidBodyInertia(m, h, Ixx, Iyx, Iyy, Izx, Izy, Izz), FrameObject(referenceFrame)
{
}
......
......@@ -30,16 +30,16 @@ TEST_F(FramePointTest, testChangeFrame)
transform1.E = RobotDynamics::Math::Xrotx(M_PI_2).E;
transform1.r.set(6., 5., 3.);
ReferenceFramePtr frameA(new unit_test_utils::RandomUnchangingFrame("A", root1, transform1, 1));
ReferenceFramePtr frameA(new ReferenceFrame("A", root1, transform1, false, 1));
transform1.E = RobotDynamics::Math::Xroty(M_PI_2).E;
transform1.r.set(3., -6., 4.);
ReferenceFramePtr frameB(new unit_test_utils::RandomUnchangingFrame("B", frameA, transform1, 2));
ReferenceFramePtr frameB(new ReferenceFrame("B", frameA, transform1, false, 2));
transform1.E = RobotDynamics::Math::Xrotz(M_PI_2).E;
transform1.r = RobotDynamics::Math::Vector3d(0., -5., 0.);
ReferenceFramePtr frameC(new unit_test_utils::RandomUnchangingFrame("C", frameB, transform1, 3));
ReferenceFramePtr frameC(new ReferenceFrame("C", frameB, transform1, false, 3));
frameA->update();
frameB->update();
......@@ -105,16 +105,16 @@ TEST_F(FramePointTest, testChangeFrameAndCopy)
transform1.E = RobotDynamics::Math::Xrotx(M_PI_2).E;
transform1.r.set(6., 5., 3.);
ReferenceFramePtr frameA(new unit_test_utils::RandomUnchangingFrame("A", root1, transform1, 1));
ReferenceFramePtr frameA(new ReferenceFrame("A", root1, transform1, false, 1));
transform1.E = RobotDynamics::Math::Xroty(M_PI_2).E;
transform1.r.set(3., -6., 4.);
ReferenceFramePtr frameB(new unit_test_utils::RandomUnchangingFrame("B", frameA, transform1, 2));
ReferenceFramePtr frameB(new ReferenceFrame("B", frameA, transform1, false, 2));
transform1.E = RobotDynamics::Math::Xrotz(M_PI_2).E;
transform1.r = RobotDynamics::Math::Vector3d(0., -5., 0.);
ReferenceFramePtr frameC(new unit_test_utils::RandomUnchangingFrame("C", frameB, transform1, 3));
ReferenceFramePtr frameC(new ReferenceFrame("C", frameB, transform1, false, 3));
frameA->update();
frameB->update();
......@@ -159,7 +159,7 @@ TEST_F(FramePointTest, testDistance)
transform1.E = RobotDynamics::Math::Xrotx(M_PI_2).E;
transform1.r.set(5., 0., 0.);
ReferenceFramePtr frameA(new unit_test_utils::RandomUnchangingFrame("A", root1, transform1, 1));
ReferenceFramePtr frameA(new ReferenceFrame("A", root1, transform1, false, 1));
FramePoint framePoint3(frameA, 1., 2., 3.);
......@@ -199,7 +199,7 @@ TEST_F(FramePointTest, testDistanceMixingTypes)
transform1.E = RobotDynamics::Math::Xrotx(M_PI_2).E;
transform1.r.set(5., 0., 0.);
ReferenceFramePtr frameA(new unit_test_utils::RandomUnchangingFrame("A", root1, transform1.inverse(), 1));
ReferenceFramePtr frameA(new ReferenceFrame("A", root1, transform1.inverse(), false, 1));
FramePoint framePoint3(frameA, 1., 2., 3.);
......@@ -225,7 +225,7 @@ TEST_F(FramePointTest, testDistanceL1)
transform1 = RobotDynamics::Math::Xrotx(M_PI_2);
transform1.r.set(5., 0., 0.);
ReferenceFramePtr frameA(new unit_test_utils::RandomUnchangingFrame("A", root1, transform1.inverse(), 1));
ReferenceFramePtr frameA(new ReferenceFrame("A", root1, transform1.inverse(), false, 1));
std::shared_ptr<FramePoint> framePoint3(new RobotDynamics::Math::FramePoint(frameA, 1., 2., 3.));
......@@ -251,7 +251,7 @@ TEST_F(FramePointTest, testDistanceL1MixingTypes)
transform1 = RobotDynamics::Math::Xrotx(M_PI_2);
transform1.r.set(5., 0., 0.);
ReferenceFramePtr frameA(new unit_test_utils::RandomUnchangingFrame("A", root1, transform1.inverse(), 1));
ReferenceFramePtr frameA(new ReferenceFrame("A", root1, transform1.inverse(), false, 1));
std::shared_ptr<FramePoint> framePoint3(new RobotDynamics::Math::FramePoint(frameA, 1., 2., 3.));
......@@ -277,7 +277,7 @@ TEST_F(FramePointTest, testDistanceLinf)
transform1 = RobotDynamics::Math::Xrotx(M_PI_2);
transform1.r.set(5., 0., 0.);
ReferenceFramePtr frameA(new unit_test_utils::RandomUnchangingFrame("A", root1, transform1.inverse(), 1));
ReferenceFramePtr frameA(new ReferenceFrame("A", root1, transform1.inverse(), false, 1));
std::shared_ptr<FramePoint> framePoint3(new RobotDynamics::Math::FramePoint(frameA, 1., 2., 3.));
......@@ -303,7 +303,7 @@ TEST_F(FramePointTest, testDistanceLinfMixingTypes)
transform1 = RobotDynamics::Math::Xrotx(M_PI_2);
transform1.r.set(5., 0., 0.);
ReferenceFramePtr frameA(new unit_test_utils::RandomUnchangingFrame("A", root1, transform1.inverse(), 1));
ReferenceFramePtr frameA(new ReferenceFrame("A", root1, transform1.inverse(), false, 1));
std::shared_ptr<FramePoint> framePoint3(new RobotDynamics::Math::FramePoint(frameA, 1., 2., 3.));
......@@ -338,7 +338,7 @@ TEST_F(FramePointTest, testAdd)
EXPECT_EQ(p3.z(), 12.);
FramePoint p4 = p3;
p4+=p2;
p4 += p2;
EXPECT_EQ(framePoint.x(), p4.x());
EXPECT_EQ(framePoint.y(), p4.y());
......
......@@ -6,12 +6,15 @@ using namespace RobotDynamics;
class FrameVectorPairTest : public ::testing::Test
{
public:
public:
FrameVectorPairTest()
{}
{
}
~FrameVectorPairTest()
{}
{
}
protected:
virtual void SetUp()
{
......
......@@ -10,15 +10,19 @@ class FrameVectorTest : public ::testing::Test
virtual void SetUp()
{
std::srand(time(NULL));
root = ReferenceFrame::createARootFrame("root1");
frame1 = unit_test_utils::createRandomUnchangingFrame("frame1", root, 1);
frame2 = unit_test_utils::createRandomUnchangingFrame("frame2", frame1, 2);
}
virtual void TearDown()
{
}
ReferenceFramePtr root = ReferenceFrame::createARootFrame("root1");
std::shared_ptr<unit_test_utils::RandomUnchangingFrame> frame1 = unit_test_utils::RandomUnchangingFrame::create("frame1", root, 1);
std::shared_ptr<unit_test_utils::RandomUnchangingFrame> frame2 = unit_test_utils::RandomUnchangingFrame::create("frame2", frame1, 2);
ReferenceFramePtr root;
ReferenceFramePtr frame1;
ReferenceFramePtr frame2;
int nTests = 1000;
......@@ -190,17 +194,17 @@ TEST_F(FrameVectorTest, testChangeFrame)
transform1.E = RobotDynamics::Math::Xrotx(M_PI_2).E;
transform1.r = RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(5., 0., 0.)).r;
ReferenceFramePtr frameA(new unit_test_utils::RandomUnchangingFrame("A", root, transform1, 3));
ReferenceFramePtr frameA(new ReferenceFrame("A", root, transform1, false, 3));
transform1.E = RobotDynamics::Math::Xroty(M_PI_2).E;
transform1.r = RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(5., 0., 0.)).r;
ReferenceFramePtr frameB(new unit_test_utils::RandomUnchangingFrame("B", frameA, transform1, 4));
ReferenceFramePtr frameB(new ReferenceFrame("B", frameA, transform1, false, 4));
transform1.E = RobotDynamics::Math::Xrotz(M_PI_2).E;
transform1.r = RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(5., 0., 0.)).r;
ReferenceFramePtr frameC(new unit_test_utils::RandomUnchangingFrame("C", frameB, transform1, 5));
ReferenceFramePtr frameC(new ReferenceFrame("C", frameB, transform1, false, 5));
double x = 3.0;
double y = 1.0;
......@@ -227,17 +231,17 @@ TEST_F(FrameVectorTest, testChangeFrameAndCopy)
transform1.E = RobotDynamics::Math::Xrotx(M_PI_2).E;
transform1.r = RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(5., 0., 0.)).r;
ReferenceFramePtr frameA(new unit_test_utils::RandomUnchangingFrame("A", root, transform1, 3));
ReferenceFramePtr frameA(new ReferenceFrame("A", root, transform1, false, 3));
transform1.E = RobotDynamics::Math::Xroty(M_PI_2).E;
transform1.r = RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(5., 0., 0.)).r;
ReferenceFramePtr frameB(new unit_test_utils::RandomUnchangingFrame("B", frameA, transform1, 4));
ReferenceFramePtr frameB(new ReferenceFrame("B", frameA, transform1, false, 4));
transform1.E = RobotDynamics::Math::Xrotz(M_PI_2).E;
transform1.r = RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(5., 0., 0.)).r;
ReferenceFramePtr frameC(new unit_test_utils::RandomUnchangingFrame("C", frameB, transform1, 5));
ReferenceFramePtr frameC(new ReferenceFrame("C", frameB, transform1, false, 5));
double x = 3.0;
double y = 1.0;
......
......@@ -147,7 +147,7 @@ TEST_F(FixedBase3DoFPlanar, testPlanar3LinkPendulum)
try
{
p+=p2;
p += p2;
}
catch (RobotDynamics::ReferenceFrameException& e)
{
......
......@@ -752,7 +752,7 @@ TEST_F(RotZRotZYXFixed, calcRelativeBodySpatialJacobian)
v = calcSpatialVelocity(*model, Q, QDot, body_fixed_id, body_b_id);
v.changeFrame(body_a_frame);
G.setZero();
calcRelativeBodySpatialJacobian(*model, Q, G, body_fixed_frame, body_b_frame, body_a_frame);
......@@ -2142,8 +2142,8 @@ TEST_F(RdlKinematicsFixture, calcSpatialVelocity)
model.fixedBodyFrames[fb_id2 - model.fixed_body_discriminator])),
-m2, unit_test_utils::TEST_PREC));
EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(
m1_frame.transform_copy(model.fixedBodyFrames[fb_id - model.fixed_body_discriminator]->getTransformToDesiredFrame(
model.fixedBodyFrames[fb_id2 - model.fixed_body_discriminator])),
m1_frame.transform_copy(
model.fixedBodyFrames[fb_id - model.fixed_body_discriminator]->getTransformToDesiredFrame(model.fixedBodyFrames[fb_id2 - model.fixed_body_discriminator])),
-m2_frame, unit_test_utils::TEST_PREC));
}
......
......@@ -196,8 +196,9 @@ TEST_F(FixedBase3DoFPlanar, testAddAndSubtract)
v7.setBodyFrame(model->getReferenceFrame("body_a"));
v7.setBaseFrame(model->getReferenceFrame("body_b"));
EXPECT_TRUE(v7.getReferenceFrame() == model->getReferenceFrame("body_a"));
EXPECT_TRUE(v7.getBodyFrame() == model->getReferenceFrame("body_a"));
EXPECT_TRUE(v7.getBaseFrame() == model->getReferenceFrame("body_b"));
EXPECT_TRUE(v7.getReferenceFrame() == model->getReferenceFrame("body_b"));
}
TEST_F(SpatialMotionTests, testTransformToSpatialVec)
......
......@@ -358,16 +358,16 @@ static inline RobotDynamics::Math::SpatialTransform createRandomSpatialTransform
static inline RobotDynamics::Math::SpatialTransform getTransformToRootByClimbingTree(ReferenceFramePtr frame)
{
if(frame->getParentFrame()==nullptr)
if (frame->getParentFrame() == nullptr)
{
return RobotDynamics::Math::SpatialTransform();
}
RobotDynamics::Math::SpatialTransform transform;
ReferenceFramePtr parent = frame;
while(parent != nullptr)
while (parent != nullptr)
{
transform = parent->getTransformToParent()*transform;
transform = parent->getTransformToParent() * transform;
parent = parent->getParentFrame();
}
......@@ -379,38 +379,6 @@ static inline ReferenceFramePtr createRandomUnchangingFrame(const std::string& f
return ReferenceFramePtr(new ReferenceFrame(frameName, parentFrame, createRandomSpatialTransform(), false, movableBodyId));
}
class RandomlyChangingFrame : public ReferenceFrame
{
public:
RandomlyChangingFrame() : ReferenceFrame()
{
}
RandomlyChangingFrame(const std::string& frameName, ReferenceFramePtr parentFrame, const unsigned int movableBodyId)
: ReferenceFrame(frameName, parentFrame, RobotDynamics::Math::SpatialTransform(), false, movableBodyId)
{
}
RandomlyChangingFrame(const std::string& frameName, ReferenceFramePtr parentFrame, RobotDynamics::Math::SpatialTransform transformFromParent,
const unsigned int movableBodyId)
: ReferenceFrame(frameName, parentFrame, transformFromParent, false, movableBodyId)
{
}
static std::shared_ptr<RandomlyChangingFrame> create(const std::string& frameName, ReferenceFramePtr parentFrame, const unsigned int movableBodyId = 1)
{
RobotDynamics::Math::SpatialTransform randomTransform = createRandomSpatialTransform();
std::shared_ptr<RandomlyChangingFrame> frame(new RandomlyChangingFrame(frameName, parentFrame, randomTransform, movableBodyId));
return frame;
}
protected:
void updateTransformFromParent(RobotDynamics::Math::SpatialTransform& transformFromParent)
{
RobotDynamics::Math::SpatialTransform randomTransform = createRandomSpatialTransform();
transformFromParent = randomTransform;
}
};
/**
*
* @brief Using simple Euler integration, integrate the system dynamics one step
......
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