Commit 6ee85f98 authored by Jordan Lack's avatar Jordan Lack

Tests segfaulting all over the place now. Expected this. Memory issues to fix now

parent ffd11c5b
Pipeline #57683182 (#926) failed with stages
in 15 minutes and 7 seconds
......@@ -143,6 +143,7 @@ class ReferenceFrame
*/
virtual ~ReferenceFrame()
{
std::cout << "DELETING " << frameName << std::endl;
}
/**
......
This diff is collapsed.
This diff is collapsed.
......@@ -16,9 +16,9 @@ class FrameVectorTest : 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);
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);
int nTests = 1000;
......@@ -27,7 +27,7 @@ class FrameVectorTest : public ::testing::Test
TEST_F(FrameVectorTest, operators)
{
FrameVector v1(root.get(), 1., 2., 3.);
FrameVector v1(root, 1., 2., 3.);
Vector3d v2(2., 3., 4.);
FrameVector v3;
......@@ -49,13 +49,13 @@ TEST_F(FrameVectorTest, operators)
EXPECT_EQ(v3.y(), -1.);
EXPECT_EQ(v3.z(), -1.);
v1.setReferenceFrame(frame1.get());
v1.setReferenceFrame(frame1);
EXPECT_STREQ(v1.getReferenceFrame()->getName().c_str(), "frame1");
}
TEST_F(FrameVectorTest, testConstructors)
{
FrameVector frameVector(root.get(), 1., 2., 3.);
FrameVector frameVector(root, 1., 2., 3.);
RobotDynamics::Math::Vector3d vec = frameVector.vec();
EXPECT_EQ(vec.x(), 1.);
......@@ -68,7 +68,7 @@ TEST_F(FrameVectorTest, testConstructors)
EXPECT_TRUE(vector(2) == 3.);
Eigen::Vector3d vector2(3., 2., 1.);
FrameVector frameVector2(root.get(), vector2);
FrameVector frameVector2(root, vector2);
Eigen::Vector3d vectorCheck = frameVector2;
EXPECT_TRUE(vectorCheck(0) == 3.);
......@@ -78,16 +78,16 @@ TEST_F(FrameVectorTest, testConstructors)
TEST_F(FrameVectorTest, testSetIncludingFrame)
{
FrameVector frameVector(root.get(), 1., 2., 3.);
FrameVector frameVector(root, 1., 2., 3.);
frameVector.setIncludingFrame(0.1, 0.2, 0.3, frame1.get());
frameVector.setIncludingFrame(0.1, 0.2, 0.3, frame1);
EXPECT_STREQ(frameVector.getReferenceFrame()->getName().c_str(), "frame1");
EXPECT_EQ(frameVector.x(), 0.1);
EXPECT_EQ(frameVector.y(), 0.2);
EXPECT_EQ(frameVector.z(), 0.3);
frameVector.setIncludingFrame(Vector3d(0.3, 0.2, 0.1), frame2.get());
frameVector.setIncludingFrame(Vector3d(0.3, 0.2, 0.1), frame2);
EXPECT_STREQ(frameVector.getReferenceFrame()->getName().c_str(), "frame2");
EXPECT_EQ(frameVector.x(), 0.3);
......@@ -115,9 +115,9 @@ TEST_F(FrameVectorTest, testSetIncludingFrame)
TEST_F(FrameVectorTest, testDot)
{
FrameVector frameVector1(frame1.get(), -1., 2., -3.);
FrameVector frameVector2(frame2.get(), 1., 2., 3.);
FrameVector frameVector3(frame1.get(), 4., 5., -6.);
FrameVector frameVector1(frame1, -1., 2., -3.);
FrameVector frameVector2(frame2, 1., 2., 3.);
FrameVector frameVector3(frame1, 4., 5., -6.);
try
{
......@@ -144,9 +144,9 @@ TEST_F(FrameVectorTest, testCross)
Eigen::Vector3d v2(unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>());
Eigen::Vector3d v3(unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>());
FrameVector frameVector1(frame1.get(), v1);
FrameVector frameVector2(frame2.get(), v2);
FrameVector frameVector3(frame1.get(), v3);
FrameVector frameVector1(frame1, v1);
FrameVector frameVector2(frame2, v2);
FrameVector frameVector3(frame1, v3);
try
{
......@@ -171,8 +171,8 @@ TEST_F(FrameVectorTest, testCross)
TEST_F(FrameVectorTest, testAngleBetweenVectors)
{
FrameVector frameVector1(frame1.get(), 2., 3., 1.);
FrameVector frameVector2(frame1.get(), 4., 1., 2.);
FrameVector frameVector1(frame1, 2., 3., 1.);
FrameVector frameVector2(frame1, 4., 1., 2.);
double angle = frameVector1.getAngleBetweenVectors(frameVector2);
double expectedResult = acos(13 / (sqrt(14) * sqrt(21)));
......@@ -190,30 +190,30 @@ 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;
std::shared_ptr<ReferenceFrame> frameA(new unit_test_utils::RandomUnchangingFrame("A", root.get(), transform1, 3));
ReferenceFramePtr frameA(new unit_test_utils::RandomUnchangingFrame("A", root, transform1, 3));
transform1.E = RobotDynamics::Math::Xroty(M_PI_2).E;
transform1.r = RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(5., 0., 0.)).r;
std::shared_ptr<ReferenceFrame> frameB(new unit_test_utils::RandomUnchangingFrame("B", frameA.get(), transform1, 4));
ReferenceFramePtr frameB(new unit_test_utils::RandomUnchangingFrame("B", frameA, transform1, 4));
transform1.E = RobotDynamics::Math::Xrotz(M_PI_2).E;
transform1.r = RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(5., 0., 0.)).r;
std::shared_ptr<ReferenceFrame> frameC(new unit_test_utils::RandomUnchangingFrame("C", frameB.get(), transform1, 5));
ReferenceFramePtr frameC(new unit_test_utils::RandomUnchangingFrame("C", frameB, transform1, 5));
double x = 3.0;
double y = 1.0;
double z = -9.0;
FrameVector frameVector(frameC.get(), x, y, z);
frameVector.changeFrame(frameB.get());
FrameVector frameVector(frameC, x, y, z);
frameVector.changeFrame(frameB);
EXPECT_NEAR(frameVector.x(), -1, unit_test_utils::TEST_PREC);
EXPECT_NEAR(frameVector.y(), 3, unit_test_utils::TEST_PREC);
EXPECT_NEAR(frameVector.z(), -9, unit_test_utils::TEST_PREC);
frameVector.changeFrame(frameA.get());
frameVector.changeFrame(frameA);
EXPECT_NEAR(frameVector.x(), -9, unit_test_utils::TEST_PREC);
EXPECT_NEAR(frameVector.y(), 3, unit_test_utils::TEST_PREC);
......@@ -227,24 +227,24 @@ 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;
std::shared_ptr<ReferenceFrame> frameA(new unit_test_utils::RandomUnchangingFrame("A", root.get(), transform1, 3));
ReferenceFramePtr frameA(new unit_test_utils::RandomUnchangingFrame("A", root, transform1, 3));
transform1.E = RobotDynamics::Math::Xroty(M_PI_2).E;
transform1.r = RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(5., 0., 0.)).r;
std::shared_ptr<ReferenceFrame> frameB(new unit_test_utils::RandomUnchangingFrame("B", frameA.get(), transform1, 4));
ReferenceFramePtr frameB(new unit_test_utils::RandomUnchangingFrame("B", frameA, transform1, 4));
transform1.E = RobotDynamics::Math::Xrotz(M_PI_2).E;
transform1.r = RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(5., 0., 0.)).r;
std::shared_ptr<ReferenceFrame> frameC(new unit_test_utils::RandomUnchangingFrame("C", frameB.get(), transform1, 5));
ReferenceFramePtr frameC(new unit_test_utils::RandomUnchangingFrame("C", frameB, transform1, 5));
double x = 3.0;
double y = 1.0;
double z = -9.0;
FrameVector v;
FrameVector frameVector(frameC.get(), x, y, z);
FrameVector frameVector(frameC, x, y, z);
v = frameVector.changeFrameAndCopy(frameB);
EXPECT_NEAR(v.x(), -1, unit_test_utils::TEST_PREC);
......@@ -252,7 +252,7 @@ TEST_F(FrameVectorTest, testChangeFrameAndCopy)
EXPECT_NEAR(v.z(), -9, unit_test_utils::TEST_PREC);
FrameVector v2;
v.changeFrameAndCopy(frameA, v2);
v2 = v.changeFrameAndCopy(frameA);
EXPECT_NEAR(v2.x(), -9, unit_test_utils::TEST_PREC);
EXPECT_NEAR(v2.y(), 3, unit_test_utils::TEST_PREC);
......@@ -268,7 +268,7 @@ TEST_F(FrameVectorTest, testVectorLength)
{
Eigen::Vector3d v1(unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>(), unit_test_utils::getRandomNumber<double>());
FrameVector frameVector1(frame1.get(), v1);
FrameVector frameVector1(frame1, v1);
double result = frameVector1.norm();
double expectedResult = v1.norm();
......
......@@ -521,13 +521,13 @@ TEST_F(RdlCustomJointMultiBodyFixture, Jacobians)
Gref.setZero();
Gcus.setZero();
ReferenceFrame* b1_ref_frame = reference_model.at(idx).bodyFrames[reference_body_id.at(idx)[0]].get();
ReferenceFrame* b2_ref_frame = reference_model.at(idx).bodyFrames[reference_body_id.at(idx)[1]].get();
ReferenceFrame* b3_ref_frame = reference_model.at(idx).bodyFrames[reference_body_id.at(idx)[2]].get();
ReferenceFramePtr b1_ref_frame = reference_model.at(idx).bodyFrames[reference_body_id.at(idx)[0]];
ReferenceFramePtr b2_ref_frame = reference_model.at(idx).bodyFrames[reference_body_id.at(idx)[1]];
ReferenceFramePtr b3_ref_frame = reference_model.at(idx).bodyFrames[reference_body_id.at(idx)[2]];
ReferenceFrame* b1_cus_frame = custom_model.at(idx).bodyFrames[custom_body_id.at(idx)[0]].get();
ReferenceFrame* b2_cus_frame = custom_model.at(idx).bodyFrames[custom_body_id.at(idx)[1]].get();
ReferenceFrame* b3_cus_frame = custom_model.at(idx).bodyFrames[custom_body_id.at(idx)[2]].get();
ReferenceFramePtr b1_cus_frame = custom_model.at(idx).bodyFrames[custom_body_id.at(idx)[0]];
ReferenceFramePtr b2_cus_frame = custom_model.at(idx).bodyFrames[custom_body_id.at(idx)[1]];
ReferenceFramePtr b3_cus_frame = custom_model.at(idx).bodyFrames[custom_body_id.at(idx)[2]];
calcRelativeBodySpatialJacobian(reference_model.at(idx), q.at(idx), Gref, b1_ref_frame, b2_ref_frame, b3_ref_frame);
calcRelativeBodySpatialJacobian(custom_model.at(idx), q.at(idx), Gcus, b1_cus_frame, b2_cus_frame, b3_cus_frame);
......@@ -626,8 +626,8 @@ TEST_F(RdlCustomJointMultiBodyFixture, Jacobians)
GrefPt = MatrixNd::Constant(6, reference_model.at(idx).qdot_size, 0.);
GcusPt = MatrixNd::Constant(6, reference_model.at(idx).qdot_size, 0.);
RobotDynamics::ReferenceFrame* frame1 = reference_model.at(idx).bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)].get();
RobotDynamics::ReferenceFrame* frame2 = custom_model.at(idx).bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)].get();
RobotDynamics::ReferenceFramePtr frame1 = reference_model.at(idx).bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
RobotDynamics::ReferenceFramePtr frame2 = custom_model.at(idx).bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
calcPointJacobian6D(reference_model.at(idx), q.at(idx), GrefPt, frame1);
calcPointJacobian6D(custom_model.at(idx), q.at(idx), GcusPt, frame2);
......@@ -643,8 +643,8 @@ TEST_F(RdlCustomJointMultiBodyFixture, Jacobians)
GrefPt = MatrixNd::Constant(6, reference_model.at(idx).qdot_size, 0.);
GcusPt = MatrixNd::Constant(6, reference_model.at(idx).qdot_size, 0.);
frame1 = reference_model.at(idx).bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)].get();
frame2 = custom_model.at(idx).bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)].get();
frame1 = reference_model.at(idx).bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
frame2 = custom_model.at(idx).bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
calcPointJacobianDot6D(reference_model.at(idx), q.at(idx), qdot.at(idx), frame1, GrefPt);
calcPointJacobianDot6D(custom_model.at(idx), q.at(idx), qdot.at(idx), frame2, GcusPt);
......@@ -660,8 +660,8 @@ TEST_F(RdlCustomJointMultiBodyFixture, Jacobians)
GrefPt = MatrixNd::Constant(3, reference_model.at(idx).qdot_size, 0.);
GcusPt = MatrixNd::Constant(3, reference_model.at(idx).qdot_size, 0.);
frame1 = reference_model.at(idx).bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)].get();
frame2 = custom_model.at(idx).bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)].get();
frame1 = reference_model.at(idx).bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
frame2 = custom_model.at(idx).bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
calcPointJacobianDot(reference_model.at(idx), q.at(idx), qdot.at(idx), frame1, GrefPt);
calcPointJacobianDot(custom_model.at(idx), q.at(idx), qdot.at(idx), frame2, GcusPt);
......@@ -671,8 +671,8 @@ TEST_F(RdlCustomJointMultiBodyFixture, Jacobians)
GrefPt = MatrixNd::Constant(3, reference_model.at(idx).qdot_size, 0.);
GcusPt = MatrixNd::Constant(3, reference_model.at(idx).qdot_size, 0.);
frame1 = reference_model.at(idx).bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)].get();
frame2 = custom_model.at(idx).bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)].get();
frame1 = reference_model.at(idx).bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
frame2 = custom_model.at(idx).bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
calcPointJacobian(reference_model.at(idx), q.at(idx), GrefPt, frame1);
calcPointJacobian(custom_model.at(idx), q.at(idx), GcusPt, frame2);
......@@ -685,10 +685,10 @@ TEST_F(RdlCustomJointMultiBodyFixture, Jacobians)
GDotrefPt = MatrixNd::Constant(6, reference_model.at(idx).qdot_size, 0.);
GDotcusPt = MatrixNd::Constant(6, reference_model.at(idx).qdot_size, 0.);
frame1 = reference_model.at(idx).bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)].get();
RobotDynamics::ReferenceFrame* frame1Rel = reference_model.at(idx).bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 2)].get();
frame2 = custom_model.at(idx).bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)].get();
RobotDynamics::ReferenceFrame* frame2Rel = custom_model.at(idx).bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 2)].get();
frame1 = reference_model.at(idx).bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
RobotDynamics::ReferenceFramePtr frame1Rel = reference_model.at(idx).bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 2)];
frame2 = custom_model.at(idx).bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
RobotDynamics::ReferenceFramePtr frame2Rel = custom_model.at(idx).bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 2)];
calcRelativePointJacobian6D(reference_model.at(idx), q.at(idx), GrefPt, frame1, frame1Rel, frame1);
calcRelativePointJacobian6D(custom_model.at(idx), q.at(idx), GcusPt, frame2, frame2Rel, frame2);
......
......@@ -845,7 +845,7 @@ TEST_F(Human36, TestForwardDynamicsWithExternalForces)
N.setZero();
unsigned int foot_r_id = model_emulated->GetBodyId("foot_r");
SpatialForce F(model_emulated->bodyFrames[foot_r_id].get(), 0.1, 0.3, -0.8, 82., -23., 500.1);
SpatialForce F(model_emulated->bodyFrames[foot_r_id], 0.1, 0.3, -0.8, 82., -23., 500.1);
MatrixNd J_r_foot(6, model_emulated->qdot_size);
J_r_foot.setZero();
......@@ -862,7 +862,7 @@ TEST_F(Human36, TestForwardDynamicsWithExternalForces)
std::shared_ptr<std::vector<Math::ForceVector>> f_ext(new std::vector<Math::ForceVector>(model_emulated->mBodies.size()));
F.changeFrame(model->worldFrame.get());
F.changeFrame(model->worldFrame);
for (int i = 0; i < model_emulated->mBodies.size(); i++)
{
......@@ -898,7 +898,7 @@ TEST_F(Human36, TestForwardDynamicsWithExternalForcesNoKinematics)
N.setZero();
unsigned int foot_r_id = model_emulated->GetBodyId("foot_r");
SpatialForce F(model_emulated->bodyFrames[foot_r_id].get(), 0.1, 0.3, -0.8, 82., -23., 500.1);
SpatialForce F(model_emulated->bodyFrames[foot_r_id], 0.1, 0.3, -0.8, 82., -23., 500.1);
MatrixNd J_r_foot(6, model_emulated->qdot_size);
J_r_foot.setZero();
......@@ -915,7 +915,7 @@ TEST_F(Human36, TestForwardDynamicsWithExternalForcesNoKinematics)
std::shared_ptr<std::vector<Math::ForceVector>> f_ext(new std::vector<Math::ForceVector>(model_emulated->mBodies.size()));
F.changeFrame(model->worldFrame.get());
F.changeFrame(model->worldFrame);
for (int i = 0; i < model_emulated->mBodies.size(); i++)
{
......
......@@ -54,8 +54,8 @@ TEST_F(FloatingBaseTestFixture, TestCalcPointTransformation)
forwardDynamics(*model, q, qdot, tau, qddot);
// test_point = calcBaseToBodyCoordinates(*model, q, base_body_id, Vector3d(0., 0., 0.), false);
FramePointd p(model->worldFrame.get(), Vector3d::Zero());
p.changeFrame(model->bodyFrames[base_body_id].get());
FramePoint p(model->worldFrame, Vector3d::Zero());
p.changeFrame(model->bodyFrames[base_body_id]);
EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0., -1., 0.), p.vec(), unit_test_utils::TEST_PREC));
}
......@@ -260,8 +260,8 @@ TEST_F(FloatingBaseTestFixture, TestCalcPointAccelerationNoQDDot)
qdot = qdot;
FramePointd point_world_position(model->bodyFrames[ref_body_id].get(), point_body_position);
point_world_position.changeFrame(model->worldFrame.get());
FramePoint point_world_position(model->bodyFrames[ref_body_id], point_body_position);
point_world_position.changeFrame(model->worldFrame);
point_world_velocity = calcPointVelocity(*model, q, qdot, ref_body_id, point_body_position);
point_world_acceleration = calcPointAcceleration(*model, q, qdot, qddot, ref_body_id, point_body_position);
......@@ -317,7 +317,7 @@ TEST_F(FloatingBaseTestFixture, TestCalcPointAccelerationOnlyQDDot)
qddot[4] = 1.5;
qddot[5] = 1.7;
FramePointd point_world_position(model->bodyFrames[ref_body_id].get(), point_body_position);
FramePoint point_world_position(model->bodyFrames[ref_body_id], point_body_position);
point_world_position.changeFrame(model->worldFrame);
point_world_velocity = calcPointVelocity(*model, q, qdot, ref_body_id, point_body_position);
......@@ -388,8 +388,8 @@ TEST_F(FloatingBaseTestFixture, TestCalcPointAccelerationFull)
// cout << "ref_body_id = " << ref_body_id << endl;
// cout << "point_body_position = " << point_body_position << endl;
FramePointd point_world_position(model->bodyFrames[ref_body_id].get(), point_body_position);
point_world_position.changeFrame(model->worldFrame.get());
FramePoint point_world_position(model->bodyFrames[ref_body_id], point_body_position);
point_world_position.changeFrame(model->worldFrame);
point_world_velocity = calcPointVelocity(*model, q, qdot, ref_body_id, point_body_position);
point_world_acceleration = calcPointAcceleration(*model, q, qdot, qddot, ref_body_id, point_body_position);
......
This diff is collapsed.
......@@ -291,10 +291,10 @@ TEST_F(RdlModelFixture, TestTransformBaseToLocal)
Vector3d base_coords_back;
updateKinematics(*model, q, qdot, qddot);
FramePointd p_base_coords(model->bodyFrames[body_id].get(), base_coords);
p_base_coords.changeFrame(model->worldFrame.get());
FramePoint p_base_coords(model->bodyFrames[body_id], base_coords);
p_base_coords.changeFrame(model->worldFrame);
p_base_coords.changeFrame(model->bodyFrames[body_id].get());
p_base_coords.changeFrame(model->bodyFrames[body_id]);
EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(base_coords, p_base_coords.vec(), TEST_PREC));
......@@ -307,9 +307,9 @@ TEST_F(RdlModelFixture, TestTransformBaseToLocal)
updateKinematics(*model, q, qdot, qddot);
p_base_coords.setIncludingFrame(base_coords, model->worldFrame.get());
p_base_coords.changeFrame(model->bodyFrames[body_id].get());
p_base_coords.changeFrame(model->worldFrame.get());
p_base_coords.setIncludingFrame(base_coords, model->worldFrame);
p_base_coords.changeFrame(model->bodyFrames[body_id]);
p_base_coords.changeFrame(model->worldFrame);
EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(base_coords, p_base_coords.vec(), TEST_PREC));
}
......@@ -705,7 +705,7 @@ TEST_F(RdlModelFixture, ModelFixedJointRotationOrderTranslationRotation)
VectorNd QDDot = VectorNd::Zero(model.dof_count);
updateKinematics(model, Q, QDot, QDDot);
FramePointd p(model.bodyFrames[body_after_fixed].get(), Vector3d(0., 1., 0.));
FramePoint p(model.bodyFrames[body_after_fixed], Vector3d(0., 1., 0.));
p.changeFrame(model.worldFrame);
EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0., 1., 0.), p.vec(), TEST_PREC));
......@@ -735,7 +735,7 @@ TEST_F(RdlModelFixture, ModelFixedJointRotationOrderRotationTranslation)
VectorNd Q(VectorNd::Zero(model.dof_count));
Q[0] = 45 * M_PI / 180.;
updateKinematicsCustom(model, &Q, nullptr, nullptr);
FramePointd p(model.bodyFrames[body_after_fixed].get(), Vector3d(0., 1., 0.));
FramePoint p(model.bodyFrames[body_after_fixed], Vector3d(0., 1., 0.));
p.changeFrame(model.worldFrame);
EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(-1., 2., 0.), p.vec(), TEST_PREC));
......
......@@ -10,29 +10,29 @@ class ReferenceFrameTest : public ::testing::Test
{
std::srand(time(NULL));
allFrames.push_back(root1.get());
allFrames.push_back(frame1.get());
allFrames.push_back(frame2.get());
allFrames.push_back(frame3.get());
allFrames.push_back(root2.get());
allFrames.push_back(frame4.get());
allFrames.push_back(frame5.get());
allFrames.push_back(frame6.get());
allFrames.push_back(frame7.get());
allFrames.push_back(frame8.get());
frames1.push_back(root1.get());
frames1.push_back(frame1.get());
frames1.push_back(frame2.get());
frames1.push_back(frame3.get());
frames2.push_back(root2.get());
frames2.push_back(frame4.get());
frames2.push_back(frame5.get());
frames2.push_back(frame6.get());
frames2.push_back(frame7.get());
frames2.push_back(frame8.get());
allFrames.push_back(root1);
allFrames.push_back(frame1);
allFrames.push_back(frame2);
allFrames.push_back(frame3);
allFrames.push_back(root2);
allFrames.push_back(frame4);
allFrames.push_back(frame5);
allFrames.push_back(frame6);
allFrames.push_back(frame7);
allFrames.push_back(frame8);
frames1.push_back(root1);
frames1.push_back(frame1);
frames1.push_back(frame2);
frames1.push_back(frame3);
frames2.push_back(root2);
frames2.push_back(frame4);
frames2.push_back(frame5);
frames2.push_back(frame6);
frames2.push_back(frame7);
frames2.push_back(frame8);
for (auto const& frame : allFrames)
{
......@@ -49,19 +49,19 @@ class ReferenceFrameTest : public ::testing::Test
std::shared_ptr<ReferenceFrame> root1 = ReferenceFrame::createARootFrame("root1");
std::shared_ptr<ReferenceFrame> root2 = ReferenceFrame::createARootFrame("root2");
std::shared_ptr<unit_test_utils::RandomUnchangingFrame> frame1 = unit_test_utils::RandomUnchangingFrame::create("frame1", root1.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> frame3 = unit_test_utils::RandomUnchangingFrame::create("frame3", frame2.get(), 3);
std::shared_ptr<unit_test_utils::RandomUnchangingFrame> frame1 = unit_test_utils::RandomUnchangingFrame::create("frame1", root1, 1);
std::shared_ptr<unit_test_utils::RandomUnchangingFrame> frame2 = unit_test_utils::RandomUnchangingFrame::create("frame2", frame1, 2);
std::shared_ptr<unit_test_utils::RandomUnchangingFrame> frame3 = unit_test_utils::RandomUnchangingFrame::create("frame3", frame2, 3);
std::shared_ptr<unit_test_utils::RandomlyChangingFrame> frame4 = unit_test_utils::RandomlyChangingFrame::create("frame4", root2.get(), 1);
std::shared_ptr<unit_test_utils::RandomlyChangingFrame> frame5 = unit_test_utils::RandomlyChangingFrame::create("frame5", frame4.get(), 2);
std::shared_ptr<unit_test_utils::RandomUnchangingFrame> frame6 = unit_test_utils::RandomUnchangingFrame::create("frame6", root2.get(), 1);
std::shared_ptr<unit_test_utils::RandomlyChangingFrame> frame7 = unit_test_utils::RandomlyChangingFrame::create("frame7", frame6.get(), 2);
std::shared_ptr<unit_test_utils::RandomlyChangingFrame> frame8 = unit_test_utils::RandomlyChangingFrame::create("frame8", frame7.get(), 3);
std::shared_ptr<unit_test_utils::RandomlyChangingFrame> frame4 = unit_test_utils::RandomlyChangingFrame::create("frame4", root2, 1);
std::shared_ptr<unit_test_utils::RandomlyChangingFrame> frame5 = unit_test_utils::RandomlyChangingFrame::create("frame5", frame4, 2);
std::shared_ptr<unit_test_utils::RandomUnchangingFrame> frame6 = unit_test_utils::RandomUnchangingFrame::create("frame6", root2, 1);
std::shared_ptr<unit_test_utils::RandomlyChangingFrame> frame7 = unit_test_utils::RandomlyChangingFrame::create("frame7", frame6, 2);
std::shared_ptr<unit_test_utils::RandomlyChangingFrame> frame8 = unit_test_utils::RandomlyChangingFrame::create("frame8", frame7, 3);
std::vector<ReferenceFrame*> allFrames;
std::vector<ReferenceFrame*> frames1;
std::vector<ReferenceFrame*> frames2;
std::vector<ReferenceFramePtr> allFrames;
std::vector<ReferenceFramePtr> frames1;
std::vector<ReferenceFramePtr> frames2;
int nTests = 20;
......@@ -70,7 +70,7 @@ class ReferenceFrameTest : public ::testing::Test
TEST_F(ReferenceFrameTest, constructors)
{
ReferenceFrame testFrame(*frame1.get());
ReferenceFrame testFrame(*frame1);
EXPECT_TRUE(
unit_test_utils::checkSpatialMatrixEpsilonClose(testFrame.getTransformToRoot().toMatrix(), frame1->getTransformToRoot().toMatrix(), unit_test_utils::TEST_PREC));
......@@ -98,7 +98,7 @@ TEST_F(ReferenceFrameTest, testRootsHaveNullParent)
TEST_F(ReferenceFrameTest, testVectorOfFramesWithRoot)
{
ReferenceFrame* worldFrame = ReferenceFrame::getWorldFrame().get();
ReferenceFramePtr worldFrame = ReferenceFrame::getWorldFrame();
EXPECT_EQ(worldFrame->getFramesStartingWithRootEndingWithThis().size(), 1);
}
......@@ -108,7 +108,7 @@ TEST_F(ReferenceFrameTest, testWorldFramePointerStuff)
const std::shared_ptr<ReferenceFrame> worldFrame1 = ReferenceFrame::getWorldFrame();
const std::shared_ptr<ReferenceFrame> worldFrame2 = ReferenceFrame::getWorldFrame();
EXPECT_EQ(worldFrame1.get(), worldFrame2.get());
EXPECT_EQ(worldFrame1, worldFrame2);
}
TEST_F(ReferenceFrameTest, testRootFramesArentTheSame)
......@@ -118,14 +118,14 @@ TEST_F(ReferenceFrameTest, testRootFramesArentTheSame)
TEST_F(ReferenceFrameTest, testGetRootFrame)
{
EXPECT_TRUE(frame2->getRootFrame() == root1.get());
EXPECT_TRUE(frame2->getRootFrame() == root1);
EXPECT_TRUE(frame7->getRootFrame() == frame5->getRootFrame());
frame7.get()->verifyFramesHaveSameRoot(frame6.get());
frame7->verifyFramesHaveSameRoot(frame6);
try
{
frame7->verifyFramesHaveSameRoot(frame1.get());
frame7->verifyFramesHaveSameRoot(frame1);
}
catch (RobotDynamics::ReferenceFrameException& e)
{
......@@ -146,7 +146,7 @@ TEST_F(ReferenceFrameTest, testCheckReferenceFramesMatch)
try
{
frame2->checkReferenceFramesMatch(frame7.get());
frame2->checkReferenceFramesMatch(frame7);
}
catch (ReferenceFrameException& e)
{
......@@ -160,8 +160,8 @@ TEST_F(ReferenceFrameTest, testGetTransformBetweenFrames)
{
unit_test_utils::updateAllFrames(allFrames);
ReferenceFrame* tmpFrame1 = unit_test_utils::getARandomFrame(frames1);
ReferenceFrame* tmpFrame2 = unit_test_utils::getARandomFrame(frames1);
ReferenceFramePtr tmpFrame1 = unit_test_utils::getARandomFrame(frames1);
ReferenceFramePtr tmpFrame2 = unit_test_utils::getARandomFrame(frames1);
RobotDynamics::Math::SpatialTransform transform1 = tmpFrame1->getTransformToDesiredFrame(tmpFrame2);
RobotDynamics::Math::SpatialTransform transform2 = tmpFrame2->getTransformToDesiredFrame(tmpFrame1);
......@@ -176,8 +176,8 @@ TEST_F(ReferenceFrameTest, testGetTransformBetweenFrames)
{
unit_test_utils::updateAllFrames(allFrames);
ReferenceFrame* tmpFrame1 = unit_test_utils::getARandomFrame(frames2);
ReferenceFrame* tmpFrame2 = unit_test_utils::getARandomFrame(frames2);
ReferenceFramePtr tmpFrame1 = unit_test_utils::getARandomFrame(frames2);
ReferenceFramePtr tmpFrame2 = unit_test_utils::getARandomFrame(frames2);
RobotDynamics::Math::SpatialTransform transform1 = tmpFrame1->getTransformToDesiredFrame(tmpFrame2);
RobotDynamics::Math::SpatialTransform transform2 = tmpFrame2->getTransformToDesiredFrame(tmpFrame1);
......@@ -193,9 +193,9 @@ TEST_F(ReferenceFrameTest, testGetTransformToParent)
{
for (int i = 1; i < allFrames.size(); i++)
{
ReferenceFrame* tmpFrame2 = allFrames[i];
ReferenceFramePtr tmpFrame2 = allFrames[i];
ReferenceFrame* parentFrame = tmpFrame2->getParentFrame();
ReferenceFramePtr parentFrame = tmpFrame2->getParentFrame();
if (parentFrame != nullptr)
{
......@@ -215,7 +215,7 @@ TEST_F(ReferenceFrameTest, testGetTransformToRoot)
for (int i = 0; i < allFrames.size(); i++)
{
ReferenceFrame* frame = allFrames[i];
ReferenceFramePtr frame = allFrames[i];
RobotDynamics::Math::SpatialTransform transformToRoot = unit_test_utils::getTransformToRootByClimbingTree(frame);
EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(transformToRoot.toMatrix(), frame->getTransformToRoot().toMatrix(), unit_test_utils::TEST_PREC));
......@@ -231,7 +231,7 @@ TEST_F(ReferenceFrameTest, testGetTransformToSelf)
for (int j = 0; j < allFrames.size(); j++)
{
ReferenceFrame* tmpFrame = allFrames[j];
ReferenceFramePtr tmpFrame = allFrames[j];
RobotDynamics::Math::SpatialTransform shouldBeIdentity = tmpFrame->getTransformToDesiredFrame(tmpFrame);
RobotDynamics::Math::SpatialMatrix identityTransform = RobotDynamics::Math::SpatialMatrixIdentity;
......
......@@ -26,8 +26,8 @@ class SpatialAccelerationTests : public testing::Test
TEST_F(FixedBase3DoFPlanar, testAdd)
{
SpatialAcceleration a1(model->bodyFrames[1].get(), ReferenceFrame::getWorldFrame().get(), ReferenceFrame::getWorldFrame().get(), 0.1, 0.2, 0.3, -0.1, -0.2, -0.3);
SpatialAcceleration a2(model->bodyFrames[2].get(), model->bodyFrames[1].get(), ReferenceFrame::getWorldFrame().get(), 0.1, 0.2, 0.3, -0.1, -0.2, -0.3);
SpatialAcceleration a1(model->bodyFrames[1], ReferenceFrame::getWorldFrame(), ReferenceFrame::getWorldFrame(), 0.1, 0.2, 0.3, -0.1, -0.2, -0.3);
SpatialAcceleration a2(model->bodyFrames[2], model->bodyFrames[1], ReferenceFrame::getWorldFrame(), 0.1, 0.2, 0.3, -0.1, -0.2, -0.3);
SpatialVector av_1 = a1;
SpatialVector av_2 = a2;
......@@ -46,7 +46,7 @@ TEST_F(FixedBase3DoFPlanar, testAdd)
try
{
SpatialAcceleration a5(model->bodyFrames[2].get(), model->bodyFrames[1].get(), ReferenceFrame::getWorldFrame().get(), 0.1, 0.2, 0.3, -0.1, -0.2, -0.3);
SpatialAcceleration a5(model->bodyFrames[2], model->bodyFrames[1], ReferenceFrame::getWorldFrame(), 0.1, 0.2, 0.3, -0.1, -0.2, -0.3);
a5 -= a1;
}
catch (ReferenceFrameException& e)
......@@ -69,10 +69,10 @@ TEST_F(FixedBase3DoFPlanar, testChangeFrame)
updateKinematics(*model, Q, QDot, QDDot);
ReferenceFrame* newFrame = model->bodyFrames[2].get();
ReferenceFrame* currentExpressedInFrame = ReferenceFrame::getWorldFrame().get();
ReferenceFramePtr newFrame = model->bodyFrames[2];
ReferenceFramePtr currentExpressedInFrame = ReferenceFrame::getWorldFrame();
SpatialAcceleration a2(model->bodyFrames[2].get(), model->bodyFrames[1].get(), currentExpressedInFrame, 0.1, 0.2, 0.3, -0.1, -0.2, -0.3);
SpatialAcceleration a2(model->bodyFrames[2], model->bodyFrames[1], currentExpressedInFrame, 0.1, 0.2, 0.3, -0.1, -0.2, -0.3);
SpatialVector a2_v = a2;
SpatialTransform X = currentExpressedInFrame->getTransformToDesiredFrame(newFrame);
......@@ -97,10 +97,10 @@ TEST_F(FixedBase3DoFPlanar, testchangeFrameWithRelativeMotion)
updateKinematics(*model, Q, QDot, QDDot);
ReferenceFrame* newFrame = model->bodyFrames[2].get();
ReferenceFrame* currentExpressedInFrame = ReferenceFrame::getWorldFrame().get();
ReferenceFrame* bodyFrame = model->bodyFrames[2].get();
ReferenceFrame* baseFrame = model->bodyFrames[1].get();
ReferenceFramePtr newFrame = model->bodyFrames[2];
ReferenceFramePtr currentExpressedInFrame = ReferenceFrame::getWorldFrame();
ReferenceFramePtr bodyFrame = model->bodyFrames[2];
ReferenceFramePtr baseFrame = model->bodyFrames[1];
SpatialAcceleration a2(bodyFrame, baseFrame, currentExpressedInFrame, 0.1, 0.2, 0.3, -0.1, -0.2, -0.3);
SpatialVector a2_v = a2;
......@@ -135,13 +135,13 @@ TEST_F(FixedBase3DoFPlanar, testchangeFrameWithRelativeMotionSameFrame)
updateKinematics(*model, Q, QDot, QDDot);
ReferenceFrame* newFrame = model->bodyFrames[2].get();
ReferenceFrame* currentExpressedInFrame = ReferenceFrame::getWorldFrame().get();
ReferenceFramePtr newFrame = model->bodyFrames[2];
ReferenceFramePtr currentExpressedInFrame = ReferenceFrame::getWorldFrame();