Merge controller into dev: bounded trajectories and shared seven-axis limits
Preserve dev TCP gizmo motion-priority handling while integrating Ruckig smoothing, continuous JTC spline validation and shared robot limits. Resolve launch and dependency conflicts without removing either feature set. Verified merged build of 8 packages and tests of 4 packages.
This commit is contained in:
Даниил Грабарь
@@ -28,6 +28,50 @@ target_compile_features(tcp_gizmo PUBLIC cxx_std_17)
|
||||
target_include_directories(tcp_gizmo PRIVATE include)
|
||||
ament_target_dependencies(tcp_gizmo rclcpp rclcpp_action control_msgs iiwa_msgs geometry_msgs trajectory_msgs
|
||||
action_msgs moveit_servo moveit_ros_planning moveit_core tf2_eigen)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(Python3 REQUIRED COMPONENTS Interpreter Development.Module)
|
||||
find_package(pybind11 REQUIRED)
|
||||
pybind11_add_module(_iiwa_spline_limits src/spline_limits_py.cpp)
|
||||
target_compile_features(_iiwa_spline_limits PRIVATE cxx_std_17)
|
||||
target_include_directories(_iiwa_spline_limits PRIVATE include)
|
||||
ament_get_python_install_dir(PYTHON_INSTALL_DIR)
|
||||
install(TARGETS _iiwa_spline_limits LIBRARY DESTINATION ${PYTHON_INSTALL_DIR})
|
||||
|
||||
add_library(iiwa_spline_limits SHARED src/limit_spline_dynamics.cpp)
|
||||
target_compile_features(iiwa_spline_limits PUBLIC cxx_std_17)
|
||||
target_include_directories(iiwa_spline_limits PRIVATE include)
|
||||
ament_target_dependencies(iiwa_spline_limits rclcpp moveit_core moveit_ros_planning pluginlib sensor_msgs)
|
||||
pluginlib_export_plugin_description_file(moveit_core planning_response_plugins.xml)
|
||||
install(TARGETS iiwa_spline_limits LIBRARY DESTINATION lib)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
enable_testing()
|
||||
add_executable(test_spline_limits test/test_spline_limits.cpp)
|
||||
target_compile_features(test_spline_limits PRIVATE cxx_std_17)
|
||||
target_compile_options(test_spline_limits PRIVATE -UNDEBUG)
|
||||
target_include_directories(test_spline_limits PRIVATE include)
|
||||
add_test(NAME spline_limits COMMAND test_spline_limits)
|
||||
find_package(joint_trajectory_controller REQUIRED)
|
||||
add_executable(test_jtc_interpolation test/test_jtc_interpolation.cpp)
|
||||
target_compile_features(test_jtc_interpolation PRIVATE cxx_std_17)
|
||||
target_compile_options(test_jtc_interpolation PRIVATE -UNDEBUG)
|
||||
target_include_directories(test_jtc_interpolation PRIVATE include)
|
||||
ament_target_dependencies(test_jtc_interpolation joint_trajectory_controller)
|
||||
add_test(NAME jtc_interpolation COMMAND test_jtc_interpolation)
|
||||
add_executable(test_response_adapter test/test_response_adapter.cpp)
|
||||
target_compile_options(test_response_adapter PRIVATE -UNDEBUG)
|
||||
ament_target_dependencies(test_response_adapter moveit_core moveit_ros_planning pluginlib)
|
||||
add_test(NAME response_adapter COMMAND test_response_adapter)
|
||||
add_executable(test_start_state_monitor test/test_start_state_monitor.cpp)
|
||||
target_compile_options(test_start_state_monitor PRIVATE -UNDEBUG)
|
||||
target_include_directories(test_start_state_monitor PRIVATE include)
|
||||
ament_target_dependencies(test_start_state_monitor rclcpp sensor_msgs)
|
||||
add_test(NAME start_state_monitor COMMAND test_start_state_monitor)
|
||||
set_tests_properties(start_state_monitor PROPERTIES ENVIRONMENT "ROS_DOMAIN_ID=231;ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST" TIMEOUT 15)
|
||||
add_test(NAME direct_trajectory COMMAND ${Python3_EXECUTABLE} -m pytest -q ${CMAKE_CURRENT_SOURCE_DIR}/test/test_prepare_trajectory.py)
|
||||
set_tests_properties(direct_trajectory PROPERTIES ENVIRONMENT "PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_CURRENT_SOURCE_DIR}:$ENV{PYTHONPATH}")
|
||||
endif()
|
||||
|
||||
add_executable(motion_planning_cpp src/motion_planning_cpp.cpp)
|
||||
target_include_directories(motion_planning_cpp PUBLIC include)
|
||||
|
||||
@@ -0,0 +1,26 @@
|
||||
"""Prepare position-only waypoints for the JTC quintic interpolator, off the RT thread."""
|
||||
import numpy as np
|
||||
from scipy.interpolate import make_interp_spline
|
||||
from _iiwa_spline_limits import retime
|
||||
|
||||
|
||||
def prepare_trajectory(times, positions, limits):
|
||||
"""Preserve waypoints, fit a C2+ rest-to-rest curve, bound its complete interpolation.
|
||||
|
||||
`times` starts at zero. Limits contain [lower, upper, velocity, acceleration, jerk].
|
||||
Returns (times, positions, velocities, accelerations, uniform_time_scale).
|
||||
Timing may increase; intermediate path can differ from position-only linear interpolation.
|
||||
No collision checking is performed by this direct-command API.
|
||||
"""
|
||||
t, q = np.asarray(times, dtype=float), np.asarray(positions, dtype=float)
|
||||
if t.ndim != 1 or q.ndim != 2 or len(t) < 2 or len(t) != len(q):
|
||||
raise ValueError('At least two complete trajectory points are required')
|
||||
if not np.isfinite(t).all() or not np.isfinite(q).all() or t[0] != 0 or np.any(np.diff(t) < 1e-6):
|
||||
raise ValueError('Finite waypoints and strictly increasing times starting at zero are required')
|
||||
if q.shape[1] != len(limits):
|
||||
raise ValueError('Joint count does not match limits')
|
||||
zero = np.zeros(q.shape[1])
|
||||
# With two derivative constraints per endpoint, knots coincide with all input times.
|
||||
# Thus JTC quintics reproduce these spline segments from their endpoint p/v/a.
|
||||
spline = make_interp_spline(t, q, k=5, bc_type=([(1, zero), (2, zero)], [(1, zero), (2, zero)]))
|
||||
return retime(t.tolist(), q.tolist(), spline(t, 1).tolist(), spline(t, 2).tolist(), limits)
|
||||
@@ -0,0 +1,147 @@
|
||||
#pragma once
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <stdexcept>
|
||||
#include <vector>
|
||||
|
||||
namespace iiwa_planning {
|
||||
struct Limit {
|
||||
double lower, upper, velocity, acceleration, jerk;
|
||||
};
|
||||
struct Sample {
|
||||
double time;
|
||||
std::vector<double> position, velocity, acceleration;
|
||||
};
|
||||
// Normalized time u=t/T; exactly the quintic used for JTC
|
||||
// position/velocity/acceleration points.
|
||||
inline std::vector<double> quintic(double p0, double p1, double v0, double v1,
|
||||
double a0, double a1, double t) {
|
||||
const double d = p1 - p0, v = v0 * t, w = v1 * t, a = a0 * t * t,
|
||||
b = a1 * t * t;
|
||||
return {p0,
|
||||
v,
|
||||
a / 2,
|
||||
10 * d - 6 * v - 4 * w - 1.5 * a + .5 * b,
|
||||
-15 * d + 8 * v + 7 * w + 1.5 * a - b,
|
||||
6 * d - 3 * v - 3 * w - .5 * a + .5 * b};
|
||||
}
|
||||
inline double choose(unsigned n, unsigned k) {
|
||||
double x = 1;
|
||||
for (unsigned i = 1; i <= k; ++i)
|
||||
x = x * (n + 1 - i) / i;
|
||||
return x;
|
||||
}
|
||||
// Convex hull bound in Bernstein basis, tightened by de Casteljau subdivision.
|
||||
// Unlike sampling, it also bounds extrema between evaluation points.
|
||||
inline std::pair<double, double> hull(const std::vector<double> &b,
|
||||
unsigned depth) {
|
||||
if (!depth)
|
||||
return {*std::min_element(b.begin(), b.end()),
|
||||
*std::max_element(b.begin(), b.end())};
|
||||
auto v = b;
|
||||
std::vector<double> left(b.size()), right(b.size());
|
||||
left[0] = v[0];
|
||||
right.back() = v.back();
|
||||
for (size_t k = 1; k < b.size(); ++k) {
|
||||
for (size_t i = 0; i < b.size() - k; ++i)
|
||||
v[i] = (v[i] + v[i + 1]) / 2;
|
||||
left[k] = v[0];
|
||||
right[b.size() - 1 - k] = v[b.size() - 1 - k];
|
||||
}
|
||||
const auto l = hull(left, depth - 1), r = hull(right, depth - 1);
|
||||
return {std::min(l.first, r.first), std::max(l.second, r.second)};
|
||||
}
|
||||
inline std::pair<double, double> bounds(const std::vector<double> &power) {
|
||||
std::vector<double> b(power.size(), 0);
|
||||
const unsigned n = power.size() - 1;
|
||||
for (unsigned i = 0; i <= n; ++i)
|
||||
for (unsigned k = 0; k <= i; ++k)
|
||||
b[i] += power[k] * choose(i, k) / choose(n, k);
|
||||
return hull(b, 5);
|
||||
}
|
||||
inline double limit_splines(std::vector<Sample> &points,
|
||||
const std::vector<Limit> &limits) {
|
||||
if (points.size() < 2 || limits.empty())
|
||||
throw std::runtime_error("Need at least two trajectory points");
|
||||
for (const auto &l : limits) {
|
||||
for (double x : {l.lower, l.upper, l.velocity, l.acceleration, l.jerk})
|
||||
if (!std::isfinite(x))
|
||||
throw std::runtime_error("Non-finite joint limit");
|
||||
if (l.lower >= l.upper || l.velocity <= 0 || l.acceleration <= 0 ||
|
||||
l.jerk <= 0)
|
||||
throw std::runtime_error("Invalid joint limits");
|
||||
}
|
||||
for (const auto &p : points) {
|
||||
if (!std::isfinite(p.time) || p.time < 0 || p.time > 2147483647.0)
|
||||
throw std::runtime_error("Invalid trajectory time");
|
||||
for (const auto *values : {&p.position, &p.velocity, &p.acceleration}) {
|
||||
if (values->size() != limits.size())
|
||||
throw std::runtime_error("Incomplete trajectory derivatives");
|
||||
for (double x : *values)
|
||||
if (!std::isfinite(x))
|
||||
throw std::runtime_error("Non-finite trajectory");
|
||||
}
|
||||
}
|
||||
if (std::abs(points.front().time) > 1e-9)
|
||||
throw std::runtime_error("Trajectory must start at t=0");
|
||||
for (const auto *p : {&points.front(), &points.back()}) {
|
||||
for (double v : p->velocity)
|
||||
if (std::abs(v) > 1e-9)
|
||||
throw std::runtime_error(
|
||||
"Only stationary trajectory boundaries are supported");
|
||||
for (double a : p->acceleration)
|
||||
if (std::abs(a) > 1e-9)
|
||||
throw std::runtime_error(
|
||||
"Boundary acceleration must be zero for bounded jerk");
|
||||
}
|
||||
double scale = 1;
|
||||
for (size_t i = 1; i < points.size(); ++i) {
|
||||
const auto &p = points[i - 1];
|
||||
const auto &q = points[i];
|
||||
const double dt = q.time - p.time;
|
||||
if (dt < 1e-6)
|
||||
throw std::runtime_error(
|
||||
"Trajectory times must strictly increase (>= 1 us)");
|
||||
for (size_t j = 0; j < limits.size(); ++j) {
|
||||
auto c = quintic(p.position[j], q.position[j], p.velocity[j],
|
||||
q.velocity[j], p.acceleration[j], q.acceleration[j], dt);
|
||||
auto position = bounds(c);
|
||||
const auto &l = limits[j];
|
||||
if (position.first < l.lower - 1e-10 || position.second > l.upper + 1e-10)
|
||||
throw std::runtime_error(
|
||||
"Interpolated position exceeds joint limit at joint index " +
|
||||
std::to_string(j));
|
||||
for (unsigned order = 1; order <= 3; ++order) {
|
||||
std::vector<double> derivative(c.size() - 1);
|
||||
for (size_t k = 1; k < c.size(); ++k)
|
||||
derivative[k - 1] = k * c[k] / dt;
|
||||
c = derivative;
|
||||
const auto b = bounds(c);
|
||||
const double peak = std::max(std::abs(b.first), std::abs(b.second));
|
||||
const double cap =
|
||||
order == 1 ? l.velocity : (order == 2 ? l.acceleration : l.jerk);
|
||||
const double ratio = peak / cap;
|
||||
if (!std::isfinite(ratio))
|
||||
throw std::runtime_error("Unrepresentable trajectory derivatives");
|
||||
scale = std::max(scale, std::pow(ratio, 1.0 / order));
|
||||
}
|
||||
}
|
||||
}
|
||||
if (scale > 1.0 + 1e-9) {
|
||||
scale *= 1.000001; // Margin for conversion of timestamps to nanoseconds.
|
||||
for (auto &p : points) {
|
||||
p.time *= scale;
|
||||
if (!std::isfinite(p.time) || p.time > 2147483647.0)
|
||||
throw std::runtime_error(
|
||||
"Trajectory duration cannot be represented by ROS Duration");
|
||||
for (double &v : p.velocity)
|
||||
v /= scale;
|
||||
for (double &a : p.acceleration)
|
||||
a /= scale * scale;
|
||||
}
|
||||
} else
|
||||
scale = 1;
|
||||
return scale;
|
||||
}
|
||||
} // namespace iiwa_planning
|
||||
@@ -0,0 +1,78 @@
|
||||
#pragma once
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <mutex>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/joint_state.hpp>
|
||||
#include <vector>
|
||||
|
||||
namespace iiwa_planning {
|
||||
// MoveIt's CurrentStateMonitor may omit velocities (copy_dynamics=false).
|
||||
// Observe the original telemetry instead of treating missing dynamics as zero.
|
||||
class StartStateMonitor {
|
||||
public:
|
||||
void initialize(const rclcpp::Node::SharedPtr &node) {
|
||||
node_ = node;
|
||||
for (const auto &item : std::vector<std::pair<std::string, double>>{
|
||||
{"iiwa_dynamics.stopped_velocity_tolerance", .01},
|
||||
{"iiwa_dynamics.state_timeout", .5}})
|
||||
if (!node->has_parameter(item.first))
|
||||
node->declare_parameter(item.first, item.second);
|
||||
tolerance_ = node->get_parameter("iiwa_dynamics.stopped_velocity_tolerance")
|
||||
.as_double();
|
||||
timeout_ = node->get_parameter("iiwa_dynamics.state_timeout").as_double();
|
||||
if (!std::isfinite(tolerance_) || tolerance_ <= 0 ||
|
||||
!std::isfinite(timeout_) || timeout_ <= 0)
|
||||
throw std::runtime_error("Invalid start-state monitoring parameters");
|
||||
subscription_ = node->create_subscription<sensor_msgs::msg::JointState>(
|
||||
"/joint_states", rclcpp::SensorDataQoS(),
|
||||
[this](sensor_msgs::msg::JointState::ConstSharedPtr msg) {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
latest_ = msg;
|
||||
received_ = std::chrono::steady_clock::now();
|
||||
});
|
||||
}
|
||||
bool stationary(const std::vector<std::string> &names,
|
||||
const sensor_msgs::msg::JointState &requested) const {
|
||||
// Explicit nonzero requested initial dynamics are never silently discarded.
|
||||
if (!requested.velocity.empty() && !velocities_ok(names, requested))
|
||||
return false;
|
||||
auto node = node_.lock();
|
||||
if (!node)
|
||||
return velocities_ok(names,
|
||||
requested); // offline use requires explicit dynamics
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (!latest_ || std::chrono::duration<double>(
|
||||
std::chrono::steady_clock::now() - received_)
|
||||
.count() > timeout_)
|
||||
return false;
|
||||
const double age =
|
||||
node->now().seconds() -
|
||||
(latest_->header.stamp.sec + latest_->header.stamp.nanosec * 1e-9);
|
||||
return std::isfinite(age) && age >= 0 && age <= timeout_ &&
|
||||
velocities_ok(names, *latest_);
|
||||
}
|
||||
|
||||
private:
|
||||
bool velocities_ok(const std::vector<std::string> &names,
|
||||
const sensor_msgs::msg::JointState &msg) const {
|
||||
for (const auto &name : names) {
|
||||
auto found = std::find(msg.name.begin(), msg.name.end(), name);
|
||||
if (found == msg.name.end())
|
||||
return false;
|
||||
const size_t i = found - msg.name.begin();
|
||||
if (i >= msg.velocity.size() || !std::isfinite(msg.velocity[i]) ||
|
||||
std::abs(msg.velocity[i]) > tolerance_)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
std::weak_ptr<rclcpp::Node> node_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr subscription_;
|
||||
mutable std::mutex mutex_;
|
||||
sensor_msgs::msg::JointState::ConstSharedPtr latest_;
|
||||
std::chrono::steady_clock::time_point received_;
|
||||
double tolerance_{.01}, timeout_{.5};
|
||||
};
|
||||
} // namespace iiwa_planning
|
||||
@@ -32,7 +32,13 @@
|
||||
<depend>action_msgs</depend>
|
||||
<depend>rcl_interfaces</depend>
|
||||
<depend>tf2_eigen</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<build_depend>pybind11-dev</build_depend>
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
|
||||
<test_depend>joint_trajectory_controller</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
<test_depend>ament_cmake_pytest</test_depend>
|
||||
|
||||
@@ -0,0 +1,8 @@
|
||||
<library path="iiwa_spline_limits">
|
||||
<class name="iiwa_planning/LimitSplineDynamics" type="iiwa_planning::LimitSplineDynamics" base_class_type="planning_interface::PlanningResponseAdapter">
|
||||
<description>Bound the complete JTC quintic interpolation, stretching time without changing its joint-space curve.</description>
|
||||
</class>
|
||||
<class name="iiwa_planning/SetRestBoundary" type="iiwa_planning::SetRestBoundary" base_class_type="planning_interface::PlanningResponseAdapter">
|
||||
<description>Zero stationary endpoint acceleration before Ruckig; reject moving endpoints.</description>
|
||||
</class>
|
||||
</library>
|
||||
@@ -1,42 +1,112 @@
|
||||
{
|
||||
"home": {
|
||||
"joints": [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0],
|
||||
"speed": 0.1
|
||||
"speed": 0.5
|
||||
},
|
||||
"waypoints": [
|
||||
{
|
||||
"x": 0.6,
|
||||
"y": 0.1,
|
||||
"z": 0.55,
|
||||
"a": 3.14,
|
||||
"b": 0.3142,
|
||||
"c": 2.7916,
|
||||
"speed": 0.4,
|
||||
"planner": "ptp"
|
||||
},
|
||||
{
|
||||
"x": 0.6,
|
||||
"y": -0.1,
|
||||
"z": 0.55,
|
||||
"a": 3.14,
|
||||
"b": 0.3142,
|
||||
"c": 2.7916,
|
||||
"speed": 0.3,
|
||||
"planner": "lin"
|
||||
},
|
||||
{
|
||||
"joints": [0.5, 0.3, 0.0, -1.2, 0.0, 1.4, 0.0],
|
||||
"speed": 0.2
|
||||
},
|
||||
{
|
||||
"x": 0.58,
|
||||
"y": 0.0,
|
||||
"z": 0.5,
|
||||
"a": 3.14,
|
||||
"b": 0.3665,
|
||||
"c": 2.8791,
|
||||
"speed": 0.3,
|
||||
"planner": "lin"
|
||||
}
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"}
|
||||
]
|
||||
}
|
||||
|
||||
@@ -0,0 +1,140 @@
|
||||
#include "iiwa_planning/spline_limits.hpp"
|
||||
#include "iiwa_planning/start_state_monitor.hpp"
|
||||
#include <moveit/planning_interface/planning_response_adapter.hpp>
|
||||
#include <moveit/planning_scene/planning_scene.hpp>
|
||||
#include <moveit/robot_trajectory/robot_trajectory.hpp>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
namespace iiwa_planning {
|
||||
// TOTG can supply nonzero endpoint acceleration. Establish rest boundary
|
||||
// conditions before Ruckig, which otherwise preserves them. Positions and
|
||||
// interior derivatives stay intact.
|
||||
class SetRestBoundary : public planning_interface::PlanningResponseAdapter {
|
||||
public:
|
||||
void initialize(const rclcpp::Node::SharedPtr &node,
|
||||
const std::string &) override {
|
||||
start_monitor_.initialize(node);
|
||||
}
|
||||
std::string getDescription() const override {
|
||||
return "Establish intended rest boundaries around Ruckig";
|
||||
}
|
||||
void adapt(const planning_scene::PlanningSceneConstPtr &,
|
||||
const planning_interface::MotionPlanRequest &req,
|
||||
planning_interface::MotionPlanResponse &res) const override {
|
||||
if (!res.trajectory || res.trajectory->getWayPointCount() < 2 ||
|
||||
!res.trajectory->getGroup()) {
|
||||
res.error_code = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
|
||||
return;
|
||||
}
|
||||
if (!start_monitor_.stationary(
|
||||
res.trajectory->getGroup()->getVariableNames(),
|
||||
req.start_state.joint_state)) {
|
||||
RCLCPP_ERROR(
|
||||
rclcpp::get_logger("iiwa_spline_limits"),
|
||||
"Fresh, stationary joint_states required for rest-to-rest planning");
|
||||
res.error_code = moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE;
|
||||
return;
|
||||
}
|
||||
for (const auto index :
|
||||
{size_t(0), res.trajectory->getWayPointCount() - 1}) {
|
||||
auto &state = *res.trajectory->getWayPointPtr(index);
|
||||
for (const auto &name : res.trajectory->getGroup()->getVariableNames()) {
|
||||
state.setVariableVelocity(name, 0.0);
|
||||
state.setVariableAcceleration(name, 0.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
StartStateMonitor start_monitor_;
|
||||
};
|
||||
class LimitSplineDynamics : public planning_interface::PlanningResponseAdapter {
|
||||
public:
|
||||
void initialize(const rclcpp::Node::SharedPtr &node,
|
||||
const std::string &) override {
|
||||
start_monitor_.initialize(node);
|
||||
}
|
||||
std::string getDescription() const override {
|
||||
return "Limit JTC quintic velocity, acceleration and jerk";
|
||||
}
|
||||
void adapt(const planning_scene::PlanningSceneConstPtr &,
|
||||
const planning_interface::MotionPlanRequest &req,
|
||||
planning_interface::MotionPlanResponse &res) const override {
|
||||
if (!res.trajectory || res.trajectory->getWayPointCount() < 2) {
|
||||
res.error_code = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
|
||||
return;
|
||||
}
|
||||
try {
|
||||
auto &trajectory = *res.trajectory;
|
||||
const auto *group = trajectory.getGroup();
|
||||
if (!group)
|
||||
throw std::runtime_error("Missing trajectory group");
|
||||
const auto &names = group->getVariableNames();
|
||||
if (!start_monitor_.stationary(names, req.start_state.joint_state))
|
||||
throw std::runtime_error(
|
||||
"Fresh stationary joint_states required before spline validation");
|
||||
const auto model = trajectory.getRobotModel();
|
||||
auto scaling = [](double x) {
|
||||
return std::isfinite(x) && x > 0 && x <= 1 ? x : 1.0;
|
||||
};
|
||||
std::vector<Limit> limits;
|
||||
for (const auto &name : names) {
|
||||
const auto &b = model->getVariableBounds(name);
|
||||
if (!b.position_bounded_ || !b.velocity_bounded_ ||
|
||||
!b.acceleration_bounded_ || !b.jerk_bounded_)
|
||||
throw std::runtime_error(
|
||||
"Missing position/velocity/acceleration/jerk bounds: " + name);
|
||||
limits.push_back(
|
||||
{b.min_position_, b.max_position_,
|
||||
b.max_velocity_ * scaling(req.max_velocity_scaling_factor),
|
||||
b.max_acceleration_ * scaling(req.max_acceleration_scaling_factor),
|
||||
b.max_jerk_});
|
||||
}
|
||||
std::vector<Sample> samples;
|
||||
double t = 0;
|
||||
for (size_t i = 0; i < trajectory.getWayPointCount(); ++i) {
|
||||
const auto &state = *trajectory.getWayPointPtr(i);
|
||||
if (!state.hasVelocities() || !state.hasAccelerations())
|
||||
throw std::runtime_error(
|
||||
"JTC guard requires positions, velocities and accelerations");
|
||||
t += trajectory.getWayPointDurationFromPrevious(i);
|
||||
Sample s{t, {}, {}, {}};
|
||||
for (const auto &name : names) {
|
||||
s.position.push_back(state.getVariablePosition(name));
|
||||
s.velocity.push_back(state.getVariableVelocity(name));
|
||||
s.acceleration.push_back(state.getVariableAcceleration(name));
|
||||
}
|
||||
samples.push_back(s);
|
||||
}
|
||||
const double scale = limit_splines(samples, limits);
|
||||
if (scale > 1) {
|
||||
for (size_t i = 0; i < samples.size(); ++i) {
|
||||
auto &state = *trajectory.getWayPointPtr(i);
|
||||
for (size_t j = 0; j < names.size(); ++j) {
|
||||
state.setVariableVelocity(names[j], samples[i].velocity[j]);
|
||||
state.setVariableAcceleration(names[j], samples[i].acceleration[j]);
|
||||
}
|
||||
trajectory.setWayPointDurationFromPrevious(
|
||||
i, trajectory.getWayPointDurationFromPrevious(i) * scale);
|
||||
}
|
||||
RCLCPP_INFO(
|
||||
rclcpp::get_logger("iiwa_spline_limits"),
|
||||
"Trajectory duration scaled by %.6f to bound JTC derivatives",
|
||||
scale);
|
||||
}
|
||||
} catch (const std::exception &e) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger("iiwa_spline_limits"),
|
||||
"Rejected trajectory: %s", e.what());
|
||||
res.error_code = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
StartStateMonitor start_monitor_;
|
||||
};
|
||||
} // namespace iiwa_planning
|
||||
PLUGINLIB_EXPORT_CLASS(iiwa_planning::LimitSplineDynamics,
|
||||
planning_interface::PlanningResponseAdapter)
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(iiwa_planning::SetRestBoundary,
|
||||
planning_interface::PlanningResponseAdapter)
|
||||
@@ -0,0 +1,30 @@
|
||||
#include "iiwa_planning/spline_limits.hpp"
|
||||
#include <pybind11/pybind11.h>
|
||||
#include <pybind11/stl.h>
|
||||
namespace py = pybind11;
|
||||
PYBIND11_MODULE(_iiwa_spline_limits, m) {
|
||||
m.def("retime", [](const std::vector<double> &t,
|
||||
const std::vector<std::vector<double>> &q,
|
||||
const std::vector<std::vector<double>> &v,
|
||||
const std::vector<std::vector<double>> &a,
|
||||
const std::vector<std::array<double, 5>> &caps) {
|
||||
if (q.size() != t.size() || v.size() != t.size() || a.size() != t.size())
|
||||
throw std::runtime_error("Point array sizes differ");
|
||||
std::vector<iiwa_planning::Sample> points;
|
||||
for (size_t i = 0; i < t.size(); ++i)
|
||||
points.push_back({t[i], q[i], v[i], a[i]});
|
||||
std::vector<iiwa_planning::Limit> limits;
|
||||
for (const auto &c : caps)
|
||||
limits.push_back({c[0], c[1], c[2], c[3], c[4]});
|
||||
const double scale = iiwa_planning::limit_splines(points, limits);
|
||||
std::vector<double> times;
|
||||
std::vector<std::vector<double>> positions, velocities, accelerations;
|
||||
for (const auto &p : points) {
|
||||
times.push_back(p.time);
|
||||
positions.push_back(p.position);
|
||||
velocities.push_back(p.velocity);
|
||||
accelerations.push_back(p.acceleration);
|
||||
}
|
||||
return py::make_tuple(times, positions, velocities, accelerations, scale);
|
||||
});
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
#include "iiwa_planning/spline_limits.hpp"
|
||||
#include <cassert>
|
||||
#include <iostream>
|
||||
#include <joint_trajectory_controller/trajectory.hpp>
|
||||
|
||||
int main() {
|
||||
using namespace iiwa_planning;
|
||||
std::vector<Sample> samples{{0, {0}, {0}, {0}}, {.2, {1}, {0}, {0}}};
|
||||
std::vector<Limit> limits{{-2, 2, 1, 1, 2}};
|
||||
const auto scale = limit_splines(samples, limits);
|
||||
assert(scale > 1);
|
||||
joint_trajectory_controller::Trajectory jtc;
|
||||
trajectory_msgs::msg::JointTrajectoryPoint a, b, out;
|
||||
a.positions = samples[0].position;
|
||||
a.velocities = samples[0].velocity;
|
||||
a.accelerations = samples[0].acceleration;
|
||||
b.positions = samples[1].position;
|
||||
b.velocities = samples[1].velocity;
|
||||
b.accelerations = samples[1].acceleration;
|
||||
const auto finish = rclcpp::Time(static_cast<int64_t>(samples[1].time * 1e9));
|
||||
const auto start = rclcpp::Time(int64_t(0));
|
||||
double prev = 0;
|
||||
const int n = 10000;
|
||||
for (int i = 0; i <= n; ++i) {
|
||||
const auto now = rclcpp::Time(finish.nanoseconds() * i / n);
|
||||
jtc.interpolate_between_points(start, a, finish, b, now, out);
|
||||
assert(std::abs(out.velocities[0]) <= 1.000001);
|
||||
assert(std::abs(out.accelerations[0]) <= 1.000001);
|
||||
if (i)
|
||||
assert(std::abs((out.accelerations[0] - prev) / (samples[1].time / n)) <=
|
||||
2.00001);
|
||||
prev = out.accelerations[0];
|
||||
}
|
||||
std::cout << "Installed JTC interpolation stays within "
|
||||
"velocity/acceleration/jerk limits\n";
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
import numpy as np
|
||||
import pytest
|
||||
from iiwa_planning.direct_trajectory import prepare_trajectory
|
||||
|
||||
LIMITS = [[-2, 2, 1, 1, 2]]
|
||||
|
||||
|
||||
def test_dense_points_are_smooth_and_retimed():
|
||||
t, q, v, a, scale = prepare_trajectory([0, .1, .2], [[0], [.5], [1]], LIMITS)
|
||||
assert scale > 1
|
||||
np.testing.assert_allclose(q, [[0], [.5], [1]])
|
||||
assert abs(v[1][0]) > 0 # do not stop at every CSV waypoint
|
||||
np.testing.assert_allclose([v[0], v[-1], a[0], a[-1]], 0, atol=1e-9)
|
||||
assert t[-1] > .2
|
||||
|
||||
|
||||
@pytest.mark.parametrize('times,positions', [([0, 0], [[0], [1]]), ([0, 1], [[0], [float('nan')]]), ([0, 1], [[0], [3]])])
|
||||
def test_bad_trajectory_is_rejected(times, positions):
|
||||
with pytest.raises((ValueError, RuntimeError)):
|
||||
prepare_trajectory(times, positions, LIMITS)
|
||||
|
||||
|
||||
def test_time_scaling_preserves_quintic_between_waypoints():
|
||||
from scipy.interpolate import BPoly
|
||||
times, positions, velocities, accelerations, scale = prepare_trajectory([0,.1,.2], [[0],[.5],[1]], LIMITS)
|
||||
curve = BPoly.from_derivatives(times, [[q,v,a] for q,v,a in zip(positions,velocities,accelerations)])
|
||||
grid = np.linspace(times[0],times[-1],10001)
|
||||
for order,cap in [(1,1),(2,1),(3,2)]:
|
||||
assert np.max(np.abs(curve(grid,nu=order))) <= cap * 1.000001
|
||||
@@ -0,0 +1,65 @@
|
||||
#include <cassert>
|
||||
#include <iostream>
|
||||
#include <moveit/planning_interface/planning_response_adapter.hpp>
|
||||
#include <moveit/robot_model/robot_model.hpp>
|
||||
#include <moveit/robot_trajectory/robot_trajectory.hpp>
|
||||
#include <moveit/trajectory_processing/ruckig_traj_smoothing.hpp>
|
||||
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.hpp>
|
||||
#include <pluginlib/class_loader.hpp>
|
||||
#include <srdfdom/model.h>
|
||||
#include <urdf_parser/urdf_parser.h>
|
||||
|
||||
int main() {
|
||||
auto urdf = urdf::parseURDF(
|
||||
R"(<robot name="test"><link name="base"/><link name="tip"/><joint name="joint1" type="revolute"><parent link="base"/><child link="tip"/><axis xyz="0 0 1"/><limit lower="-2" upper="2" velocity="1" effort="1"/></joint></robot>)");
|
||||
auto srdf = std::make_shared<srdf::Model>();
|
||||
assert(srdf->initString(
|
||||
*urdf,
|
||||
R"(<robot name="test"><group name="arm"><joint name="joint1"/></group></robot>)"));
|
||||
auto model = std::make_shared<moveit::core::RobotModel>(urdf, srdf);
|
||||
auto bounds = model->getVariableBounds("joint1");
|
||||
bounds.acceleration_bounded_ = bounds.jerk_bounded_ = true;
|
||||
bounds.min_acceleration_ = -1;
|
||||
bounds.max_acceleration_ = 1;
|
||||
bounds.min_jerk_ = -2;
|
||||
bounds.max_jerk_ = 2;
|
||||
model->getJointModel("joint1")->setVariableBounds("joint1", bounds);
|
||||
auto trajectory =
|
||||
std::make_shared<robot_trajectory::RobotTrajectory>(model, "arm");
|
||||
moveit::core::RobotState state(model);
|
||||
state.setToDefaultValues();
|
||||
state.setVariableVelocity("joint1", 0);
|
||||
state.setVariableAcceleration("joint1", 0);
|
||||
trajectory->addSuffixWayPoint(state, 0);
|
||||
state.setVariablePosition("joint1", 1);
|
||||
trajectory->addSuffixWayPoint(state, .2);
|
||||
assert(trajectory_processing::TimeOptimalTrajectoryGeneration()
|
||||
.computeTimeStamps(*trajectory));
|
||||
pluginlib::ClassLoader<planning_interface::PlanningResponseAdapter> loader(
|
||||
"moveit_core", "planning_interface::PlanningResponseAdapter");
|
||||
auto guard = loader.createSharedInstance("iiwa_planning/LimitSplineDynamics");
|
||||
planning_interface::MotionPlanRequest req;
|
||||
req.group_name = "arm";
|
||||
req.max_velocity_scaling_factor = req.max_acceleration_scaling_factor = 1;
|
||||
planning_interface::MotionPlanResponse res;
|
||||
res.trajectory = trajectory;
|
||||
res.error_code = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
|
||||
auto boundary = loader.createSharedInstance("iiwa_planning/SetRestBoundary");
|
||||
req.start_state.joint_state.name = {"joint1"};
|
||||
req.start_state.joint_state.position = {0};
|
||||
req.start_state.joint_state.velocity = {0};
|
||||
boundary->adapt(nullptr, req, res);
|
||||
assert(bool(res));
|
||||
assert(trajectory_processing::RuckigSmoothing::applySmoothing(*trajectory));
|
||||
boundary->adapt(nullptr, req, res);
|
||||
assert(bool(res));
|
||||
guard->adapt(nullptr, req, res);
|
||||
assert(bool(res));
|
||||
assert(trajectory->getDuration() > .2);
|
||||
state.setVariablePosition("joint1", 3);
|
||||
trajectory->addSuffixWayPoint(state, 1);
|
||||
guard->adapt(nullptr, req, res);
|
||||
assert(!bool(res));
|
||||
std::cout << "Installed Ruckig + pluginlib response adapter: success and "
|
||||
"rejection OK\n";
|
||||
}
|
||||
@@ -0,0 +1,58 @@
|
||||
#include "iiwa_planning/spline_limits.hpp"
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
using namespace iiwa_planning;
|
||||
int main() {
|
||||
// Rest-to-rest quintic: endpoint acceleration is zero, interior peak > 5.
|
||||
std::vector<Sample> points{{0, {0}, {0}, {0}}, {1, {1}, {0}, {0}}};
|
||||
std::vector<Limit> limits{{-2, 2, 2, 1, 2}};
|
||||
const double scale = limit_splines(points, limits);
|
||||
assert(scale > 3.0);
|
||||
assert(std::abs(points.back().position[0] - 1) < 1e-12);
|
||||
assert(limit_splines(points, limits) <= 1.000001);
|
||||
// Consistent time scaling keeps the geometric quintic unchanged.
|
||||
auto p = quintic(0, 1, 0, 0, 0, 0, 1);
|
||||
auto q = quintic(0, 1, 0, 0, 0, 0, points.back().time);
|
||||
for (int i = 0; i <= 5; ++i)
|
||||
assert(std::abs(p[i] - q[i]) < 1e-10);
|
||||
bool bad = false;
|
||||
try {
|
||||
std::vector<Sample> s{{0, {0}, {0}, {0}}, {0, {1}, {0}, {0}}};
|
||||
limit_splines(s, limits);
|
||||
} catch (const std::exception &) {
|
||||
bad = true;
|
||||
}
|
||||
assert(bad);
|
||||
bad = false;
|
||||
// Positions are in range but large endpoint velocities overshoot between
|
||||
// them.
|
||||
try {
|
||||
std::vector<Sample> s{{0, {0}, {0}, {0}},
|
||||
{1, {0}, {12}, {0}},
|
||||
{2, {0}, {-12}, {0}},
|
||||
{3, {0}, {0}, {0}}};
|
||||
limit_splines(s, limits);
|
||||
} catch (const std::exception &) {
|
||||
bad = true;
|
||||
}
|
||||
assert(bad);
|
||||
bad = false;
|
||||
try {
|
||||
std::vector<Sample> s{{0, {NAN}, {0}, {0}}, {1, {0}, {0}, {0}}};
|
||||
limit_splines(s, limits);
|
||||
} catch (const std::exception &) {
|
||||
bad = true;
|
||||
}
|
||||
assert(bad);
|
||||
// Non-rest boundaries must not slip past the finite-segment jerk check.
|
||||
bad = false;
|
||||
try {
|
||||
std::vector<Sample> s{{0, {0}, {0}, {1}}, {1, {1}, {0}, {0}}};
|
||||
limit_splines(s, limits);
|
||||
} catch (const std::exception &) {
|
||||
bad = true;
|
||||
}
|
||||
assert(bad);
|
||||
std::cout << "spline limits OK\n";
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
#include "iiwa_planning/start_state_monitor.hpp"
|
||||
#include <cassert>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
int main(int argc, char **argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<rclcpp::Node>(
|
||||
"iiwa_start_monitor_test", rclcpp::NodeOptions().enable_rosout(false));
|
||||
iiwa_planning::StartStateMonitor monitor;
|
||||
monitor.initialize(node);
|
||||
auto pub = node->create_publisher<sensor_msgs::msg::JointState>(
|
||||
"/joint_states", rclcpp::SensorDataQoS());
|
||||
sensor_msgs::msg::JointState
|
||||
requested; // Normal MoveIt request has no velocities.
|
||||
assert(!monitor.stationary({"joint1"}, requested));
|
||||
sensor_msgs::msg::JointState state;
|
||||
state.name = {"joint1"};
|
||||
state.position = {0};
|
||||
state.velocity = {0};
|
||||
auto publish = [&] {
|
||||
for (int i = 0; i < 30; ++i) {
|
||||
state.header.stamp = node->now();
|
||||
pub->publish(state);
|
||||
rclcpp::spin_some(node);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
};
|
||||
publish();
|
||||
assert(monitor.stationary({"joint1"}, requested));
|
||||
state.velocity = {.1};
|
||||
publish();
|
||||
assert(!monitor.stationary({"joint1"}, requested));
|
||||
state.velocity = {0};
|
||||
publish();
|
||||
assert(monitor.stationary({"joint1"}, requested));
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(550));
|
||||
assert(!monitor.stationary({"joint1"}, requested));
|
||||
rclcpp::shutdown();
|
||||
std::cout << "Position-only MoveIt requests validated using fresh real "
|
||||
"velocity feedback\n";
|
||||
}
|
||||
Reference in New Issue
Block a user