Skip to content

Re-added max cartesian speed limiter. Partial from: #2212 #3656

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

Open
JensVanhooydonck wants to merge 1 commit into
base: main
Choose a base branch
from
+682 −3
Open

Re-added max cartesian speed limiter. Partial from: #2212 #3656

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
7 changes: 6 additions & 1 deletion moveit_core/trajectory_processing/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
add_library(
moveit_trajectory_processing SHARED
src/ruckig_traj_smoothing.cpp src/trajectory_tools.cpp
src/time_optimal_trajectory_generation.cpp)
src/time_optimal_trajectory_generation.cpp src/limit_cartesian_speed.cpp)
target_include_directories(
moveit_trajectory_processing
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
Expand Down Expand Up @@ -45,6 +45,11 @@ if(BUILD_TESTING)
target_link_libraries(test_ruckig_traj_smoothing moveit_trajectory_processing
moveit_test_utils)

ament_add_gtest(test_limit_cartesian_speed
test/test_limit_cartesian_speed.cpp)
target_link_libraries(test_limit_cartesian_speed moveit_trajectory_processing
moveit_test_utils)

ament_add_google_benchmark(robot_trajectory_benchmark
test/robot_trajectory_benchmark.cpp)
target_link_libraries(
Expand Down
15 changes: 15 additions & 0 deletions ...t_core/trajectory_processing/include/moveit/trajectory_processing/limit_cartesian_speed.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@

/*********************************************************************
* All MoveIt 2 headers have been updated to use the .hpp extension.
*
* .h headers are now autogenerated via create_deprecated_headers.py,
* and will import the corresponding .hpp with a deprecation warning.
*
* imports via .h files may be removed in future releases, so please
* modify your imports to use the corresponding .hpp imports.
*
* See https://github.com/moveit/moveit2/pull/3113 for extra details.
*********************************************************************/
#pragma once
#pragma message(".h header is obsolete. Please use the .hpp header instead.")
#include <moveit/trajectory_processing/limit_cartesian_speed.hpp>
50 changes: 50 additions & 0 deletions ...core/trajectory_processing/include/moveit/trajectory_processing/limit_cartesian_speed.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,50 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Benjamin Scholz
* Copyright (c) 2021, Thies Oelerich
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the authors nor the names of other
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Authors: Benjamin Scholz, Thies Oelerich */

#pragma once

#include <moveit/macros/class_forward.hpp>
#include <string>
#include <moveit/robot_trajectory/robot_trajectory.hpp>

namespace trajectory_processing
{
bool limitMaxCartesianLinkSpeed(robot_trajectory::RobotTrajectory& trajectory, const double max_speed,
const moveit::core::LinkModel* link_model);
bool limitMaxCartesianLinkSpeed(robot_trajectory::RobotTrajectory& trajectory, const double max_speed,
const std::string& link_name = "");
} // namespace trajectory_processing
9 changes: 8 additions & 1 deletion moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#include <moveit_msgs/msg/robot_trajectory.hpp>
#include <moveit/robot_trajectory/robot_trajectory.hpp>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.hpp>
#include <moveit/trajectory_processing/limit_cartesian_speed.hpp>

namespace trajectory_processing
{
Expand Down Expand Up @@ -80,11 +81,17 @@ bool applyTOTGTimeParameterization(robot_trajectory::RobotTrajectory& trajectory
bool applyRuckigSmoothing(robot_trajectory::RobotTrajectory& trajectory, double velocity_scaling_factor,
double acceleration_scaling_factor, bool mitigate_overshoot = false,
double overshoot_threshold = 0.01);

/**
* @brief Converts a `trajectory_processing::Trajectory` into a `JointTrajectory` message with a given sampling rate.
*/
[[nodiscard]] trajectory_msgs::msg::JointTrajectory
createTrajectoryMessage(const std::vector<std::string>& joint_names,
const trajectory_processing::Trajectory& trajectory, const int sampling_rate);

/**
* \brief Updates the time stamps of a `robot_trajectory::RobotTrajectory` based on the provided time differences.
* \param [in,out] rob_trajectory The robot trajectory to be updated.
* \param [in] time_diff A vector containing the time differences between consecutive waypoints.
*/
void updateTrajectory(robot_trajectory::RobotTrajectory& rob_trajectory, const std::vector<double>& time_diff);
} // namespace trajectory_processing
127 changes: 127 additions & 0 deletions moveit_core/trajectory_processing/src/limit_cartesian_speed.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,127 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Benjamin Scholz
* Copyright (c) 2021, Thies Oelerich
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the authors nor the names of other
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Authors: Benjamin Scholz, Thies Oelerich */

#include <moveit/trajectory_processing/limit_cartesian_speed.hpp>
#include <moveit/trajectory_processing/trajectory_tools.hpp>

#include <rclcpp/logger.hpp>
#include <rclcpp/logging.hpp>
#include <moveit/utils/logger.hpp>

rclcpp::Logger getLogger()
{
return moveit::getLogger("moveit.core.limit_cartesian_speed");
}

namespace trajectory_processing
{

bool limitMaxCartesianLinkSpeed(robot_trajectory::RobotTrajectory& trajectory, const double max_speed,
const std::string& link_name)
{
std::vector<const moveit::core::LinkModel*> links;

if (!link_name.empty())
{
const moveit::core::RobotModel& model{ *trajectory.getRobotModel() };
bool found = false;
const auto* link = model.getLinkModel(link_name, &found);

if (!found)
RCLCPP_ERROR_STREAM(moveit::getLogger("moveit.core.limit_cartesian_speed"),
"Unknown link model '" << link_name << "'");
else
links.push_back(link);
}
// In case the link name is not given but the trajectory belongs to a group,
// retrieve the end effectors from that joint model group
else if (trajectory.getGroup())
{
trajectory.getGroup()->getEndEffectorTips(links);
if (links.empty())
{
RCLCPP_ERROR_STREAM(moveit::getLogger("moveit.core.limit_cartesian_speed"), "No link(s) specified");
}
}

// Call function for speed setting using the created link model
for (const auto& link : links)
{
if (!limitMaxCartesianLinkSpeed(trajectory, max_speed, link))
return false;
}

return !links.empty();
}

bool limitMaxCartesianLinkSpeed(robot_trajectory::RobotTrajectory& trajectory, const double max_speed,
const moveit::core::LinkModel* link_model)
{
if (max_speed <= 0.0)
{
RCLCPP_ERROR_STREAM(moveit::getLogger("moveit.core.limit_cartesian_speed"), "Link speed must be greater than 0.");
return false;
}

size_t num_waypoints = trajectory.getWayPointCount();
if (num_waypoints == 0)
return false;

// do forward kinematics to get Cartesian positions of link for current waypoint
double euclidean_distance, new_time_diff, old_time_diff;
std::vector<double> time_diff(num_waypoints - 1, 0.0);

for (size_t i = 0; i < num_waypoints - 1; i++)
{
// get link state for current and next waypoint
const Eigen::Isometry3d& current_link_state = trajectory.getWayPointPtr(i)->getGlobalLinkTransform(link_model);
const Eigen::Isometry3d& next_link_state = trajectory.getWayPointPtr(i + 1)->getGlobalLinkTransform(link_model);

// get Euclidean distance between the two waypoints
euclidean_distance = (next_link_state.translation() - current_link_state.translation()).norm();

new_time_diff = (euclidean_distance / max_speed);
old_time_diff = trajectory.getWayPointDurationFromPrevious(i + 1);

// slow-down segment if it was too fast before
time_diff[i] = std::max(new_time_diff, old_time_diff);
}
// update time stamps, velocities and accelerations of the trajectory
updateTrajectory(trajectory, time_diff);
return true;
}
} // namespace trajectory_processing
110 changes: 110 additions & 0 deletions moveit_core/trajectory_processing/src/trajectory_tools.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,4 +107,114 @@ trajectory_msgs::msg::JointTrajectory createTrajectoryMessage(const std::vector<
}
return trajectory_msg;
}

// Takes the time differences, and updates the timestamps, velocities and accelerations
// in the trajectory.
void updateTrajectory(robot_trajectory::RobotTrajectory& rob_trajectory, const std::vector<double>& time_diff)
{
// Error check
if (time_diff.empty())
return;

double time_sum = 0.0;

moveit::core::RobotStatePtr prev_waypoint;
moveit::core::RobotStatePtr curr_waypoint;
moveit::core::RobotStatePtr next_waypoint;

const moveit::core::JointModelGroup* group = rob_trajectory.getGroup();
const std::vector<std::string>& vars = group->getVariableNames();
const std::vector<int>& idx = group->getVariableIndexList();

int num_points = rob_trajectory.getWayPointCount();

rob_trajectory.setWayPointDurationFromPrevious(0, time_sum);

// Times
for (int i = 1; i < num_points; ++i)
// Update the time between the waypoints in the robot_trajectory.
rob_trajectory.setWayPointDurationFromPrevious(i, time_diff[i - 1]);

// Return if there is only one point in the trajectory!
if (num_points <= 1)
return;

// Accelerations
for (int i = 0; i < num_points; ++i)
{
curr_waypoint = rob_trajectory.getWayPointPtr(i);

if (i > 0)
prev_waypoint = rob_trajectory.getWayPointPtr(i - 1);

if (i < num_points - 1)
next_waypoint = rob_trajectory.getWayPointPtr(i + 1);

for (std::size_t j = 0; j < vars.size(); ++j)
{
double q1;
double q2;
double q3;
double dt1;
double dt2;

if (i == 0)
{
// First point
q1 = next_waypoint->getVariablePosition(idx[j]);
q2 = curr_waypoint->getVariablePosition(idx[j]);
q3 = q1;

dt1 = dt2 = time_diff[i];
}
else if (i < num_points - 1)
{
// middle points
q1 = prev_waypoint->getVariablePosition(idx[j]);
q2 = curr_waypoint->getVariablePosition(idx[j]);
q3 = next_waypoint->getVariablePosition(idx[j]);

dt1 = time_diff[i - 1];
dt2 = time_diff[i];
}
else
{
// last point
q1 = prev_waypoint->getVariablePosition(idx[j]);
q2 = curr_waypoint->getVariablePosition(idx[j]);
q3 = q1;

dt1 = dt2 = time_diff[i - 1];
}

double v1, v2, a;

bool start_velocity = false;
if (dt1 == 0.0 || dt2 == 0.0)
{
v1 = 0.0;
v2 = 0.0;
a = 0.0;
}
else
{
if (i == 0)
{
if (curr_waypoint->hasVelocities())
{
start_velocity = true;
v1 = curr_waypoint->getVariableVelocity(idx[j]);
}
}
v1 = start_velocity ? v1 : (q2 - q1) / dt1;
// v2 = (q3-q2)/dt2;
v2 = start_velocity ? v1 : (q3 - q2) / dt2; // Needed to ensure continuous velocity for first point
a = 2.0 * (v2 - v1) / (dt1 + dt2);
}

curr_waypoint->setVariableVelocity(idx[j], (v2 + v1) / 2.0);
curr_waypoint->setVariableAcceleration(idx[j], a);
}
}
}
} // namespace trajectory_processing
Loading
Loading