Commit ffd11c5b authored by Jordan Lack's avatar Jordan Lack

Fixing unit tests to work with API changes

parent 659ef190
Pipeline #57668829 (#925) failed with stages
in 14 minutes and 41 seconds
......@@ -15,6 +15,7 @@
* is important.
*/
#include "rdl_dynamics/ReferenceFrame.hpp"
#include "rdl_dynamics/rdl_eigenmath.h"
namespace RobotDynamics
......
......@@ -400,12 +400,6 @@ inline std::ostream& operator<<(std::ostream& output, const FramePoint& framePoi
output << "x = " << framePoint.x() << " y = " << framePoint.y() << " z = " << framePoint.z() << std::endl;
return output;
}
/**
* @typedef FramePoint<double> FramePointd
* @brief A type definition for a FramePoint<double>
*/
typedef FramePoint FramePointd;
} // namespace Math
} // namespace RobotDynamics
#endif // ifndef __RDL_FRAME_POINT_HPP__
......@@ -70,7 +70,7 @@ void calcCenterOfMass(Model& model, const Math::VectorNd& q, Math::Vector3d& com
* @param com (output) location of the Center of Mass of the model in world frame
* @param update_kinematics (optional input) whether the kinematics should be updated (defaults to true)
*/
void calcCenterOfMass(Model& model, const Math::VectorNd& q, Math::FramePointd& com, bool update_kinematics = true);
void calcCenterOfMass(Model& model, const Math::VectorNd& q, Math::FramePoint& com, bool update_kinematics = true);
/** @brief Computes the Center of Mass (COM) and optionally its linear velocity.
*
......@@ -106,7 +106,7 @@ void calcCenterOfMass(Model& model, const Math::VectorNd& q, const Math::VectorN
* the world frame, but located at the center of mass
* @param update_kinematics (optional input) whether the kinematics should be updated (defaults to true)
*/
void calcCenterOfMass(Model& model, const Math::VectorNd& q, const Math::VectorNd& qdot, double& mass, Math::FramePointd& com, Math::FrameVector* com_velocity = nullptr,
void calcCenterOfMass(Model& model, const Math::VectorNd& q, const Math::VectorNd& qdot, double& mass, Math::FramePoint& com, Math::FrameVector* com_velocity = nullptr,
Math::FrameVector* angular_momentum = nullptr, bool update_kinematics = true);
/** @brief Computes the Center of Mass (COM) and optionally its linear velocity.
......@@ -123,7 +123,7 @@ void calcCenterOfMass(Model& model, const Math::VectorNd& q, const Math::VectorN
* @param com_velocity (optional output) linear velocity of the COM in world frame
* @param update_kinematics (optional input) whether the kinematics should be updated (defaults to true)
*/
void calcCenterOfMass(Model& model, const Math::VectorNd& q, const Math::VectorNd& qdot, Math::FramePointd& com, Math::FrameVector* com_velocity = nullptr,
void calcCenterOfMass(Model& model, const Math::VectorNd& q, const Math::VectorNd& qdot, Math::FramePoint& com, Math::FrameVector* com_velocity = nullptr,
bool update_kinematics = true);
/** @brief Computes the potential energy of the full model.
......@@ -166,7 +166,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::FramePointd calcSubtreeCenterOfMassScaledByMass(Model& model, const unsigned int bodyId, const Math::VectorNd& q, bool updateKinematics = true);
Math::FramePoint 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
......
......@@ -728,7 +728,7 @@ namespace RobotDynamics
// Now we can compute and apply the test forces and use their net effect
// to compute the inverse articlated inertia to fill K.
Math::FramePointd p;
Math::FramePoint p;
for(ci = 0; ci < CS.size(); ci++)
{
unsigned int body_id = CS.body[ci];
......
......@@ -685,7 +685,7 @@ namespace RobotDynamics
updateKinematicsCustom(model, &Q, nullptr, nullptr);
}
Math::FramePointd p;
Math::FramePoint p;
unsigned int reference_body_id = body_id;
......@@ -1155,7 +1155,7 @@ void calcRelativePointJacobianAndJacobianDot6D(Model& model, const Math::VectorN
updateKinematicsCustom(model, &Q, &QDot, nullptr);
}
Math::FramePointd p;
Math::FramePoint p;
unsigned int reference_body_id = body_id;
......@@ -1352,7 +1352,7 @@ void calcRelativePointJacobianAndJacobianDot6D(Model& model, const Math::VectorN
}
unsigned int reference_body_id = body_id;
Math::FramePointd p;
Math::FramePoint p;
if(model.IsFixedBodyId(body_id))
{
......@@ -1390,7 +1390,7 @@ void calcRelativePointJacobianAndJacobianDot6D(Model& model, const Math::VectorN
}
unsigned int reference_body_id = body_id;
Math::FramePointd p;
Math::FramePoint p;
if(model.IsFixedBodyId(body_id))
{
......@@ -1521,7 +1521,7 @@ void calcRelativePointJacobianAndJacobianDot6D(Model& model, const Math::VectorN
}
unsigned int reference_body_id = body_id;
Math::FramePointd p;
Math::FramePoint p;
if(model.IsFixedBodyId(body_id))
{
......@@ -1558,7 +1558,7 @@ void calcRelativePointJacobianAndJacobianDot6D(Model& model, const Math::VectorN
}
unsigned int reference_body_id = body_id;
Math::FramePointd p;
Math::FramePoint p;
if(model.IsFixedBodyId(body_id))
{
......@@ -1803,7 +1803,7 @@ void calcRelativePointJacobianAndJacobianDot6D(Model& model, const Math::VectorN
MatrixNd J = MatrixNd::Zero(3 * body_id.size(), model.qdot_size);
VectorNd e = VectorNd::Zero(3 * body_id.size());
MatrixNd G = model.three_x_qd0;
Math::FramePointd p;
Math::FramePoint p;
Vector3d tmp_vec;
Qres = Qinit;
......
......@@ -236,7 +236,7 @@ namespace RobotDynamics
void calcCenterOfMass(Model & model,
const Math::VectorNd& q,
FramePointd & com,
FramePoint & com,
bool update_kinematics)
{
Vector3d com_v;
......@@ -245,7 +245,7 @@ namespace RobotDynamics
}
void calcCenterOfMass(Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot,
double &mass, Math::FramePointd &com, Math::FrameVector *com_velocity,
double &mass, Math::FramePoint &com, Math::FrameVector *com_velocity,
Math::FrameVector *angular_momentum, bool update_kinematics)
{
Vector3d comPosition, comVelocity, angularMomentum;
......@@ -317,7 +317,7 @@ namespace RobotDynamics
}
void calcCenterOfMass(Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot,
Math::FramePointd &com, Math::FrameVector *com_velocity, bool update_kinematics)
Math::FramePoint &com, Math::FrameVector *com_velocity, bool update_kinematics)
{
if (update_kinematics)
{
......@@ -388,7 +388,7 @@ namespace RobotDynamics
jCom /= calcSubtreeMass(model, 0);
}
Math::FramePointd
Math::FramePoint
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::FramePointd comScaledByMass(model.worldFrame, (model.bodyCenteredFrames[bodyId]->getInverseTransformToRoot().r * model.mBodies[bodyId].mMass));
Math::FramePoint comScaledByMass(model.worldFrame, (model.bodyCenteredFrames[bodyId]->getInverseTransformToRoot().r * model.mBodies[bodyId].mMass));
for (unsigned int j = 0; j < childBodyIds.size(); j++)
{
......@@ -507,7 +507,7 @@ namespace RobotDynamics
updateKinematicsCustom(model,&q,&qdot,nullptr);
}
FramePointd com;
FramePoint com;
FrameVector com_velocity;
calcCenterOfMass(model,q,qdot,com,&com_velocity,false);
SpatialTransform X_com = Xtrans(com.vec());
......
......@@ -26,8 +26,8 @@ class FrameOrientationTest : public ::testing::Test
TEST_F(FrameOrientationTest, setters)
{
ReferenceFrame f1("f1", ReferenceFrame::getWorldFrame(), SpatialTransform(), true, 0);
FrameOrientation orientation(&f1, Quaternion());
ReferenceFramePtr f1(new ReferenceFrame("f1", ReferenceFrame::getWorldFrame(), SpatialTransform(), true, 0));
FrameOrientation orientation(f1, Quaternion());
Quaternion q(1., 2., 3., 4.);
......@@ -51,14 +51,14 @@ TEST_F(FrameOrientationTest, setters)
TEST_F(FrameOrientationTest, changeFrame)
{
SpatialTransform X1 = Xrotz(M_PI_2);
ReferenceFrame f1("f1", ReferenceFrame::getWorldFrame(), X1, true, 0);
ReferenceFramePtr f1(new ReferenceFrame("f1", ReferenceFrame::getWorldFrame(), X1, true, 0));
SpatialTransform X2 = Xrotz(-M_PI_2);
ReferenceFrame f2("f2", &f1, X2, true, 1);
ReferenceFramePtr f2(new ReferenceFrame("f2", f1, X2, true, 1));
SpatialTransform X3 = Xrotz(-M_PI_2);
ReferenceFrame f3("f3", &f2, X3, true, 1);
ReferenceFramePtr f3(new ReferenceFrame("f3", f2, X3, true, 1));
FrameOrientation orientation(&f2, Quaternion());
orientation.changeFrame(&f1);
FrameOrientation orientation(f2, Quaternion());
orientation.changeFrame(f1);
Quaternion expected(Quaternion::fromMatrix(X2.E));
EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation, expected, unit_test_utils::TEST_PREC));
......@@ -70,7 +70,7 @@ TEST_F(FrameOrientationTest, changeFrame)
EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation, expected, unit_test_utils::TEST_PREC));
EXPECT_STREQ(orientation.getReferenceFrame()->getName().c_str(), "World");
orientation.changeFrame(&f3);
orientation.changeFrame(f3);
expected = Quaternion::fromMatrix(Xrotz(M_PI_2).E);
EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation, expected, unit_test_utils::TEST_PREC));
EXPECT_STREQ(orientation.getReferenceFrame()->getName().c_str(), "f3");
......@@ -94,7 +94,7 @@ TEST_F(FrameOrientationTest, changeFrameAndCopy)
EXPECT_STREQ(orientation_copy.getReferenceFrame()->getName().c_str(), "f1");
orientation = orientation_copy;
orientation.changeFrameAndCopy(ReferenceFrame::getWorldFrame(), orientation_copy);
orientation_copy = orientation.changeFrameAndCopy(ReferenceFrame::getWorldFrame());
expected = Quaternion(0., 0., 0., 1.);
EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation_copy, expected, unit_test_utils::TEST_PREC));
......@@ -102,7 +102,7 @@ TEST_F(FrameOrientationTest, changeFrameAndCopy)
EXPECT_STREQ(orientation_copy.getReferenceFrame()->getName().c_str(), "World");
orientation = orientation_copy;
orientation.changeFrameAndCopy(f3, orientation_copy);
orientation_copy = orientation.changeFrameAndCopy(f3);
expected = Quaternion::fromMatrix(Xrotz(M_PI_2).E);
EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation_copy, expected, unit_test_utils::TEST_PREC));
EXPECT_STREQ(orientation.getReferenceFrame()->getName().c_str(), "World");
......
......@@ -17,8 +17,8 @@ class FrameVectorPairTest : public ::testing::Test
}
std::shared_ptr<ReferenceFrame> root = ReferenceFrame::createARootFrame("root1");
std::shared_ptr<unit_test_utils::RandomUnchangingFrame> frame1 = unit_test_utils::RandomUnchangingFrame::create("frame1", root.get(), 1);
std::shared_ptr<unit_test_utils::RandomUnchangingFrame> frame2 = unit_test_utils::RandomUnchangingFrame::create("frame2", frame1.get(), 2);
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);
int nTests = 1000;
......@@ -76,7 +76,7 @@ TEST_F(FrameVectorPairTest, simple2)
ASSERT_TRUE(pair.linear().isApprox(v1, 1e-12));
ASSERT_TRUE(pair.angular().isApprox(v2, 1e-12));
pair.changeFrame(frame2.get());
pair.changeFrame(frame2);
ASSERT_FALSE(pair.linear().isApprox(v1, 1e-12));
ASSERT_FALSE(pair.angular().isApprox(v2, 1e-12));
......@@ -92,7 +92,7 @@ TEST_F(FrameVectorPairTest, simple2)
ASSERT_TRUE(pair.linear().isApprox(pair2.linear(), 1e-12));
ASSERT_TRUE(pair.angular().isApprox(pair2.angular(), 1e-12));
FrameVectorPair pair3 = pair2.changeFrameAndCopy(frame1.get());
FrameVectorPair pair3 = pair2.changeFrameAndCopy(frame1);
v1.changeFrame(frame1);
v2.changeFrame(frame1);
......@@ -118,7 +118,7 @@ TEST_F(FrameVectorPairTest, simple3)
ASSERT_TRUE(pair.linear().isApprox(v1, 1e-12));
ASSERT_TRUE(pair.angular().isApprox(v2, 1e-12));
pair.changeFrame(frame2.get());
pair.changeFrame(frame2);
ASSERT_FALSE(pair.linear().isApprox(v1, 1e-12));
ASSERT_FALSE(pair.angular().isApprox(v2, 1e-12));
......@@ -134,7 +134,7 @@ TEST_F(FrameVectorPairTest, simple3)
ASSERT_TRUE(pair.linear().isApprox(pair2.linear(), 1e-12));
ASSERT_TRUE(pair.angular().isApprox(pair2.angular(), 1e-12));
FrameVectorPair pair3 = pair2.changeFrameAndCopy(frame1.get());
FrameVectorPair pair3 = pair2.changeFrameAndCopy(frame1);
v1.changeFrame(frame1);
v2.changeFrame(frame1);
......@@ -160,7 +160,7 @@ TEST_F(FrameVectorPairTest, simple4)
ASSERT_TRUE(pair.linear().isApprox(v1, 1e-12));
ASSERT_TRUE(pair.angular().isApprox(v2, 1e-12));
pair.changeFrame(frame2.get());
pair.changeFrame(frame2);
ASSERT_FALSE(pair.linear().isApprox(v1, 1e-12));
ASSERT_FALSE(pair.angular().isApprox(v2, 1e-12));
......@@ -176,7 +176,7 @@ TEST_F(FrameVectorPairTest, simple4)
ASSERT_TRUE(pair.linear().isApprox(pair2.linear(), 1e-12));
ASSERT_TRUE(pair.angular().isApprox(pair2.angular(), 1e-12));
FrameVectorPair pair3 = pair2.changeFrameAndCopy(frame1.get());
FrameVectorPair pair3 = pair2.changeFrameAndCopy(frame1);
v1.changeFrame(frame1);
v2.changeFrame(frame1);
......
This diff is collapsed.
......@@ -131,7 +131,7 @@ TEST_F(Human36, TestInverseForwardDynamicsFloatingBaseExternalForces)
N.setZero();
unsigned int foot_r_id = model_emulated->GetBodyId("foot_r");
SpatialForce F(model_emulated->bodyFrames[foot_r_id].get(), -1., 2., 3.1, 82., -23., 500.1);
SpatialForce F(model_emulated->bodyFrames[foot_r_id], -1., 2., 3.1, 82., -23., 500.1);
MatrixNd J_r_foot(6, model_emulated->qdot_size);
J_r_foot.setZero();
......@@ -148,7 +148,7 @@ TEST_F(Human36, TestInverseForwardDynamicsFloatingBaseExternalForces)
std::shared_ptr<std::vector<Math::ForceVector>> f_ext(new std::vector<Math::ForceVector>(model_emulated->mBodies.size()));
F.changeFrame(model_emulated->worldFrame.get());
F.changeFrame(model_emulated->worldFrame);
for (int i = 0; i < model_emulated->mBodies.size(); i++)
{
(*f_ext)[i].setZero();
......@@ -178,7 +178,7 @@ TEST_F(Human36, TestInverseForwardDynamicsFloatingBaseExternalForcesNoKinematics
N.setZero();
unsigned int foot_r_id = model_emulated->GetBodyId("foot_r");
SpatialForce F(model_emulated->bodyFrames[foot_r_id].get(), -1., 2., 3.1, 82., -23., 500.1);
SpatialForce F(model_emulated->bodyFrames[foot_r_id], -1., 2., 3.1, 82., -23., 500.1);
MatrixNd J_r_foot(6, model_emulated->qdot_size);
J_r_foot.setZero();
......@@ -195,7 +195,7 @@ TEST_F(Human36, TestInverseForwardDynamicsFloatingBaseExternalForcesNoKinematics
std::shared_ptr<std::vector<Math::ForceVector>> f_ext(new std::vector<Math::ForceVector>(model_emulated->mBodies.size()));
F.changeFrame(model_emulated->worldFrame.get());
F.changeFrame(model_emulated->worldFrame);
for (int i = 0; i < model_emulated->mBodies.size(); i++)
{
(*f_ext)[i].setZero();
......
......@@ -110,7 +110,7 @@ TEST_F(RdlUtilsTests, TestCOMSimple)
double mass;
Vector3d com;
Vector3d com_velocity;
FramePointd p_com, pcom_2;
FramePoint p_com, pcom_2;
FrameVector v_com;
Utils::calcCenterOfMass(model, q, qdot, mass, com, &com_velocity);
Utils::calcCenterOfMass(model, q, qdot, mass, p_com, &v_com);
......@@ -151,7 +151,7 @@ TEST_F(Human36, TestCOM)
Vector3d com_velocity;
Utils::calcCenterOfMass(*model, q, qdot, mass, com, &com_velocity);
FramePointd p_com, pcom_2;
FramePoint p_com, pcom_2;
FrameVector v_com;
Utils::calcCenterOfMass(*model, q, qdot, p_com, &v_com);
Utils::calcCenterOfMass(*model, q, pcom_2);
......@@ -260,7 +260,7 @@ TEST_F(RdlUtilsTests, TestAngularMomentumSimple)
Vector3d com;
Vector3d angular_momentum;
FramePointd p_com;
FramePoint p_com;
FrameVector v_com, ang_momentum;
qdot << 1., 0., 0.;
......
......@@ -26,7 +26,7 @@ class SpatialForceTests : public ::testing::Test
TEST_F(FixedBase3DoFPlanar, changeFrameAndCopy)
{
SpatialForce v1(model->getBodyFrame("body_b"), 1., 2., 3., 4., 5., 6.);
SpatialForce v1(model->getReferenceFrame("body_b"), 1., 2., 3., 4., 5., 6.);
SpatialForce v2;
randomizeStates();
......@@ -50,10 +50,10 @@ TEST_F(FixedBase3DoFPlanar, changeFrameAndCopy)
EXPECT_TRUE(v2.fy() == v1.fy());
EXPECT_TRUE(v2.fz() == v1.fz());
SpatialForce v3(model->getBodyFrame("body_b"), 1., 2., 3., 4., 5., 6.);
SpatialForce v3(model->getReferenceFrame("body_b"), 1., 2., 3., 4., 5., 6.);
SpatialForce v4;
v3.changeFrameAndCopy(ReferenceFrame::getWorldFrame(), v4);
v4 = v3.changeFrameAndCopy(ReferenceFrame::getWorldFrame());
EXPECT_FALSE(v3.mx() == v4.mx());
EXPECT_FALSE(v3.my() == v4.my());
......@@ -74,7 +74,7 @@ TEST_F(FixedBase3DoFPlanar, changeFrameAndCopy)
TEST_F(FixedBase3DoFPlanar, test_framed_accessors)
{
SpatialForce v1(model->getBodyFrame("body_a"), 1., 2., 3., 4., 5., 6.);
SpatialForce v1(model->getReferenceFrame("body_a"), 1., 2., 3., 4., 5., 6.);
FrameVector fvl = v1.getFramedLinearPart();
FrameVector fva = v1.getFramedAngularPart();
......@@ -89,7 +89,7 @@ TEST_F(FixedBase3DoFPlanar, test_framed_accessors)
EXPECT_EQ(fvl.y(), v1.fy());
EXPECT_EQ(fvl.z(), v1.fz());
v1.setIncludingFrame(model->getBodyFrame("body_b"), ForceVector(0.1, -0.1, 0.2, -0.2, 0.3, -0.3));
v1.setIncludingFrame(model->getReferenceFrame("body_b"), ForceVector(0.1, -0.1, 0.2, -0.2, 0.3, -0.3));
EXPECT_STREQ(v1.getReferenceFrame()->getName().c_str(), "body_b");
EXPECT_EQ(v1.mx(), 0.1);
......@@ -100,7 +100,7 @@ TEST_F(FixedBase3DoFPlanar, test_framed_accessors)
EXPECT_EQ(v1.fy(), 0.3);
EXPECT_EQ(v1.fz(), -0.3);
v1.setIncludingFrame(model->getBodyFrame("body_c"), 3., 4., 5., 6., 7., 8.);
v1.setIncludingFrame(model->getReferenceFrame("body_c"), 3., 4., 5., 6., 7., 8.);
EXPECT_STREQ(v1.getReferenceFrame()->getName().c_str(), "body_c");
EXPECT_EQ(v1.mx(), 3.);
......@@ -114,8 +114,8 @@ TEST_F(FixedBase3DoFPlanar, test_framed_accessors)
TEST_F(FixedBase3DoFPlanar, testAdd)
{
SpatialForce v1(model->getBodyFrame("body_a"), 0., 1., 0., 0., 0., 0.);
SpatialForce v2(model->getBodyFrame("body_b"), 0., 1., 0., 0., 0., 0.);
SpatialForce v1(model->getReferenceFrame("body_a"), 0., 1., 0., 0., 0., 0.);
SpatialForce v2(model->getReferenceFrame("body_b"), 0., 1., 0., 0., 0., 0.);
try
{
......@@ -127,7 +127,7 @@ TEST_F(FixedBase3DoFPlanar, testAdd)
EXPECT_STREQ("Reference frames do not match!", e.what());
}
SpatialForce v4(model->getBodyFrame("body_b"), 0., 1., 0., 0., 0., 0.);
SpatialForce v4(model->getReferenceFrame("body_b"), 0., 1., 0., 0., 0., 0.);
SpatialForce v5 = v2 + v4;
......@@ -137,7 +137,7 @@ TEST_F(FixedBase3DoFPlanar, testAdd)
EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(v5, SpatialVector(0., 2., 0., 0., 0., 0.), unit_test_utils::TEST_PREC));
SpatialForce v7(model->getBodyFrame("body_b"), -0.2, 0.3, -0.4, 0.5, -0.6, 0.7);
SpatialForce v7(model->getReferenceFrame("body_b"), -0.2, 0.3, -0.4, 0.5, -0.6, 0.7);
EXPECT_EQ(v7.mx(), -0.2);
EXPECT_EQ(v7.my(), 0.3);
......@@ -163,27 +163,27 @@ TEST_F(SpatialForceTests, testChangeFrame)
X_66_matrix = Xrotz_mat(rot[2]) * Xroty_mat(rot[1]) * Xrotx_mat(rot[0]) * Xtrans_mat(trans);
X_st.E = X_66_matrix.block<3, 3>(0, 0);
std::shared_ptr<ReferenceFrame> frameA(new ReferenceFrame("frameA", ReferenceFrame::getWorldFrame().get(), X_st, 0, 0));
std::shared_ptr<ReferenceFrame> frameA(new ReferenceFrame("frameA", ReferenceFrame::getWorldFrame(), X_st, 0, 0));
ForceVector v(1.1, 2.1, 3.1, 4.1, 5.1, 6.1);
SpatialForce m(frameA.get(), v);
SpatialForce m(frameA, v);
v.transform(X_st.inverse());
m.changeFrame(ReferenceFrame::getWorldFrame().get());
m.changeFrame(ReferenceFrame::getWorldFrame());
EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(m, v, unit_test_utils::TEST_PREC));
}
TEST_F(SpatialForceTests, testFrameCorrectness)
{
SpatialForce F(ReferenceFrame::getWorldFrame().get(), 0., 0., 0., 1., 2., 3.);
SpatialForce F(ReferenceFrame::getWorldFrame(), 0., 0., 0., 1., 2., 3.);
SpatialTransform X_z = Xrotz(M_PI_2);
std::shared_ptr<ReferenceFrame> R_2(new ReferenceFrame("Frame2", ReferenceFrame::getWorldFrame().get(), X_z, true, 1));
std::shared_ptr<ReferenceFrame> R_2(new ReferenceFrame("Frame2", ReferenceFrame::getWorldFrame(), X_z, true, 1));
F.changeFrame(R_2.get());
F.changeFrame(R_2);
EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(F, SpatialVector(0., 0., 0., 2., -1., 3.), unit_test_utils::TEST_PREC));
......
......@@ -330,7 +330,7 @@ static inline bool checkSpatialVectorsEq(const RobotDynamics::Math::SpatialVecto
return true;
}
static inline void updateAllFrames(std::vector<ReferenceFrame*> frames)
static inline void updateAllFrames(std::vector<ReferenceFramePtr> frames)
{
for (int i = 0; i < frames.size(); i++)
{
......@@ -338,7 +338,7 @@ static inline void updateAllFrames(std::vector<ReferenceFrame*> frames)
}
}
static inline ReferenceFrame* getARandomFrame(std::vector<ReferenceFrame*> frames)
static inline ReferenceFramePtr getARandomFrame(std::vector<ReferenceFramePtr> frames)
{
int index = rand() % frames.size();
......@@ -356,9 +356,9 @@ static inline RobotDynamics::Math::SpatialTransform createRandomSpatialTransform
return X;
}
static inline RobotDynamics::Math::SpatialTransform getTransformToRootByClimbingTree(ReferenceFrame* frame)
static inline RobotDynamics::Math::SpatialTransform getTransformToRootByClimbingTree(ReferenceFramePtr frame)
{
const std::vector<ReferenceFrame*> framesStartingWithRootEndingWithFrame = frame->getFramesStartingWithRootEndingWithThis();
const std::vector<ReferenceFramePtr> framesStartingWithRootEndingWithFrame = frame->getFramesStartingWithRootEndingWithThis();
RobotDynamics::Math::SpatialTransform transform;
......@@ -380,13 +380,13 @@ class RandomUnchangingFrame : public ReferenceFrame
{
}
RandomUnchangingFrame(const std::string& frameName, ReferenceFrame* parentFrame, RobotDynamics::Math::SpatialTransform transformFromParent,
RandomUnchangingFrame(const std::string& frameName, ReferenceFramePtr parentFrame, RobotDynamics::Math::SpatialTransform transformFromParent,
const unsigned int movableBodyId)
: ReferenceFrame(frameName, parentFrame, transformFromParent, false, movableBodyId)
{
}
static std::shared_ptr<RandomUnchangingFrame> create(const std::string& frameName, ReferenceFrame* parentFrame, const unsigned int movableBodyId = 1)
static std::shared_ptr<RandomUnchangingFrame> create(const std::string& frameName, ReferenceFramePtr parentFrame, const unsigned int movableBodyId = 1)
{
RobotDynamics::Math::SpatialTransform randomTransform = createRandomSpatialTransform();
......@@ -406,18 +406,18 @@ class RandomlyChangingFrame : public ReferenceFrame
RandomlyChangingFrame() : ReferenceFrame()
{
}
RandomlyChangingFrame(const std::string& frameName, ReferenceFrame* parentFrame, const unsigned int movableBodyId)
: ReferenceFrame(frameName, parentFrame, false, movableBodyId)
RandomlyChangingFrame(const std::string& frameName, ReferenceFramePtr parentFrame, const unsigned int movableBodyId)
: ReferenceFrame(frameName, parentFrame, RobotDynamics::Math::SpatialTransform(), false, movableBodyId)
{
}
RandomlyChangingFrame(const std::string& frameName, ReferenceFrame* parentFrame, RobotDynamics::Math::SpatialTransform transformFromParent,
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, ReferenceFrame* parentFrame, const unsigned int movableBodyId = 1)
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));
......
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