Skip to content

Commit

Permalink
Require velocity and acceleration limits in TOTG (#1794)
Browse files Browse the repository at this point in the history 
* Require vel/accel limits for TOTG

* Comment improvements

Co-authored-by: Sebastian Jahr <sebastian.jahr@tuta.io>

Co-authored-by: Sebastian Jahr <sebastian.jahr@tuta.io>
  • Loading branch information
@AndyZe @sjahr
AndyZe and sjahr committed Dec 20, 2022
1 parent 43b06e9 commit 937be8e
Show file tree
Hide file tree
Showing 3 changed files with 69 additions and 116 deletions.
37 changes: 19 additions & 18 deletions moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -924,7 +924,6 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]);

// Limits need to be non-zero, otherwise we never exit
max_velocity[j] = 1.0;
if (bounds.velocity_bounded_)
{
if (bounds.max_velocity_ <= 0.0)
Expand All @@ -938,12 +937,12 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
}
else
{
RCLCPP_WARN_STREAM_ONCE(
LOGGER, "Joint velocity limits are not defined. Using the default "
<< max_velocity[j] << " rad/s. You can define velocity limits in the URDF or joint_limits.yaml.");
RCLCPP_ERROR_STREAM(LOGGER, "No velocity limit was defined for joint "
<< vars[j].c_str()
<< "! You have to define velocity limits in the URDF or joint_limits.yaml");
return false;
}

max_acceleration[j] = 1.0;
if (bounds.acceleration_bounded_)
{
if (bounds.max_acceleration_ < 0.0)
Expand All @@ -957,10 +956,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
}
else
{
RCLCPP_WARN_STREAM_ONCE(LOGGER,
"Joint acceleration limits are not defined. Using the default "
<< max_acceleration[j]
<< " rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.");
RCLCPP_ERROR_STREAM(LOGGER, "No acceleration limit was defined for joint "
<< vars[j].c_str()
<< "! You have to define acceleration limits in the URDF or "
"joint_limits.yaml");
return false;
}
}

Expand Down Expand Up @@ -1016,10 +1016,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(

if (!set_velocity_limit)
{
max_velocity[j] = 1.0;
RCLCPP_WARN_STREAM_ONCE(
LOGGER, "Joint velocity limits are not defined. Using the default "
<< max_velocity[j] << " rad/s. You can define velocity limits in the URDF or joint_limits.yaml.");
RCLCPP_ERROR_STREAM(LOGGER, "No velocity limit was defined for joint "
<< vars[j].c_str()
<< "! You have to define velocity limits in the URDF or "
"joint_limits.yaml");
return false;
}

// ACCELERATION LIMIT
Expand All @@ -1046,11 +1047,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
}
if (!set_acceleration_limit)
{
max_acceleration[j] = 1.0;
RCLCPP_WARN_STREAM_ONCE(LOGGER,
"Joint acceleration limits are not defined. Using the default "
<< max_acceleration[j]
<< " rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.");
RCLCPP_ERROR_STREAM(LOGGER, "No acceleration limit was defined for joint "
<< vars[j].c_str()
<< "! You have to define acceleration limits in the URDF or "
"joint_limits.yaml");
return false;
}
}

Expand Down
142 changes: 47 additions & 95 deletions moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,29 @@ using trajectory_processing::Path;
using trajectory_processing::TimeOptimalTrajectoryGeneration;
using trajectory_processing::Trajectory;

namespace
{
// The URDF used in moveit::core::loadTestingRobotModel() does not contain acceleration limits,
// so add them here.
// TODO(andyz): Function won't be needed once this issue has been addressed:
// https://github.com/ros/urdfdom/issues/177
void set_acceleration_limits(const moveit::core::RobotModelPtr& robot_model)
{
const std::vector<moveit::core::JointModel*> joint_models = robot_model->getActiveJointModels();
for (auto& joint_model : joint_models)
{
std::vector<moveit_msgs::msg::JointLimits> joint_bounds_msg(joint_model->getVariableBoundsMsg());
// TODO: update individual bounds
for (auto& joint_bound : joint_bounds_msg)
{
joint_bound.has_acceleration_limits = true;
joint_bound.max_acceleration = 1.0;
}
joint_model->setVariableBounds(joint_bounds_msg);
}
}
} // namespace

TEST(time_optimal_trajectory_generation, test1)
{
Eigen::VectorXd waypoint(4);
Expand Down Expand Up @@ -153,13 +176,14 @@ TEST(time_optimal_trajectory_generation, test3)
}

// Test the version of computeTimeStamps that takes custom velocity/acceleration limits
TEST(time_optimal_trajectory_generation, test_custom_limits)
TEST(time_optimal_trajectory_generation, testCustomLimits)
{
constexpr auto robot_name{ "panda" };
constexpr auto group_name{ "panda_arm" };

auto robot_model = moveit::core::loadTestingRobotModel(robot_name);
ASSERT_TRUE(robot_model) << "Failed to load robot model" << robot_name;
set_acceleration_limits(robot_model);
auto group = robot_model->getJointModelGroup(group_name);
ASSERT_TRUE(group) << "Failed to load joint model group " << group_name;
moveit::core::RobotState waypoint_state(robot_model);
Expand All @@ -174,8 +198,14 @@ TEST(time_optimal_trajectory_generation, test_custom_limits)

TimeOptimalTrajectoryGeneration totg;
// Custom velocity & acceleration limits for some joints
std::unordered_map<std::string, double> vel_limits{ { "panda_joint1", 1.3 } };
std::unordered_map<std::string, double> accel_limits{ { "panda_joint2", 2.3 }, { "panda_joint3", 3.3 } };
std::unordered_map<std::string, double> vel_limits{ { "panda_joint1", 1.3 }, { "panda_joint2", 2.3 },
{ "panda_joint3", 3.3 }, { "panda_joint4", 4.3 },
{ "panda_joint5", 5.3 }, { "panda_joint6", 6.3 },
{ "panda_joint7", 7.3 } };
std::unordered_map<std::string, double> accel_limits{ { "panda_joint1", 1.3 }, { "panda_joint2", 2.3 },
{ "panda_joint3", 3.3 }, { "panda_joint4", 4.3 },
{ "panda_joint5", 5.3 }, { "panda_joint6", 6.3 },
{ "panda_joint7", 7.3 } };
ASSERT_TRUE(totg.computeTimeStamps(trajectory, vel_limits, accel_limits)) << "Failed to compute time stamps";
}

Expand Down Expand Up @@ -256,14 +286,16 @@ TEST(time_optimal_trajectory_generation, testLargeAccel)
}
}

// Test parameterizing a trajectory would always produce a trajectory with output end waypoint same as the input end waypoint
// Test parameterizing a trajectory would always produce a trajectory with output end waypoint same as the input end
// waypoint
TEST(time_optimal_trajectory_generation, testLastWaypoint)
{
constexpr auto robot_name{ "panda" };
constexpr auto group_name{ "hand" };
constexpr auto group_name{ "panda_arm" };

auto robot_model = moveit::core::loadTestingRobotModel(robot_name);
ASSERT_TRUE(robot_model) << "Failed to load robot model" << robot_name;
set_acceleration_limits(robot_model);
auto group = robot_model->getJointModelGroup(group_name);
ASSERT_TRUE(group) << "Failed to load joint model group " << group_name;
moveit::core::RobotState waypoint_state(robot_model);
Expand All @@ -274,96 +306,14 @@ TEST(time_optimal_trajectory_generation, testLastWaypoint)
waypoint_state.setJointGroupPositions(group, waypoint);
trajectory.addSuffixWayPoint(waypoint_state, 0.1);
};
add_waypoint({ 0.000000000, 0.000000000 });
add_waypoint({ 0.000396742, 0.000396742 });
add_waypoint({ 0.000793484, 0.000793484 });
add_waypoint({ 0.001190226, 0.001190226 });
add_waypoint({ 0.001586968, 0.001586968 });
add_waypoint({ 0.001983710, 0.001983710 });
add_waypoint({ 0.002380452, 0.002380452 });
add_waypoint({ 0.002777194, 0.002777194 });
add_waypoint({ 0.003173936, 0.003173936 });
add_waypoint({ 0.003570678, 0.003570678 });
add_waypoint({ 0.003967420, 0.003967420 });
add_waypoint({ 0.004364162, 0.004364162 });
add_waypoint({ 0.004760904, 0.004760904 });
add_waypoint({ 0.005157646, 0.005157646 });
add_waypoint({ 0.005554388, 0.005554388 });
add_waypoint({ 0.005951130, 0.005951130 });
add_waypoint({ 0.006347872, 0.006347872 });
add_waypoint({ 0.006744614, 0.006744614 });
add_waypoint({ 0.007141356, 0.007141356 });
add_waypoint({ 0.007538098, 0.007538098 });
add_waypoint({ 0.007934840, 0.007934840 });
add_waypoint({ 0.008331582, 0.008331582 });
add_waypoint({ 0.008728324, 0.008728324 });
add_waypoint({ 0.009125066, 0.009125066 });
add_waypoint({ 0.009521808, 0.009521808 });
add_waypoint({ 0.009918550, 0.009918550 });
add_waypoint({ 0.010315292, 0.010315292 });
add_waypoint({ 0.010712034, 0.010712034 });
add_waypoint({ 0.011108776, 0.011108776 });
add_waypoint({ 0.011505518, 0.011505518 });
add_waypoint({ 0.011902261, 0.011902261 });
add_waypoint({ 0.012299003, 0.012299003 });
add_waypoint({ 0.012695745, 0.012695745 });
add_waypoint({ 0.013092487, 0.013092487 });
add_waypoint({ 0.013489229, 0.013489229 });
add_waypoint({ 0.013885971, 0.013885971 });
add_waypoint({ 0.014282713, 0.014282713 });
add_waypoint({ 0.014679455, 0.014679455 });
add_waypoint({ 0.015076197, 0.015076197 });
add_waypoint({ 0.015472939, 0.015472939 });
add_waypoint({ 0.015869681, 0.015869681 });
add_waypoint({ 0.016266423, 0.016266423 });
add_waypoint({ 0.016663165, 0.016663165 });
add_waypoint({ 0.017059907, 0.017059907 });
add_waypoint({ 0.017456649, 0.017456649 });
add_waypoint({ 0.017853391, 0.017853391 });
add_waypoint({ 0.018250133, 0.018250133 });
add_waypoint({ 0.018646875, 0.018646875 });
add_waypoint({ 0.019043617, 0.019043617 });
add_waypoint({ 0.019440359, 0.019440359 });
add_waypoint({ 0.019837101, 0.019837101 });
add_waypoint({ 0.020233843, 0.020233843 });
add_waypoint({ 0.020630585, 0.020630585 });
add_waypoint({ 0.021027327, 0.021027327 });
add_waypoint({ 0.021424069, 0.021424069 });
add_waypoint({ 0.021820811, 0.021820811 });
add_waypoint({ 0.022217553, 0.022217553 });
add_waypoint({ 0.022614295, 0.022614295 });
add_waypoint({ 0.023011037, 0.023011037 });
add_waypoint({ 0.023407779, 0.023407779 });
add_waypoint({ 0.023804521, 0.023804521 });
add_waypoint({ 0.024201263, 0.024201263 });
add_waypoint({ 0.024598005, 0.024598005 });
add_waypoint({ 0.024994747, 0.024994747 });
add_waypoint({ 0.025391489, 0.025391489 });
add_waypoint({ 0.025788231, 0.025788231 });
add_waypoint({ 0.026184973, 0.026184973 });
add_waypoint({ 0.026581715, 0.026581715 });
add_waypoint({ 0.026978457, 0.026978457 });
add_waypoint({ 0.027375199, 0.027375199 });
add_waypoint({ 0.027771941, 0.027771941 });
add_waypoint({ 0.028168683, 0.028168683 });
add_waypoint({ 0.028565425, 0.028565425 });
add_waypoint({ 0.028962167, 0.028962167 });
add_waypoint({ 0.029358909, 0.029358909 });
add_waypoint({ 0.029755651, 0.029755651 });
add_waypoint({ 0.030152393, 0.030152393 });
add_waypoint({ 0.030549135, 0.030549135 });
add_waypoint({ 0.030945877, 0.030945877 });
add_waypoint({ 0.031342619, 0.031342619 });
add_waypoint({ 0.031739361, 0.031739361 });
add_waypoint({ 0.032136103, 0.032136103 });
add_waypoint({ 0.032532845, 0.032532845 });
add_waypoint({ 0.032929587, 0.032929587 });
add_waypoint({ 0.033326329, 0.033326329 });
add_waypoint({ 0.033723071, 0.033723071 });
add_waypoint({ 0.034119813, 0.034119813 });
add_waypoint({ 0.034516555, 0.034516555 });

const std::vector<double> expected_last_waypoint = { 0.034913297, 0.034913297 };
add_waypoint({ 0.000000000, 0.000000000, 0, 0, 0, 0, 0 });
add_waypoint({ 0.009521808, 0.009521808, 0, 0, 0, 0, 0 });
add_waypoint({ 0.011902261, 0.011902261, 0, 0, 0, 0, 0 });
add_waypoint({ 0.016663165, 0.016663165, 0, 0, 0, 0, 0 });
add_waypoint({ 0.026184973, 0.026184973, 0, 0, 0, 0, 0 });
add_waypoint({ 0.034516555, 0.034516555, 0, 0, 0, 0, 0 });

const std::vector<double> expected_last_waypoint = { 0.034516555, 0.034516555, 0, 0, 0, 0, 0 };
add_waypoint(expected_last_waypoint);

TimeOptimalTrajectoryGeneration totg;
Expand All @@ -384,6 +334,7 @@ TEST(time_optimal_trajectory_generation, testPluginAPI)

auto robot_model = moveit::core::loadTestingRobotModel(robot_name);
ASSERT_TRUE(robot_model) << "Failed to load robot model" << robot_name;
set_acceleration_limits(robot_model);
auto group = robot_model->getJointModelGroup(group_name);
ASSERT_TRUE(group) << "Failed to load joint model group " << group_name;
moveit::core::RobotState waypoint_state(robot_model);
Expand Down Expand Up @@ -506,6 +457,7 @@ TEST(time_optimal_trajectory_generation, testFixedNumWaypoints)

auto robot_model = moveit::core::loadTestingRobotModel(robot_name);
ASSERT_TRUE(robot_model) << "Failed to load robot model" << robot_name;
set_acceleration_limits(robot_model);
auto group = robot_model->getJointModelGroup(group_name);
ASSERT_TRUE(group) << "Failed to load joint model group " << group_name;
moveit::core::RobotState waypoint_state(robot_model);
Expand Down
6 changes: 3 additions & 3 deletions moveit_core/utils/src/robot_model_test_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,14 +34,14 @@

/* Author: Bryce Willey */

#include <ament_index_cpp/get_package_share_directory.hpp>
#include <boost/algorithm/string_regex.hpp>
#include <filesystem>
#include <geometry_msgs/msg/pose.hpp>
#include <urdf_parser/urdf_parser.h>
#include <moveit/utils/robot_model_test_utils.h>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <filesystem>
#include <rclcpp/logger.hpp>
#include <rclcpp/logging.hpp>
#include <urdf_parser/urdf_parser.h>

namespace moveit
{
Expand Down

0 comments on commit 937be8e

Please sign in to comment.