├── .gitignore ├── config └── demo.yaml ├── external └── spline │ ├── spline.sh │ ├── spline.hpp │ └── LICENSE.txt ├── scripts ├── getPPString.m ├── splines.py ├── display_PP.m~ ├── display_TOPP_traj.m ├── load_moveit_traj.m ├── display_PP.m ├── simple_parameterization_original.m └── convert_joint_traj_to_pp.m ├── package.xml ├── launch └── demo.launch ├── src ├── demo │ ├── demo_joint_traj_to_pp.cpp │ ├── demo_pp_to_optimal.cpp │ ├── demo_joint_traj_to_optimal.cpp │ └── demo_traj_msg_to_optimal.cpp ├── spline_fitting.cpp └── moveit_topp.cpp ├── include └── moveit_topp │ ├── spline_fitting.h │ └── moveit_topp.h ├── README.md └── CMakeLists.txt /.gitignore: -------------------------------------------------------------------------------- 1 | data/ -------------------------------------------------------------------------------- /config/demo.yaml: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /external/spline/spline.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # 3 | cp spline.hpp /$HOME/include 4 | # 5 | g++ -c -I /$HOME/include spline.cpp 6 | if [ $? -ne 0 ]; then 7 | echo "Errors compiling spline.cpp" 8 | exit 9 | fi 10 | # 11 | mv spline.o ~/libcpp/$ARCH/spline.o 12 | # 13 | echo "Library installed as ~/libcpp/$ARCH/spline.o" 14 | -------------------------------------------------------------------------------- /scripts/getPPString.m: -------------------------------------------------------------------------------- 1 | function output = getPPString(time, waypoints) 2 | 3 | piecewise_polyn = pchip(time, waypoints); 4 | output = ''; 5 | 6 | for piece = 1:piecewise_polyn.pieces 7 | duration = piecewise_polyn.breaks(piece + 1) - piecewise_polyn.breaks(piece) 8 | output = sprintf('%s%.5f\n%i \n', output, duration, piecewise_polyn.dim); 9 | for dim = 1:piecewise_polyn.dim 10 | chunk = piecewise_polyn.coefs(piece*piecewise_polyn.dim+dim-piecewise_polyn.dim,:); 11 | output = sprintf('%s%f %f %f %f \n', output, chunk(4), chunk(3), chunk(2), chunk(1)); 12 | end 13 | end 14 | end -------------------------------------------------------------------------------- /scripts/splines.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from scipy.interpolate import PPoly, splrep, splev 4 | 5 | # Data points 6 | x = [0, 1, 2, 3, 4, 5] 7 | y = [0, 1, 4, 0, 2, 2] 8 | 9 | # Fit B-spline. This is a bunch of cubic polinomials that blend well (C^2) 10 | # Calls fortran's FITPACK under the hood 11 | tck = splrep(x, y) 12 | # Make piecewise polynomial from the B-spline 13 | pp = PPoly.from_spline(tck) 14 | 15 | 16 | # Some plotting to show how the coefficients are stored in the PPoly. 17 | # Basically columns in pp.c are coefficients of [x^3, x^2, x, 1] 18 | # and pp.x contains the bounds for each polynomial. The complete thing 19 | # consists of evaluating the polynomial in pp.c[:, i] in the range pp.x[i:i+1] 20 | 21 | xx = np.linspace(0, max(x)) 22 | yy = splev(xx, tck) # Also evaluates derivatives by setting der=desired order 23 | 24 | lw = 20 25 | for i in range(len(pp.c.T)): 26 | plt.plot(xx, np.poly1d(pp.c[:, i])(xx-pp.x[i]), 'k', alpha=0.3, lw=lw) 27 | lw -= 2 28 | plt.ylim(-1, 5) 29 | plt.plot(xx, yy) 30 | plt.show() 31 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | moveit_topp 4 | 0.0.1 5 | Wrapper for Time-Optimal Path Parameterization 6 | 7 | Dave Coleman 8 | 9 | BSD 10 | 11 | https://github.com/davetcoleman/moveit_topp 12 | https://github.com/davetcoleman/moveit_topp/issues 13 | https://github.com/davetcoleman/moveit_topp/ 14 | 15 | Dave Coleman 16 | 17 | catkin 18 | 19 | roscpp 20 | roslint 21 | eigen 22 | cmake_modules 23 | rosparam_shortcuts 24 | moveit_msgs 25 | moveit_ros_planning 26 | 27 | roscpp 28 | roslint 29 | eigen 30 | rosparam_shortcuts 31 | moveit_ros_planning 32 | 33 | 34 | -------------------------------------------------------------------------------- /scripts/display_PP.m~: -------------------------------------------------------------------------------- 1 | % read a optimized trajectory from a text file and plot 2 | 3 | clear 4 | clc 5 | format long g 6 | clf 7 | 8 | % input 9 | num_joints = 7; 10 | filename = '/home/dave/ros/current/ws_acme/src/moveit_topp/data/spline_pp_traj.csv'; 11 | 12 | timestamp = []; 13 | 14 | % read 15 | %raw = csvread(filename,0,0); 16 | fid = fopen(filename); 17 | 18 | while ischar(line) 19 | % add time 20 | line = fgetl(fid); 21 | timestamp = [timestamp str2num(line)]; 22 | % get dimension and ignore - assume all are num_joints 23 | line = fgetl(fid); 24 | % get coefficients 25 | for i = 1:num_joints 26 | line = fgetl(fid); 27 | disp(line) 28 | end 29 | end 30 | fclose(fid); 31 | 32 | return 33 | 34 | joints = [1:1:num_joints]; 35 | timestamp = raw(:,1)'; 36 | 37 | % Create struct 38 | piecewise_polyn2.form = 'pp'; 39 | piecewise_polyn2.breaks = [timestamp 1]; 40 | piecewise_polyn2.coefs = raw(:,2:5); 41 | piecewise_polyn2.pieces = size(timestamp,2); 42 | piecewise_polyn2.order = 4; 43 | piecewise_polyn2.dim = 1; 44 | 45 | % Interpolate 46 | discretization = 0.01; 47 | time = [0:discretization:size(timestamp,2)]; 48 | pos = ppval(piecewise_polyn2, time); 49 | 50 | % Plot 51 | plot(time, pos, '--','DisplayName','Position'); -------------------------------------------------------------------------------- /scripts/display_TOPP_traj.m: -------------------------------------------------------------------------------- 1 | % read a optimized trajectory from a text file and plot 2 | 3 | clear 4 | clc 5 | format long g 6 | clf 7 | 8 | % settings 9 | show_pos = 1; 10 | show_vel = 0; 11 | 12 | % input 13 | num_joints = 7; 14 | filename = '/home/dave/ros/current/ws_acme/src/moveit_topp/data/topp_optimized_traj.csv'; 15 | 16 | % read 17 | traj_data = load_moveit_traj(filename, num_joints); 18 | 19 | hold on; 20 | if show_pos 21 | h = plot(traj_data.timestamp, traj_data.pos); 22 | set(h,{'DisplayName'},{'PP Fitted Pos 1';'PP Fitted Pos 2';'PP Fitted Pos 3';'PP Fitted Pos 4';'PP Fitted Pos 5';'PP Fitted Pos 6';'PP Fitted Pos 7'}); 23 | end 24 | if show_vel 25 | h = plot(traj_data.timestamp, traj_data.vel); 26 | set(h,{'DisplayName'},{'PP Fitted Vel 1';'PP Fitted Vel 2';'PP Fitted Vel 3';'PP Fitted Vel 4';'PP Fitted Vel 5';'PP Fitted Vel 6';'PP Fitted Vel 7'}); 27 | end 28 | %plot(traj_data.timestamp, traj_data.acc(:,1), 'DisplayName','PP Fitted Acceleration'); 29 | 30 | % Adjust size of plot 31 | plot_gca = get(gca, 'Position'); 32 | plot_gca(1) = 0.03; % x 33 | plot_gca(2) = 0.06; % y 34 | plot_gca(3) = 0.95; % percent width 35 | plot_gca(4) = 0.9; % percent height 36 | set(gca, 'Position', plot_gca) 37 | 38 | title('Joint 1') 39 | xlabel('Time') 40 | ylabel('Radians') 41 | legend('Location','best'); 42 | figure(1) 43 | -------------------------------------------------------------------------------- /scripts/load_moveit_traj.m: -------------------------------------------------------------------------------- 1 | function data = load_moveit_traj(filename, num_joints) 2 | % 3 | % This function converts a CSV file from a MoveIt! trajectory into a Matlab 4 | % struct 5 | % 6 | raw = csvread(filename,1,0); 7 | i=1; 8 | data.timestamp = raw(:,i);i=i+1; 9 | %data.timestamp = data.timestamp - data.timestamp(1); 10 | 11 | joints = [1:1:num_joints]; 12 | 13 | % Preallocate memory 14 | data.pos= zeros(size(data.timestamp,1),7); 15 | data.vel= zeros(size(data.timestamp,1),7); 16 | data.acc = zeros(size(data.timestamp,1),7); 17 | 18 | % Convert input data to better structures 19 | for j = joints 20 | data.pos(:,j)=raw(:,i);i=i+1; 21 | data.vel(:,j)=raw(:,i);i=i+1; 22 | data.acc(:,j)=raw(:,i);i=i+1; 23 | 24 | % calculate the derivatives 25 | m_t_delta=diff(data.timestamp); 26 | m_pos_delta(:,j)=diff(data.pos(:,j)); 27 | data.calc_vel(:,j)=[0; m_pos_delta(:,j) ./ m_t_delta]; % add a zero in front to make same size as other data structures 28 | m_vel_delta(:,j)=diff(data.calc_vel(:,j)); 29 | data.calc_acc(:,j)=[0; m_vel_delta(:,j) ./ m_t_delta]; % add a zero in front to make same size as other data structures 30 | m_acc_delta(:,j)=diff(data.calc_acc(:,j)); 31 | data.calc_jer(:,j)=[0; m_acc_delta(:,j) ./ m_t_delta]; % add a zero in front to make same size as other data structures 32 | end 33 | end -------------------------------------------------------------------------------- /scripts/display_PP.m: -------------------------------------------------------------------------------- 1 | % read a optimized trajectory from a text file and plot 2 | 3 | clear 4 | clc 5 | format long g 6 | clf 7 | 8 | % input 9 | num_joints = 7; 10 | filename = '/home/dave/ros/current/ws_acme/src/moveit_topp/data/spline_pp_traj.csv'; 11 | 12 | % Create struct 13 | piecewise_polyn2.form = 'pp'; 14 | piecewise_polyn2.breaks = []; 15 | piecewise_polyn2.coefs = []; 16 | piecewise_polyn2.pieces = 0; 17 | piecewise_polyn2.order = 4; 18 | piecewise_polyn2.dim = num_joints; 19 | 20 | % read 21 | %raw = csvread(filename,0,0); 22 | fid = fopen(filename); 23 | 24 | while true 25 | % add time 26 | line = fgetl(fid); 27 | if ischar(line) == false 28 | break 29 | end 30 | piecewise_polyn2.breaks = [piecewise_polyn2.breaks, str2num(line)]; 31 | % get dimension and ignore - assume all are num_joints 32 | line = fgetl(fid); 33 | % get coefficients 34 | for i = 1:num_joints 35 | line = fgetl(fid); 36 | c = strsplit(line,','); 37 | num_line = [str2num(cell2mat(c(1))), str2num(cell2mat(c(2))), str2num(cell2mat(c(3))), str2num(cell2mat(c(4)))]; 38 | piecewise_polyn2.coefs = [piecewise_polyn2.coefs; num_line]; 39 | end 40 | piecewise_polyn2.pieces = piecewise_polyn2.pieces + 1; 41 | end 42 | fclose(fid); 43 | 44 | piecewise_polyn2.breaks = [piecewise_polyn2.breaks, 1]; 45 | 46 | joints = [1:1:num_joints]; 47 | 48 | % Interpolate 49 | discretization = 0.01; 50 | time = [0:discretization:size(piecewise_polyn2.breaks,2)]; 51 | pos = ppval(piecewise_polyn2, time); 52 | 53 | % Plot 54 | h = plot(time, pos, '-'); 55 | set(h,{'DisplayName'},{'Discretized PP Pos 1';'Discretized PP Pos 2';'Discretized PP Pos 3';'Discretized PP Pos 4';'Discretized PP Pos 5';'Discretized PP Pos 6';'Discretized PP Pos 7'}); 56 | 57 | % Adjust size of plot 58 | plot_gca = get(gca, 'Position'); 59 | plot_gca(1) = 0.03; % x 60 | plot_gca(2) = 0.06; % y 61 | plot_gca(3) = 0.95; % percent width 62 | plot_gca(4) = 0.9; % percent height 63 | set(gca, 'Position', plot_gca) 64 | 65 | xlabel('Time') 66 | ylabel('Radians') 67 | legend('Location','best'); 68 | figure(1) 69 | 70 | -------------------------------------------------------------------------------- /scripts/simple_parameterization_original.m: -------------------------------------------------------------------------------- 1 | % my crappy attempt to create a smooth trajectory 2 | 3 | clear 4 | clc 5 | format long g 6 | clf 7 | 8 | % Input trajectory 9 | in_pos = [0 4 5 0 -20 -5 -2 2 3 3 0]; 10 | 11 | %in_pos = [0 1 4 5 3 0 -5 -10 -15 -19 -20 -19 -15 -10 -5 -2 2 3 3 1 0]; 12 | 13 | in_pos = [0 1 2 4 5 0 -5 -10 -12 -15 -18 -20 -18 -15 -10 -5 -2 2 3 3 1 0]; 14 | 15 | in_time = 0:1:size(in_pos,2)-1; 16 | 17 | % Constraints 18 | max_velocity = 1; 19 | max_acceleration = 1; 20 | 21 | % Guess 22 | max_time = size(in_pos,2) 23 | target_time = 5; 24 | scale_time = max_time / target_time 25 | 26 | 27 | % Show input 28 | hold on 29 | plot(in_time / scale_time, in_pos, 'o-','DisplayName','Input Command'); 30 | 31 | % ------------------------------------------------- 32 | for i = 1:1:1 33 | 34 | % Interpolate 35 | discretization = 0.01; 36 | time = [0:discretization:max_time-1]; 37 | cs = spline(in_time, [0 in_pos 0]); 38 | pos = ppval(cs, time); 39 | time_delta = zeros(size(pos,2),1); % change in time between points, starts at zero 40 | 41 | 42 | % scale down time 43 | time = time / scale_time; 44 | 45 | % Derive 46 | time_delta = diff(time); 47 | pos_delta = diff(pos); 48 | vel = [0, pos_delta ./ time_delta]; % add a zero in front to make same size as other data structures 49 | vel_delta = diff(vel); 50 | acc = [0, vel_delta ./ time_delta]; % add a zero in front to make same size as other data structures 51 | 52 | % find max 53 | [max_acc idx] = max(acc); 54 | max_acc 55 | max_acc_pos = pos(idx) 56 | plot([time(idx) time(idx)],[-30 30],'DisplayName','Max Acceleration'); 57 | 58 | [min_acc idx] = min(acc); 59 | max_acc 60 | min_acc_pos = pos(idx) 61 | plot([time(idx) time(idx)],[-30 30],'DisplayName','Min Acceleration'); 62 | 63 | % Plot 64 | plot(time, pos,'DisplayName','Position'); 65 | plot(time, vel,'DisplayName','Velocity'); 66 | plot(time, acc,'DisplayName','Acceleration'); 67 | 68 | % Adjust size of plot 69 | plot_gca = get(gca, 'Position'); 70 | plot_gca(1) = 0.03; % x 71 | plot_gca(2) = 0.06; % y 72 | plot_gca(3) = 0.95; % percent width 73 | plot_gca(4) = 0.9; % percent height 74 | set(gca, 'Position', plot_gca) 75 | 76 | xlabel('Time') 77 | ylabel('Radians') 78 | legend('Location','northwest'); 79 | figure(1) 80 | %w = waitforbuttonpress; 81 | 82 | end -------------------------------------------------------------------------------- /launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 29 | 30 | 31 | 33 | 34 | 35 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /src/demo/demo_joint_traj_to_pp.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, PickNik LLC 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of PickNik LLC nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Test for converting a discretized joint trajectory into piecewise polynomials using splines 37 | */ 38 | 39 | // ROS 40 | #include 41 | 42 | // ROS parameter loading 43 | #include 44 | 45 | // this package 46 | #include 47 | 48 | int main(int argc, char** argv) 49 | { 50 | // Initialize ROS 51 | ros::init(argc, argv, "demo_joint_traj_to_pp"); 52 | ROS_INFO_STREAM_NAMED("main", "Starting MoveItTopp demo..."); 53 | 54 | // Allow the action server to recieve and send ros messages 55 | ros::AsyncSpinner spinner(2); 56 | spinner.start(); 57 | 58 | // Initialize main class 59 | moveit_topp::SplineFitting spline_fitting; 60 | 61 | // Load pre-generated (from Matlab) piecewise polynomial from file 62 | std::size_t num_joints = 7; 63 | spline_fitting.readJointTrajFromFile("/home/dave/ros/current/ws_acme/src/moveit_topp/data/moveit_joint_traj.csv", num_joints); 64 | 65 | // Fit a spline to waypoints 66 | spline_fitting.fitSpline(); 67 | 68 | // Write coefficients to file (for use with Matlab) 69 | spline_fitting.writeCoefficientsToFile("/home/dave/ros/current/ws_acme/src/moveit_topp/data/spline_pp_traj.csv"); 70 | 71 | // Shutdown 72 | ROS_INFO_STREAM_NAMED("main", "Finished."); 73 | ros::shutdown(); 74 | 75 | return 0; 76 | } 77 | -------------------------------------------------------------------------------- /scripts/convert_joint_traj_to_pp.m: -------------------------------------------------------------------------------- 1 | % read a text file of waypoints and convert to splines 2 | 3 | clear 4 | clc 5 | format long g 6 | clf 7 | hold on 8 | 9 | % Settings 10 | use_moveit_data = 1; 11 | show_pos = 1; 12 | show_vel = 0; 13 | show_acc = 0; 14 | show_optimized = 1; 15 | 16 | % ------------------------------------------------------------------------ 17 | % Input trajectory 18 | if (use_moveit_data) 19 | num_joints = 7; 20 | %filename = '/home/dave/ros/current/ws_acme/src/moveit_topp/data/moveit_joint_traj.csv'; 21 | filename = '/home/dave/ros/current/iiwa_trajectory_data/arm_moveit_trajectory_0.csv'; 22 | moveit_data = load_moveit_traj(filename, num_joints); 23 | 24 | % scale time 25 | time_scaling_factor = 1; %0.05; 26 | moveit_data.timestamp = moveit_data.timestamp / time_scaling_factor; 27 | 28 | only_use_one_joint = false; 29 | if (only_use_one_joint) 30 | num_joints = 1; 31 | joint_id = 1; 32 | in_pos = moveit_data.pos(:,joint_id)'; 33 | in_time = moveit_data.timestamp; 34 | else 35 | in_pos = moveit_data.pos'; 36 | in_time = moveit_data.timestamp; 37 | end 38 | else 39 | in_pos = [0 0.2 0 -0.2 -0.5 0; 40 | 0.25 0.5 0 -0.5 0 0]; 41 | num_joints = 2; 42 | %in_pos = [0 3.14 1.57 0 -5 -3 -2 2 3 3 0]; 43 | in_time = 0:1:size(in_pos,2)-1; 44 | end 45 | 46 | % ------------------------------------------------------------------------ 47 | % Input PP Fitted Trajectory 48 | if (show_optimized) 49 | filename2 = '/home/dave/ros/current/ws_acme/src/moveit_topp/data/topp_optimized_traj.csv'; 50 | 51 | % read 52 | traj_data = load_moveit_traj(filename2, num_joints); 53 | 54 | % Show fitted 55 | for joint = 1:num_joints 56 | title_str = sprintf(' Joint %i',joint); 57 | if (show_pos); plot(traj_data.timestamp, traj_data.pos(:,joint), 'DisplayName',strcat('TOPP ',title_str,' Position')); end; 58 | if (show_vel); plot(traj_data.timestamp, traj_data.vel(:,joint), 'DisplayName',strcat('TOPP ',title_str,' Velocity')); end; 59 | if (show_acc); plot(traj_data.timestamp, traj_data.acc(:,joint), 'DisplayName',strcat('TOPP ',title_str,' Acceleration')); end; 60 | end 61 | end 62 | % ------------------------------------------------------------------------ 63 | 64 | % Show input 65 | if (show_pos); plot(in_time, in_pos, 'o','DisplayName','Input Command'); end; 66 | 67 | % Fit to spline 68 | piecewise_polyn = spline(in_time, in_pos); 69 | 70 | % Interpolate 71 | discretization = 0.01; 72 | time = [0:discretization:in_time(end)]; 73 | pos = ppval(piecewise_polyn, time); 74 | % vel_time = time(:,1:end-1); 75 | % vel_time=repmat(vel_time,num_joints,1); 76 | % size(vel_time) 77 | % size(diff(pos')') 78 | % vel = diff(pos')'./diff(vel_time); 79 | % vel = [vel vel(end)]; %TODO this repeats the final velocity value twice 80 | % acc = diff(vel)./diff(time); 81 | % acc = [acc acc(end)]; %TODO this repeats the final velocity value twice 82 | 83 | % Plot 84 | if (show_pos); plot(time, pos, '--', 'DisplayName','PP Pos'); end; 85 | %if (show_vel); plot(time, vel,'DisplayName','Velocity'); end; 86 | %if (show_acc); plot(time, acc,'DisplayName','Acceleration'); end; 87 | 88 | % Adjust size of plot 89 | plot_gca = get(gca, 'Position'); 90 | plot_gca(1) = 0.03; % x 91 | plot_gca(2) = 0.06; % y 92 | plot_gca(3) = 0.95; % percent width 93 | plot_gca(4) = 0.9; % percent height 94 | set(gca, 'Position', plot_gca) 95 | 96 | xlabel('Time') 97 | ylabel('Radians') 98 | legend('Location','best'); 99 | figure(1) 100 | 101 | % output as trajectory string 102 | filename = '/home/dave/ros/current/ws_acme/src/moveit_topp/data/matlab_pp_traj.csv'; 103 | fileID = fopen(filename,'w'); 104 | 105 | polyn_string = getPPString(in_time, in_pos); 106 | 107 | fprintf(fileID, polyn_string ); 108 | -------------------------------------------------------------------------------- /src/demo/demo_pp_to_optimal.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, PickNik LLC 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of PickNik LLC nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Test for time optimal path parameterization 37 | */ 38 | 39 | // ROS 40 | #include 41 | 42 | // ROS parameter loading 43 | #include 44 | 45 | // this package 46 | #include 47 | 48 | int main(int argc, char** argv) 49 | { 50 | // Initialize ROS 51 | ros::init(argc, argv, "demo_pp_to_optimal"); 52 | ROS_INFO_STREAM_NAMED("main", "Starting MoveItTopp demo..."); 53 | 54 | // Allow the action server to recieve and send ros messages 55 | ros::AsyncSpinner spinner(2); 56 | spinner.start(); 57 | 58 | // Initialize main class 59 | std::vector vel_limits = {0.7895, 0.7895, 0.973, 1.2162, 1.7143, 2.6471, 3.3962}; 60 | std::vector acc_limits = {0.7895, 0.7895, 0.973, 1.2162, 1.7143, 2.6471, 3.3962}; 61 | moveit_topp::MoveItTopp optimizer(vel_limits, acc_limits); 62 | 63 | // Load pre-generated (from Matlab) piecewise polynomial from file 64 | TOPP::Trajectory orig_trajectory; 65 | //optimizer.readPPTrajFromFile("/home/dave/ros/current/ws_acme/src/moveit_topp/data/matlab_pp_traj.csv", orig_trajectory); 66 | optimizer.readPPTrajFromFile("/home/dave/ros/current/ws_acme/src/moveit_topp/data/spline_pp_traj.csv", orig_trajectory); 67 | 68 | bool debug = false; 69 | if (debug) 70 | { 71 | // Write joint trajectory to CSV file (for use with Matlab) 72 | optimizer.writeTrajectoryToFile("/home/dave/ros/current/ws_acme/src/moveit_topp/data/topp_optimized_traj.csv", 73 | orig_trajectory); 74 | return 0; 75 | } 76 | 77 | // Time-Optimize with respect to constraints 78 | TOPP::Trajectory new_trajectory; 79 | optimizer.optimizeTrajectory(orig_trajectory, new_trajectory); 80 | 81 | // Write joint trajectory to CSV file (for use with Matlab) 82 | optimizer.writeTrajectoryToFile("/home/dave/ros/current/ws_acme/src/moveit_topp/data/topp_optimized_traj.csv", 83 | new_trajectory); 84 | 85 | // Shutdown 86 | ROS_INFO_STREAM_NAMED("main", "Finished."); 87 | ros::shutdown(); 88 | 89 | return 0; 90 | } 91 | -------------------------------------------------------------------------------- /include/moveit_topp/spline_fitting.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, PickNik LLC 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of PickNik LLC nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Wrapper for John Burkardt's spline funcitonality 37 | */ 38 | 39 | #ifndef MOVEIT_TOPP_SPLINE_FITTING_H 40 | #define MOVEIT_TOPP_SPLINE_FITTING_H 41 | 42 | // C++ 43 | #include 44 | #include 45 | #include 46 | 47 | // ROS 48 | #include 49 | 50 | // ROS parameter loading 51 | #include 52 | 53 | // Spline 54 | #include 55 | 56 | // TOPP 57 | #include 58 | 59 | namespace moveit_topp 60 | { 61 | class SplineFitting 62 | { 63 | public: 64 | /** 65 | * \brief Constructor 66 | */ 67 | SplineFitting(); 68 | 69 | void readJointTrajFromFile(const std::string& filename, std::size_t num_joints); 70 | 71 | void setJointPositions(const std::vector >& joint_positions, 72 | const std::vector& timetamps); 73 | 74 | void fitSpline(); 75 | 76 | void writeCoefficientsToFile(const std::string& file_path); 77 | 78 | /** 79 | * \brief Convert the coefficients into the TOPP trajectory format 80 | */ 81 | void getPPTrajectory(TOPP::Trajectory& trajectory); 82 | 83 | void calcSimpleDerivative(); 84 | 85 | private: 86 | // -------------------------------------------------------- 87 | 88 | // The short name of this class 89 | std::string name_ = "spline_fitting"; 90 | 91 | // A shared node handle 92 | ros::NodeHandle nh_; 93 | 94 | std::size_t num_joints_; 95 | 96 | // Read data 97 | std::vector timestamps_; 98 | std::vector> joint_positions_; 99 | std::vector> joint_velocities_; 100 | std::vector coefficients_; // SPLINE_HERMITE_SET 101 | 102 | }; // end class 103 | 104 | // Create boost pointers for this class 105 | typedef boost::shared_ptr SplineFittingPtr; 106 | typedef boost::shared_ptr SplineFittingConstPtr; 107 | 108 | } // namespace moveit_topp 109 | 110 | #endif // MOVEIT_TOPP_SPLINE_FITTING_H 111 | -------------------------------------------------------------------------------- /src/demo/demo_joint_traj_to_optimal.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, PickNik LLC 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of PickNik LLC nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Test for converting a discretized joint trajectory into piecewise polynomials using splines 37 | */ 38 | 39 | // ROS 40 | #include 41 | 42 | // ROS parameter loading 43 | #include 44 | 45 | // this package 46 | #include 47 | #include 48 | 49 | int main(int argc, char** argv) 50 | { 51 | // Initialize ROS 52 | ros::init(argc, argv, "demo_joint_traj_to_optimal"); 53 | ROS_INFO_STREAM_NAMED("main", "Starting MoveItTopp demo..."); 54 | 55 | // Allow the action server to recieve and send ros messages 56 | ros::AsyncSpinner spinner(2); 57 | spinner.start(); 58 | 59 | // Initialize interpolater 60 | moveit_topp::SplineFitting spline_fitting; 61 | 62 | // Initialize optimizer 63 | std::vector vel_limits = {0.7895, 0.7895, 0.973, 1.2162, 1.7143, 2.6471, 3.3962}; 64 | std::vector acc_limits = {0.7895, 0.7895, 0.973, 1.2162, 1.7143, 2.6471, 3.3962}; 65 | moveit_topp::MoveItTopp optimizer(vel_limits, acc_limits); 66 | 67 | // Load pre-generated (from Matlab) piecewise polynomial from file 68 | std::size_t num_joints = 7; 69 | spline_fitting.readJointTrajFromFile("/home/dave/ros/current/ws_acme/src/moveit_topp/data/moveit_joint_traj_short.csv", num_joints); 70 | 71 | // Benchmark runtime 72 | ros::Time start_time = ros::Time::now(); 73 | 74 | // Fit a spline to waypoinst 75 | spline_fitting.fitSpline(); 76 | 77 | TOPP::Trajectory orig_trajectory; 78 | spline_fitting.getPPTrajectory(orig_trajectory); 79 | 80 | bool debug = false; 81 | if (debug) 82 | { 83 | // Write PP trajectory to CSV file (for use with Matlab) 84 | ROS_WARN_STREAM_NAMED("main", "converting orig trajectory to joint space"); 85 | optimizer.writeTrajectoryToFile("/home/dave/ros/current/ws_acme/src/moveit_topp/data/topp_optimized_traj.csv", 86 | orig_trajectory); 87 | return 0; 88 | } 89 | 90 | // Time-Optimize with respect to constraints 91 | TOPP::Trajectory new_trajectory; 92 | optimizer.optimizeTrajectory(orig_trajectory, new_trajectory); 93 | 94 | // Benchmark runtime 95 | double duration = (ros::Time::now() - start_time).toSec(); 96 | ROS_INFO_STREAM_NAMED("main", "Total time: " << duration << " seconds (" << 1.0/duration << " hz)"); 97 | 98 | // Write joint trajectory to CSV file (for use with Matlab) 99 | optimizer.writeTrajectoryToFile("/home/dave/ros/current/ws_acme/src/moveit_topp/data/topp_optimized_traj.csv", 100 | new_trajectory); 101 | 102 | // Shutdown 103 | ROS_INFO_STREAM_NAMED("main", "Finished."); 104 | ros::shutdown(); 105 | 106 | return 0; 107 | } 108 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MoveIt! Time Optimal Path Parameterization 2 | 3 | Description: Time optimizes a kinematic path, say from sampling-based motion planning or IK solvers, to be as fast as possible under velocity and acceleration constraints. 4 | 5 | Features: 6 | 7 | - Converts a kinematic path into piecewise polynomials using spline fitting 8 | - Spline fitting accomplished using cubic Hermite interpolant by the algorithm CALCCF from Conte and deBoor. 9 | 10 | Developed by [Dave Coleman](http://dav.ee/) at the University of Colorado Boulder, using the [TOPP library by Quang-Cuong Pham](https://github.com/quangounet/TOPP) 11 | 12 | Status: 13 | 14 | * [![Build Status](https://travis-ci.org/davetcoleman/moveit_topp.svg)](https://travis-ci.org/davetcoleman/moveit_topp) Indigo Travis CI 15 | * [![Devel Job Status](http://jenkins.ros.org/buildStatus/icon?job=devel-indigo-moveit_topp)](http://jenkins.ros.org/job/devel-indigo-moveit_topp) Indigo Devel Job Status 16 | * [![Build Status](http://jenkins.ros.org/buildStatus/icon?job=ros-indigo-moveit-topp_binarydeb_trusty_amd64)](http://jenkins.ros.org/job/ros-indigo-moveit-topp_binarydeb_trusty_amd64/) Indigo AMD64 Debian Job Status 17 | 18 | ![](resources/screenshot.png) 19 | 20 | ## Licensing 21 | 22 | Be aware there are multiple licenses in this project: 23 | 24 | Spline Fitting - GNU LGPL v3 25 | TOPP Library - GNU LGPL v3 26 | ROS Package - BSD 3-Clause 27 | 28 | ## Install 29 | 30 | ### Ubuntu Debian 31 | 32 | Not available yet: 33 | 34 | sudo apt-get install ros-indigo-moveit-topp 35 | 36 | ### Build from Source 37 | 38 | To build this package, ``git clone`` this repo into a [catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace) and be sure to install necessary dependencies by running the following command in the root of your catkin workspace: 39 | 40 | rosdep install -y --from-paths src --ignore-src --rosdistro indigo 41 | 42 | ## Code API 43 | 44 | See [Class Reference](http://docs.ros.org/indigo/api/moveit_topp/html/) 45 | 46 | ## Usage 47 | 48 | ### Demo: Convert Piecewise Polynomial Trajectory to Time-Optimal Discretized Path 49 | 50 | - Use the Matlab script ``scripts/convert_joint_traj_to_pp.m`` to generate ``data/matlab_pp_traj.csv`` 51 | - Convert the file into discretized optimized waypoints in C++ using TOPP via: 52 | 53 | rosrun moveit_topp demo_pp_to_optimal 54 | 55 | - This generates ``data/topp_optimized_traj.csv`` 56 | - View the generated discretized waypoints: 57 | 58 | matlab scripts/display_TOPP_traj.m 59 | 60 | ### Demo: Convert Joint Trajectory Waypoints into Piecewise Polynomial Trajectory 61 | 62 | - Generate a joint-space waypoints trajectory using MoveIt! and save into ``data/matlab_joint_traj.csv`` 63 | - Convert the file into piecewise polynomials in C++ using via: 64 | 65 | rosrun moveit_topp demo_joint_traj_to_pp 66 | 67 | - This generates ``data/spline_pp_traj.csv`` 68 | - View the generated PP spline in Matlab: 69 | 70 | matlab scripts/display_PP.m 71 | 72 | ### Demo: Convert Joint Trajectory Waypoints into Time-Optimal Discretized Path 73 | 74 | - Generate a joint-space waypoints trajectory using MoveIt! and save into ``data/matlab_joint_traj.csv`` 75 | - Convert the file into discretized optimized waypoints in C++ using TOPP via: 76 | 77 | rosrun moveit_topp demo_joint_traj_to_optimal 78 | 79 | - This generates ``data/topp_optimized_traj.csv`` 80 | - View the generated discretized waypoints: 81 | 82 | matlab scripts/display_TOPP_traj.m 83 | 84 | ## Demo: Convert ROS Trajectory Message Waypoints into Time-Optimal Discretized Path 85 | 86 | UNFINISHED 87 | 88 | - Generate a joint-space waypoints trajectory using MoveIt! and save into ``data/matlab_joint_traj.csv`` 89 | - Convert the file into discretized optimized waypoints in C++ using TOPP via: 90 | 91 | rosrun moveit_topp demo_traj_msg_to_optimal 92 | 93 | - This generates ``data/topp_optimized_traj.csv`` 94 | - View the generated discretized waypoints: 95 | 96 | matlab scripts/display_TOPP_traj.m 97 | 98 | ## Testing and Linting 99 | 100 | To run [roslint](http://wiki.ros.org/roslint), use the following command with [catkin-tools](https://catkin-tools.readthedocs.org/): 101 | 102 | catkin build --no-status --no-deps --this --make-args roslint 103 | 104 | To run [catkin lint](https://pypi.python.org/pypi/catkin_lint), use the following command with [catkin-tools](https://catkin-tools.readthedocs.org/): 105 | 106 | catkin lint -W2 107 | 108 | There are currently no unit or integration tests for this package. If there were you would use the following command with [catkin-tools](https://catkin-tools.readthedocs.org/): 109 | 110 | catkin run_tests --no-deps --this -i 111 | 112 | ## Contribute 113 | 114 | Please send PRs for new helper functions, fixes, etc! 115 | -------------------------------------------------------------------------------- /include/moveit_topp/moveit_topp.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, PickNik LLC 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of PickNik LLC nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Wrapper for time opitmal path parameterization 37 | */ 38 | 39 | #ifndef MOVEIT_TOPP_MOVEIT_TOPP_H 40 | #define MOVEIT_TOPP_MOVEIT_TOPP_H 41 | 42 | // C++ 43 | #include 44 | #include 45 | #include 46 | 47 | // ROS 48 | #include 49 | 50 | // ROS parameter loading 51 | #include 52 | 53 | // TOPP 54 | #include 55 | #include 56 | #include 57 | #include 58 | 59 | // Spline 60 | #include 61 | 62 | // MoveIt 63 | #include 64 | 65 | // this package 66 | #include 67 | 68 | namespace moveit_topp 69 | { 70 | 71 | class MoveItTopp 72 | { 73 | public: 74 | 75 | /** 76 | * \brief Constructor 77 | */ 78 | MoveItTopp(ros::NodeHandle &nh, const moveit::core::JointModelGroup* jmg); 79 | 80 | /** 81 | * \brief Constructor 82 | */ 83 | MoveItTopp(const std::vector &vel_limits, const std::vector &acc_limits); 84 | 85 | void init(const std::vector &vel_limits, const std::vector &acc_limits); 86 | 87 | void computeTimeStamps(robot_trajectory::RobotTrajectory& robot_traj); 88 | 89 | void convertMoveItTrajToPP(const robot_trajectory::RobotTrajectory& robot_traj); 90 | 91 | void readPPTrajFromFile(const std::string& filename, TOPP::Trajectory &trajectory); 92 | 93 | void optimizeTrajectory(const TOPP::Trajectory &old_trajectory, TOPP::Trajectory &new_trajectory); 94 | 95 | bool writeTrajectoryToFile(const std::string &file_path, const TOPP::Trajectory &trajectory); 96 | 97 | bool convertTrajToMoveItTraj(robot_trajectory::RobotTrajectory& robot_traj, const TOPP::Trajectory &trajectory); 98 | 99 | private: 100 | 101 | // -------------------------------------------------------- 102 | 103 | // The short name of this class 104 | std::string name_ = "moveit_topp"; 105 | 106 | // A shared node handle 107 | ros::NodeHandle nh_; 108 | 109 | // TOPP vars 110 | boost::shared_ptr pconstraints_; 111 | 112 | // Group of joints to use 113 | const moveit::core::JointModelGroup* jmg_; 114 | 115 | // Initialize interpolater 116 | moveit_topp::SplineFitting spline_fitting_; 117 | 118 | // Used by convertMoveItTrajToPP() 119 | std::vector > tmp_joint_positions_; 120 | std::vector tmp_jmg_positions_; 121 | std::vector tmp_timestamps_; 122 | 123 | // Settings 124 | bool debug_write_coeff_to_file_ = false; 125 | bool debug_write_joint_traj_to_file_ = false; 126 | 127 | }; // end class 128 | 129 | // Create boost pointers for this class 130 | typedef boost::shared_ptr MoveItToppPtr; 131 | typedef boost::shared_ptr MoveItToppConstPtr; 132 | 133 | } // namespace moveit_topp 134 | 135 | #endif // MOVEIT_TOPP_MOVEIT_TOPP_H 136 | -------------------------------------------------------------------------------- /external/spline/spline.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This program is free software; you can redistribute it and/or modify 3 | * it under the terms of the GNU General Public License as published by 4 | * the Free Software Foundation; either version 3 of the License, or 5 | * (at your option) any later version. 6 | */ 7 | 8 | /* Author: John Burkardt 9 | Desc: Collection of algorithms for spline interpolation 10 | Source: http://people.sc.fsu.edu/~jburkardt/cpp_src/spline/spline.html 11 | */ 12 | 13 | #include 14 | 15 | double basis_function_b_val ( double tdata[], double tval ); 16 | double basis_function_beta_val ( double beta1, double beta2, double tdata[], 17 | double tval ); 18 | double *basis_matrix_b_uni ( ); 19 | double *basis_matrix_beta_uni ( double beta1, double beta2 ); 20 | double *basis_matrix_bezier ( ); 21 | double *basis_matrix_hermite ( ); 22 | double *basis_matrix_overhauser_nonuni ( double alpha, double beta ); 23 | double *basis_matrix_overhauser_nul ( double alpha ); 24 | double *basis_matrix_overhauser_nur ( double beta ); 25 | double *basis_matrix_overhauser_uni ( ); 26 | double *basis_matrix_overhauser_uni_l ( ); 27 | double *basis_matrix_overhauser_uni_r ( ); 28 | double basis_matrix_tmp ( int left, int n, double mbasis[], int ndata, 29 | double tdata[], double ydata[], double tval ); 30 | void bc_val ( int n, double t, double xcon[], double ycon[], double *xval, 31 | double *yval ); 32 | double bez_val ( int n, double x, double a, double b, double y[] ); 33 | double bpab_approx ( int n, double a, double b, double ydata[], double xval ); 34 | double *bp01 ( int n, double x ); 35 | double *bpab ( int n, double a, double b, double x ); 36 | int chfev ( double x1, double x2, double f1, double f2, double d1, double d2, 37 | int ne, double xe[], double fe[], int next[] ); 38 | int d3_fs ( double a1[], double a2[], double a3[], int n, double b[], double x[] ); 39 | double *d3_mxv ( int n, double a[], double x[] ); 40 | double *d3_np_fs ( int n, double a[], double b[] ); 41 | void d3_print ( int n, double a[], std::string title ); 42 | void d3_print_some ( int n, double a[], int ilo, int jlo, int ihi, int jhi ); 43 | double *d3_uniform ( int n, int *seed ); 44 | void data_to_dif ( int ntab, double xtab[], double ytab[], double diftab[] ); 45 | double dif_val ( int ntab, double xtab[], double diftab[], double xval ); 46 | int i4_max ( int i1, int i2 ); 47 | int i4_min ( int i1, int i2 ); 48 | void least_set ( int point_num, double x[], double f[], double w[], 49 | int nterms, double b[], double c[], double d[] ); 50 | double least_val ( int nterms, double b[], double c[], double d[], 51 | double x ); 52 | void least_val2 ( int nterms, double b[], double c[], double d[], double x, 53 | double *px, double *pxp ); 54 | void least_set_old ( int ntab, double xtab[], double ytab[], int ndeg, 55 | double ptab[], double b[], double c[], double d[], double *eps, int *ierror ); 56 | double least_val_old ( double x, int ndeg, double b[], double c[], double d[] ); 57 | void parabola_val2 ( int ndim, int ndata, double tdata[], double ydata[], 58 | int left, double tval, double yval[] ); 59 | double pchst ( double arg1, double arg2 ); 60 | double *penta ( int n, double a1[], double a2[], double a3[], double a4[], 61 | double a5[], double b[] ); 62 | double r8_max ( double x, double y ); 63 | double r8_min ( double x, double y ); 64 | double r8_uniform_01 ( int *seed ); 65 | double *r8ge_fs_new ( int n, double a[], double b[] ); 66 | void r8vec_bracket ( int n, double x[], double xval, int *left, int *right ); 67 | void r8vec_bracket3 ( int n, double t[], double tval, int *left ); 68 | double *r8vec_even_new ( int n, double alo, double ahi ); 69 | double *r8vec_indicator_new ( int n ); 70 | int r8vec_order_type ( int n, double x[] ); 71 | void r8vec_print ( int n, double a[], std::string title ); 72 | void r8vec_sort_bubble_a ( int n, double a[] ); 73 | double *r8vec_uniform_new ( int n, double b, double c, int *seed ); 74 | int r8vec_unique_count ( int n, double a[], double tol ); 75 | void r8vec_zero ( int n, double a[] ); 76 | double spline_b_val ( int ndata, double tdata[], double ydata[], double tval ); 77 | double spline_beta_val ( double beta1, double beta2, int ndata, double tdata[], 78 | double ydata[], double tval ); 79 | double spline_constant_val ( int ndata, double tdata[], double ydata[], 80 | double tval ); 81 | double *spline_cubic_set ( int n, double t[], double y[], int ibcbeg, 82 | double ybcbeg, int ibcend, double ybcend ); 83 | double spline_cubic_val ( int n, double t[], double y[], double ypp[], 84 | double tval, double *ypval, double *yppval ); 85 | void spline_cubic_val2 ( int n, double t[], double tval, int *left, double y[], 86 | double ypp[], double *yval, double *ypval, double *yppval ); 87 | double *spline_hermite_set ( int ndata, double tdata[], double ydata[], 88 | double ypdata[] ); 89 | void spline_hermite_val ( int ndata, double tdata[], double c[], double tval, 90 | double *sval, double *spval ); 91 | double spline_linear_int ( int ndata, double tdata[], double ydata[], double a, 92 | double b ); 93 | void spline_linear_intset ( int int_n, double int_x[], double int_v[], 94 | double data_x[], double data_y[] ); 95 | void spline_linear_val ( int ndata, double tdata[], double ydata[], 96 | double tval, double *yval, double *ypval ); 97 | double spline_overhauser_nonuni_val ( int ndata, double tdata[], 98 | double ydata[], double tval ); 99 | double spline_overhauser_uni_val ( int ndata, double tdata[], double ydata[], 100 | double tval ); 101 | void spline_overhauser_val ( int ndim, int ndata, double tdata[], double ydata[], 102 | double tval, double yval[] ); 103 | void spline_pchip_set ( int n, double x[], double f[], double d[] ); 104 | void spline_pchip_val ( int n, double x[], double f[], double d[], int ne, 105 | double xe[], double fe[] ); 106 | void spline_quadratic_val ( int ndata, double tdata[], double ydata[], 107 | double tval, double *yval, double *ypval ); 108 | void timestamp ( ); 109 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(moveit_topp) 3 | 4 | # C++ 11 5 | set(CMAKE_CXX_FLAGS "-std=c++11 -Wall ${CMAKE_CXX_FLAGS}") 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | roslint 10 | rosparam_shortcuts 11 | cmake_modules 12 | moveit_msgs 13 | moveit_ros_planning 14 | ) 15 | 16 | find_package(Eigen REQUIRED) 17 | find_package(Boost REQUIRED) 18 | 19 | catkin_package( 20 | CATKIN_DEPENDS 21 | roscpp 22 | rosparam_shortcuts 23 | moveit_ros_planning 24 | DEPENDS 25 | Eigen 26 | INCLUDE_DIRS 27 | include 28 | external 29 | LIBRARIES 30 | ${PROJECT_NAME}_spline 31 | ${PROJECT_NAME}_spline_fitting 32 | ${PROJECT_NAME}_wrapper 33 | ) 34 | 35 | ########### 36 | ## Build ## 37 | ########### 38 | 39 | include_directories( 40 | include 41 | external 42 | ${catkin_INCLUDE_DIRS} 43 | ${EIGEN_INCLUDE_DIRS} 44 | ) 45 | 46 | # -------------------------------------------- 47 | # Configure Locations 48 | set(topp_LOCATION ${PROJECT_SOURCE_DIR}/external/TOPP/build/src/TOPPbindings.so) 49 | set(topp_DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}/lib${PROJECT_NAME}.so) 50 | MESSAGE( STATUS "topp_LOCATION: " ${topp_LOCATION} ) 51 | MESSAGE( STATUS "topp_DESTINATION: " ${topp_DESTINATION} ) 52 | 53 | # Topp library 54 | add_library(${PROJECT_NAME} SHARED IMPORTED) 55 | set_property(TARGET ${PROJECT_NAME} PROPERTY IMPORTED_LOCATION ${topp_LOCATION}) 56 | 57 | # Unfortuantly an imported target is not copied to the devel/lib folder, so we do it manually 58 | # so that installation of a catkin workspace is not required 59 | # See http://answers.ros.org/question/223866/cmakecatkin-how-to-export-imported-target/ 60 | file(COPY ${topp_LOCATION} 61 | DESTINATION ${topp_DESTINATION} 62 | ) 63 | 64 | # -------------------------------------------- 65 | # Spline Library 66 | add_library(${PROJECT_NAME}_spline 67 | external/spline/spline.cpp 68 | ) 69 | # target_link_libraries(${PROJECT_NAME}_spline 70 | # ) 71 | # -------------------------------------------- 72 | 73 | # -------------------------------------------- 74 | # Spline Fitting Wrapper Library 75 | add_library(${PROJECT_NAME}_spline_fitting 76 | src/spline_fitting.cpp 77 | ) 78 | target_link_libraries(${PROJECT_NAME}_spline_fitting 79 | ${PROJECT_NAME}_spline 80 | ${PROJECT_NAME} 81 | ${catkin_LIBRARIES} 82 | ) 83 | # -------------------------------------------- 84 | 85 | # -------------------------------------------- 86 | # TOPP Wrapper Library 87 | add_library(${PROJECT_NAME}_wrapper 88 | src/moveit_topp.cpp 89 | ) 90 | target_link_libraries(${PROJECT_NAME}_wrapper 91 | ${PROJECT_NAME} 92 | ${PROJECT_NAME}_spline_fitting 93 | ${catkin_LIBRARIES} 94 | ) 95 | # -------------------------------------------- 96 | 97 | # Demo: Convert Piecewise Polynomial Trajectory to Time-Optimal Discretized Path 98 | add_executable(${PROJECT_NAME}_demo_pp_to_optimal 99 | src/demo/demo_pp_to_optimal.cpp 100 | ) 101 | set_target_properties(${PROJECT_NAME}_demo_pp_to_optimal 102 | PROPERTIES OUTPUT_NAME demo_pp_to_optimal PREFIX "" 103 | ) 104 | target_link_libraries(${PROJECT_NAME}_demo_pp_to_optimal 105 | ${PROJECT_NAME}_wrapper 106 | ${catkin_LIBRARIES} 107 | ) 108 | 109 | # Demo: Convert Joint Trajectory Waypoints into Piecewise Polynomial Trajectory 110 | add_executable(${PROJECT_NAME}_demo_joint_traj_to_pp 111 | src/demo/demo_joint_traj_to_pp.cpp 112 | ) 113 | set_target_properties(${PROJECT_NAME}_demo_joint_traj_to_pp 114 | PROPERTIES OUTPUT_NAME demo_joint_traj_to_pp PREFIX "" 115 | ) 116 | target_link_libraries(${PROJECT_NAME}_demo_joint_traj_to_pp 117 | ${PROJECT_NAME}_wrapper 118 | ${catkin_LIBRARIES} 119 | ) 120 | 121 | # Demo: Convert Joint Trajectory Waypoints into Time-Optimal Discretized Path 122 | add_executable(${PROJECT_NAME}_demo_joint_traj_to_optimal 123 | src/demo/demo_joint_traj_to_optimal.cpp 124 | ) 125 | set_target_properties(${PROJECT_NAME}_demo_joint_traj_to_optimal 126 | PROPERTIES OUTPUT_NAME demo_joint_traj_to_optimal PREFIX "" 127 | ) 128 | target_link_libraries(${PROJECT_NAME}_demo_joint_traj_to_optimal 129 | ${PROJECT_NAME}_wrapper 130 | ${catkin_LIBRARIES} 131 | ) 132 | 133 | # Demo: Convert ROS Trajectory Message Waypoints into Time-Optimal Discretized Path 134 | add_executable(${PROJECT_NAME}_demo_traj_msg_to_optimal 135 | src/demo/demo_traj_msg_to_optimal.cpp 136 | ) 137 | set_target_properties(${PROJECT_NAME}_demo_traj_msg_to_optimal 138 | PROPERTIES OUTPUT_NAME demo_traj_msg_to_optimal PREFIX "" 139 | ) 140 | target_link_libraries(${PROJECT_NAME}_demo_traj_msg_to_optimal 141 | ${PROJECT_NAME}_wrapper 142 | ${catkin_LIBRARIES} 143 | ) 144 | 145 | ############# 146 | ## testing ## 147 | ############# 148 | 149 | ## Test for correct C++ source code 150 | roslint_cpp() 151 | 152 | ############# 153 | ## Install ## 154 | ############# 155 | 156 | ## Mark executables and/or libraries for installation 157 | # install(TARGETS ${PROJECT_NAME} #${PROJECT_NAME}_wrapper 158 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | # # Mark cpp header files for installation 164 | # install(DIRECTORY include/${PROJECT_NAME}/ 165 | # FILES_MATCHING PATTERN "*.h" 166 | # PATTERN ".svn" EXCLUDE 167 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 168 | # ) 169 | 170 | # # Mark roslaunch files for installation 171 | # # install(FILES launch 172 | # # FILES_MATCHING PATTERN "*.launch" 173 | # # PATTERN ".svn" EXCLUDE 174 | # # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 175 | # # ) 176 | # install(DIRECTORY launch 177 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 178 | # ) 179 | 180 | # # Mark config files for installation 181 | # install(FILES config 182 | # FILES_MATCHING PATTERN "*.yaml" 183 | # PATTERN ".svn" EXCLUDE 184 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 185 | # ) 186 | 187 | 188 | # Mark imported library for installation using a workaround since imported 189 | # targets do not have proper support for installation in CMakeLists 190 | install(FILES ${foo_LOCATION} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) -------------------------------------------------------------------------------- /src/demo/demo_traj_msg_to_optimal.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, PickNik LLC 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of PickNik LLC nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Test for converting a discretized joint trajectory into piecewise polynomials using splines 37 | */ 38 | 39 | // ROS 40 | #include 41 | 42 | // ROS parameter loading 43 | #include 44 | 45 | // this package 46 | #include 47 | #include 48 | 49 | // MoveIt 50 | #include 51 | #include 52 | #include 53 | 54 | namespace moveit_topp 55 | { 56 | 57 | static const std::string ROBOT_DESCRIPTION = "robot_description"; 58 | 59 | class ToppMoveItDemo 60 | { 61 | public: 62 | 63 | /** 64 | * \brief Constructor 65 | */ 66 | ToppMoveItDemo() 67 | : name_("topp_moveit_demo") 68 | { 69 | std::string joint_model_group; 70 | 71 | // Load rosparams 72 | ros::NodeHandle rpnh(nh_, name_); 73 | int error = 0; 74 | error += !rosparam_shortcuts::get(name_, rpnh, "joint_model_group", joint_model_group); 75 | rosparam_shortcuts::shutdownIfError(name_, error); 76 | 77 | // Load the loader 78 | robot_model_loader_.reset(new robot_model_loader::RobotModelLoader(ROBOT_DESCRIPTION)); 79 | 80 | // Load the robot model 81 | robot_model_ = robot_model_loader_->getModel(); // Get a shared pointer to the robot 82 | 83 | // Choose planning group 84 | jmg_ = robot_model_->getJointModelGroup(joint_model_group); 85 | 86 | 87 | ROS_INFO_STREAM_NAMED(name_,"ToppMoveItDemo Ready."); 88 | } 89 | 90 | void run() 91 | { 92 | // Initialize interpolater 93 | moveit_topp::SplineFitting spline_fitting; 94 | 95 | // Initialize optimizer 96 | std::vector vel_limits = {0.7895, 0.7895, 0.973, 1.2162, 1.7143, 2.6471, 3.3962}; 97 | std::vector acc_limits = {0.7895, 0.7895, 0.973, 1.2162, 1.7143, 2.6471, 3.3962}; 98 | moveit_topp::MoveItTopp optimizer(vel_limits, acc_limits); 99 | 100 | 101 | // Copy the vector of RobotStates to a RobotTrajectory 102 | robot_trajectory::RobotTrajectoryPtr robot_traj(new robot_trajectory::RobotTrajectory(robot_model_, jmg_)); 103 | //double dummy_dt = 1; // dummy value until parameterization 104 | //robot_traj->addSuffixWayPoint(interpolated_state, dummy_dt); 105 | 106 | // Compare to MoveIt's default parameterizer 107 | trajectory_processing::IterativeParabolicTimeParameterization default_time_parameterizer; 108 | default_time_parameterizer.computeTimeStamps(*robot_traj); 109 | 110 | 111 | // Load pre-generated (from Matlab) piecewise polynomial from file 112 | std::size_t num_joints = 7; 113 | spline_fitting.readJointTrajFromFile("/home/dave/ros/current/ws_acme/src/moveit_topp/data/moveit_joint_traj.csv", num_joints); 114 | 115 | // Benchmark runtime 116 | ros::Time start_time = ros::Time::now(); 117 | 118 | // Fit a spline to waypoinst 119 | spline_fitting.fitSpline(); 120 | 121 | TOPP::Trajectory orig_trajectory; 122 | spline_fitting.getPPTrajectory(orig_trajectory); 123 | 124 | bool debug = false; 125 | if (debug) 126 | { 127 | // Write PP trajectory to CSV file (for use with Matlab) 128 | ROS_WARN_STREAM_NAMED("main", "converting orig trajectory to joint space"); 129 | optimizer.writeTrajectoryToFile("/home/dave/ros/current/ws_acme/src/moveit_topp/data/topp_optimized_traj.csv", 130 | orig_trajectory); 131 | return; 132 | } 133 | 134 | // Time-Optimize with respect to constraints 135 | TOPP::Trajectory new_trajectory; 136 | optimizer.optimizeTrajectory(orig_trajectory, new_trajectory); 137 | 138 | // Benchmark runtime 139 | double duration = (ros::Time::now() - start_time).toSec(); 140 | ROS_INFO_STREAM_NAMED("main", "Total time: " << duration << " seconds (" << 1.0/duration << " hz)"); 141 | 142 | // Write joint trajectory to CSV file (for use with Matlab) 143 | optimizer.writeTrajectoryToFile("/home/dave/ros/current/ws_acme/src/moveit_topp/data/topp_optimized_traj.csv", 144 | new_trajectory); 145 | } 146 | 147 | private: 148 | 149 | // -------------------------------------------------------- 150 | 151 | // The short name of this class 152 | std::string name_; 153 | 154 | // A shared node handle 155 | ros::NodeHandle nh_; 156 | 157 | // Core MoveIt components 158 | robot_model_loader::RobotModelLoaderPtr robot_model_loader_; 159 | robot_model::RobotModelPtr robot_model_; 160 | 161 | // Desired planning group to work with 162 | moveit::core::JointModelGroup *jmg_; 163 | }; // end class 164 | 165 | } // namespace moveit_topp 166 | 167 | int main(int argc, char** argv) 168 | { 169 | // Initialize ROS 170 | ros::init(argc, argv, "demo_traj_msg_to_optimal"); 171 | ROS_INFO_STREAM_NAMED("main", "Starting MoveItTopp demo..."); 172 | 173 | // Allow the action server to recieve and send ros messages 174 | ros::AsyncSpinner spinner(2); 175 | spinner.start(); 176 | 177 | moveit_topp::ToppMoveItDemo demo; 178 | demo.run(); 179 | 180 | // Shutdown 181 | ROS_INFO_STREAM_NAMED("main", "Finished."); 182 | ros::shutdown(); 183 | 184 | return 0; 185 | } 186 | -------------------------------------------------------------------------------- /external/spline/LICENSE.txt: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /src/spline_fitting.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, PickNik LLC 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of PickNik LLC nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Wrapper for John Burkardt's spline funcitonality 37 | */ 38 | 39 | // this package 40 | #include 41 | 42 | namespace moveit_topp 43 | { 44 | 45 | SplineFitting::SplineFitting() 46 | { 47 | // Load rosparams 48 | // ros::NodeHandle rpnh(nh_, name_); 49 | // std::size_t error = 0; 50 | // error += !rosparam_shortcuts::get(name_, rpnh, "control_rate", control_rate_); 51 | // add more parameters here to load if desired 52 | // rosparam_shortcuts::shutdownIfError(name_, error); 53 | 54 | ROS_INFO_STREAM_NAMED(name_, "SplineFitting Ready."); 55 | } 56 | 57 | void SplineFitting::readJointTrajFromFile(const std::string& filename, std::size_t num_joints) 58 | { 59 | num_joints_ = num_joints; 60 | 61 | // Open file 62 | std::ifstream input_file; 63 | input_file.open(filename.c_str()); 64 | 65 | // Settings 66 | std::string line; 67 | std::string cell; 68 | 69 | // Store data by joints, THEN waypoints 70 | joint_positions_.resize(num_joints_); 71 | 72 | // Skip header 73 | std::getline(input_file, line); 74 | 75 | bool first_row_read_file = true; 76 | double first_timestamp; 77 | 78 | // For each row/trajectory waypoint 79 | while (std::getline(input_file, line)) 80 | { 81 | std::stringstream lineStream(line); 82 | 83 | // TIME FROM START 84 | if (!std::getline(lineStream, cell, ',')) 85 | ROS_ERROR_STREAM_NAMED("csv_to_controller", "no time value"); 86 | double timestamp = atof(cell.c_str()); 87 | // Make first timestamp zero 88 | if (first_row_read_file) 89 | { 90 | first_timestamp = timestamp; 91 | first_row_read_file = false; 92 | } 93 | timestamps_.push_back(timestamp - first_timestamp); 94 | 95 | // For each joint 96 | for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id) 97 | { 98 | // POSITION 99 | if (!std::getline(lineStream, cell, ',')) 100 | ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value"); 101 | joint_positions_[joint_id].push_back(atof(cell.c_str())); 102 | 103 | // VELOCITY 104 | if (!std::getline(lineStream, cell, ',')) 105 | ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value"); 106 | // UNUSED 107 | 108 | // ACCELERATION 109 | if (!std::getline(lineStream, cell, ',')) 110 | ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value"); 111 | // UNUSED 112 | } 113 | } // while 114 | } 115 | 116 | void SplineFitting::setJointPositions(const std::vector >& joint_positions, 117 | const std::vector& timetamps) 118 | { 119 | // Error check 120 | if (joint_positions.empty()) 121 | { 122 | ROS_ERROR_STREAM_NAMED(name_, "No waypoints passed in"); 123 | return; 124 | } 125 | if (joint_positions.front().empty()) 126 | { 127 | ROS_ERROR_STREAM_NAMED(name_, "No joint values passed in"); 128 | return; 129 | } 130 | // Copy in num joints 131 | num_joints_ = joint_positions.size(); 132 | 133 | // Save joint positions 134 | joint_positions_ = joint_positions; 135 | timestamps_ = timetamps; 136 | } 137 | 138 | void SplineFitting::fitSpline() 139 | { 140 | ROS_DEBUG_STREAM_NAMED(name_, "Fitting spline"); 141 | 142 | // Error check 143 | if (joint_positions_.empty()) 144 | { 145 | ROS_ERROR_STREAM_NAMED(name_, "No data loaded to fit"); 146 | return; 147 | } 148 | if (joint_positions_.size() != num_joints_) 149 | { 150 | ROS_ERROR_STREAM_NAMED(name_, "Incorrect number of joints passed in " << joint_positions_.size()); 151 | return; 152 | } 153 | if (timestamps_.size() != joint_positions_.front().size()) 154 | { 155 | ROS_ERROR_STREAM_NAMED(name_, "Data size invalid between timestamps and joint_positions"); 156 | return; 157 | } 158 | 159 | // Get the velocities 160 | bool use_simple_derivative = false; 161 | if (use_simple_derivative) 162 | { 163 | calcSimpleDerivative(); 164 | 165 | // Error check 166 | if (timestamps_.size() != joint_velocities_.front().size()) 167 | { 168 | ROS_ERROR_STREAM_NAMED(name_, "Data size invalid between timestamps and joint_velocities"); 169 | return; 170 | } 171 | } 172 | 173 | int ndata = timestamps_.size(); 174 | double* tdata = ×tamps_[0]; 175 | coefficients_.resize(num_joints_); 176 | 177 | // Benchmark runtime 178 | ros::Time start_time = ros::Time::now(); 179 | 180 | for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id) 181 | { 182 | double* ydata = &joint_positions_[joint_id][0]; 183 | double* ypdata; 184 | if (use_simple_derivative) 185 | ypdata = &joint_velocities_[joint_id][0]; 186 | else 187 | ypdata = new double[ndata]; 188 | 189 | // Set derivatives for a piecewise cubic Hermite interpolant. 190 | //ROS_INFO_STREAM_NAMED(name_, "Calculating spline pchip set"); 191 | spline_pchip_set(ndata, tdata, ydata, ypdata); 192 | 193 | // Set up a piecewise cubic Hermite interpolant 194 | //ROS_INFO_STREAM_NAMED(name_, "Calculating spline hermite set"); 195 | coefficients_[joint_id] = spline_hermite_set(ndata, tdata, ydata, ypdata); 196 | 197 | // Output coefficients 198 | bool verbose = true; 199 | if (verbose) 200 | { 201 | std::cout << "Timestamp Position "; 202 | if (use_simple_derivative) 203 | std::cout << "Velocity "; 204 | std::cout << "SVelocity Coefficient1 Coefficient2 Coefficient3 Coefficient4 " << std::endl; 205 | for (std::size_t j = 0; j < std::size_t(ndata); ++j) 206 | { 207 | std::cout << std::fixed << tdata[j] << "\t" << ydata[j] << "\t"; 208 | if (use_simple_derivative) 209 | std::cout << joint_velocities_[joint_id][j] << "\t"; 210 | std::cout << ypdata[j] << "\t"; 211 | for (std::size_t i = 0; i < 4; ++i) 212 | { 213 | std::cout << coefficients_[joint_id][j * 4 + i] << "\t"; 214 | } 215 | std::cout << std::endl; 216 | } 217 | } 218 | } // end for 219 | 220 | // Benchmark runtime 221 | double duration = (ros::Time::now() - start_time).toSec(); 222 | ROS_INFO_STREAM_NAMED(name_, "Conversion to polynomial: " << duration << " seconds"); 223 | } 224 | 225 | void SplineFitting::writeCoefficientsToFile(const std::string& file_path) 226 | { 227 | ROS_INFO_STREAM_NAMED(name_, "Writing coefficients to file"); 228 | 229 | // Open file 230 | std::ofstream output_handle; 231 | output_handle.open(file_path.c_str()); 232 | 233 | for (std::size_t i = 0; i < timestamps_.size() - 1; ++i) 234 | { 235 | std::cout << "timestamp " << timestamps_[i] << std::endl; 236 | // Timestamp 237 | if (i == 0) 238 | { 239 | output_handle << timestamps_[i] << std::endl; 240 | } 241 | else 242 | { 243 | output_handle << timestamps_[i] - timestamps_[i-1] << std::endl; 244 | } 245 | 246 | // Num joints 247 | output_handle << num_joints_ << std::endl; 248 | 249 | // Coefficients 250 | output_handle.precision(10); 251 | for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id) 252 | { 253 | for (std::size_t c = 0; c < 4; ++c) 254 | { 255 | //std::cout << "joint_id " << joint_id << " then " << i << " then " << c << " = " << (i * 4 + (3 - c)) << std::endl; 256 | output_handle << coefficients_[joint_id][i * 4 + (3 - c)]; 257 | if (c < 3) 258 | output_handle << ","; 259 | } 260 | output_handle << std::endl; 261 | } 262 | } 263 | 264 | // Save 265 | output_handle.close(); 266 | ROS_INFO_STREAM_NAMED(name_, "Saved coefficients to file " << file_path); 267 | } 268 | 269 | void SplineFitting::getPPTrajectory(TOPP::Trajectory& trajectory) 270 | { 271 | ROS_INFO_STREAM_NAMED(name_, "Converting coefficients to new format"); 272 | 273 | std::list chunks_list(timestamps_.size()); 274 | std::list::iterator chunk_it = chunks_list.begin(); 275 | std::vector polynomials_vector(num_joints_); 276 | std::vector coefficients_vector(4); 277 | 278 | for (std::size_t chunk_id = 0; chunk_id < timestamps_.size() - 1; ++chunk_id) // TODO(davetcoleman): why is it minus one? 279 | { 280 | for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id) 281 | { 282 | for (std::size_t coeff_id = 0; coeff_id < 4; ++coeff_id) 283 | { 284 | coefficients_vector[coeff_id] = coefficients_[joint_id][chunk_id * 4 + coeff_id]; 285 | } 286 | polynomials_vector[joint_id] = TOPP::Polynomial(coefficients_vector); 287 | } 288 | 289 | double duration = timestamps_[chunk_id + 1] - timestamps_[chunk_id]; 290 | //chunks_list[chunk_id] = TOPP::Chunk(duration, polynomials_vector); 291 | (*chunk_it) = TOPP::Chunk(duration, polynomials_vector); 292 | chunk_it++; 293 | } 294 | 295 | // Create final trajectory 296 | trajectory.InitFromChunksList(chunks_list); 297 | } 298 | 299 | void SplineFitting::calcSimpleDerivative() 300 | { 301 | ROS_INFO_STREAM_NAMED(name_, "Calculating simple derivatives"); 302 | joint_velocities_.resize(num_joints_); 303 | 304 | bool verbose = false; 305 | 306 | // For each joint 307 | for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id) 308 | { 309 | // For each waypoint 310 | for (std::size_t i = 1; i < joint_positions_[joint_id].size(); ++i) 311 | { 312 | double pos_diff = joint_positions_[joint_id][i] - joint_positions_[joint_id][i - 1]; 313 | double t_diff = timestamps_[i] - timestamps_[i - 1]; 314 | double vel = pos_diff / t_diff; 315 | if (verbose) 316 | std::cout << "joint_id: " << joint_id << " i: " << std::setfill('0') << std::setw(2) << i 317 | << " pos_diff: " << std::fixed << pos_diff << " \tt_diff: " << t_diff << " \tvel: " << vel 318 | << std::endl; 319 | 320 | joint_velocities_[joint_id].push_back(vel); 321 | } 322 | joint_velocities_[joint_id].push_back(0.0); // TODO(davetcoleman): how to calculate final derivative? 323 | } 324 | } 325 | 326 | } // namespace moveit_topp 327 | -------------------------------------------------------------------------------- /src/moveit_topp.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, PickNik LLC 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of PickNik LLC nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Wrapper for time opitmal path parameterization 37 | */ 38 | 39 | // this package 40 | #include 41 | 42 | namespace moveit_topp 43 | { 44 | 45 | MoveItTopp::MoveItTopp(ros::NodeHandle &nh, const moveit::core::JointModelGroup* jmg) 46 | : nh_(nh) 47 | , jmg_(jmg) 48 | { 49 | std::vector vel_limits; 50 | std::vector acc_limits; 51 | for (std::size_t i = 0; i < jmg->getActiveJointModels().size(); ++i) 52 | { 53 | const moveit::core::JointModel::Bounds& bounds = jmg->getActiveJointModels()[i]->getVariableBounds(); 54 | if (bounds.size() != 1) 55 | { 56 | ROS_ERROR_STREAM_NAMED(name_, "Invalid number of bounds"); 57 | continue; 58 | } 59 | const moveit::core::VariableBounds& bound = bounds.front(); 60 | vel_limits.push_back(bound.max_velocity_); 61 | acc_limits.push_back(bound.max_acceleration_); 62 | //std::cout << "bound.max_velocity_: " << bound.max_velocity_ << std::endl; 63 | //std::cout << "bound.max_acceleration_: " << bound.max_acceleration_ << std::endl; 64 | } 65 | 66 | // Resize datastructures for convertMoveItTrajToPP() 67 | ROS_INFO_STREAM_NAMED(name_, "Joint model group var count: " << jmg_->getVariableCount()); 68 | tmp_joint_positions_.resize(jmg_->getVariableCount()); 69 | tmp_jmg_positions_.resize(jmg_->getVariableCount()); 70 | 71 | // Load rosparams 72 | ros::NodeHandle rpnh(nh_, name_); 73 | std::size_t error = 0; 74 | error += !rosparam_shortcuts::get(name_, rpnh, "debug/write_coeff_to_file", debug_write_coeff_to_file_); 75 | error += !rosparam_shortcuts::get(name_, rpnh, "debug/write_joint_traj_to_file", debug_write_joint_traj_to_file_); 76 | rosparam_shortcuts::shutdownIfError(name_, error); 77 | 78 | init(vel_limits, acc_limits); 79 | } 80 | 81 | MoveItTopp::MoveItTopp(const std::vector &vel_limits, const std::vector &acc_limits) 82 | { 83 | init(vel_limits, acc_limits); 84 | } 85 | 86 | void MoveItTopp::init(const std::vector &vel_limits, const std::vector &acc_limits) 87 | { 88 | // Error checking 89 | if (vel_limits.size() != acc_limits.size() || 90 | vel_limits.empty()) 91 | { 92 | ROS_ERROR_STREAM_NAMED(name_, "Invalid vel/acc limits"); 93 | exit(-1); 94 | } 95 | 96 | // Convert constraints into a weird string format required by TOPP 97 | double discrtimestep = 0.005; // TODO(davetcoleman): no hardcode 98 | std::string constraint_string = std::to_string(discrtimestep); 99 | constraint_string.append("\n"); 100 | for (std::size_t i = 0; i < vel_limits.size(); ++i) 101 | { 102 | std::string temp = std::to_string(vel_limits[i]).append(" "); 103 | constraint_string.append(temp); // velocity 104 | } 105 | constraint_string.append("\n"); 106 | for (std::size_t i = 0; i < vel_limits.size(); ++i) 107 | { 108 | std::string temp = std::to_string(acc_limits[i]).append(" "); 109 | constraint_string.append(temp); // acceleration 110 | } 111 | bool verbose = false; 112 | if (verbose) 113 | { 114 | std::cout << "constraint_string:\n" << constraint_string << std::endl; 115 | } 116 | 117 | // Setup constraints 118 | pconstraints_.reset(new TOPP::KinematicLimits(constraint_string)); 119 | 120 | ROS_INFO_STREAM_NAMED(name_,"MoveItTopp Ready."); 121 | } 122 | 123 | void MoveItTopp::computeTimeStamps(robot_trajectory::RobotTrajectory& robot_traj) 124 | { 125 | // Convert MoveIt format to Spline format and send to spline fitting 126 | convertMoveItTrajToPP(robot_traj); 127 | 128 | // Fit a spline to waypoinst 129 | spline_fitting_.fitSpline(); 130 | 131 | // Write coefficients to file (for use with Matlab) 132 | if (debug_write_coeff_to_file_) 133 | { 134 | spline_fitting_.writeCoefficientsToFile("/home/dave/ros/current/ws_acme/src/moveit_topp/data/spline_pp_traj.csv"); 135 | } 136 | 137 | // Convert to TOPP format 138 | TOPP::Trajectory orig_trajectory; 139 | spline_fitting_.getPPTrajectory(orig_trajectory); 140 | 141 | // Time-Optimize with respect to constraints 142 | TOPP::Trajectory new_trajectory; 143 | optimizeTrajectory(orig_trajectory, new_trajectory); 144 | 145 | // Debug 146 | if (debug_write_joint_traj_to_file_) 147 | { 148 | writeTrajectoryToFile("/home/dave/ros/current/ws_acme/src/moveit_topp/data/topp_optimized_traj.csv", 149 | new_trajectory); 150 | } 151 | 152 | // Convert trajectory back to MoveIt's format 153 | convertTrajToMoveItTraj(robot_traj, new_trajectory); 154 | } 155 | 156 | void MoveItTopp::convertMoveItTrajToPP(const robot_trajectory::RobotTrajectory& robot_traj) 157 | { 158 | // Populate joint_positions with robot trajectory 159 | 160 | // Resize 161 | ROS_INFO_STREAM_NAMED(name_, "Converting MoveIt! robot trajectory with " << jmg_->getVariableCount() << " dof"); 162 | tmp_timestamps_.resize(robot_traj.getWayPointCount()); 163 | for (std::size_t joint_id = 0; joint_id < jmg_->getVariableCount(); ++joint_id) // for each joint 164 | { 165 | tmp_joint_positions_[joint_id].resize(robot_traj.getWayPointCount()); 166 | } 167 | 168 | // Copy - this requires transposing the matrix 169 | for (std::size_t i = 0; i < robot_traj.getWayPointCount(); ++i) // for each waypoint 170 | { 171 | robot_traj.getWayPoint(i).copyJointGroupPositions(jmg_, &tmp_jmg_positions_[0]); // TODO this is inefficient 172 | for (std::size_t joint_id = 0; joint_id < jmg_->getVariableCount(); ++joint_id) // for each joint 173 | { 174 | tmp_joint_positions_[joint_id][i] = tmp_jmg_positions_[joint_id]; 175 | } 176 | 177 | tmp_timestamps_[i] = i + 1.0; // dummy value TODO(davetcoleman): is this ok? 178 | } 179 | 180 | // Setup Spline Fitting 181 | spline_fitting_.setJointPositions(tmp_joint_positions_, tmp_timestamps_); 182 | } 183 | 184 | void MoveItTopp::readPPTrajFromFile(const std::string& filename, TOPP::Trajectory &trajectory) 185 | { 186 | ROS_INFO_STREAM_NAMED(name_, "Reading PP from filename: " << filename); 187 | 188 | std::string trajectory_string; 189 | std::ifstream filehandle(filename.c_str()); 190 | 191 | filehandle.seekg(0, std::ios::end); 192 | trajectory_string.reserve(filehandle.tellg()); 193 | filehandle.seekg(0, std::ios::beg); 194 | 195 | trajectory_string.assign((std::istreambuf_iterator(filehandle)), 196 | std::istreambuf_iterator()); 197 | 198 | // Convert to trajectory 199 | trajectory.InitFromString(trajectory_string); 200 | } 201 | 202 | void MoveItTopp::optimizeTrajectory(const TOPP::Trajectory &old_trajectory, TOPP::Trajectory &new_trajectory) 203 | { 204 | // Benchmark runtime 205 | ros::Time start_time = ros::Time::now(); 206 | 207 | ROS_INFO_STREAM_NAMED(name_, "Optimizing tractory with " << old_trajectory.dimension << " dims, " 208 | << old_trajectory.chunkslist.size() << " waypoints"); 209 | 210 | // Error check 211 | if (!pconstraints_) 212 | { 213 | ROS_ERROR_STREAM_NAMED(name_, "Constraints object not initialized"); 214 | exit(-1); 215 | } 216 | 217 | pconstraints_->trajectory = old_trajectory; 218 | 219 | // Set default private tuning parameters 220 | pconstraints_->bisectionprecision = 0.01; 221 | pconstraints_->loweringcoef = 0.95; 222 | 223 | // Run TOPP 224 | ROS_INFO_STREAM_NAMED(name_, "Computing profiles"); 225 | TOPP::dReal sdbeg = 0; 226 | TOPP::dReal sdend= 0; 227 | pconstraints_->integrationtimestep = 0; 228 | pconstraints_->passswitchpointnsteps = 5; 229 | pconstraints_->extrareps = 0; 230 | pconstraints_->stepthresh = 0.01; 231 | ros::Time start_time2 = ros::Time::now(); 232 | TOPP::ComputeProfiles(*pconstraints_, sdbeg, sdend); 233 | 234 | // Benchmark runtime 235 | double duration2 = (ros::Time::now() - start_time2).toSec(); 236 | ROS_INFO_STREAM_NAMED(name_, "Profiles time: " << duration2 << " seconds (" << 1.0/duration2 << " hz)"); 237 | 238 | // Reparameterize 239 | ROS_INFO_STREAM_NAMED(name_, "Parameterizing"); 240 | pconstraints_->reparamtimestep = 0; // Original value - sets automatically 241 | //pconstraints_->reparamtimestep = 0.01; // DTC Causes less points to be created, faster 242 | ros::Time start_time3 = ros::Time::now(); 243 | pconstraints_->trajectory.Reparameterize(*pconstraints_, new_trajectory); 244 | 245 | // Benchmark runtime 246 | double duration3 = (ros::Time::now() - start_time3).toSec(); 247 | ROS_INFO_STREAM_NAMED(name_, "Parameterize time: " << duration3 << " seconds (" << 1.0/duration3 << " hz)"); 248 | 249 | // Benchmark runtime 250 | double duration = (ros::Time::now() - start_time).toSec(); 251 | ROS_WARN_STREAM_NAMED(name_, "Total time: " << duration << " seconds (" << 1.0/duration << " hz)"); 252 | } 253 | 254 | bool MoveItTopp::writeTrajectoryToFile(const std::string &file_path, const TOPP::Trajectory &trajectory) 255 | { 256 | ROS_INFO_STREAM_NAMED(name_, "Writing discretized trajectory to file"); 257 | 258 | std::ofstream output_handle; 259 | output_handle.open(file_path.c_str()); 260 | 261 | // Output header ------------------------------------------------------- 262 | output_handle << "time_from_start,"; 263 | for (std::size_t i = 0; i < std::size_t(trajectory.dimension); ++i) 264 | { 265 | output_handle << "j" << std::to_string(i) << "_pos,"; 266 | output_handle << "j" << std::to_string(i) << "_vel,"; 267 | output_handle << "j" << std::to_string(i) << "_acc,"; 268 | } 269 | output_handle << std::endl; 270 | 271 | // Debug 272 | ROS_INFO_STREAM_NAMED(name_, " - Number of waypoints: " << trajectory.chunkslist.size()); 273 | ROS_INFO_STREAM_NAMED(name_, " - Trajectory duration: " << trajectory.duration); 274 | ROS_INFO_STREAM_NAMED(name_, " - Trajectory dimension: " << trajectory.dimension); 275 | 276 | // Discretize back into waypoints 277 | double dt = 0.01; 278 | 279 | std::vector position; 280 | std::vector velocity; 281 | std::vector acceleration; 282 | position.resize(trajectory.dimension); 283 | velocity.resize(trajectory.dimension); 284 | acceleration.resize(trajectory.dimension); 285 | 286 | for (double time = 0; time < trajectory.duration; time+=dt) 287 | { 288 | trajectory.Eval(time, position); 289 | trajectory.Evald(time, velocity); 290 | trajectory.Evaldd(time, acceleration); 291 | 292 | output_handle.precision(10); 293 | output_handle << time << ","; 294 | for (std::size_t j = 0; j < position.size(); ++j) 295 | { 296 | output_handle << position[j] << ", "; 297 | output_handle << velocity[j] << ", "; 298 | output_handle << acceleration[j] << ", "; 299 | } 300 | output_handle << std::endl; 301 | } 302 | 303 | 304 | output_handle.close(); 305 | ROS_INFO_STREAM_NAMED(name_, "Saved trajectory to " << file_path); 306 | return true; 307 | } 308 | 309 | bool MoveItTopp::convertTrajToMoveItTraj(robot_trajectory::RobotTrajectory& robot_traj, const TOPP::Trajectory &trajectory) 310 | { 311 | ROS_INFO_STREAM_NAMED(name_, "Converting TOPP PP trajectory to MoveIt! discretized trajectory"); 312 | 313 | // Delete previous trajectory 314 | robot_traj.clear(); 315 | 316 | // Debug 317 | ROS_INFO_STREAM_NAMED(name_, " - Number of waypoints: " << trajectory.chunkslist.size()); 318 | ROS_INFO_STREAM_NAMED(name_, " - Trajectory duration: " << trajectory.duration); 319 | ROS_INFO_STREAM_NAMED(name_, " - Trajectory dimension: " << trajectory.dimension); 320 | 321 | // Discretize back into waypoints 322 | double dt = 0.05; 323 | 324 | std::vector position; 325 | std::vector velocity; 326 | std::vector acceleration; 327 | position.resize(trajectory.dimension); 328 | velocity.resize(trajectory.dimension); 329 | acceleration.resize(trajectory.dimension); 330 | 331 | // Re-usable robot state 332 | robot_state::RobotState state(robot_traj.getRobotModel()); 333 | 334 | // Loop through trajectory at discretization dt 335 | for (double time = 0; time <= trajectory.duration; time += dt) 336 | { 337 | // Evaluate PP at this point in time 338 | trajectory.Eval(time, position); 339 | trajectory.Evald(time, velocity); 340 | //trajectory.Evaldd(time, acceleration); 341 | 342 | // Copy to robot state 343 | state.setJointGroupPositions(jmg_, position); 344 | state.setJointGroupVelocities(jmg_, velocity); 345 | //state.setJointGroupAccelerations(jmg_, acceleration); 346 | 347 | // Add state to trajectory 348 | robot_traj.addSuffixWayPoint(state, dt); 349 | } 350 | 351 | return true; 352 | } 353 | 354 | } // namespace moveit_topp 355 | --------------------------------------------------------------------------------