From 0509af51532000cd21dbbd08ef8f15e11230b182 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Mon, 20 Nov 2023 23:31:35 -0500 Subject: [PATCH 1/2] Fix angular distance calculation in floating joint model (#2538) (cherry picked from commit 83ff55a4adb5ab1c4b0813e1220e4b3242190c88) # Conflicts: # moveit_core/robot_trajectory/test/test_robot_trajectory.cpp --- .../robot_model/src/floating_joint_model.cpp | 15 ++--- .../test/test_robot_trajectory.cpp | 55 ++++++++++++++++++- 2 files changed, 58 insertions(+), 12 deletions(-) diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 1cd5204808..15b46a9e92 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -36,6 +36,7 @@ /* Author: Ioan Sucan */ #include +#include #include #include #include @@ -121,16 +122,10 @@ double FloatingJointModel::distanceTranslation(const double* values1, const doub double FloatingJointModel::distanceRotation(const double* values1, const double* values2) const { - double dq = - fabs(values1[3] * values2[3] + values1[4] * values2[4] + values1[5] * values2[5] + values1[6] * values2[6]); - if (dq + std::numeric_limits::epsilon() >= 1.0) - { - return 0.0; - } - else - { - return acos(dq); - } + // The values are in "xyzw" order but Eigen expects "wxyz". + const auto q1 = Eigen::Quaterniond(values1[6], values1[3], values1[4], values1[5]).normalized(); + const auto q2 = Eigen::Quaterniond(values2[6], values2[3], values2[4], values2[5]).normalized(); + return q2.angularDistance(q1); } void FloatingJointModel::interpolate(const double* from, const double* to, const double t, double* state) const diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp index a2de86e8df..0f6436f2bb 100644 --- a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -55,7 +55,7 @@ class RobotTrajectoryTestFixture : public testing::Test { robot_model_ = moveit::core::loadTestingRobotModel(robot_model_name_); robot_state_ = std::make_shared(robot_model_); - robot_state_->setToDefaultValues(arm_jmg_name_, arm_state_name_); + robot_state_->setToDefaultValues(); robot_state_->setVariableVelocity(/*index*/ 0, /*value*/ 1.0); robot_state_->setVariableAcceleration(/*index*/ 0, /*value*/ -0.1); robot_state_->update(); @@ -67,7 +67,7 @@ class RobotTrajectoryTestFixture : public testing::Test void initTestTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory) { - // Init a traj + // Init a trajectory ASSERT_TRUE(robot_model_->hasJointModelGroup(arm_jmg_name_)) << "Robot model does not have group: " << arm_jmg_name_; @@ -79,7 +79,10 @@ class RobotTrajectoryTestFixture : public testing::Test double duration_from_previous = 0.1; std::size_t waypoint_count = 5; for (std::size_t ix = 0; ix < waypoint_count; ++ix) + { trajectory->addSuffixWayPoint(*robot_state_, duration_from_previous); + } + // Quick check that getDuration is working correctly EXPECT_EQ(trajectory->getDuration(), duration_from_previous * waypoint_count) << "Generated trajectory duration incorrect"; @@ -528,7 +531,22 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryLength) { robot_trajectory::RobotTrajectoryPtr trajectory; initTestTrajectory(trajectory); +<<<<<<< HEAD EXPECT_GT(robot_trajectory::path_length(*trajectory), 0.0); +======= + EXPECT_FLOAT_EQ(robot_trajectory::pathLength(*trajectory), 0.0); + + // modify joint values so the smoothness is nonzero + std::vector positions; + for (size_t i = 0; i < trajectory->size(); ++i) + { + auto waypoint = trajectory->getWayPointPtr(i); + waypoint->copyJointGroupPositions(arm_jmg_name_, positions); + positions[0] += 0.01 * i; + waypoint->setJointGroupPositions(arm_jmg_name_, positions); + } + EXPECT_GT(robot_trajectory::pathLength(*trajectory), 0.0); +>>>>>>> 83ff55a4a (Fix angular distance calculation in floating joint model (#2538)) } TEST_F(RobotTrajectoryTestFixture, RobotTrajectorySmoothness) @@ -536,6 +554,16 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectorySmoothness) robot_trajectory::RobotTrajectoryPtr trajectory; initTestTrajectory(trajectory); + // modify joint values so the smoothness is nonzero + std::vector positions; + for (size_t i = 0; i < trajectory->size(); ++i) + { + auto waypoint = trajectory->getWayPointPtr(i); + waypoint->copyJointGroupPositions(arm_jmg_name_, positions); + positions[0] += 0.01 * i; + waypoint->setJointGroupPositions(arm_jmg_name_, positions); + } + const auto smoothness = robot_trajectory::smoothness(*trajectory); ASSERT_TRUE(smoothness.has_value()); EXPECT_GT(smoothness.value(), 0.0); @@ -550,13 +578,36 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDensity) robot_trajectory::RobotTrajectoryPtr trajectory; initTestTrajectory(trajectory); +<<<<<<< HEAD const auto density = robot_trajectory::waypoint_density(*trajectory); +======= + // If trajectory has all equal state, and length zero, density should be null. + auto density = robot_trajectory::waypointDensity(*trajectory); + ASSERT_FALSE(density.has_value()); + + // modify joint values so the density is nonzero + std::vector positions; + for (size_t i = 0; i < trajectory->size(); ++i) + { + auto waypoint = trajectory->getWayPointPtr(i); + waypoint->copyJointGroupPositions(arm_jmg_name_, positions); + positions[0] += 0.01 * i; + waypoint->setJointGroupPositions(arm_jmg_name_, positions); + } + + density = robot_trajectory::waypointDensity(*trajectory); +>>>>>>> 83ff55a4a (Fix angular distance calculation in floating joint model (#2538)) ASSERT_TRUE(density.has_value()); EXPECT_GT(density.value(), 0.0); // Check for empty trajectory trajectory->clear(); +<<<<<<< HEAD EXPECT_FALSE(robot_trajectory::waypoint_density(*trajectory).has_value()); +======= + density = robot_trajectory::waypointDensity(*trajectory); + EXPECT_FALSE(density.has_value()); +>>>>>>> 83ff55a4a (Fix angular distance calculation in floating joint model (#2538)) } TEST_F(OneRobot, Unwind) From 5a690f8cc9deea995ce78c601af4333565d25066 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Tue, 21 Nov 2023 13:48:39 -0500 Subject: [PATCH 2/2] Fix conflicts --- .../test/test_robot_trajectory.cpp | 22 +++++-------------- 1 file changed, 5 insertions(+), 17 deletions(-) diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp index 0f6436f2bb..43c4fb1d61 100644 --- a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -531,10 +531,7 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryLength) { robot_trajectory::RobotTrajectoryPtr trajectory; initTestTrajectory(trajectory); -<<<<<<< HEAD - EXPECT_GT(robot_trajectory::path_length(*trajectory), 0.0); -======= - EXPECT_FLOAT_EQ(robot_trajectory::pathLength(*trajectory), 0.0); + EXPECT_FLOAT_EQ(robot_trajectory::path_length(*trajectory), 0.0); // modify joint values so the smoothness is nonzero std::vector positions; @@ -545,8 +542,7 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryLength) positions[0] += 0.01 * i; waypoint->setJointGroupPositions(arm_jmg_name_, positions); } - EXPECT_GT(robot_trajectory::pathLength(*trajectory), 0.0); ->>>>>>> 83ff55a4a (Fix angular distance calculation in floating joint model (#2538)) + EXPECT_GT(robot_trajectory::path_length(*trajectory), 0.0); } TEST_F(RobotTrajectoryTestFixture, RobotTrajectorySmoothness) @@ -578,11 +574,8 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDensity) robot_trajectory::RobotTrajectoryPtr trajectory; initTestTrajectory(trajectory); -<<<<<<< HEAD - const auto density = robot_trajectory::waypoint_density(*trajectory); -======= // If trajectory has all equal state, and length zero, density should be null. - auto density = robot_trajectory::waypointDensity(*trajectory); + auto density = robot_trajectory::waypoint_density(*trajectory); ASSERT_FALSE(density.has_value()); // modify joint values so the density is nonzero @@ -595,19 +588,14 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDensity) waypoint->setJointGroupPositions(arm_jmg_name_, positions); } - density = robot_trajectory::waypointDensity(*trajectory); ->>>>>>> 83ff55a4a (Fix angular distance calculation in floating joint model (#2538)) + density = robot_trajectory::waypoint_density(*trajectory); ASSERT_TRUE(density.has_value()); EXPECT_GT(density.value(), 0.0); // Check for empty trajectory trajectory->clear(); -<<<<<<< HEAD - EXPECT_FALSE(robot_trajectory::waypoint_density(*trajectory).has_value()); -======= - density = robot_trajectory::waypointDensity(*trajectory); + density = robot_trajectory::waypoint_density(*trajectory); EXPECT_FALSE(density.has_value()); ->>>>>>> 83ff55a4a (Fix angular distance calculation in floating joint model (#2538)) } TEST_F(OneRobot, Unwind)