From 12465394cb8039c1f7992f12971edc6d22e0305b Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Wed, 18 Dec 2024 13:37:59 -0600 Subject: [PATCH] Fixed clang-format in command language test suite --- .../cartesian_waypoint_poly_unit.hpp | 303 ++++++------ .../test_suite/joint_waypoint_poly_unit.hpp | 409 ++++++++-------- .../test_suite/state_waypoint_poly_unit.hpp | 447 +++++++++--------- 3 files changed, 581 insertions(+), 578 deletions(-) diff --git a/tesseract_command_language/include/tesseract_command_language/test_suite/cartesian_waypoint_poly_unit.hpp b/tesseract_command_language/include/tesseract_command_language/test_suite/cartesian_waypoint_poly_unit.hpp index 73e0be72e7..e599918da6 100644 --- a/tesseract_command_language/include/tesseract_command_language/test_suite/cartesian_waypoint_poly_unit.hpp +++ b/tesseract_command_language/include/tesseract_command_language/test_suite/cartesian_waypoint_poly_unit.hpp @@ -63,168 +63,169 @@ void runCartesianWaypointTest() EXPECT_FALSE(wp.isToleranced()); } - { // Set/Get Transform - { // Test construction providing pose + { // Set/Get Transform + { // Test construction providing pose Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; - pose.translation() = Eigen::Vector3d(1, 2, 3); - CartesianWaypointPoly wp{ T(pose) }; - EXPECT_TRUE(wp.getTransform().isApprox(pose)); - EXPECT_FALSE(wp.isToleranced()); -} - -{ // Test construction providing pose - Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; - pose.translation() = Eigen::Vector3d(1, 2, 3); - CartesianWaypointPoly wp{ T(pose, Eigen::VectorXd::Constant(3, -5), Eigen::VectorXd::Constant(3, 5)) }; - EXPECT_TRUE(wp.getTransform().isApprox(pose)); - EXPECT_TRUE(wp.getLowerTolerance().isApprox(Eigen::VectorXd::Constant(3, -5))); - EXPECT_TRUE(wp.getUpperTolerance().isApprox(Eigen::VectorXd::Constant(3, 5))); - EXPECT_TRUE(wp.isToleranced()); -} + pose.translation() = Eigen::Vector3d(1, 2, 3); + CartesianWaypointPoly wp{ T(pose) }; + EXPECT_TRUE(wp.getTransform().isApprox(pose)); + EXPECT_FALSE(wp.isToleranced()); + } -{ // Test set pose - Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; - pose.translation() = Eigen::Vector3d(1, 2, 3); - CartesianWaypointPoly wp{ T() }; - wp.setTransform(pose); - EXPECT_TRUE(wp.getTransform().isApprox(pose)); - EXPECT_FALSE(wp.isToleranced()); -} - -{ // Test assigning pose - Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; - pose.translation() = Eigen::Vector3d(1, 2, 3); - CartesianWaypointPoly wp{ T() }; - wp.getTransform() = pose; - EXPECT_TRUE(std::as_const(wp).getTransform().isApprox(pose)); - EXPECT_FALSE(wp.isToleranced()); -} -} // namespace tesseract_planning::test_suite - -{ // Test Set Tolerances - CartesianWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.isToleranced()); - - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); - EXPECT_TRUE(wp.isToleranced()); + { // Test construction providing pose + Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; + pose.translation() = Eigen::Vector3d(1, 2, 3); + CartesianWaypointPoly wp{ T(pose, Eigen::VectorXd::Constant(3, -5), Eigen::VectorXd::Constant(3, 5)) }; + EXPECT_TRUE(wp.getTransform().isApprox(pose)); + EXPECT_TRUE(wp.getLowerTolerance().isApprox(Eigen::VectorXd::Constant(3, -5))); + EXPECT_TRUE(wp.getUpperTolerance().isApprox(Eigen::VectorXd::Constant(3, 5))); + EXPECT_TRUE(wp.isToleranced()); + } + + { // Test set pose + Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; + pose.translation() = Eigen::Vector3d(1, 2, 3); + CartesianWaypointPoly wp{ T() }; + wp.setTransform(pose); + EXPECT_TRUE(wp.getTransform().isApprox(pose)); + EXPECT_FALSE(wp.isToleranced()); + } + + { // Test assigning pose + Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; + pose.translation() = Eigen::Vector3d(1, 2, 3); + CartesianWaypointPoly wp{ T() }; + wp.getTransform() = pose; + EXPECT_TRUE(std::as_const(wp).getTransform().isApprox(pose)); + EXPECT_FALSE(wp.isToleranced()); + } + } // namespace tesseract_planning::test_suite + + { // Test Set Tolerances + CartesianWaypointPoly wp{ T() }; + EXPECT_FALSE(wp.isToleranced()); - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, -5)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); - EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); + EXPECT_TRUE(wp.isToleranced()); - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 5)); - EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, -5)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); + EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 0)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 0)); - EXPECT_FALSE(wp.isToleranced()); -} + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 5)); + EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT -{ // Test Equality and Serialization - { CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; -CartesianWaypointPoly wp2(wp1); // NOLINT -EXPECT_TRUE(wp1 == wp2); -EXPECT_TRUE(wp2 == wp1); -EXPECT_FALSE(wp2 != wp1); -runWaypointSerializationTest(wp1); -} -{ - CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; - wp1.getTransform().translate(Eigen::Vector3d(1e6, 0, 0)); - CartesianWaypointPoly wp2(wp1); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); -} -{ - CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; - wp1.getUpperTolerance().resize(3); - wp1.getUpperTolerance() << 1, 2, 3; - wp1.getLowerTolerance().resize(3); - wp1.getLowerTolerance() << -4, -5, -6; - CartesianWaypointPoly wp2(wp1); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); -} -// Not equal -{ - CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; - CartesianWaypointPoly wp2{ T(Eigen::Isometry3d::Identity()) }; - wp2.getTransform().rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d(0, 0, 1))); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp2); -} -{ - CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; - CartesianWaypointPoly wp2{ T(Eigen::Isometry3d::Identity()) }; - wp2.getTransform().translate(Eigen::Vector3d(1e6, 0, 0)); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp2); -} -{ - CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; - CartesianWaypointPoly wp2(wp1); - wp2.getUpperTolerance().resize(3); - wp2.getUpperTolerance() << 1, 2, 3; - wp2.getLowerTolerance().resize(3); - wp2.getLowerTolerance() << -4, -5, -6; - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp2); -} -} - -{ // Set/Get Seed - tesseract_common::JointState seed_state; - seed_state.joint_names = { "joint_1", "joint_2", "joint_3" }; - seed_state.position.resize(3); - seed_state.position << .01, .02, .03; - seed_state.velocity.resize(3); - seed_state.velocity << .1, .2, .3; - seed_state.acceleration.resize(3); - seed_state.acceleration << 1, 2, 3; - - { // Test default construction pose - CartesianWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.hasSeed()); - EXPECT_FALSE(std::as_const(wp).getSeed() == seed_state); - } - - { // Test assigning pose - CartesianWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.hasSeed()); - wp.setSeed(seed_state); - EXPECT_TRUE(wp.hasSeed()); - EXPECT_TRUE(std::as_const(wp).getSeed() == seed_state); + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 0)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 0)); + EXPECT_FALSE(wp.isToleranced()); } - { // Test assigning pose - CartesianWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.hasSeed()); - wp.getSeed() = seed_state; - EXPECT_TRUE(wp.hasSeed()); - EXPECT_TRUE(std::as_const(wp).getSeed() == seed_state); + { // Test Equality and Serialization + { + CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; + CartesianWaypointPoly wp2(wp1); // NOLINT + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + { + CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; + wp1.getTransform().translate(Eigen::Vector3d(1e6, 0, 0)); + CartesianWaypointPoly wp2(wp1); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + { + CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; + wp1.getUpperTolerance().resize(3); + wp1.getUpperTolerance() << 1, 2, 3; + wp1.getLowerTolerance().resize(3); + wp1.getLowerTolerance() << -4, -5, -6; + CartesianWaypointPoly wp2(wp1); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + // Not equal + { + CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; + CartesianWaypointPoly wp2{ T(Eigen::Isometry3d::Identity()) }; + wp2.getTransform().rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d(0, 0, 1))); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp2); + } + { + CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; + CartesianWaypointPoly wp2{ T(Eigen::Isometry3d::Identity()) }; + wp2.getTransform().translate(Eigen::Vector3d(1e6, 0, 0)); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp2); + } + { + CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; + CartesianWaypointPoly wp2(wp1); + wp2.getUpperTolerance().resize(3); + wp2.getUpperTolerance() << 1, 2, 3; + wp2.getLowerTolerance().resize(3); + wp2.getLowerTolerance() << -4, -5, -6; + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp2); + } } - { // Test clear seed - CartesianWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.hasSeed()); - wp.getSeed() = seed_state; - EXPECT_TRUE(wp.hasSeed()); - wp.clearSeed(); - EXPECT_FALSE(wp.hasSeed()); + { // Set/Get Seed + tesseract_common::JointState seed_state; + seed_state.joint_names = { "joint_1", "joint_2", "joint_3" }; + seed_state.position.resize(3); + seed_state.position << .01, .02, .03; + seed_state.velocity.resize(3); + seed_state.velocity << .1, .2, .3; + seed_state.acceleration.resize(3); + seed_state.acceleration << 1, 2, 3; + + { // Test default construction pose + CartesianWaypointPoly wp{ T() }; + EXPECT_FALSE(wp.hasSeed()); + EXPECT_FALSE(std::as_const(wp).getSeed() == seed_state); + } + + { // Test assigning pose + CartesianWaypointPoly wp{ T() }; + EXPECT_FALSE(wp.hasSeed()); + wp.setSeed(seed_state); + EXPECT_TRUE(wp.hasSeed()); + EXPECT_TRUE(std::as_const(wp).getSeed() == seed_state); + } + + { // Test assigning pose + CartesianWaypointPoly wp{ T() }; + EXPECT_FALSE(wp.hasSeed()); + wp.getSeed() = seed_state; + EXPECT_TRUE(wp.hasSeed()); + EXPECT_TRUE(std::as_const(wp).getSeed() == seed_state); + } + + { // Test clear seed + CartesianWaypointPoly wp{ T() }; + EXPECT_FALSE(wp.hasSeed()); + wp.getSeed() = seed_state; + EXPECT_TRUE(wp.hasSeed()); + wp.clearSeed(); + EXPECT_FALSE(wp.hasSeed()); + } } } -} } // namespace tesseract_planning::test_suite #endif // TESSERACT_COMMAND_LANGUAGE_CARTESIAN_WAYPOINT_POLY_UNIT_HPP diff --git a/tesseract_command_language/include/tesseract_command_language/test_suite/joint_waypoint_poly_unit.hpp b/tesseract_command_language/include/tesseract_command_language/test_suite/joint_waypoint_poly_unit.hpp index 5353ab1545..ca119e4d08 100644 --- a/tesseract_command_language/include/tesseract_command_language/test_suite/joint_waypoint_poly_unit.hpp +++ b/tesseract_command_language/include/tesseract_command_language/test_suite/joint_waypoint_poly_unit.hpp @@ -52,227 +52,228 @@ void runJointWaypointTest() EXPECT_FALSE(base.isStateWaypoint()); } - { // Test construction - { JointWaypointPoly wp{ T() }; - EXPECT_TRUE(wp.getNames().empty()); - EXPECT_TRUE(wp.getPosition().rows() == 0); - EXPECT_TRUE(std::as_const(wp).getUpperTolerance().rows() == 0); - EXPECT_TRUE(std::as_const(wp).getLowerTolerance().rows() == 0); - EXPECT_FALSE(wp.isConstrained()); -} -{ - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - JointWaypointPoly wp{ T(names, positions) }; - EXPECT_EQ(wp.getNames(), names); - EXPECT_TRUE(wp.getPosition().isApprox(positions)); - EXPECT_TRUE(std::as_const(wp).getUpperTolerance().rows() == 0); - EXPECT_TRUE(std::as_const(wp).getLowerTolerance().rows() == 0); - EXPECT_TRUE(wp.isConstrained()); -} -{ - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(3, -5); - Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(3, 5); - JointWaypointPoly wp{ T(names, positions, lower_tol, uppert_tol) }; - EXPECT_EQ(wp.getNames(), names); - EXPECT_TRUE(wp.getPosition().isApprox(positions)); - EXPECT_TRUE(wp.getLowerTolerance().isApprox(lower_tol)); - EXPECT_TRUE(wp.getUpperTolerance().isApprox(uppert_tol)); - EXPECT_TRUE(wp.isConstrained()); -} -{ - std::vector names{ "j1", "j2" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions) }); // NOLINT -} -{ - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(2, 0.0); - EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions) }); // NOLINT -} -{ - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(2, -5); - Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(3, 5); - EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions, lower_tol, uppert_tol) }); // NOLINT -} -{ - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(3, -5); - Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(2, 5); - EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions, lower_tol, uppert_tol) }); // NOLINT -} -} // namespace tesseract_planning::test_suite + { // Test construction + { + JointWaypointPoly wp{ T() }; + EXPECT_TRUE(wp.getNames().empty()); + EXPECT_TRUE(wp.getPosition().rows() == 0); + EXPECT_TRUE(std::as_const(wp).getUpperTolerance().rows() == 0); + EXPECT_TRUE(std::as_const(wp).getLowerTolerance().rows() == 0); + EXPECT_FALSE(wp.isConstrained()); + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + JointWaypointPoly wp{ T(names, positions) }; + EXPECT_EQ(wp.getNames(), names); + EXPECT_TRUE(wp.getPosition().isApprox(positions)); + EXPECT_TRUE(std::as_const(wp).getUpperTolerance().rows() == 0); + EXPECT_TRUE(std::as_const(wp).getLowerTolerance().rows() == 0); + EXPECT_TRUE(wp.isConstrained()); + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(3, -5); + Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(3, 5); + JointWaypointPoly wp{ T(names, positions, lower_tol, uppert_tol) }; + EXPECT_EQ(wp.getNames(), names); + EXPECT_TRUE(wp.getPosition().isApprox(positions)); + EXPECT_TRUE(wp.getLowerTolerance().isApprox(lower_tol)); + EXPECT_TRUE(wp.getUpperTolerance().isApprox(uppert_tol)); + EXPECT_TRUE(wp.isConstrained()); + } + { + std::vector names{ "j1", "j2" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions) }); // NOLINT + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(2, 0.0); + EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions) }); // NOLINT + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(2, -5); + Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(3, 5); + EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions, lower_tol, uppert_tol) }); // NOLINT + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(3, -5); + Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(2, 5); + EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions, lower_tol, uppert_tol) }); // NOLINT + } + } // namespace tesseract_planning::test_suite -{ // Test is constrained - JointWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.isConstrained()); - wp.setIsConstrained(true); - EXPECT_TRUE(wp.isConstrained()); -} - -{ // Set/Get Names - const std::vector names{ "j1", "j2", "j3" }; - { // Test set + { // Test is constrained JointWaypointPoly wp{ T() }; - wp.setNames(names); - EXPECT_TRUE(wp.getNames() == names); + EXPECT_FALSE(wp.isConstrained()); + wp.setIsConstrained(true); + EXPECT_TRUE(wp.isConstrained()); } - { // Test assigning - JointWaypointPoly wp{ T() }; - wp.getNames() = names; - EXPECT_TRUE(std::as_const(wp).getNames() == names); + { // Set/Get Names + const std::vector names{ "j1", "j2", "j3" }; + { // Test set + JointWaypointPoly wp{ T() }; + wp.setNames(names); + EXPECT_TRUE(wp.getNames() == names); + } + + { // Test assigning + JointWaypointPoly wp{ T() }; + wp.getNames() = names; + EXPECT_TRUE(std::as_const(wp).getNames() == names); + } } -} -{ // Set/Get Positions - Eigen::VectorXd positions; - positions.resize(3); - positions << 1.0, 2.0, 3.0; + { // Set/Get Positions + Eigen::VectorXd positions; + positions.resize(3); + positions << 1.0, 2.0, 3.0; - { // Test set - JointWaypointPoly wp{ T() }; - wp.setPosition(positions); - EXPECT_TRUE(wp.getPosition().isApprox(positions)); - } + { // Test set + JointWaypointPoly wp{ T() }; + wp.setPosition(positions); + EXPECT_TRUE(wp.getPosition().isApprox(positions)); + } - { // Test assigning - JointWaypointPoly wp{ T() }; - wp.getPosition() = positions; - EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(positions)); + { // Test assigning + JointWaypointPoly wp{ T() }; + wp.getPosition() = positions; + EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(positions)); + } } -} - -{ // Test Set Tolerances - JointWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.isToleranced()); - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); - EXPECT_TRUE(wp.isToleranced()); + { // Test Set Tolerances + JointWaypointPoly wp{ T() }; + EXPECT_FALSE(wp.isToleranced()); - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, -5)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); - EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); + EXPECT_TRUE(wp.isToleranced()); - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 5)); - EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, -5)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); + EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 0)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 0)); - EXPECT_FALSE(wp.isToleranced()); -} + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 5)); + EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT -{ // Test Equality and Serialization - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0.0, 0.0, 0.0 }) }; - const JointWaypointPoly wp2(wp1); // NOLINT - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 0)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 0)); + EXPECT_FALSE(wp.isToleranced()); } - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }) }; - JointWaypointPoly wp2(wp1); // NOLINT - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions, upper_tol, lower_tol; - positions.resize(3); - upper_tol.resize(3); - lower_tol.resize(3); - positions << 0, -1e6, 1e6; - upper_tol << 1, 2, 3; - lower_tol << -4, -5, -6; - JointWaypointPoly wp1{ T(names, positions, lower_tol, upper_tol) }; - JointWaypointPoly wp2(wp1); - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - // Not equal - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - JointWaypointPoly wp2{ T(std::vector({ "j1" }), Eigen::VectorXd::Zero(1)) }; - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - JointWaypointPoly wp2{ T({ "j1", "j2", "j4" }, { 0, 0, 0 }) }; - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - JointWaypointPoly wp2{ T({ "j1", "j2", "j3" }, { 0.001, 0, 0 }) }; - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }, { 1, 2, 3 }, { -4, -5, -6 }) }; - JointWaypointPoly wp2(wp1); - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - Eigen::VectorXd upper_tol, lower_tol; - upper_tol.resize(3); - lower_tol.resize(3); - upper_tol << 1, 2, 3; - lower_tol << -4, -5, -6; - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - JointWaypointPoly wp2(wp1); - wp1.getUpperTolerance() = upper_tol; - wp1.getLowerTolerance() = lower_tol; - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); + { // Test Equality and Serialization + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0.0, 0.0, 0.0 }) }; + const JointWaypointPoly wp2(wp1); // NOLINT + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }) }; + JointWaypointPoly wp2(wp1); // NOLINT + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions, upper_tol, lower_tol; + positions.resize(3); + upper_tol.resize(3); + lower_tol.resize(3); + positions << 0, -1e6, 1e6; + upper_tol << 1, 2, 3; + lower_tol << -4, -5, -6; + + JointWaypointPoly wp1{ T(names, positions, lower_tol, upper_tol) }; + JointWaypointPoly wp2(wp1); + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + // Not equal + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + JointWaypointPoly wp2{ T(std::vector({ "j1" }), Eigen::VectorXd::Zero(1)) }; + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + JointWaypointPoly wp2{ T({ "j1", "j2", "j4" }, { 0, 0, 0 }) }; + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + JointWaypointPoly wp2{ T({ "j1", "j2", "j3" }, { 0.001, 0, 0 }) }; + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }, { 1, 2, 3 }, { -4, -5, -6 }) }; + JointWaypointPoly wp2(wp1); + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } + { + Eigen::VectorXd upper_tol, lower_tol; + upper_tol.resize(3); + lower_tol.resize(3); + upper_tol << 1, 2, 3; + lower_tol << -4, -5, -6; + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + JointWaypointPoly wp2(wp1); + wp1.getUpperTolerance() = upper_tol; + wp1.getLowerTolerance() = lower_tol; + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } } } -} } // namespace tesseract_planning::test_suite #endif // TESSERACT_COMMAND_LANGUAGE_JOINT_WAYPOINT_POLY_UNIT_HPP diff --git a/tesseract_command_language/include/tesseract_command_language/test_suite/state_waypoint_poly_unit.hpp b/tesseract_command_language/include/tesseract_command_language/test_suite/state_waypoint_poly_unit.hpp index 08a14b851a..991a77d5c5 100644 --- a/tesseract_command_language/include/tesseract_command_language/test_suite/state_waypoint_poly_unit.hpp +++ b/tesseract_command_language/include/tesseract_command_language/test_suite/state_waypoint_poly_unit.hpp @@ -52,256 +52,257 @@ void runStateWaypointTest() EXPECT_TRUE(base.isStateWaypoint()); } - { // Test construction - { StateWaypointPoly wp{ T() }; - EXPECT_TRUE(wp.getNames().empty()); - EXPECT_TRUE(wp.getPosition().rows() == 0); - EXPECT_TRUE(wp.getVelocity().rows() == 0); - EXPECT_TRUE(wp.getAcceleration().rows() == 0); - EXPECT_TRUE(wp.getEffort().rows() == 0); - EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 0.0)); -} -{ - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - StateWaypointPoly wp{ T(names, positions) }; - EXPECT_EQ(wp.getNames(), names); - EXPECT_TRUE(wp.getPosition().isApprox(positions)); - EXPECT_TRUE(wp.getVelocity().rows() == 0); - EXPECT_TRUE(wp.getAcceleration().rows() == 0); - EXPECT_TRUE(wp.getEffort().rows() == 0); - EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 0.0)); -} -{ - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd velocities = Eigen::VectorXd::Constant(3, -5); - Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(3, 5); - StateWaypointPoly wp{ T(names, positions, velocities, accelerations, 5) }; - EXPECT_EQ(wp.getNames(), names); - EXPECT_TRUE(wp.getPosition().isApprox(positions)); - EXPECT_TRUE(wp.getVelocity().isApprox(velocities)); - EXPECT_TRUE(wp.getAcceleration().isApprox(accelerations)); - EXPECT_TRUE(wp.getEffort().rows() == 0); - EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 5)); -} -{ - std::vector names{ "j1", "j2" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions) }); // NOLINT -} -{ - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(2, 0.0); - EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions) }); // NOLINT -} -{ - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd velocities = Eigen::VectorXd::Constant(2, -5); - Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(3, 5); - EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions, velocities, accelerations, 5) }); // NOLINT -} -{ - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd velocities = Eigen::VectorXd::Constant(3, -5); - Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(2, 5); - EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions, velocities, accelerations, 5) }); // NOLINT -} -} // namespace tesseract_planning::test_suite + { // Test construction + { + StateWaypointPoly wp{ T() }; + EXPECT_TRUE(wp.getNames().empty()); + EXPECT_TRUE(wp.getPosition().rows() == 0); + EXPECT_TRUE(wp.getVelocity().rows() == 0); + EXPECT_TRUE(wp.getAcceleration().rows() == 0); + EXPECT_TRUE(wp.getEffort().rows() == 0); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 0.0)); + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + StateWaypointPoly wp{ T(names, positions) }; + EXPECT_EQ(wp.getNames(), names); + EXPECT_TRUE(wp.getPosition().isApprox(positions)); + EXPECT_TRUE(wp.getVelocity().rows() == 0); + EXPECT_TRUE(wp.getAcceleration().rows() == 0); + EXPECT_TRUE(wp.getEffort().rows() == 0); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 0.0)); + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd velocities = Eigen::VectorXd::Constant(3, -5); + Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(3, 5); + StateWaypointPoly wp{ T(names, positions, velocities, accelerations, 5) }; + EXPECT_EQ(wp.getNames(), names); + EXPECT_TRUE(wp.getPosition().isApprox(positions)); + EXPECT_TRUE(wp.getVelocity().isApprox(velocities)); + EXPECT_TRUE(wp.getAcceleration().isApprox(accelerations)); + EXPECT_TRUE(wp.getEffort().rows() == 0); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 5)); + } + { + std::vector names{ "j1", "j2" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions) }); // NOLINT + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(2, 0.0); + EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions) }); // NOLINT + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd velocities = Eigen::VectorXd::Constant(2, -5); + Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(3, 5); + EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions, velocities, accelerations, 5) }); // NOLINT + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd velocities = Eigen::VectorXd::Constant(3, -5); + Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(2, 5); + EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions, velocities, accelerations, 5) }); // NOLINT + } + } // namespace tesseract_planning::test_suite -{ // Set/Get Names - const std::vector names{ "j1", "j2", "j3" }; - { // Test set - StateWaypointPoly wp{ T() }; - wp.setNames(names); - EXPECT_TRUE(wp.getNames() == names); - } + { // Set/Get Names + const std::vector names{ "j1", "j2", "j3" }; + { // Test set + StateWaypointPoly wp{ T() }; + wp.setNames(names); + EXPECT_TRUE(wp.getNames() == names); + } - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getNames() = names; - EXPECT_TRUE(std::as_const(wp).getNames() == names); + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getNames() = names; + EXPECT_TRUE(std::as_const(wp).getNames() == names); + } } -} -{ // Set/Get Positions - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Set/Get Positions + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test set - StateWaypointPoly wp{ T() }; - wp.setPosition(data); - EXPECT_TRUE(wp.getPosition().isApprox(data)); - } + { // Test set + StateWaypointPoly wp{ T() }; + wp.setPosition(data); + EXPECT_TRUE(wp.getPosition().isApprox(data)); + } - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getPosition() = data; - EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(data)); + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getPosition() = data; + EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(data)); + } } -} -{ // Set/Get Velocity - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Set/Get Velocity + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test set - StateWaypointPoly wp{ T() }; - wp.setVelocity(data); - EXPECT_TRUE(wp.getVelocity().isApprox(data)); - } + { // Test set + StateWaypointPoly wp{ T() }; + wp.setVelocity(data); + EXPECT_TRUE(wp.getVelocity().isApprox(data)); + } - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getVelocity() = data; - EXPECT_TRUE(std::as_const(wp).getVelocity().isApprox(data)); + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getVelocity() = data; + EXPECT_TRUE(std::as_const(wp).getVelocity().isApprox(data)); + } } -} -{ // Set/Get Acceleration - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Set/Get Acceleration + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test set - StateWaypointPoly wp{ T() }; - wp.setAcceleration(data); - EXPECT_TRUE(wp.getAcceleration().isApprox(data)); - } + { // Test set + StateWaypointPoly wp{ T() }; + wp.setAcceleration(data); + EXPECT_TRUE(wp.getAcceleration().isApprox(data)); + } - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getAcceleration() = data; - EXPECT_TRUE(std::as_const(wp).getAcceleration().isApprox(data)); + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getAcceleration() = data; + EXPECT_TRUE(std::as_const(wp).getAcceleration().isApprox(data)); + } } -} -{ // Set/Get Effort - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Set/Get Effort + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test set - StateWaypointPoly wp{ T() }; - wp.setEffort(data); - EXPECT_TRUE(wp.getEffort().isApprox(data)); + { // Test set + StateWaypointPoly wp{ T() }; + wp.setEffort(data); + EXPECT_TRUE(wp.getEffort().isApprox(data)); + } + + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getEffort() = data; + EXPECT_TRUE(std::as_const(wp).getEffort().isApprox(data)); + } } - { // Test assigning + { // Set/Get Time + double data{ 3 }; StateWaypointPoly wp{ T() }; - wp.getEffort() = data; - EXPECT_TRUE(std::as_const(wp).getEffort().isApprox(data)); + wp.setTime(data); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), data)); } -} - -{ // Set/Get Time - double data{ 3 }; - StateWaypointPoly wp{ T() }; - wp.setTime(data); - EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), data)); -} -{ // Test Equality and Serialization - { - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0.0, 0.0, 0.0 }) }; - const StateWaypointPoly wp2(wp1); // NOLINT - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - { - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }) }; - StateWaypointPoly wp2(wp1); // NOLINT - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions, velocities, accelerations, efforts; - positions.resize(3); - velocities.resize(3); - accelerations.resize(3); - efforts.resize(3); - positions << 0, -1e6, 1e6; - velocities << 1, 2, 3; - accelerations << -4, -5, -6; - efforts << 4, 5, 6; + { // Test Equality and Serialization + { + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0.0, 0.0, 0.0 }) }; + const StateWaypointPoly wp2(wp1); // NOLINT + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + { + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }) }; + StateWaypointPoly wp2(wp1); // NOLINT + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions, velocities, accelerations, efforts; + positions.resize(3); + velocities.resize(3); + accelerations.resize(3); + efforts.resize(3); + positions << 0, -1e6, 1e6; + velocities << 1, 2, 3; + accelerations << -4, -5, -6; + efforts << 4, 5, 6; - StateWaypointPoly wp1{ T(names, positions, velocities, accelerations, 5) }; - wp1.getEffort() = efforts; - StateWaypointPoly wp2(wp1); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - { - Eigen::VectorXd efforts; - efforts.resize(3); - efforts << 4, 5, 6; + StateWaypointPoly wp1{ T(names, positions, velocities, accelerations, 5) }; + wp1.getEffort() = efforts; + StateWaypointPoly wp2(wp1); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + { + Eigen::VectorXd efforts; + efforts.resize(3); + efforts << 4, 5, 6; - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }, { 1, 2, 3 }, { -4, -5, -6 }, 5) }; - wp1.getEffort() = efforts; - StateWaypointPoly wp2(wp1); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - // Not equal - { - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - StateWaypointPoly wp2{ T(std::vector({ "j1" }), Eigen::VectorXd::Zero(1)) }; - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - StateWaypointPoly wp2{ T({ "j1", "j2", "j4" }, { 0, 0, 0 }) }; - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions, velocities, accelerations, efforts; - positions.resize(3); - velocities.resize(3); - accelerations.resize(3); - efforts.resize(3); - positions << 0, -1e6, 1e6; - velocities << 1, 2, 3; - accelerations << -4, -5, -6; - efforts << 4, 5, 6; + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }, { 1, 2, 3 }, { -4, -5, -6 }, 5) }; + wp1.getEffort() = efforts; + StateWaypointPoly wp2(wp1); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + // Not equal + { + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + StateWaypointPoly wp2{ T(std::vector({ "j1" }), Eigen::VectorXd::Zero(1)) }; + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } + { + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + StateWaypointPoly wp2{ T({ "j1", "j2", "j4" }, { 0, 0, 0 }) }; + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions, velocities, accelerations, efforts; + positions.resize(3); + velocities.resize(3); + accelerations.resize(3); + efforts.resize(3); + positions << 0, -1e6, 1e6; + velocities << 1, 2, 3; + accelerations << -4, -5, -6; + efforts << 4, 5, 6; - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - StateWaypointPoly wp2{ wp1 }; - wp1.getNames() = names; - wp1.getPosition() = positions; - wp1.getVelocity() = velocities; - wp1.getAcceleration() = accelerations; - wp1.getEffort() = efforts; - wp1.setTime(5); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + StateWaypointPoly wp2{ wp1 }; + wp1.getNames() = names; + wp1.getPosition() = positions; + wp1.getVelocity() = velocities; + wp1.getAcceleration() = accelerations; + wp1.getEffort() = efforts; + wp1.setTime(5); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } } } -} } // namespace tesseract_planning::test_suite #endif // TESSERACT_COMMAND_LANGUAGE_STATE_WAYPOINT_POLY_UNIT_HPP