Skip to content

Commit

Permalink
Trajectory functions (#9)
Browse files Browse the repository at this point in the history
  • Loading branch information
@sjahr
sjahr authored Aug 15, 2022
1 parent 83dc341 commit 0ee1154
Show file tree
Hide file tree
Showing 4 changed files with 60 additions and 84 deletions.
23 changes: 10 additions & 13 deletions moveit_core/robot_trajectory/src/robot_trajectory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -653,21 +653,18 @@ std::optional<double> smoothness(RobotTrajectory const& trajectory)

std::optional<double> waypoint_density(RobotTrajectory const& trajectory)
{
if (trajectory.getWayPointCount() == 0)
// Only calculate density if more than one waypoint exists
if (trajectory.getWayPointCount() > 1)
{
// Cannot calculate trajectory density because the trajectory does not
// contain any points
return std::nullopt;
}

// Calculate path length
auto const length = path_length(trajectory);
if (length > 0.0)
{
auto density = length / (double)trajectory.getWayPointCount();
return density;
// Calculate path length
auto const length = path_length(trajectory);
if (length > 0.0)
{
auto density = (double)trajectory.getWayPointCount() / length;
return density;
}
}
// In case the path length is zero, no value is returned
// Trajectory is empty, a single point or path length is zero
return std::nullopt;
}

Expand Down
38 changes: 37 additions & 1 deletion moveit_core/robot_trajectory/test/test_robot_trajectory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,13 +47,14 @@ class RobotTrajectoryTestFixture : public testing::Test
moveit::core::RobotStatePtr robot_state_;
const std::string robot_model_name_ = "panda";
const std::string arm_jmg_name_ = "panda_arm";
const std::string arm_state_name_ = "ready";

protected:
void SetUp() override
{
robot_model_ = moveit::core::loadTestingRobotModel(robot_model_name_);
robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
robot_state_->setToDefaultValues();
robot_state_->setToDefaultValues(arm_jmg_name_, arm_state_name_);
robot_state_->setVariableVelocity(/*index*/ 0, /*value*/ 1.0);
robot_state_->setVariableAcceleration(/*index*/ 0, /*value*/ -0.1);
robot_state_->update();
Expand Down Expand Up @@ -297,6 +298,41 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryIterator)
EXPECT_EQ(++(++(++(++(++trajectory->begin())))), trajectory->end());
}

TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryLength)
{
robot_trajectory::RobotTrajectoryPtr trajectory;
initTestTrajectory(trajectory);
EXPECT_GT(robot_trajectory::path_length(*trajectory), 0.0);
}

TEST_F(RobotTrajectoryTestFixture, RobotTrajectorySmoothness)
{
robot_trajectory::RobotTrajectoryPtr trajectory;
initTestTrajectory(trajectory);

const auto smoothness = robot_trajectory::smoothness(*trajectory);
ASSERT_TRUE(smoothness.has_value());
EXPECT_GT(smoothness.value(), 0.0);

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

TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDensity)
{
robot_trajectory::RobotTrajectoryPtr trajectory;
initTestTrajectory(trajectory);

const auto 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());
}

int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
Expand Down
38 changes: 5 additions & 33 deletions moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -814,14 +814,11 @@ void moveit_benchmarks::BenchmarkExecution::collectMetrics(RunData& rundata,
for (std::size_t j = 0; j < mp_res.trajectory_.size(); ++j)
{
correct = true;
L = 0.0;
clearance = 0.0;
smoothness = 0.0;
const robot_trajectory::RobotTrajectory& p = *mp_res.trajectory_[j];

// compute path length
for (std::size_t k = 1; k < p.getWayPointCount(); ++k)
L += p.getWayPoint(k - 1).distance(p.getWayPoint(k));
L = robot_trajectory::length(p);

// compute correctness and clearance
collision_detection::CollisionRequest req;
Expand All @@ -840,36 +837,11 @@ void moveit_benchmarks::BenchmarkExecution::collectMetrics(RunData& rundata,
clearance /= (double)p.getWayPointCount();

// compute smoothness
if (p.getWayPointCount() > 2)
{
double a = p.getWayPoint(0).distance(p.getWayPoint(1));
for (std::size_t k = 2; k < p.getWayPointCount(); ++k)
{
// view the path as a sequence of segments, and look at the triangles it forms:
// s1
// /\ s4
// a / \ b |
// / \ |
// /......\_______|
// s0 c s2 s3
//
// use Pythagoras generalized theorem to find the cos of the angle between segments a and b
double b = p.getWayPoint(k - 1).distance(p.getWayPoint(k));
double cdist = p.getWayPoint(k - 2).distance(p.getWayPoint(k));
double acosValue = (a * a + b * b - cdist * cdist) / (2.0 * a * b);
if (acosValue > -1.0 && acosValue < 1.0)
{
// the smoothness is actually the outside angle of the one we compute
double angle = (M_PI - acos(acosValue));
const auto smoothness = [&]() {
const auto s = robot_trajectory::smoothness(p);
return s.has_value() ? s.value() : 0.0;
}();

// and we normalize by the length of the segments
double u = 2.0 * angle; /// (a + b);
smoothness += u * u;
}
a = b;
}
smoothness /= (double)p.getWayPointCount();
}
rundata["path_" + mp_res.description_[j] + "_correct BOOLEAN"] = correct ? "true" : "false";
rundata["path_" + mp_res.description_[j] + "_length REAL"] = std::to_string(L);
rundata["path_" + mp_res.description_[j] + "_clearance REAL"] = std::to_string(clearance);
Expand Down
45 changes: 8 additions & 37 deletions moveit_ros/benchmarks/src/BenchmarkExecutor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -875,23 +875,20 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics,
if (solved)
{
// Analyzing the trajectory(ies) geometrically
double traj_len = 0.0; // trajectory length
double clearance = 0.0; // trajectory clearance (average)
double smoothness = 0.0; // trajectory smoothness (average)
bool correct = true; // entire trajectory collision free and in bounds
double traj_len = 0.0; // trajectory length
double clearance = 0.0; // trajectory clearance (average)
bool correct = true; // entire trajectory collision free and in bounds

double process_time = total_time;
for (std::size_t j = 0; j < mp_res.trajectory_.size(); ++j)
{
correct = true;
traj_len = 0.0;
clearance = 0.0;
smoothness = 0.0;
const robot_trajectory::RobotTrajectory& p = *mp_res.trajectory_[j];

// compute path length
for (std::size_t k = 1; k < p.getWayPointCount(); ++k)
traj_len += p.getWayPoint(k - 1).distance(p.getWayPoint(k));
traj_len = robot_trajectory::path_length(p);

// compute correctness and clearance
collision_detection::CollisionRequest req;
Expand All @@ -910,37 +907,11 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics,
clearance /= (double)p.getWayPointCount();

// compute smoothness
if (p.getWayPointCount() > 2)
{
double a = p.getWayPoint(0).distance(p.getWayPoint(1));
for (std::size_t k = 2; k < p.getWayPointCount(); ++k)
{
// view the path as a sequence of segments, and look at the triangles it forms:
// s1
// /\ s4
// a / \ b |
// / \ |
// /......\_______|
// s0 c s2 s3
//

// use Pythagoras generalized theorem to find the cos of the angle between segments a and b
double b = p.getWayPoint(k - 1).distance(p.getWayPoint(k));
double cdist = p.getWayPoint(k - 2).distance(p.getWayPoint(k));
double acos_value = (a * a + b * b - cdist * cdist) / (2.0 * a * b);
if (acos_value > -1.0 && acos_value < 1.0)
{
// the smoothness is actually the outside angle of the one we compute
double angle = (M_PI - acos(acos_value));
const auto smoothness = [&]() {
const auto s = robot_trajectory::smoothness(p);
return s.has_value() ? s.value() : 0.0;
}();

// and we normalize by the length of the segments
double u = 2.0 * angle; /// (a + b);
smoothness += u * u;
}
a = b;
}
smoothness /= (double)p.getWayPointCount();
}
metrics["path_" + mp_res.description_[j] + "_correct BOOLEAN"] = correct ? "true" : "false";
metrics["path_" + mp_res.description_[j] + "_length REAL"] = moveit::core::toString(traj_len);
metrics["path_" + mp_res.description_[j] + "_clearance REAL"] = moveit::core::toString(clearance);
Expand Down

0 comments on commit 0ee1154

Please sign in to comment.