Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix angular distance calculation in floating joint model (backport #2538) #2544

Merged
merged 2 commits into from
Dec 15, 2023
Merged

Fix angular distance calculation in floating joint model (backport #2538) #2544

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
0509af5
Fix angular distance calculation in floating joint model (#2538)
sea-bass Nov 21, 2023
5a690f8
Fix conflicts
sea-bass Nov 21, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 5 additions & 10 deletions moveit_core/robot_model/src/floating_joint_model.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@
/* Author: Ioan Sucan */

#include <cmath>
#include <Eigen/Geometry>
#include <geometric_shapes/check_isometry.h>
#include <limits>
#include <moveit/robot_model/floating_joint_model.h>
Expand Down Expand Up @@ -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<double>::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
Expand Down
47 changes: 43 additions & 4 deletions moveit_core/robot_trajectory/test/test_robot_trajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ class RobotTrajectoryTestFixture : public testing::Test
{
robot_model_ = moveit::core::loadTestingRobotModel(robot_model_name_);
robot_state_ = std::make_shared<moveit::core::RobotState>(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();
Expand All @@ -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_;

Expand All @@ -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";
Expand Down Expand Up @@ -528,6 +531,17 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryLength)
{
robot_trajectory::RobotTrajectoryPtr trajectory;
initTestTrajectory(trajectory);
EXPECT_FLOAT_EQ(robot_trajectory::path_length(*trajectory), 0.0);

// modify joint values so the smoothness is nonzero
std::vector<double> 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::path_length(*trajectory), 0.0);
}

Expand All @@ -536,6 +550,16 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectorySmoothness)
robot_trajectory::RobotTrajectoryPtr trajectory;
initTestTrajectory(trajectory);

// modify joint values so the smoothness is nonzero
std::vector<double> 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);
Expand All @@ -550,13 +574,28 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDensity)
robot_trajectory::RobotTrajectoryPtr trajectory;
initTestTrajectory(trajectory);

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::waypoint_density(*trajectory);
ASSERT_FALSE(density.has_value());

// modify joint values so the density is nonzero
std::vector<double> 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::waypoint_density(*trajectory);
ASSERT_TRUE(density.has_value());
EXPECT_GT(density.value(), 0.0);

// Check for empty trajectory
trajectory->clear();
EXPECT_FALSE(robot_trajectory::waypoint_density(*trajectory).has_value());
density = robot_trajectory::waypoint_density(*trajectory);
EXPECT_FALSE(density.has_value());
}

TEST_F(OneRobot, Unwind)
Expand Down
Loading