├── .gitignore ├── LICENSE ├── README.md └── traj ├── CMakeLists.txt ├── package.xml ├── scripts ├── generate_exception_test_cases.py ├── generate_random_test_cases.py └── traj_segment_example ├── setup.py ├── src └── traj │ ├── __init__.py │ ├── cubic_eq_roots.py │ ├── segment_planning.py │ ├── traj_plot.py │ └── traj_segment.py └── test ├── test_min_real_root_cubic_eq.py ├── test_traj_segment.py └── test_traj_segment_middle_points.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *autogenerated* 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # scurve_traj_generation 2 | This package generates jerk limited motion profile using s-curve [1-5] 3 | 4 | 5 | ### Usage 6 | - To generate a jerk limited motion profile for any trajectory segment, call the `fit_traj_segment` function. this function requires the following arguments 7 | ``` 8 | p_start: initial position 9 | p_end : final position 10 | v_start: initial velocity 11 | v_end : final velocity 12 | p_max : position limitation 13 | v_max : velocity limit 14 | a_max : acceleration limit 15 | j_max : jerk limit 16 | ``` 17 | The output (return value) of the function will be a set of a PiecewiseFunction describes the motion profile of the segment: 18 | ``` 19 | position : a PiecewiseFunction describes the position w.r.t time 20 | velocity : a PiecewiseFunction describes the velocity w.r.t time 21 | acceleration : a PiecewiseFunction describes the acceleration w.r.t time 22 | jerk : a PiecewiseFunction describes the jerk w.r.t time 23 | ``` 24 | 25 | - To visualize the trajectory segment, call the function `fit_and_plot_segment` using the same argument of the `fit_traj_segment` function. this function will fit and plot the trajectory segment using matplotlib. 26 | 27 | ### Example 28 | - `traj_segment_example.py`: this scripts contains predefined trajectory segments with different initial and final values for both position and velocity, this values will result in different motion profiles. By running this script, it will fit the predefined segments into the corresponding motion profile and then it will plot each segment in a separate figure. 29 | 30 | ### Exceptions 31 | This high level functions `fit_traj_segment` will first check the feasibility of generating a jerk limited motion profile based on the given arguments. If the given arguments are not feasible, then the function will throw an error message with a description why the given a arguments are not feasible. Examples of non-feasible motion: 32 | - if any of the initial of final values violates the corresponding limits of the position, velocity, acceleration, and jerk 33 | - If the final velocity can not be reached from the initial position within the given motion in position (the position difference between the initial position and final position). because reaching any velocity from initial value requires a min position difference considering the acceleration and jerk limits. 34 | - if the velocity direction is opposite to the motion direction required to achieve final position. for example, if initial and final position have 35 | 36 | ### Test Files 37 | - `test_traj_segment.py`: it has 34 test cases, each of them generates a different profile structure based on the given inputs. For each of them, the initial and final values (position & velocity) of the generated profile are tested against the given values to ensure that the generated profile reflects the given inputs. To run all the test cases inside this file: 38 | ``` 39 | nosetests test_traj_segment.py 40 | ``` 41 | 42 | - `test_traj_segment_middel_points.py`: it has 38 test cases, each of them generates a different profile structure. For each of them, the values at the transient points* of the position, velocity, acceleration, and jerk of the generated profiles are tested if they are within the corresponding limits or not. To run all the test cases inside this file: 43 | ``` 44 | nosetests test_traj_segment_middel_points.py 45 | ``` 46 | 47 | *transient points: it used here to represent the points at which the profile move from one phase to the next phase (e.g. from constant_jerk_phase to constant_acceleration_phase) 48 | 49 | ### Random Test Cases: 50 | beside the provided test files which include around 38 test case, you can generate as many test cases as you wish using the following scripts: 51 | - `generate_random_test_cases.py`: 52 | This scripts generate random test cases (random initial and final position & velocity) which results in feasible motion. you can set the number of the test cases using the variable `num_of_cases`. you can change the position, velocity, acceleration, and jerk limits as well be setting the variables `p_max`, `v_max`, `a_max`, `j_max` respectively. You also can make the script generate random limits by resetting the variable `fixed_limit` to `false`. This scripts generates two scripts: 53 | - `test_autogenerated_random_cases.py`: this files contains all test cases which randomly generated using. To run it type ```nosetests test_autogenerated_random_cases.py``` 54 | - `plot_autogenerated_random_cases.py`: you can run this script to visualize all the generated test cases. 55 | 56 | - `generate_exception_test_cases.py`: 57 | it does the same job as the previuos one `generate_random_test_cases.py`, However, it generates only test cases which results in non-feasible motion, see Exceptions section. 58 | 59 | ### Assumption 60 | * The current version considers that initial and final acceleration values are zeros. 61 | * The current version assumes that the minimum limit for position, velocity, acceleration, and jerk are equal to the minus value the corresponding maximum value (e.g. minimum velocity = - maximum velocity) 62 | 63 | 64 | ### References 65 | 1. [CONSTANT JERK EQUATIONS FOR A TRAJECTORY GENERATOR] 66 | 2. [Optimize S-Curve Velocity for Motion Control] 67 | 3. [On Algorithms for Planning S-curve Motion Profiles] 68 | 4. [A New Seven-Segment Profile Algorithm for an Open Source Architecture in a Hybrid Electronic Platform] 69 | 5. [Soft-Motion and Visual Control for service robots] 70 | 6. [A New Approach to Time-Optimal Path Parameterization Based on Reachability Analysis] 71 | 7. [Online Trajectory Generation: Basic Concepts for Instantaneous Reactions to Unforeseen Events] 72 | 73 | 74 | [CONSTANT JERK EQUATIONS FOR A TRAJECTORY GENERATOR]: http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf 75 | 76 | [Optimize S-Curve Velocity for Motion Control]: https://www.researchgate.net/publication/257576837_Optimize_S-Curve_Velocity_for_Motion_Control 77 | 78 | [On Algorithms for Planning S-curve Motion Profiles]: https://journals-sagepub-com.tudelft.idm.oclc.org/doi/pdf/10.5772/5652 79 | 80 | [Soft-Motion and Visual Control for service robots]: https://www.researchgate.net/publication/228570194_Soft-Motion_and_Visual_Control_for_service_robots 81 | 82 | [A New Seven-Segment Profile Algorithm for an Open Source Architecture in a Hybrid Electronic Platform]: https://www.researchgate.net/publication/333676286_A_New_Seven-Segment_Profile_Algorithm_for_an_Open_Source_Architecture_in_a_Hybrid_Electronic_Platform 83 | 84 | [A New Approach to Time-Optimal Path Parameterization Based on Reachability Analysis]: https://www.researchgate.net/publication/318671280_A_New_Approach_to_Time-Optimal_Path_Parameterization_Based_on_Reachability_Analysis 85 | 86 | [Online Trajectory Generation: Basic Concepts for Instantaneous Reactions to Unforeseen Events]: https://www-cs.stanford.edu/groups/manips/publications/pdfs/Kroeger_2010_TRO.pdf 87 | -------------------------------------------------------------------------------- /traj/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(traj) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_python_setup() 7 | 8 | catkin_package() 9 | 10 | catkin_add_nosetests(test) 11 | 12 | -------------------------------------------------------------------------------- /traj/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | traj 4 | 0.0.0 5 | Python library for trjactory manipulation 6 | 7 | Jon Binney 8 | mahmoud ali 9 | 10 | Apache 2.0 11 | 12 | catkin 13 | 14 | python-matplotlib 15 | python-numpy 16 | python-sympy 17 | 18 | -------------------------------------------------------------------------------- /traj/scripts/generate_exception_test_cases.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ''' python script to auto-generate test-cases to test the "traj.fit_traj_segment" function. 3 | this file rejects all the feasible cases that can fit into one segment, 4 | and it generates the cases that should raise an error inside the function! 5 | So, this files to test that the function can identify illogical/unfeasible cases and raise an error 6 | ''' 7 | 8 | import random 9 | import os 10 | import stat 11 | import traj 12 | 13 | def generate_exception_test_cases(num_of_cases, p_max_abs, v_max_abs, a_max_abs, j_max_abs, fixed_limit): 14 | test_auto_generated_cases = os.getcwd() +"/test_autogenerated_exception_cases.py" 15 | plot_random_cases = os.getcwd() +"/plot_autogenerated_exception_test_cases.py" 16 | 17 | with open(test_auto_generated_cases, 'w') as random_cases: 18 | with open(plot_random_cases, 'w') as plot_segment: 19 | print >> random_cases, ''' 20 | #this is an auto-generated test file, generated using python script: generate_exception_test_cases --> 21 | import nose 22 | import numpy as np 23 | import random 24 | import traj 25 | 26 | ''' 27 | print >> plot_segment, '''#!/usr/bin/env python 28 | #this is an auto-generated test file, generated using python script: generate_exception_test_cases --> 29 | import traj 30 | ''' 31 | ## assign the limits if it is fixed_limits case 32 | p_max = p_max_abs 33 | v_max = v_max_abs 34 | a_max = a_max_abs 35 | j_max = j_max_abs 36 | 37 | 38 | case = 0 39 | while case < num_of_cases: 40 | add_case = False 41 | violate_pmax= False 42 | 43 | # random limits option: fixed_limit= False 44 | if not fixed_limit: 45 | p_max = random.uniform(0.1, p_max_abs) 46 | v_max = random.uniform(0.1, v_max_abs) 47 | a_max = random.uniform(0.1, j_max_abs) 48 | j_max = random.uniform(0.1, a_max_abs) 49 | 50 | 51 | ############## choose random start/stop pos/vel within the limits (given or random generated limits) 52 | p_start = random.uniform(-p_max, p_max) 53 | p_end = random.uniform(-p_max, p_max) 54 | v_start = random.uniform(-v_max, v_max) 55 | v_end = random.uniform(-v_max, v_max) 56 | 57 | 58 | 59 | ############## 1] check cases when pos_diff less than the min pos_diff to reach v_end from v_start 60 | if(v_start*v_end>0): 61 | abs_minPos_to_vf, acc_to_vf, t_jrk_to_vf, t_acc_to_vf = traj.calculate_min_pos_reached_acc_jrk_time_acc_time_to_reach_final_vel(abs(v_start), abs(v_end), v_max, a_max, j_max) 62 | if abs_minPos_to_vf > abs(p_end-p_start): 63 | add_case = True 64 | ###print the test statement for this case in the testing generated script 65 | print>> random_cases, ''' 66 | def test_exception_{}(): 67 | with nose.tools.assert_raises_regexp(ValueError, "non feasible case: violate min_pos_to_vf"): 68 | position, velocity, jerk, acceleration = traj.fit_traj_segment({}, {}, {}, {}, {}, {}, {}, {}) 69 | '''.format(case, p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max ) 70 | 71 | 72 | 73 | ############## 2] check cases when combination of: motion & pos_diff requires that position violate p_max 74 | if(v_start*v_end<0): 75 | minPos_to_zero, acc_to_zero, t_jrk_to_zero, t_acc_to_zero = traj.calculate_min_pos_reached_acc_jrk_time_acc_time_to_reach_final_vel(v_start, 0.0, v_max, a_max, j_max) 76 | minPos_to_vf, acc_to_vf, t_jrk_to_vf, t_acc_to_vf = traj.calculate_min_pos_reached_acc_jrk_time_acc_time_to_reach_final_vel( 0.0, v_end, v_max, a_max, j_max) 77 | pos_diff = p_end - p_start 78 | pos_dominant = pos_diff - minPos_to_zero - minPos_to_vf 79 | 80 | ######## A) complex positive motion case ######################## 81 | if pos_dominant > 0: ## positive dominant case 82 | if v_start < 0 and v_end > 0: # from negative to positive 83 | if abs(p_start+minPos_to_zero) > p_max or abs(p_start+minPos_to_zero+minPos_to_vf) > p_max or abs(p_start+minPos_to_zero+minPos_to_vf+pos_dominant) > p_max: 84 | add_case = True 85 | violate_pmax=True 86 | elif v_start > 0 and v_end < 0: #from positive to negative 87 | if abs(p_start+pos_dominant) > p_max or abs(p_start+pos_dominant+minPos_to_zero) > p_max or abs(p_start+pos_dominant+minPos_to_zero+minPos_to_vf) > p_max: 88 | add_case = True 89 | violate_pmax=True 90 | 91 | ######## B) complex negative motion case ######################## 92 | if pos_dominant < 0: ## negative dominant case 93 | if v_start < 0 and v_end > 0: # from negative to positive 94 | if abs(p_start+pos_dominant) > p_max or abs(p_start+pos_dominant+minPos_to_zero) > p_max or abs(p_start+pos_dominant+minPos_to_zero+minPos_to_vf) > p_max: 95 | add_case = True 96 | violate_pmax=True 97 | elif v_start > 0 and v_end < 0: #from positive to negative 98 | if abs(p_start+minPos_to_zero) > p_max or abs(p_start+minPos_to_zero+minPos_to_vf) > p_max or abs(p_start+minPos_to_zero+minPos_to_vf+pos_dominant) > p_max: 99 | add_case = True 100 | violate_pmax=True 101 | 102 | 103 | 104 | ############## 3] check cases when motion direction is opposit pos_diff direction 105 | if (v_start>0 and v_end>0 and (p_end-p_start)<0) or (v_start<0 and v_end<0 and (p_end-p_start)>0): # +ve motion vs -ve pos_diff or -ve motion vs +ve pos_diff 106 | add_case = True 107 | ###print the test statement for this case in the testing generated script 108 | print>> random_cases, ''' 109 | def test_exception_{}(): 110 | with nose.tools.assert_raises_regexp(ValueError, "non feasible case: vel_motion opposite to pos_motion"): 111 | position, velocity, jerk, acceleration = traj.fit_traj_segment({}, {}, {}, {}, {}, {}, {}, {}) 112 | '''.format(case, p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max ) 113 | 114 | 115 | ############## add plotting statement ############## 116 | if violate_pmax: 117 | ###print the test statement for this case in the testing generated script 118 | print>> random_cases, ''' 119 | def test_exception_{}(): 120 | with nose.tools.assert_raises_regexp(ValueError, "non feasible case: violate p_max"): 121 | position, velocity, jerk, acceleration = traj.fit_traj_segment({}, {}, {}, {}, {}, {}, {}, {}) 122 | '''.format(case, p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max ) 123 | 124 | 125 | 126 | ############## add plotting statement ############## 127 | if add_case: 128 | case +=1 129 | ###print the plot statement for this case in the plotting generated script 130 | print>> plot_segment, '''#case_{} 131 | traj.traj_plot.fit_and_plot_segment({}, {}, {}, {}, {}, {}, {}, {}) 132 | '''.format( case, p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max ) 133 | 134 | 135 | 136 | 137 | st = os.stat(test_auto_generated_cases) 138 | os.chmod(test_auto_generated_cases, st.st_mode | stat.S_IEXEC) 139 | st = os.stat(plot_random_cases) 140 | os.chmod(plot_random_cases, st.st_mode | stat.S_IEXEC) 141 | 142 | 143 | 144 | 145 | 146 | ##### generate random cases 147 | num_of_cases = 1000 148 | 149 | ## random cases with fixed limits 150 | p_max = 5.0 151 | v_max = 2.0 152 | a_max = 3.0 153 | j_max = 1.0 154 | 155 | ## random cases with variable limits: [the sollowing are limits for the limits] 156 | p_max = 10.0 157 | v_max = 10.0 158 | a_max = 10.0 159 | j_max = 10.0 160 | 161 | fixed_limit = False 162 | generate_exception_test_cases(num_of_cases, p_max, v_max, a_max, j_max, fixed_limit) 163 | 164 | 165 | 166 | 167 | -------------------------------------------------------------------------------- /traj/scripts/generate_random_test_cases.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ''' python script to auto-generate random test-cases to test "traj.fit_traj_segment" function, 3 | this file rejects all the unfeasible/illogical cases that can not be fit into one segment, 4 | and it accepts all the feasible cases that can be traeted as one segment, 5 | ''' 6 | 7 | import random 8 | import os 9 | import stat 10 | import traj 11 | 12 | def generate_random_test_cases(num_of_cases, p_max_abs, v_max_abs, a_max_abs, j_max_abs, fixed_limit): 13 | test_auto_generated_cases = os.getcwd() +"/test_autogenerated_random_cases.py" 14 | plot_random_cases = os.getcwd() +"/plot_autogenerated_random_test_cases.py" 15 | 16 | with open(test_auto_generated_cases, 'w') as random_cases: 17 | with open(plot_random_cases, 'w') as plot_segment: 18 | print >> random_cases, ''' 19 | #this is an auto-generated test file, generated using python script: generate_random_test_cases --> 20 | import nose 21 | import numpy as np 22 | import random 23 | import traj 24 | 25 | 26 | def check_fit_traj_segment(p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max): 27 | position, velocity, acceleration, jerk = traj.fit_traj_segment(p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max) 28 | 29 | t_start = position.boundaries[0] 30 | t_end = position.boundaries[-1] 31 | p_start_computed = position(t_start) 32 | v_start_computed = velocity(t_start) 33 | p_end_computed = position(t_end)[0] 34 | v_end_computed = velocity(t_end)[0] 35 | 36 | ### test_1: test if the calculated start/end pos/vel are equal to the given ones 37 | assert np.isclose(p_start, p_start_computed) 38 | assert np.isclose(p_end, p_end_computed) 39 | assert np.isclose(v_start, v_start_computed) 40 | assert np.isclose(v_end, v_end_computed) 41 | 42 | ### 2) test if the calculated pos/vel/acc/jrk are within the given limits p_max, v_max, a_max, j_max at each time_instatnt the segment_phase changes 43 | for phase_time in position.boundaries: 44 | p_computed = position(phase_time) 45 | v_computed = velocity(phase_time) 46 | a_computed = acceleration(phase_time) 47 | j_computed = jerk(phase_time) 48 | 49 | assert np.less_equal( abs(p_computed), p_max+1e-6) #added 1e-6 because sometime it gives fails even if the difference is less than (1e-6) 50 | assert np.less_equal( abs(v_computed), v_max+1e-6) 51 | assert np.less_equal( abs(a_computed), a_max+1e-6) 52 | assert np.less_equal( abs(j_computed), j_max+1e-6) 53 | 54 | ''' 55 | print >> plot_segment, '''#!/usr/bin/env python 56 | #this is an auto-generated test file, generated using python script: generate_random_test_cases --> 57 | import traj 58 | ''' 59 | ## assign limits if it is fixed 60 | p_max = p_max_abs 61 | v_max = v_max_abs 62 | a_max = a_max_abs 63 | j_max = j_max_abs 64 | 65 | 66 | case = 0 67 | while case < num_of_cases: 68 | 69 | # random limits option: fixed_limit= False 70 | if not fixed_limit: 71 | p_max = random.uniform(0.1, p_max_abs) 72 | v_max = random.uniform(0.1, v_max_abs) 73 | a_max = random.uniform(0.1, j_max_abs) 74 | j_max = random.uniform(0.1, a_max_abs) 75 | 76 | 77 | ## choose random start/stop pos/vel within the limits (given or random generated limits) 78 | p_start = random.uniform(-p_max, p_max) 79 | p_end = random.uniform(-p_max, p_max) 80 | v_start = random.uniform(-v_max, v_max) 81 | v_end = random.uniform(-v_max, v_max) 82 | 83 | 84 | 85 | ##################### check cases when pos_diff less than the min pos_diff to reach v_end from v_start 86 | if(v_start*v_end>0): 87 | abs_minPos_to_vf, acc_to_vf, t_jrk_to_vf, t_acc_to_vf = traj.calculate_min_pos_reached_acc_jrk_time_acc_time_to_reach_final_vel(abs(v_start), abs(v_end), v_max, a_max, j_max) 88 | if abs_minPos_to_vf > abs(p_end-p_start): 89 | continue 90 | 91 | 92 | 93 | ##################### check cases when combination of: motion & pos_diff requires that position violate p_max 94 | if(v_start*v_end<0): 95 | minPos_to_zero, acc_to_zero, t_jrk_to_zero, t_acc_to_zero = traj.calculate_min_pos_reached_acc_jrk_time_acc_time_to_reach_final_vel(v_start, 0.0, v_max, a_max, j_max) 96 | minPos_to_vf, acc_to_vf, t_jrk_to_vf, t_acc_to_vf = traj.calculate_min_pos_reached_acc_jrk_time_acc_time_to_reach_final_vel( 0.0, v_end, v_max, a_max, j_max) 97 | pos_diff = p_end - p_start 98 | pos_dominant = pos_diff - minPos_to_zero - minPos_to_vf 99 | 100 | # A) complex positive motion case ######################## 101 | if pos_dominant > 0: ## positive dominant case 102 | if v_start < 0 and v_end > 0: # from negative to positive 103 | if abs(p_start+minPos_to_zero) > p_max or abs(p_start+minPos_to_zero+minPos_to_vf) > p_max or abs(p_start+minPos_to_zero+minPos_to_vf+pos_dominant) > p_max: 104 | continue 105 | elif v_start > 0 and v_end < 0: #from positive to negative 106 | if abs(p_start+pos_dominant) > p_max or abs(p_start+pos_dominant+minPos_to_zero) > p_max or abs(p_start+pos_dominant+minPos_to_zero+minPos_to_vf) > p_max: 107 | continue 108 | 109 | # B) complex negative motion case ######################## 110 | if pos_dominant < 0: ## negative dominant case 111 | if v_start < 0 and v_end > 0: # from negative to positive 112 | if abs(p_start+pos_dominant) > p_max or abs(p_start+pos_dominant+minPos_to_zero) > p_max or abs(p_start+pos_dominant+minPos_to_zero+minPos_to_vf) > p_max: 113 | continue 114 | elif v_start > 0 and v_end < 0: #from positive to negative 115 | if abs(p_start+minPos_to_zero) > p_max or abs(p_start+minPos_to_zero+minPos_to_vf) > p_max or abs(p_start+minPos_to_zero+minPos_to_vf+pos_dominant) > p_max: 116 | continue 117 | 118 | 119 | 120 | ##################### check cases when motion direction is opposit pos_diff direction 121 | if (v_start>0 and v_end>0 and (p_end-p_start)<0): # +ve motion vs -ve pos_diff 122 | continue 123 | elif (v_start<0 and v_end<0 and (p_end-p_start)>0): # -ve motion vs +ve pos_diff 124 | continue 125 | 126 | 127 | case +=1 128 | ## print the test statement for this case in the generated test script 129 | print>> random_cases, ''' 130 | def test_case_{}(): 131 | check_fit_traj_segment({}, {}, {}, {}, {}, {}, {}, {}) 132 | '''.format(case, p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max ) 133 | 134 | 135 | ## print the plot statement for this case in the plotting generated script 136 | print>> plot_segment, '''#case_{} 137 | traj.traj_plot.fit_and_plot_segment({}, {}, {}, {}, {}, {}, {}, {}) 138 | '''.format( case, p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max ) 139 | 140 | 141 | 142 | ## make the generated file executable 143 | st = os.stat(test_auto_generated_cases) 144 | os.chmod(test_auto_generated_cases, st.st_mode | stat.S_IEXEC) 145 | st = os.stat(plot_random_cases) 146 | os.chmod(plot_random_cases, st.st_mode | stat.S_IEXEC) 147 | 148 | 149 | 150 | 151 | 152 | ##### generate random cases 153 | num_of_cases = 100 154 | 155 | ## random cases with fixed limits 156 | p_max = 5.0 157 | v_max = 2.0 158 | a_max = 3.0 159 | j_max = 1.0 160 | 161 | ## random cases with variable limits: [the sollowing are limits for the limits] 162 | p_max = 10.0 163 | v_max = 10.0 164 | a_max = 10.0 165 | j_max = 10.0 166 | 167 | 168 | fixed_limit = False 169 | generate_random_test_cases(num_of_cases, p_max, v_max, a_max, j_max, fixed_limit) 170 | 171 | 172 | 173 | 174 | -------------------------------------------------------------------------------- /traj/scripts/traj_segment_example: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | Simple example that fits general trajectory segment, given the start and 4 | end positions/velocities. Start and end acceleration/jerk are assumed to be zero. 5 | """ 6 | from matplotlib import pyplot as plt 7 | import traj 8 | 9 | p_max=30 10 | v_max=3.0 11 | a_max=4.0 12 | j_max=10.0 13 | 14 | 15 | ############### CASE A: v_start = v_end = 0 [normal case, start and end velocities are zeros] 16 | #case A1: no limit is reached 17 | traj.traj_plot.fit_and_plot_segment(0.0, 1.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 18 | 19 | #case A2: acc_limit is reached 20 | traj.traj_plot.fit_and_plot_segment(0.0, 3.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 21 | 22 | #case A3: vel_limit and acc_limit are reached 23 | traj.traj_plot.fit_and_plot_segment(0.0, 5.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 24 | 25 | 26 | ############## CASE B: v_start = v_end !=0 [ start and end velocities are equal but not null] 27 | #case B1: no limit is reached 28 | traj.traj_plot.fit_and_plot_segment(0.0, 2.0, 1.0, 1.0, p_max, v_max, a_max, j_max) 29 | 30 | #case B2: vel_limit is reached 31 | traj.traj_plot.fit_and_plot_segment(0.0, 3.0, 2.5, 2.5, p_max, v_max, a_max, j_max) 32 | 33 | #case B3: acc_limit is reached 34 | traj.traj_plot.fit_and_plot_segment(0.0, 3.0, 0.5, 0.5, p_max, v_max, a_max, j_max) 35 | 36 | #case B4: vel_limit and acc_limit are reached 37 | traj.traj_plot.fit_and_plot_segment(0.0, 10.0, 0.5, 0.5, p_max, v_max, a_max, j_max) 38 | 39 | 40 | 41 | ############## CASE C: v_start < v_end [acceleration] 42 | #case C1: no limit is reached 43 | traj.traj_plot.fit_and_plot_segment(0.0, 2.0, 0.5, 1.5, p_max, v_max, a_max, j_max) 44 | 45 | ##case C2: vel_limit is reached 46 | traj.traj_plot.fit_and_plot_segment(0.0, 5.0, 2.0, 2.5, p_max, v_max, a_max, j_max) 47 | 48 | ##case C3: acc_limit is reached 49 | traj.traj_plot.fit_and_plot_segment(0.0, 3.0, 0.5, 2.5, p_max, v_max, a_max, j_max) 50 | 51 | ##case C4: vel_limit and acc_limit are reached 52 | traj.traj_plot.fit_and_plot_segment(0.0, 5.0, 0.5, 2.5, p_max, v_max, a_max, j_max) 53 | 54 | 55 | 56 | ############## CASE D: v_start > v_end [deceleration] 57 | #case D1: no limit is reached 58 | traj.traj_plot.fit_and_plot_segment(0.0, 2.0, 1.5, 0.5, p_max, v_max, a_max, j_max) 59 | 60 | ##case D2: vel_limit is reached 61 | traj.traj_plot.fit_and_plot_segment(0.0, 5.0, 2.5, 2.0, p_max, v_max, a_max, j_max) 62 | 63 | ##case D3: acc_limit is reached 64 | traj.traj_plot.fit_and_plot_segment(0.0, 3.0, 2.5, 0.5, p_max, v_max, a_max, j_max) 65 | 66 | ##case D4: vel_limit and acc_limit are reached 67 | traj.traj_plot.fit_and_plot_segment(0.0, 5.0, 2.5, 0.5, p_max, v_max, a_max, j_max) 68 | 69 | 70 | 71 | 72 | ############### CASE A-: v_start = v_end = 0 [normal case, negative motion] 73 | #case A1-: no limit is reached 74 | traj.traj_plot.fit_and_plot_segment(0.0, -1.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 75 | 76 | #case A2-: acc_limit is reached 77 | traj.traj_plot.fit_and_plot_segment(0.0, -3.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 78 | 79 | #case A3-: vel_limit and acc_limit are reached 80 | traj.traj_plot.fit_and_plot_segment(0.0, -5.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 81 | 82 | 83 | ############## CASE B-: v_start = v_end !=0 [ start and end velocities are equal but not null] 84 | #case B1-: no limit is reached 85 | traj.traj_plot.fit_and_plot_segment(0.0, -2.0, -1.0, -1.0, p_max, v_max, a_max, j_max) 86 | 87 | #case B2-: vel_limit is reached 88 | traj.traj_plot.fit_and_plot_segment(0.0, -3.0, -2.5, -2.5, p_max, v_max, a_max, j_max) 89 | 90 | #case B3-: acc_limit is reached 91 | traj.traj_plot.fit_and_plot_segment(0.0, -3.0, -0.5, -0.5, p_max, v_max, a_max, j_max) 92 | 93 | #case B4-: vel_limit and acc_limit are reached 94 | traj.traj_plot.fit_and_plot_segment(0.0, -10.0, -0.5, -0.5, p_max, v_max, a_max, j_max) 95 | 96 | 97 | ############## CASE C-: v_start < v_end [acceleration] 98 | #case C1-: no limit is reached 99 | traj.traj_plot.fit_and_plot_segment(0.0, -2.0, -0.5, -1.5, p_max, v_max, a_max, j_max) 100 | 101 | ##case C2-: vel_limit is reached 102 | traj.traj_plot.fit_and_plot_segment(0.0, -5.0, -2.0, -2.5, p_max, v_max, a_max, j_max) 103 | 104 | ##case C3-: acc_limit is reached 105 | traj.traj_plot.fit_and_plot_segment(0.0, -3.0, -0.5, -2.5, p_max, v_max, a_max, j_max) 106 | 107 | ##case C4-: vel_limit and acc_limit are reached 108 | traj.traj_plot.fit_and_plot_segment(0.0, -5.0, -0.5, -2.5, p_max, v_max, a_max, j_max) 109 | 110 | 111 | ############## CASE D-: v_start > v_end [deceleration] 112 | #case D1: no limit is reached 113 | traj.traj_plot.fit_and_plot_segment(0.0, -2.0, -1.5, -0.5, p_max, v_max, a_max, j_max) 114 | 115 | ##case D2: vel_limit is reached 116 | traj.traj_plot.fit_and_plot_segment(0.0, -5.0, -2.5, -2.0, p_max, v_max, a_max, j_max) 117 | 118 | ##case D3: acc_limit is reached 119 | traj.traj_plot.fit_and_plot_segment(0.0, -3.0, -2.5, -0.5, p_max, v_max, a_max, j_max) 120 | 121 | ##case D4: vel_limit and acc_limit are reached 122 | traj.traj_plot.fit_and_plot_segment(0.0, -5.0, -2.5, -0.5, p_max, v_max, a_max, j_max) 123 | 124 | 125 | 126 | 127 | ############################################################################################### 128 | ################ complex motion: v_start*v_end <0, +ve and -ve parts ########################## 129 | ############################################################################################### 130 | 131 | 132 | ############ CASE X: positive dominant motion: pos_diff > 0 133 | ### starting from +ve to -ve 134 | traj.traj_plot.fit_and_plot_segment(0.0, 8.0, 1.5, -1.0, p_max, v_max, a_max, j_max) 135 | ### starting from -ve to +ve 136 | traj.traj_plot.fit_and_plot_segment(0.0, 5.0, -1.5, +1.0, p_max, v_max, a_max, j_max) 137 | 138 | 139 | #############CASE Y: negative dominant motion: pos_diff < 0 140 | ### starting from +ve to -ve 141 | traj.traj_plot.fit_and_plot_segment(0.0, -8.0, 1.5, -1.0, p_max, v_max, a_max, j_max) 142 | ### starting from -ve to +ve 143 | traj.traj_plot.fit_and_plot_segment(0.0, -5.0, -1.5, +1.0, p_max, v_max, a_max, j_max) 144 | -------------------------------------------------------------------------------- /traj/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages=['traj'], 6 | package_dir={'': 'src'} 7 | ) 8 | 9 | setup(**d) 10 | 11 | -------------------------------------------------------------------------------- /traj/src/traj/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | from traj_plot import plot_segment 3 | from traj_plot import fit_and_plot_segment 4 | 5 | 6 | from traj_segment import fit_traj_segment 7 | from traj_segment import calculate_jerk_sign_and_duration 8 | 9 | from segment_planning import traj_segment_planning 10 | from segment_planning import calculate_min_pos_reached_acc_jrk_time_acc_time_to_reach_final_vel 11 | 12 | from cubic_eq_roots import real_roots_cubic_eq 13 | from cubic_eq_roots import quad_eq_real_root 14 | from cubic_eq_roots import min_positive_root2 15 | from cubic_eq_roots import min_positive_root3 16 | 17 | -------------------------------------------------------------------------------- /traj/src/traj/cubic_eq_roots.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import math 3 | 4 | 5 | def real_roots_cubic_eq ( a, b, c, d): 6 | ''' 7 | This function finds the real roots of a cubic equation, 8 | ''' 9 | # print "real_roots_cubic_eq, eq_info: a={} b={} c={} d={} ".format(a, b, c, d) 10 | rt1=0 11 | rt2=0 12 | rt3=0 13 | if (a == 0.0000000): 14 | # print "The coefficient of the cube of x is 0. Please use the utility for a SECOND degree quadratic. No further action taken." 15 | rt1, rt2, n_rts = quad_eq_real_root ( b, c, d) 16 | return rt1, rt2, -100, n_rts 17 | 18 | if ( abs(d) < 1e-20): 19 | # print "One root is 0. Now divide through by x and use the utility for a SECOND degree quadratic to solve the resulting equation for the other two roots. No further action taken." 20 | rt1 = 0 21 | rt2, rt3, n_rts = quad_eq_real_root (a, b, c) 22 | return rt1, rt2, rt3, n_rts+1 23 | 24 | b /= a 25 | c /= a 26 | d /= a 27 | 28 | q = (3.0*c - (b*b))/9.0 29 | r = -(27.0*d) + b*(9.0*c - 2.0*(b*b)) 30 | r /= 54.0 31 | disc = q*q*q + r*r 32 | # print "disc= {}".format(disc) 33 | term1 = (b/3.0) 34 | if (disc > 0.0): # one root real, two are complex 35 | s = r + math.sqrt(disc) 36 | if s<0: 37 | mins_s=-s 38 | s= - mins_s**(1.0/3.0) 39 | else: 40 | s= s**(1.0/3.0) 41 | 42 | t = r - math.sqrt(disc) 43 | if t<0: 44 | mins_t=-t 45 | t= - mins_t**(1.0/3.0) 46 | else: 47 | t= t**(1.0/3.0) 48 | 49 | x1r= -term1 + s + t 50 | term1 += (s + t)/2.0 51 | x3r = -term1 52 | x2r = -term1 53 | term1 = math.sqrt(3.0)*(-t + s)/2 54 | x2r = term1 55 | rt1 = x1r 56 | rt2 = -100.0 57 | rt3 = -100.0 58 | return rt1, rt2, rt3, 1 59 | # End if (disc > 0) 60 | 61 | #The remaining options are all real 62 | if disc==0.000 : # All roots real, at least two are equal. 63 | disc=0 64 | if r<0: 65 | mins_r=-r 66 | r13= - mins_r**(1.0/3.0) 67 | else: 68 | r13= r**(1.0/3.0) 69 | 70 | x1r= -term1 + 2.0*r13 71 | x3r = -(r13 + term1) 72 | x2r = -(r13 + term1) 73 | rt1= x1r 74 | rt2 = x2r 75 | rt3= x3r 76 | return rt1, rt2, rt3, 2 77 | #End if (disc == 0) 78 | 79 | #Only one option left is that all roots are real and unequal (to get here, q < 0) 80 | # print "last case: disc < 0 , disc= " 81 | # print disc 82 | else: 83 | q = -q 84 | dum1 = q*q*q 85 | dum1 = math.acos(r/math.sqrt(dum1)) 86 | r13 = 2.0*math.sqrt(q) 87 | x1r= -term1 + r13*math.cos(dum1/3.0) 88 | x2r = -term1 + r13*math.cos((dum1 + 2.0*math.pi)/3.0) 89 | x3r = -term1 + r13*math.cos((dum1 + 4.0*math.pi)/3.0) 90 | rt1= x1r 91 | rt2= x2r 92 | rt3= x3r 93 | return rt1, rt2, rt3, 3 94 | 95 | 96 | def quad_eq_real_root (a, b, c): 97 | ''' 98 | This function finds the real roots of a quadratic equation, 99 | ''' 100 | disc= b*b - 4*a*c 101 | if(a==0.000000): 102 | if(b==0.000000): 103 | rt1= 0 104 | rt2= 0 105 | return rt1, rt2, 0 106 | else : 107 | rt1= -c/b 108 | rt2= -c/b 109 | return rt1, rt2, 1 110 | 111 | else : 112 | if(disc > 0): 113 | rt1 = (-b - math.sqrt(disc) ) / (2*a) 114 | rt2 = (-b + math.sqrt(disc) ) / (2*a) 115 | return rt1, rt2, 2 116 | else : 117 | rt1= 0 118 | rt2= 0 119 | return rt1, rt2, 0 120 | 121 | 122 | def min_positive_root2( r1, r2): 123 | ''' 124 | This function finds the minimum positive number of two numbers 125 | ''' 126 | min_rt = -1 127 | if (r1>0 and r2>0 ): 128 | if r10 ): 133 | min_rt= r1 134 | elif (r2>0 ): 135 | min_rt= r2 136 | else: 137 | raise ValueError("there is no real positive roots!" ) 138 | return min_rt 139 | 140 | 141 | def min_positive_root3( r1, r2, r3): 142 | ''' 143 | This function finds the minimum positive number of three numbers 144 | ''' 145 | min_rt=-1 146 | if (r1>0 and r2>0 and r3>0): 147 | if r10 and r2>0): 152 | min_rt=min_positive_root2( r1, r2) 153 | elif (r1>0 and r3>0): 154 | min_rt=min_positive_root2( r1, r3) 155 | elif (r2>0 and r3>0): 156 | min_rt=min_positive_root2( r2, r3) 157 | elif (r1>0): 158 | min_rt= r1 159 | elif (r2>0): 160 | min_rt= r2 161 | elif (r3>0 ): 162 | min_rt= r3 163 | else: 164 | raise ValueError("there is no real positive roots!" ) 165 | return min_rt 166 | -------------------------------------------------------------------------------- /traj/src/traj/segment_planning.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | this file contains main low level planning function "traj_segment_planning" to to calculate the values of t_jrk, t_acc, t_vel for each phase of the segment 4 | """ 5 | import rospy 6 | import math 7 | import cubic_eq_roots as rt 8 | 9 | 10 | def calculate_min_pos_reached_acc_to_reach_max_vel(v, vm, am, jm): 11 | ''' 12 | This function calculates the following variables to reach the absolute Maximum Velocity "vm" starting with initial_velocity "v": 13 | 1. the minimum position required to reach the maximum absolute velocity "vm" 14 | 2. the acceleration "acc" that has been reached to reach the maximum absolute velocity starting with "v" 15 | ''' 16 | p0=0.0 17 | a0=0.0 18 | v0=v 19 | ar = math.sqrt( jm*(vm-v0) ) 20 | t= ar/jm 21 | ta=0.0 22 | tv=0.0 23 | 24 | a1 = jm*t + a0 25 | a2 = a1 26 | a3 = -jm*t + a2 27 | a4 = a3 28 | a5 = -jm*t + a4 29 | a6 = a5 30 | 31 | v1 = jm*t*t/2.0 + a0*t + v0 32 | v2 = a1*ta + v1 33 | v3 = -jm*t*t/2.0 + a2*t + v2 34 | v4 = a3*tv + v3 35 | v5 = -jm*t*t/2.0 + a4*t + v4 36 | v6 = a5*ta + v5 37 | 38 | p1 = jm*t*t*t/6.0 + a0*t*t/2.0 + v0*t + p0 39 | p2 = a1*ta*ta/2.0 + v1*ta + p1 40 | p3 = -jm*t*t*t/6.0 + a2*t*t/2.0 + v2*t + p2 41 | p4 = a3*t*t/2.0 + v3*tv + p3 42 | p5 = -jm*t*t*t/6.0 + a4*t*t/2.0 + v4*t + p4 43 | p6 = a5*ta*ta/2.0 + v5*ta + p5 44 | p7 = jm*t*t*t/6.0 + a6*t*t/2.0 + v6*t + p6 45 | min_pos_to_max_vel = p7 46 | reached_acc_to_max_vel = ar 47 | return min_pos_to_max_vel, reached_acc_to_max_vel 48 | 49 | 50 | def calculate_reached_acc_in_case_no_const_acc_phase(Dp, v, vm, am ,jm): 51 | ''' 52 | This function calculates the acceleration that has been reached to move the joint a position equals to "DP" starting with velocity "v" 53 | this considers the case where there is no a constant acceleration phase. 54 | ''' 55 | p0=0.0 56 | v0=v 57 | a=2.0 58 | b=0.0 59 | c=4*v0*jm 60 | d=p0*jm**2-jm**2*Dp 61 | rt1, rt2, rt3, n_rts = rt.real_roots_cubic_eq ( a, b, c, d) 62 | if n_rts==1: 63 | ar = rt1 64 | elif n_rts ==2: 65 | ar = rt.min_positive_root2(rt1, rt2) 66 | elif n_rts ==3: 67 | ar = rt.min_positive_root3(rt1, rt2, rt3) 68 | return ar 69 | 70 | 71 | def calculate_min_pos_const_acc_time_to_reach_max_acc_and_max_vel(v, vm, am, jm): 72 | ''' 73 | This function calculates the following variables to reach the absolute Maximum Velocity "vm" 74 | starting with initial_velocity "v" considering that the absolute maximum acceleration "am" will be reached: 75 | 1. the minimum position required to reach the maximum absolute velocity "vm" 76 | 2. the acceleration_phase time "ta" to reach the maximum absolute velocity "vm" 77 | ''' 78 | p0=0.0 79 | a0=0.0 80 | v0=v 81 | t = am/jm 82 | tv= 0.0 83 | ta= ( vm-v0-(am**2/jm) )/am 84 | 85 | a1 = jm*t + a0; 86 | a2 = a1; 87 | a3 = -jm*t + a2; 88 | a4 = a3; 89 | a5 = -jm*t + a4; 90 | a6 = a5; 91 | 92 | v1 = jm*t*t/2.0 + a0*t + v0; 93 | v2 = a1*ta + v1; 94 | v3 = -jm*t*t/2.0 + a2*t + v2; 95 | v4 = a3*tv + v3; 96 | v5 = -jm*t*t/2.0 + a4*t + v4; 97 | v6 = a5*ta + v5; 98 | 99 | p1 = jm*t*t*t/6.0 + a0*t*t/2.0 + v0*t + p0; 100 | p2 = a1*ta*ta/2.0 + v1*ta + p1; 101 | p3 = -jm*t*t*t/6.0 + a2*t*t/2.0 + v2*t + p2; 102 | p4 = a3*t*t/2.0 + v3*tv + p3; 103 | p5 = -jm*t*t*t/6.0 + a4*t*t/2.0 + v4*t + p4; 104 | p6 = a5*ta*ta/2.0 + v5*ta + p5; 105 | p7 = jm*t*t*t/6.0 + a6*t*t/2.0 + v6*t + p6; 106 | min_pos_to_max_vel= p7-p0 107 | t_max_acc = ta 108 | return min_pos_to_max_vel, t_max_acc 109 | 110 | 111 | def calculate_min_pos_to_reach_max_acc(v, vm, am, jm): 112 | ''' 113 | This function calculates the minimum position required to reach the absolute maximum acceleration "am", starting with initial velocity "v" 114 | ''' 115 | p0=0.0 116 | a0=0.0 117 | v0=v 118 | t = am/jm 119 | tv= 0.0 120 | ta= 0.0 121 | 122 | a1 = jm*t + a0; 123 | a2 = a1; 124 | a3 = -jm*t + a2; 125 | a4 = a3; 126 | a5 = -jm*t + a4; 127 | a6 = a5; 128 | 129 | v1 = jm*t*t/2.0 + a0*t + v0; 130 | v2 = a1*ta + v1; 131 | v3 = -jm*t*t/2.0 + a2*t + v2; 132 | v4 = a3*tv + v3; 133 | v5 = -jm*t*t/2.0 + a4*t + v4; 134 | v6 = a5*ta + v5; 135 | 136 | p1 = jm*t*t*t/6.0 + a0*t*t/2.0 + v0*t + p0; 137 | p2 = a1*ta*ta/2.0 + v1*ta + p1; 138 | p3 = -jm*t*t*t/6.0 + a2*t*t/2.0 + v2*t + p2; 139 | p4 = a3*t*t/2.0 + v3*tv + p3; 140 | p5 = -jm*t*t*t/6.0 + a4*t*t/2.0 + v4*t + p4; 141 | p6 = a5*ta*ta/2.0 + v5*ta + p5; 142 | p7 = jm*t*t*t/6.0 + a6*t*t/2.0 + v6*t + p6; 143 | min_pos_to_max_acc= p7-p0 144 | return min_pos_to_max_acc 145 | 146 | 147 | def calculate_const_acc_time(Dp, v, vm, am, jm): 148 | ''' 149 | This function calculates the acceleration_phase time "ta" which is required to move the joint 150 | to new position equal to the current position plus a position difference "DP" starting with initial velocity "v" 151 | ''' 152 | # here we wont reach max vel but still we reach max acc 153 | p0=0.0 154 | v0=v 155 | # P7 equation is: (2*am^3 + 3*am^2*jm*ta + am*jm^2*ta^2 + 4*v0*am*jm + 2*v0*jm^2*ta + p0*jm^2)/jm^2 156 | a=0.0 157 | b= am*jm**2 158 | c=3*am**2*jm + 2*v0*jm**2 159 | d=2*am**3 + 4*v0*am*jm + p0*jm**2 - jm**2*Dp 160 | rt1, rt2, rt3, n_rts = rt.real_roots_cubic_eq ( a, b, c, d) 161 | 162 | if n_rts==1: 163 | ta = rt1 164 | elif n_rts ==2: 165 | ta = rt.min_positive_root2(rt1, rt2) 166 | elif n_rts ==3: 167 | ta = rt.min_positive_root3(rt1, rt2, rt3) 168 | const_acc_time = ta 169 | return const_acc_time 170 | 171 | 172 | def calculate_min_pos_reached_acc_jrk_time_acc_time_to_reach_final_vel(v0, vf, vm, am, jm): 173 | ''' 174 | This function calculates the following variables to reach the final Velocity "vf" starting with initial_vel "v0": 175 | 1. the minimum position required to reach the final Velocity "vf" 176 | 2. the acceleration "acc" that has been reached to reach the final Velocity "vf" starting with "v0" 177 | 3. phases times to reach the final Velocity "vf": jerk_phase time "tj" and acceleration_phase time "ta" 178 | ''' 179 | p0=0.0 180 | a0=0.0 181 | ar = math.sqrt( jm*abs(vf-v0) ) 182 | tj= ar/jm 183 | ta=0.0 184 | t=tj 185 | if vf==v0: 186 | return 0,0,0,0 187 | else: 188 | jm = math.copysign(jm, (vf-v0) ) 189 | 190 | a1 = jm*t + a0; 191 | a2 = a1; 192 | v1 = jm*t*t/2.0 + a0*t + v0; 193 | v2 = a1*ta + v1; 194 | p1 = jm*t*t*t/6.0 + a0*t*t/2.0 + v0*t + p0; 195 | p2 = a1*ta*ta/2.0 + v1*ta + p1; 196 | p3 = -jm*t*t*t/6.0 + a2*t*t/2.0 + v2*t + p2; 197 | 198 | if abs(a2)<=abs(am): 199 | acc_to_vf = a2 #math.copysign(a2, (vf-v0) ) 200 | min_pos_to_vf = p3 #math.copysign(p3, (vf-v0) ) 201 | else: 202 | tj =abs(am/jm) 203 | ta= ( abs(vf-v0) - abs((am**2/jm)) )/am 204 | 205 | t = tj 206 | a1 = jm*t + a0; 207 | a2 = a1; 208 | v1 = jm*t*t/2.0 + a0*t + v0; 209 | v2 = a1*ta + v1; 210 | p1 = jm*t*t*t/6.0 + a0*t*t/2.0 + v0*t + p0; 211 | p2 = a1*ta*ta/2.0 + v1*ta + p1; 212 | p3 = -jm*t*t*t/6.0 + a2*t*t/2.0 + v2*t + p2; 213 | acc_to_vf = a2 #math.copysign(a2, (vf-v0) ) 214 | min_pos_to_vf = p3 #math.copysign(p3, (vf-v0) ) 215 | return min_pos_to_vf, acc_to_vf, tj, ta 216 | 217 | 218 | def equal_vel_case_planning (pos_diff, v, abs_max_vel, abs_max_acc, abs_max_jrk): 219 | ''' 220 | this function selects a motion profile for a trajectory segment in case of the final veoclity equals the starting velocity, both equal "v" 221 | ''' 222 | # check if a const_acc phase is required or not by calcuating the reached_acc_to_max_vel to reach vf 223 | min_pos_to_max_vel, reached_acc_to_max_vel = calculate_min_pos_reached_acc_to_reach_max_vel(v, abs_max_vel, abs_max_acc, abs_max_jrk) 224 | # check if reached_acc_to_max_vel < abs_max_acc: 225 | #if yes: then no const_acc phase, check if a const_vel phase is required or not (to satisfy pos_diff) 226 | if(reached_acc_to_max_vel<= abs_max_acc): 227 | rospy.logdebug("case a: maxAcc won't be reached ! /\\/ ") 228 | reached_vel = abs_max_vel 229 | reached_acc = reached_acc_to_max_vel 230 | t_max_jrk = reached_acc_to_max_vel/abs_max_jrk 231 | t_max_acc = 0.0 232 | t_max_vel = 0.0 233 | 234 | if(pos_diff > min_pos_to_max_vel): 235 | rospy.logdebug("\n >>> case a1: require const_vel_phase=zero_acc_phase [ /\-----\/ ]" ) 236 | t_max_vel= (pos_diff - min_pos_to_max_vel )/ abs_max_vel 237 | else: 238 | rospy.logdebug("\n >>> case a2: calculate Acc corresponds to pos_diff [ /\\/ ]") 239 | acc = calculate_reached_acc_in_case_no_const_acc_phase(pos_diff, v, abs_max_vel, abs_max_acc, abs_max_jrk) 240 | t_max_jrk = acc/abs_max_jrk 241 | 242 | #if no: check if a const_acc phase is required or not (to satisfy pos_diff) 243 | elif(reached_acc_to_max_vel > abs_max_acc): 244 | rospy.logdebug("case b: maxAcc will be reached ! /'''\\.../") 245 | min_pos_to_max_vel, t_max_acc = calculate_min_pos_const_acc_time_to_reach_max_acc_and_max_vel(v, abs_max_vel, abs_max_acc, abs_max_jrk) 246 | reached_vel = abs_max_vel 247 | reached_acc = abs_max_acc 248 | t_max_jrk = abs_max_acc/abs_max_jrk 249 | t_max_vel = 0.0 250 | 251 | if(pos_diff >= min_pos_to_max_vel): 252 | rospy.logdebug("\n >>> case b1: require const_vel_phase=zero_acc_phase [ /```\------\.../ ]" ) 253 | t_max_vel= (pos_diff - min_pos_to_max_vel )/ abs_max_vel 254 | else: 255 | min_pos_to_max_acc = calculate_min_pos_to_reach_max_acc(v, abs_max_vel, abs_max_acc, abs_max_jrk) 256 | rospy.logdebug("case b2: min_pos_to_max_acc= {}, Dp= {} ".format(min_pos_to_max_acc, pos_diff) ) 257 | if(pos_diff >= min_pos_to_max_acc): 258 | rospy.logdebug( "\n >>> case b2a: calculate acc_time-reached_vel corresponds to pos_diff [ /````\\..../ ]" ) 259 | acc_time = calculate_const_acc_time(pos_diff, v, abs_max_vel, abs_max_acc, abs_max_jrk) 260 | t_max_acc = acc_time 261 | else: 262 | rospy.logdebug( "\n >>> case b2b: calculate acc corresponds to pos_diff [ /\\/ ]") 263 | acc = calculate_reached_acc_in_case_no_const_acc_phase(pos_diff, v, abs_max_vel, abs_max_acc, abs_max_jrk) 264 | t_max_jrk = acc/abs_max_jrk 265 | t_max_acc = 0.0 266 | t_max_vel = 0.0 267 | 268 | return t_max_jrk, t_max_acc, t_max_vel, reached_vel, reached_acc 269 | 270 | 271 | ### the main function to plan motion profile for a general_velocity-to-general_velocity segment 272 | def traj_segment_planning(p_start, p_end, abs_v_start, abs_v_end, abs_max_vel, abs_max_acc, abs_max_jrk): 273 | ''' 274 | this function selects a motion profile for a trajectory segment with a given start and end velocities/positions, 275 | considering the start and end accelerations/jerks are zeros! 276 | ''' 277 | #calculate min_pos required to reach vf from v0 278 | abs_min_pos_to_vf, acc_to_vf, t_jrk_to_vf, t_acc_to_vf = calculate_min_pos_reached_acc_jrk_time_acc_time_to_reach_final_vel(abs_v_start, abs_v_end, abs_max_vel, abs_max_acc, abs_max_jrk) 279 | if abs_min_pos_to_vf > abs(p_end-p_start) and abs_min_pos_to_vf - abs(p_end-p_start) > 1e-5: # if abs_min_pos_to_vf> abs(p_end-p_start), then these values are not feasible 280 | print">>> min required position difference to reach v_end from v_start= {} > abs(p_end-p_start)={} ".format(abs_min_pos_to_vf, abs(p_end-p_start) ) 281 | raise ValueError("non feasible case: violate min_pos_to_vf" ) 282 | return 0, 0, 0, 0, 0 283 | 284 | else: ## then the motion will be: 285 | if abs_v_end > abs_v_start: #from v0 to reach vf with dis=minPos_to_vf, then then plan the rest of the required distance (pos_diff - minPos_to_vf) using vf (as vf>v0) 286 | abs_v=abs_v_end 287 | else: #plan the rest of the required distance (pos_diff - minPos_to_vf) using v0 (as v0>vf), from v0 to reach vf with dis= minPos_to_vf 288 | abs_v=abs_v_start 289 | 290 | if abs( abs(p_end-p_start) - abs_min_pos_to_vf)> 1e-7: 291 | pos_diff= p_end - p_start - math.copysign( abs_min_pos_to_vf, p_end - p_start) 292 | ## plan the rest of the motion using the equal start/end vel case 293 | t_jrk, t_acc, t_vel, reached_vel, reached_acc = equal_vel_case_planning ( abs(pos_diff), abs_v, abs_max_vel, abs_max_acc, abs_max_jrk) 294 | else: 295 | t_jrk=0.0 296 | t_acc=0.0 297 | t_vel=0.0 298 | # return time for both: from v0_to_vf case and for equal_vel_case 299 | rospy.logdebug(">>> output of traj_segment_planning: t_jrk_to_vf, t_acc_to_vf, t_jrk, t_acc, t_vel: ") 300 | rospy.logdebug("{}, {}, {}, {}, {}".format(t_jrk_to_vf, t_acc_to_vf, t_jrk, t_acc, t_vel) ) 301 | return t_jrk_to_vf, t_acc_to_vf, t_jrk, t_acc, t_vel 302 | -------------------------------------------------------------------------------- /traj/src/traj/traj_plot.py: -------------------------------------------------------------------------------- 1 | from matplotlib import pyplot as plt 2 | import numpy as np 3 | import traj 4 | 5 | joint_colors = ['r', 'b', 'g', 'c', 'm', 'y', 'k', 'w'] 6 | 7 | """ 8 | Plot a 1 dimensional trajectory. Plots position, velocity, acceleration, and jerk versus time 9 | in separate subplots. 10 | Uses the entire provided figure, adding subplots as needed. 11 | """ 12 | def plot_segment(figure, position, velocity, acceleration, jerk, n_points=500, j_max=None, 13 | a_max=None, v_max=None, p_max=None): 14 | boundaries = jerk.boundaries 15 | plot_times = np.linspace(position.boundaries[0], position.boundaries[-1], n_points) 16 | positions = np.array([position(t) for t in plot_times]) 17 | velocities = np.array([velocity(t) for t in plot_times]) 18 | accelerations = np.array([acceleration(t) for t in plot_times]) 19 | jerks = np.array([jerk(t) for t in plot_times]) 20 | axes = figure.subplots(4, sharex=True) 21 | for joint_i in range(positions.shape[1]): 22 | c = joint_colors[joint_i] 23 | axes[0].plot(plot_times, positions[:,joint_i], c=c) 24 | axes[0].set_ylabel('position') 25 | axes[1].plot(plot_times, velocities[:,joint_i], c=c) 26 | axes[1].set_ylabel('velocity') 27 | axes[2].plot(plot_times, accelerations[:,joint_i], c=c) 28 | axes[2].set_ylabel('acceleration') 29 | axes[3].plot(plot_times, jerks[:,joint_i], c=c) 30 | axes[3].set_ylabel('jerk') 31 | 32 | axes[0].vlines(boundaries, positions.min(), positions.max(), color=(0.8, 0.8, 0.8)) 33 | 34 | if p_max is not None: 35 | axes[0].plot(plot_times, [p_max] * len(plot_times), '--', color='red') 36 | axes[0].plot(plot_times, [-p_max] * len(plot_times), '--', color='red') 37 | 38 | if v_max is not None: 39 | axes[1].plot(plot_times, [v_max] * len(plot_times), '--', color='red') 40 | axes[1].plot(plot_times, [-v_max] * len(plot_times), '--', color='red') 41 | axes[1].vlines(boundaries, -v_max, v_max, color=(0.8, 0.8, 0.8)) 42 | 43 | if a_max is not None: 44 | axes[2].plot(plot_times, [a_max] * len(plot_times), '--', color='red') 45 | axes[2].plot(plot_times, [-a_max] * len(plot_times), '--', color='red') 46 | axes[2].vlines(boundaries, -a_max, a_max, color=(0.8, 0.8, 0.8)) 47 | 48 | if j_max is not None: 49 | axes[3].plot(plot_times, [j_max] * len(plot_times), '--', color='red') 50 | axes[3].plot(plot_times, [-j_max] * len(plot_times), '--', color='red') 51 | axes[3].vlines(boundaries, -j_max, j_max, color=(0.8, 0.8, 0.8)) 52 | plt.legend() 53 | plt.suptitle("traj_segment") 54 | 55 | 56 | 57 | 58 | def fit_and_plot_segment(p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max): 59 | ''' 60 | this function plots a trajectory segment, given the start/end positions/velocities, 61 | Start and end acceleration/jerk are assumed to be zero. 62 | ''' 63 | position, velocity, acceleration, jerk = traj.fit_traj_segment(p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max) 64 | plot_segment(plt.figure(), position, velocity, acceleration, jerk, n_points=100, v_max=v_max, a_max=a_max, j_max=j_max) 65 | plt.show() 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | def find_phase(t, phase_times): 74 | ''' 75 | this functions find the phase at which the time instant t belongs. 76 | phase_times: contains the start and end time of each phase 77 | ''' 78 | phase_times = np.asarray(phase_times) 79 | ph = np.searchsorted(phase_times, t, side='left') -1 80 | return ph 81 | 82 | 83 | def sample( t, p_start, v_start, phase_times, phase_jrk, infl_points_acc, infl_points_vel, infl_points_pos): 84 | ''' 85 | this functions is to sample a trajectory segment, it takes as argument: 86 | 1. starting time "t_start", starting position "p_start", and starting velocity "v_start" 87 | 2. jerk value and jerk duration of each phase: J: jerk value "Jrk_v", duration "T_ph" 88 | 3. the time instant "t" at which we need to calculate pos, vel, acc, jrk 89 | 4. pos,vel, and acc at each inflection point where phase changes: Pos_v, Vel_v, Acc_v 90 | it returns: 91 | pos, vel, acc, and jrk at that time instant "t" 92 | J, T are vectors of size equal to number of phases 93 | ''' 94 | ph = find_phase(t, phase_times) 95 | if ph < 0: #before segment 96 | jrk = 0.0 97 | acc = infl_points_acc[0] 98 | vel = infl_points_vel[0] 99 | pos = infl_points_pos[0] 100 | return pos, vel, acc, jrk 101 | 102 | elif ph > 9: #after segment 103 | jrk = 0.0 104 | acc = infl_points_acc[-1] 105 | vel = infl_points_vel[-1] 106 | pos = infl_points_pos[-1] 107 | return pos, vel, acc, jrk 108 | 109 | else: 110 | t = t - phase_times[ph] 111 | jrk = phase_jrk[ph] 112 | acc = phase_jrk[ph]*t + infl_points_acc[ph] 113 | vel = phase_jrk[ph]*t**2/2.0 + infl_points_acc[ph]*t + infl_points_vel[ph] 114 | pos = phase_jrk[ph]*t**3/6.0 + infl_points_acc[ph]*t**2/2.0 + infl_points_vel[ph]*t + infl_points_pos[ph] 115 | return pos, vel, acc, jrk 116 | 117 | 118 | def sample_segment(t, t_start, p_start, v_start, phase_jrk, phase_dur): 119 | ''' 120 | this functions is to sample a trajectory segment, it takes as argument: 121 | 1. starting time "t_start", starting position "p_start", and starting velocity "v_start" 122 | 2. jerk value and jerk duration of each phase: J: jerk value "J", duration "T" 123 | 3. the time instant "t" at which we need to calculate pos, vel, acc, jrk 124 | it returns: 125 | pos, vel, acc, and jrk at that time instant "t" 126 | 127 | J, T are vectors of size equal to number of phases 128 | ''' 129 | #convert durations to times 130 | phase_times = [ sum(phase_dur[0:i]) for i in range(0, len(phase_dur)+1) ] 131 | 132 | #calculate pos,vel,acc at each inflection point where phase changes: 133 | infl_points_acc=[0.0] 134 | infl_points_vel=[v_start] 135 | infl_points_pos=[p_start] 136 | for i in range (0, 10): 137 | infl_points_acc.append( phase_jrk[i]*phase_dur[i] + infl_points_acc[i] ) 138 | infl_points_vel.append( phase_jrk[i]*phase_dur[i]**2/2.0 + infl_points_acc[i]*phase_dur[i] + infl_points_vel[i] ) 139 | infl_points_pos.append( phase_jrk[i]*phase_dur[i]**3/6.0 + infl_points_acc[i]*phase_dur[i]**2/2.0 + infl_points_vel[i]*phase_dur[i] + infl_points_pos[i] ) 140 | 141 | return sample( t-t_start, p_start, v_start, phase_times, phase_jrk, infl_points_acc, infl_points_vel, infl_points_pos) 142 | -------------------------------------------------------------------------------- /traj/src/traj/traj_segment.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ''' 3 | this file contains the main high level planning function "fit_traj_segment" to fit a trajectory segment for given start/end velocities/positions. 4 | fit_traj_segment does high level planning for the segment: 5 | 1. check the given start/end pos/vel values, if they are within the limits or not 6 | 2. check the given start/end pos/vel values, if they form feasible/logical case for generic segment or not 7 | 3. check motion type (simple +ve/-ve motion or it is complex motion where vel direction will change) 8 | 4. it calls low level planning function "traj_segment_planning" to calculate the values of t_jrk, t_acc, t_vel 9 | 5. it generates pos, vel, acc, jrk vectors using the abovemention times: t_jr, t_acc, t_vel 10 | 6. it returns vectors of pos, vel, acc, jrk 11 | ''' 12 | from sympy import integrate, Symbol 13 | from sympy.core.numbers import Float 14 | from .piecewise_function import PiecewiseFunction 15 | import traj 16 | import math 17 | import rospy 18 | 19 | # Function to assign jerk sign for each phase based on the motion (+ve/-ve): it is determined by start/end vel, and pos_diff 20 | def assign_jerk_sign_According_to_motion_type(p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max): 21 | ''' 22 | This function assigns jerk sign for each phase of the segment based on the motion type (+ve/-ve) 23 | ''' 24 | abs_v_start = abs(v_start) 25 | abs_v_end = abs(v_end) 26 | if v_start == v_end: 27 | j_max_to_vf=0 28 | j_max = math.copysign(j_max, (p_end-p_start)) 29 | 30 | else:# v_end != v_start: 31 | if v_start*v_end < 0: #won't be need it in complex motion case 32 | rospy.logdebug("this a complex motion, stop point will be calculated to join the +ve/-ve motion part " ) 33 | elif abs_v_start < abs_v_end : #acc motion 34 | if(v_start >= 0 and v_end >= 0): # positive motion 35 | j_max_to_vf = j_max #math.copysign(j_max, v_end) 36 | j_max = math.copysign(j_max, v_end) 37 | elif (v_start <= 0 and v_end <= 0): # negative motion 38 | j_max_to_vf = -j_max #math.copysign(j_max, v_end) 39 | j_max = math.copysign(j_max, v_end) 40 | else:# v_start > v_end : #dec motion 41 | if(v_start >= 0 and v_end >= 0): # positive motion 42 | j_max_to_vf = -j_max #math.copysign(j_max, v_end) 43 | j_max = math.copysign(j_max, v_end) 44 | elif (v_start <= 0 and v_end <= 0): # negative motion 45 | j_max_to_vf = j_max #math.copysign(j_max, v_end) 46 | j_max = math.copysign(j_max, v_end) 47 | return j_max_to_vf, j_max 48 | 49 | 50 | def calculate_jerk_sign_and_duration(p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max, independent_variable=Symbol('t')): 51 | ''' 52 | this function calculates the jerk_value && the duration associated with each phase of the segment 53 | ''' 54 | assert(a_max > 0.0) 55 | assert(j_max > 0.0) 56 | assert(v_max > 0.0) 57 | # Step_1: check limits for given start/end velocities/positions 58 | # if absolute values v_start/v_end/p_end is greater than v_max/p_max, we replace the values with max one 59 | # another option is to raise error and exit 60 | # for p_start: it depends on direction of v_start, as we can not put p_start as p_max if v_start is in +ve direction 61 | if(abs(v_start) > v_max): 62 | v_start = math.copysign(v_max, v_start) 63 | rospy.logdebug("\nWarning: \n>>> these values are not feasible: v_start should be within the limit v_max !") 64 | rospy.logdebug(">>> v_start: {}, v_max: {}".format(v_start, v_max) ) 65 | #if abs(v_start) - v_max >1e-15: 66 | raise ValueError("non feasible case: violate v_max, v_start: {}, v_max: {}".format(v_start, v_max) ) 67 | 68 | if(abs(v_end) > v_max): 69 | v_end = math.copysign(v_max, v_end) 70 | rospy.logdebug("\nWarning: \n>>> these values are not feasible, v_end should be within the limit v_max !") 71 | rospy.logdebug(">>> v_end: {}, v_max: {}".format(v_end, v_max) ) 72 | raise ValueError("non feasible case: violate v_max, v_end: {}, v_max: {}".format(v_end, v_max) ) 73 | 74 | if(abs(p_end) > p_max): 75 | rospy.logdebug("\nWarning: \n>>> these values are not feasible, p_end should be within the limit p_max !") 76 | p_end = math.copysign(p_max, p_end) 77 | 78 | if(abs(p_start) > p_max): 79 | p_start = math.copysign(p_max, p_start) 80 | if (p_start*v_start>0.0) or (v_start==0 and p_start*v_end>0.0): #direction of motion 81 | print"\nWarning: \n>>> these values are not feasible, p_start = p_max, and motion in the direction of v_start will violate p_max!" 82 | raise ValueError("non feasible case: violate p_max" ) 83 | 84 | # reject unfeasible/iillogical cases 85 | if (v_start>0 and v_end>0 and (p_end-p_start)<0): # +ve motion vs -ve pos_diff 86 | raise ValueError("non feasible case: vel_motion opposite to pos_motion" ) 87 | elif (v_start<0 and v_end<0 and (p_end-p_start)>0): # -ve motion vs +ve pos_diff 88 | raise ValueError("non feasible case: vel_motion opposite to pos_motion" ) 89 | 90 | # absolute value of the velocities 91 | abs_v_start = abs(v_start) 92 | abs_v_end = abs(v_end) 93 | 94 | # Step_2: check motion type: complex or simple motion 95 | # 1) complex motion: positive and negative velocities, v_start*v_end<0 #### 96 | if (v_start * v_end) < 0.0 : #complex motion: positive and negative velocity, check min distance to change diraction of the motion 97 | minPos_to_zero, acc_to_zero, t_jrk_to_zero, t_acc_to_zero = traj.calculate_min_pos_reached_acc_jrk_time_acc_time_to_reach_final_vel(v_start, 0.0, v_max, a_max, j_max) 98 | minPos_to_vf, acc_to_vf, t_jrk_to_vf, t_acc_to_vf = traj.calculate_min_pos_reached_acc_jrk_time_acc_time_to_reach_final_vel( 0.0, v_end, v_max, a_max, j_max) 99 | pos_diff = p_end - p_start 100 | pos_dominant = pos_diff - minPos_to_zero - minPos_to_vf 101 | 102 | # A) complex positive motion case 103 | if pos_dominant > 0.0: # positive dominant case, main part of the motion is in the +ve direction 104 | if v_start < 0.0 and v_end > 0.0: # from negative to positive 105 | if abs(p_start+minPos_to_zero) > p_max or abs(p_start+minPos_to_zero+minPos_to_vf) > p_max or abs(p_start+minPos_to_zero+minPos_to_vf+pos_dominant) > p_max: 106 | raise ValueError("non feasible case: violate p_max") 107 | rospy.logdebug("\n\n>>>positive dominant case: negative to positive: {}, {}, {}, {}".format(p_start, p_end, v_start, v_end) ) 108 | t_jrk_not_used, t_acc_not_used, t_jrk_dominant, t_acc_dominant, t_vel_dominant = traj.traj_segment_planning(p_start, p_end - minPos_to_zero - minPos_to_vf, abs_v_end, abs_v_end, v_max, a_max, j_max) 109 | segment_jerks_and_durations = [( j_max, t_jrk_to_zero), (0.0, t_acc_to_zero), (-j_max, t_jrk_to_zero ), 110 | ( j_max, t_jrk_to_vf), (0.0, t_acc_to_vf), (-j_max, t_jrk_to_vf ), 111 | ( j_max, t_jrk_dominant), (0.0, t_acc_dominant), (-j_max, t_jrk_dominant), (0, t_vel_dominant),(-j_max, t_jrk_dominant), (0.0, t_acc_dominant), (j_max, t_jrk_dominant) ] 112 | elif v_start > 0.0 and v_end < 0.0: #from positive to negative 113 | if abs(p_start+pos_dominant) > p_max or abs(p_start+pos_dominant+minPos_to_zero) > p_max or abs(p_start+pos_dominant+minPos_to_zero+minPos_to_vf) > p_max: 114 | raise ValueError("non feasible case: violate p_max") 115 | rospy.logdebug("\n\n>>>positive dominant case: positive to negative: {}, {}, {}, {}".format(p_start, p_end, v_start, v_end)) 116 | t_jrk_not_used, t_acc_not_used, t_jrk_dominant, t_acc_dominant, t_vel_dominant = traj.traj_segment_planning(p_start, p_end-minPos_to_zero-minPos_to_vf, abs_v_start, abs_v_start, v_max, a_max, j_max) 117 | segment_jerks_and_durations = [( j_max, t_jrk_dominant), (0.0, t_acc_dominant), (-j_max, t_jrk_dominant), (0, t_vel_dominant), (-j_max, t_jrk_dominant), (0.0, t_acc_dominant), (j_max, t_jrk_dominant), 118 | (-j_max, t_jrk_to_zero), (0.0, t_acc_to_zero), ( j_max, t_jrk_to_zero ), 119 | (-j_max, t_jrk_to_vf), (0.0, t_acc_to_vf), (j_max, t_jrk_to_vf ) ] 120 | else: 121 | raise ValueError("\n>> should be simple motion instead of complex motion case!") 122 | 123 | # B) complex negative motion case 124 | if pos_dominant < 0.0: # negative dominant case, main part of the motion is in the -ve direction 125 | if v_start < 0.0 and v_end > 0.0: # from negative to positive 126 | if abs(p_start+pos_dominant) > p_max or abs(p_start+pos_dominant+minPos_to_zero) > p_max or abs(p_start+pos_dominant+minPos_to_zero+minPos_to_vf) > p_max: 127 | raise ValueError("non feasible case: violate p_max") 128 | rospy.logdebug("\n\n>>>negative dominant case: negative to positive: {}, {}, {}, {}".format(p_start, p_end, v_start, v_end)) 129 | t_jrk_not_used, t_acc_not_used, t_jrk_dominant, t_acc_dominant, t_vel_dominant = traj.traj_segment_planning(p_start, p_end-minPos_to_zero-minPos_to_vf, abs_v_start, abs_v_start, v_max, a_max, j_max) 130 | segment_jerks_and_durations = [(-j_max, t_jrk_dominant), (0.0, t_acc_dominant), ( j_max, t_jrk_dominant), (0, t_vel_dominant),(j_max, t_jrk_dominant), (0.0, t_acc_dominant), (-j_max, t_jrk_dominant), 131 | ( j_max, t_jrk_to_zero), (0.0, t_acc_to_zero), (-j_max, t_jrk_to_zero ), 132 | ( j_max, t_jrk_to_vf), (0.0, t_acc_to_vf), (-j_max, t_jrk_to_vf ) ] 133 | elif v_start > 0.0 and v_end < 0.0: #from positive to negative 134 | if abs(p_start+minPos_to_zero) > p_max or abs(p_start+minPos_to_zero+minPos_to_vf) > p_max or abs(p_start+minPos_to_zero+minPos_to_vf+pos_dominant) > p_max: 135 | raise ValueError("non feasible case: violate p_max") 136 | rospy.logdebug("\n\n>>>negative dominant case: positive to negative: {}, {}, {}, {}".format(p_start, p_end, v_start, v_end) ) 137 | t_jrk_not_used, t_acc_not_used, t_jrk_dominant, t_acc_dominant, t_vel_dominant = traj.traj_segment_planning(p_start+ minPos_to_zero + minPos_to_vf, p_end , abs_v_end, abs_v_end, v_max, a_max, j_max) 138 | segment_jerks_and_durations = [(-j_max, t_jrk_to_zero), (0.0, t_acc_to_zero), ( j_max, t_jrk_to_zero ), 139 | (-j_max, t_jrk_to_vf), (0.0, t_acc_to_vf), ( j_max, t_jrk_to_vf ), 140 | (-j_max, t_jrk_dominant), (0.0, t_acc_dominant), ( j_max, t_jrk_dominant), (0, t_vel_dominant), ( j_max, t_jrk_dominant), (0.0, t_acc_dominant), (-j_max, t_jrk_dominant) ] 141 | else: 142 | raise ValueError("\n>> should be simple motion instead of complex motion case!") 143 | 144 | # check if final_velocity value gives optimal motion to change from +ve/-ve to -ve/+ve 145 | # this part can be used later to assign velocity vf in the parameterizarion part 146 | minPos_v02vf = minPos_to_zero + minPos_to_vf 147 | if v_start < 0 and v_end > 0: #from -ve to +ve 148 | if pos_diff < minPos_v02vf: 149 | rospy.logdebug(">>>>>> non optimal case <<<<<<< ") 150 | else: 151 | if pos_diff > minPos_v02vf: 152 | rospy.logdebug(">>>>>> non optimal case <<<<<<< ") 153 | 154 | # 2)simple motion: positive or negative velocity, v0 and vf have same sign 155 | else: 156 | # same action will be performed in both simple +ve or simple -ve motion, this part can be used later 157 | # A) simple positive motion 158 | if(v_start >= 0 and v_end >= 0): # case one: both are positive 159 | rospy.logdebug("\n\n>>>simple postive motion: {}, {}, {}, {} ".format(p_start, p_end, v_start, v_end)) 160 | 161 | # B) simple negative motion 162 | elif (v_start <= 0 and v_end <= 0): # case two: both are negative 163 | rospy.logdebug("\n\n>>>simple negative motion: {}, {}, {}, {} ".format(p_start, p_end, v_start, v_end)) 164 | t_jrk_to_vf, t_acc_to_vf, t_jrk, t_acc, t_vel = traj.traj_segment_planning(p_start, p_end, abs_v_start, abs_v_end, v_max, a_max, j_max) 165 | j_max_to_vf, j_max = assign_jerk_sign_According_to_motion_type(p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max) 166 | if abs_v_end > abs_v_start: 167 | segment_jerks_and_durations = [(j_max_to_vf, t_jrk_to_vf), (0.0, t_acc_to_vf), (-j_max_to_vf, t_jrk_to_vf), (j_max, t_jrk), (0.0, t_acc), (-j_max, t_jrk), (0.0, t_vel), (-j_max,t_jrk), (0.0, t_acc), (j_max, t_jrk)] 168 | else: 169 | segment_jerks_and_durations = [(j_max, t_jrk), (0.0, t_acc), (-j_max, t_jrk), (0.0, t_vel), (-j_max,t_jrk), (0.0, t_acc), (j_max, t_jrk), (j_max_to_vf, t_jrk_to_vf), (0.0, t_acc_to_vf), (-j_max_to_vf, t_jrk_to_vf)] 170 | 171 | # one option to retun segment_jerks_and_durations and send it to JTC and then use it for interpolation on the JTC side 172 | return segment_jerks_and_durations 173 | 174 | 175 | # the main function to fit traj segment with generic start/end velocities 176 | def fit_traj_segment(p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max, independent_variable=Symbol('t')): 177 | ''' 178 | This function selects a motion profile for a general trajectory segment with a given start/end velocities/positions 179 | considering the start and end accelerations/jerks are zeros 180 | ''' 181 | 182 | # Step_1. calculate jerk_sign_and_duration 183 | segment_jerks_and_durations = calculate_jerk_sign_and_duration(p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max, independent_variable=Symbol('t')) 184 | 185 | # Step_2: generate pos, vel, acc, jrk using the calculated "segment_jerks_and_durations" 186 | p0 = p_start 187 | v0 = v_start 188 | a0 = 0.0 189 | times = [0.0] 190 | jerk_functions = [] 191 | acceleration_functions = [] 192 | velocity_functions = [] 193 | position_functions = [] 194 | # Integrate jerk starting from the start of the trajectory and going all the way through the end. 195 | for j0, T in segment_jerks_and_durations: 196 | times.append(times[-1] + T) 197 | j = Float(j0) 198 | a = integrate(j, independent_variable) + a0 199 | v = integrate(a, independent_variable) + v0 200 | p = integrate(v, independent_variable) + p0 201 | jerk_functions.append(j) 202 | acceleration_functions.append(a) 203 | velocity_functions.append(v) 204 | position_functions.append(p) 205 | a0 = a.subs({independent_variable: T}) 206 | v0 = v.subs({independent_variable: T}) 207 | p0 = p.subs({independent_variable: T}) 208 | position = PiecewiseFunction(times, position_functions, independent_variable) 209 | velocity = PiecewiseFunction(times, velocity_functions, independent_variable) 210 | acceleration = PiecewiseFunction(times, acceleration_functions, independent_variable) 211 | jerk = PiecewiseFunction(times, jerk_functions, independent_variable) 212 | return position, velocity, acceleration, jerk 213 | -------------------------------------------------------------------------------- /traj/test/test_min_real_root_cubic_eq.py: -------------------------------------------------------------------------------- 1 | import nose 2 | import traj 3 | import numpy as np 4 | ''' 5 | to test cubic_eq_roos.py, main objective to get the min positive real root for cubic equation, 6 | if non positive real roots exist, it should raise error 7 | ''' 8 | 9 | 10 | def check_min_positive_real_root_for_cubic_eq(a, b, c, d, root_value): 11 | r1, r2, r3, n_rts= traj.real_roots_cubic_eq ( a, b, c, d) 12 | if n_rts==1: 13 | r = r1 14 | elif n_rts ==2: 15 | r = traj.min_positive_root2(r1, r2) 16 | elif n_rts ==3: 17 | r = traj.min_positive_root3(r1, r2, r3) 18 | 19 | print r1, r2, r3, n_rts 20 | assert np.isclose(r, root_value) 21 | 22 | 23 | 24 | def test_3_different_positive_real_roots(): 25 | check_min_positive_real_root_for_cubic_eq(1, -6, 11, -6, 1) 26 | 27 | 28 | def test_two_positive_real_root(): 29 | check_min_positive_real_root_for_cubic_eq(0, 1, -3, 2, 1) 30 | 31 | 32 | def test_two_positive_real_root_2(): 33 | check_min_positive_real_root_for_cubic_eq(1, -3, 2, 0, 1) 34 | 35 | 36 | def test_one_positive_real_root(): 37 | check_min_positive_real_root_for_cubic_eq(1, 1, 1, -3, 1) 38 | 39 | 40 | 41 | ### when error should be raised 42 | def test_3_different_negative_real_roots(): 43 | with nose.tools.assert_raises_regexp(ValueError, "there is no real positive roots!"): 44 | check_min_positive_real_root_for_cubic_eq(1, 6, 11, 6, 0) 45 | 46 | -------------------------------------------------------------------------------- /traj/test/test_traj_segment.py: -------------------------------------------------------------------------------- 1 | import nose 2 | import numpy as np 3 | import traj 4 | 5 | 6 | 7 | def check_fit_traj_segment(p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max): 8 | position, velocity, jerk, acceleration = traj.fit_traj_segment(p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max) 9 | 10 | t_start = position.boundaries[0] 11 | t_end = position.boundaries[-1] 12 | p_start_computed = position(t_start) 13 | v_start_computed = velocity(t_start) 14 | p_end_computed = position(t_end)[0] 15 | v_end_computed = velocity(t_end)[0] 16 | 17 | #print p_start_computed, p_end_computed, v_start_computed, v_end_computed 18 | #print " ", p_start, p_end, v_start, v_end 19 | 20 | assert np.isclose(p_start, p_start_computed) 21 | assert np.isclose(p_end, p_end_computed) 22 | assert np.isclose(v_start, v_start_computed) 23 | assert np.isclose(v_end, v_end_computed) 24 | 25 | 26 | #limits 27 | p_max=30 28 | v_max=3.0 29 | a_max=4.0 30 | j_max=10.0 31 | 32 | 33 | 34 | ############### CASE A: v_start = v_end = 0 [normal case, start and end velocities are zeros] 35 | #case A1: no limit is reached 36 | def test_not_max_vel_nor_max_acc_zero_equal_velocity(): 37 | check_fit_traj_segment(0.0, 1.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 38 | 39 | #case A2: acc_limit is reached 40 | def test_max_acc_but_not_max_vel_zero_equal_velocity(): 41 | check_fit_traj_segment(0.0, 3.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 42 | 43 | #case A3: vel_limit and acc_limit are reached 44 | def test_max_vel_and_max_acc_zero_equal_velocity(): 45 | check_fit_traj_segment(0.0, 5.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 46 | 47 | 48 | ############## CASE B: v_start = v_end !=0 [ start and end velocities are equal but not null] 49 | #case B1: no limit is reached 50 | def test_not_max_vel_nor_max_acc_nonzero_equal_veolcity(): 51 | check_fit_traj_segment(0.0, 2.0, 1.0, 1.0, p_max, v_max, a_max, j_max) 52 | 53 | #case B2: vel_limit is reached 54 | def test_max_vel_but_not_max_acc_nonzero_equal_veolcity(): 55 | check_fit_traj_segment(0.0, 3.0, 2.5, 2.5, p_max, v_max, a_max, j_max) 56 | 57 | #case B3: acc_limit is reached 58 | def test_max_acc_but_not_max_vel_nonzero_equal_veolcity(): 59 | check_fit_traj_segment(0.0, 3.0, 0.5, 0.5, p_max, v_max, a_max, j_max) 60 | 61 | #case B4: vel_limit and acc_limit are reached 62 | def test_max_vel_and_max_acc_nonzero_equal_veolcity(): 63 | check_fit_traj_segment(0.0, 10.0, 0.5, 0.5, p_max, v_max, a_max, j_max) 64 | 65 | 66 | 67 | ############## CASE C: v_start < v_end [acceleration] 68 | #case C1: no limit is reached 69 | def test_not_max_vel_nor_max_acc_acceleration(): 70 | check_fit_traj_segment(0.0, 2.0, 0.5, 1.5, p_max, v_max, a_max, j_max) 71 | 72 | ##case C2: vel_limit is reached 73 | def test_max_vel_but_not_max_acc_acceleration(): 74 | check_fit_traj_segment(0.0, 5.0, 2.0, 2.5, p_max, v_max, a_max, j_max) 75 | 76 | ##case C3: acc_limit is reached 77 | def test_max_acc_but_not_max_vel_acceleration(): 78 | check_fit_traj_segment(0.0, 3.0, 0.5, 2.5, p_max, v_max, a_max, j_max) 79 | 80 | ##case C4: vel_limit and acc_limit are reached 81 | def test_max_vel_and_max_acc_acceleration(): 82 | check_fit_traj_segment(0.0, 5.0, 0.5, 2.5, p_max, v_max, a_max, j_max) 83 | 84 | 85 | 86 | ############## CASE D: v_start > v_end [deceleration] 87 | #case D1: no limit is reached 88 | def test_not_max_vel_nor_max_acc_deceleration(): 89 | check_fit_traj_segment(0.0, 2.0, 1.5, 0.5, p_max, v_max, a_max, j_max) 90 | 91 | ##case D2: vel_limit is reached 92 | def test_max_vel_but_not_max_acc_deceleration(): 93 | check_fit_traj_segment(0.0, 5.0, 2.5, 2.0, p_max, v_max, a_max, j_max) 94 | 95 | ##case D3: acc_limit is reached 96 | def test_max_acc_but_not_max_vel_deceleration(): 97 | check_fit_traj_segment(0.0, 3.0, 2.5, 0.5, p_max, v_max, a_max, j_max) 98 | 99 | ##case D4: vel_limit and acc_limit are reached 100 | def test_max_vel_and_max_acc_deceleration(): 101 | check_fit_traj_segment(0.0, 5.0, 2.5, 0.5, p_max, v_max, a_max, j_max) 102 | 103 | 104 | 105 | 106 | ############### CASE A-: v_start = v_end = 0 [normal case, negative motion] 107 | #case A1-: no limit is reached 108 | def test_not_max_vel_nor_max_acc_zero_equal_velocity_negative_motion(): 109 | check_fit_traj_segment(0.0, -1.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 110 | 111 | #case A2-: acc_limit is reached 112 | def test_max_acc_but_not_max_vel_zero_equal_velocity_negative_motion(): 113 | check_fit_traj_segment(0.0, -3.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 114 | 115 | #case A3-: vel_limit and acc_limit are reached 116 | def test_max_vel_and_max_acc_zero_equal_velocity_negative_motion(): 117 | check_fit_traj_segment(0.0, -5.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 118 | 119 | 120 | ############## CASE B-: v_start = v_end !=0 [ start and end velocities are equal but not null] 121 | #case B1-: no limit is reached 122 | def test_not_max_vel_nor_max_acc_nonzero_equal_veolcity_negative_motion(): 123 | check_fit_traj_segment(0.0, -2.0, -1.0, -1.0, p_max, v_max, a_max, j_max) 124 | 125 | #case B2-: vel_limit is reached 126 | def test_max_vel_but_not_max_acc_nonzero_equal_veolcity_negative_motion(): 127 | check_fit_traj_segment(0.0, -3.0, -2.5, -2.5, p_max, v_max, a_max, j_max) 128 | 129 | #case B3-: acc_limit is reached 130 | def test_max_acc_but_not_max_vel_nonzero_equal_veolcity_negative_motion(): 131 | check_fit_traj_segment(0.0, -3.0, -0.5, -0.5, p_max, v_max, a_max, j_max) 132 | 133 | #case B4-: vel_limit and acc_limit are reached 134 | def test_max_vel_and_max_acc_nonzero_equal_veolcity_negative_motion(): 135 | check_fit_traj_segment(0.0, -10.0, -0.5, -0.5, p_max, v_max, a_max, j_max) 136 | 137 | 138 | ############## CASE C-: v_start < v_end [acceleration] 139 | #case C1-: no limit is reached 140 | def test_not_max_vel_nor_max_acc_acceleration_negative_motion(): 141 | check_fit_traj_segment(0.0, -2.0, -0.5, -1.5, p_max, v_max, a_max, j_max) 142 | 143 | ##case C2-: vel_limit is reached 144 | def test_max_vel_but_not_max_acc_acceleration_negative_motion(): 145 | check_fit_traj_segment(0.0, -5.0, -2.0, -2.5, p_max, v_max, a_max, j_max) 146 | 147 | ##case C3-: acc_limit is reached 148 | def test_max_acc_but_not_max_vel_acceleration_negative_motion(): 149 | check_fit_traj_segment(0.0, -3.0, -0.5, -2.5, p_max, v_max, a_max, j_max) 150 | 151 | ##case C4-: vel_limit and acc_limit are reached 152 | def test_max_vel_and_max_acc_acceleration_negative_motion(): 153 | check_fit_traj_segment(0.0, -5.0, -0.5, -2.5, p_max, v_max, a_max, j_max) 154 | 155 | 156 | ############## CASE D-: v_start > v_end [deceleration] 157 | #case D1: no limit is reached 158 | def test_not_max_vel_nor_max_acc_deceleration_negative_motion(): 159 | check_fit_traj_segment(0.0, -2.0, -1.5, -0.5, p_max, v_max, a_max, j_max) 160 | 161 | ##case D2: vel_limit is reached 162 | def test_max_vel_but_not_max_acc_deceleration_negative_motion(): 163 | check_fit_traj_segment(0.0, -5.0, -2.5, -2.0, p_max, v_max, a_max, j_max) 164 | 165 | ##case D3: acc_limit is reached 166 | def test_max_acc_but_not_max_vel_deceleration_negative_motion(): 167 | check_fit_traj_segment(0.0, -3.0, -2.5, -0.5, p_max, v_max, a_max, j_max) 168 | 169 | ##case D4: vel_limit and acc_limit are reached 170 | def test_max_vel_and_max_acc_deceleration_negative_motion(): 171 | check_fit_traj_segment(0.0, -5.0, -2.5, -0.5, p_max, v_max, a_max, j_max) 172 | 173 | 174 | 175 | 176 | ############################################################################################## 177 | ############### complex motion: v_start*v_end <0, +ve and -ve parts ########################## 178 | ############################################################################################## 179 | 180 | 181 | ############ CASE X: positive dominant motion: pos_diff > 0 182 | ### starting from +ve to -ve 183 | def test_X1_complex_positive_dominant_motion(): 184 | check_fit_traj_segment(0.0, 10.0, 1.5, -1.0, p_max, v_max, a_max, j_max) 185 | ### starting from -ve to +ve 186 | def test_X2_complex_positive_dominant_motion(): 187 | check_fit_traj_segment(0.0, 5.0, -1.5, 1.0, p_max, v_max, a_max, j_max) 188 | 189 | 190 | #############CASE Y: negative dominant motion: pos_diff < 0 191 | ### starting from +ve to -ve 192 | def test_Y1_complex_positive_dominant_motion(): 193 | check_fit_traj_segment(0.0, -10.0, 1.5, -1.0, p_max, v_max, a_max, j_max) 194 | ### starting from -ve to +ve 195 | def test_Y2_complex_positive_dominant_motion(): 196 | check_fit_traj_segment(0.0, -5.0, -1.5, 1.0, p_max, v_max, a_max, j_max) 197 | 198 | 199 | 200 | 201 | -------------------------------------------------------------------------------- /traj/test/test_traj_segment_middle_points.py: -------------------------------------------------------------------------------- 1 | import nose 2 | import numpy as np 3 | import traj 4 | 5 | 6 | ### main test function to check start/end and intermediate pos, vel, acc, jrk 7 | def check_fit_traj_segment(p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max): 8 | position, velocity, acceleration, jerk = traj.fit_traj_segment(p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max) 9 | 10 | #### 1) test if the calculated start/end pos/vel are equal to the given ones 11 | t_start = position.boundaries[0] 12 | t_end = position.boundaries[-1] 13 | p_start_computed = position(t_start) 14 | v_start_computed = velocity(t_start) 15 | p_end_computed = position(t_end)[0] 16 | v_end_computed = velocity(t_end)[0] 17 | 18 | assert np.isclose(p_start, p_start_computed) 19 | assert np.isclose(p_end, p_end_computed) 20 | assert np.isclose(v_start, v_start_computed) 21 | assert np.isclose(v_end, v_end_computed) 22 | 23 | #### 2) test if the calculated pos/vel/acc/jrk [at each time_instatnt has change in segment_phase] are within the given limits p_max, v_max, a_max, j_max 24 | for phase_time in position.boundaries: 25 | p_computed = position(phase_time) 26 | v_computed = velocity(phase_time) 27 | a_computed = acceleration(phase_time) 28 | j_computed = jerk(phase_time) 29 | # print "p_computed, v_computed, a_computed, j_computed :" 30 | # print p_computed, v_computed, a_computed, j_computed 31 | assert np.less_equal( abs(p_computed), p_max+1e-6) #added 1e-6 because sometime it gives fails even if the difference is less than (1e-6) 32 | assert np.less_equal( abs(v_computed), v_max+1e-6) 33 | assert np.less_equal( abs(a_computed), a_max+1e-6) 34 | assert np.less_equal( abs(j_computed), j_max+1e-6) 35 | 36 | 37 | 38 | 39 | p_max=10 40 | v_max=3.0 41 | a_max=4.0 42 | j_max=10.0 43 | 44 | 45 | 46 | ############### CASE A: v_start = v_end = 0 [normal case, start and end velocities are zeros] 47 | #case A1: no limit is reached 48 | def test_not_max_vel_nor_max_acc_zero_equal_velocity(): 49 | check_fit_traj_segment(0.0, 1.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 50 | 51 | #case A2: acc_limit is reached 52 | def test_max_acc_but_not_max_vel_zero_equal_velocity(): 53 | check_fit_traj_segment(0.0, 3.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 54 | 55 | #case A3: vel_limit and acc_limit are reached 56 | def test_max_vel_and_max_acc_zero_equal_velocity(): 57 | check_fit_traj_segment(0.0, 5.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 58 | 59 | 60 | ############## CASE B: v_start = v_end !=0 [ start and end velocities are equal but not null] 61 | #case B1: no limit is reached 62 | def test_not_max_vel_nor_max_acc_nonzero_equal_veolcity(): 63 | check_fit_traj_segment(0.0, 2.0, 1.0, 1.0, p_max, v_max, a_max, j_max) 64 | 65 | #case B2: vel_limit is reached 66 | def test_max_vel_but_not_max_acc_nonzero_equal_veolcity(): 67 | check_fit_traj_segment(0.0, 3.0, 2.5, 2.5, p_max, v_max, a_max, j_max) 68 | 69 | #case B3: acc_limit is reached 70 | def test_max_acc_but_not_max_vel_nonzero_equal_veolcity(): 71 | check_fit_traj_segment(0.0, 3.0, 0.5, 0.5, p_max, v_max, a_max, j_max) 72 | 73 | #case B4: vel_limit and acc_limit are reached 74 | def test_max_vel_and_max_acc_nonzero_equal_veolcity(): 75 | check_fit_traj_segment(0.0, 10.0, 0.5, 0.5, p_max, v_max, a_max, j_max) 76 | 77 | 78 | 79 | ############## CASE C: v_start < v_end [acceleration] 80 | #case C1: no limit is reached 81 | def test_not_max_vel_nor_max_acc_acceleration(): 82 | check_fit_traj_segment(0.0, 2.0, 0.5, 1.5, p_max, v_max, a_max, j_max) 83 | 84 | ##case C2: vel_limit is reached 85 | def test_max_vel_but_not_max_acc_acceleration(): 86 | check_fit_traj_segment(0.0, 5.0, 2.0, 2.5, p_max, v_max, a_max, j_max) 87 | 88 | ##case C3: acc_limit is reached 89 | def test_max_acc_but_not_max_vel_acceleration(): 90 | check_fit_traj_segment(0.0, 3.0, 0.5, 2.5, p_max, v_max, a_max, j_max) 91 | 92 | ##case C4: vel_limit and acc_limit are reached 93 | def test_max_vel_and_max_acc_acceleration(): 94 | check_fit_traj_segment(0.0, 5.0, 0.5, 2.5, p_max, v_max, a_max, j_max) 95 | 96 | 97 | 98 | ############## CASE D: v_start > v_end [deceleration] 99 | #case D1: no limit is reached 100 | def test_not_max_vel_nor_max_acc_deceleration(): 101 | check_fit_traj_segment(0.0, 2.0, 1.5, 0.5, p_max, v_max, a_max, j_max) 102 | 103 | ##case D2: vel_limit is reached 104 | def test_max_vel_but_not_max_acc_deceleration(): 105 | check_fit_traj_segment(0.0, 5.0, 2.5, 2.0, p_max, v_max, a_max, j_max) 106 | 107 | ##case D3: acc_limit is reached 108 | def test_max_acc_but_not_max_vel_deceleration(): 109 | check_fit_traj_segment(0.0, 3.0, 2.5, 0.5, p_max, v_max, a_max, j_max) 110 | 111 | ##case D4: vel_limit and acc_limit are reached 112 | def test_max_vel_and_max_acc_deceleration(): 113 | check_fit_traj_segment(0.0, 5.0, 2.5, 0.5, p_max, v_max, a_max, j_max) 114 | 115 | 116 | 117 | 118 | ############### CASE A-: v_start = v_end = 0 [normal case, negative motion] 119 | #case A1-: no limit is reached 120 | def test_not_max_vel_nor_max_acc_zero_equal_velocity_negative_motion(): 121 | check_fit_traj_segment(0.0, -1.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 122 | 123 | #case A2-: acc_limit is reached 124 | def test_max_acc_but_not_max_vel_zero_equal_velocity_negative_motion(): 125 | check_fit_traj_segment(0.0, -3.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 126 | 127 | #case A3-: vel_limit and acc_limit are reached 128 | def test_max_vel_and_max_acc_zero_equal_velocity_negative_motion(): 129 | check_fit_traj_segment(0.0, -5.0, 0.0, 0.0, p_max, v_max, a_max, j_max) 130 | 131 | 132 | ############## CASE B-: v_start = v_end !=0 [ start and end velocities are equal but not null] 133 | #case B1-: no limit is reached 134 | def test_not_max_vel_nor_max_acc_nonzero_equal_veolcity_negative_motion(): 135 | check_fit_traj_segment(0.0, -2.0, -1.0, -1.0, p_max, v_max, a_max, j_max) 136 | 137 | #case B2-: vel_limit is reached 138 | def test_max_vel_but_not_max_acc_nonzero_equal_veolcity_negative_motion(): 139 | check_fit_traj_segment(0.0, -3.0, -2.5, -2.5, p_max, v_max, a_max, j_max) 140 | 141 | #case B3-: acc_limit is reached 142 | def test_max_acc_but_not_max_vel_nonzero_equal_veolcity_negative_motion(): 143 | check_fit_traj_segment(0.0, -3.0, -0.5, -0.5, p_max, v_max, a_max, j_max) 144 | 145 | #case B4-: vel_limit and acc_limit are reached 146 | def test_max_vel_and_max_acc_nonzero_equal_veolcity_negative_motion(): 147 | check_fit_traj_segment(0.0, -10.0, -0.5, -0.5, p_max, v_max, a_max, j_max) 148 | 149 | 150 | ############## CASE C-: v_start < v_end [acceleration] 151 | #case C1-: no limit is reached 152 | def test_not_max_vel_nor_max_acc_acceleration_negative_motion(): 153 | check_fit_traj_segment(0.0, -2.0, -0.5, -1.5, p_max, v_max, a_max, j_max) 154 | 155 | ##case C2-: vel_limit is reached 156 | def test_max_vel_but_not_max_acc_acceleration_negative_motion(): 157 | check_fit_traj_segment(0.0, -5.0, -2.0, -2.5, p_max, v_max, a_max, j_max) 158 | 159 | ##case C3-: acc_limit is reached 160 | def test_max_acc_but_not_max_vel_acceleration_negative_motion(): 161 | check_fit_traj_segment(0.0, -3.0, -0.5, -2.5, p_max, v_max, a_max, j_max) 162 | 163 | ##case C4-: vel_limit and acc_limit are reached 164 | def test_max_vel_and_max_acc_acceleration_negative_motion(): 165 | check_fit_traj_segment(0.0, -5.0, -0.5, -2.5, p_max, v_max, a_max, j_max) 166 | 167 | 168 | ############## CASE D-: v_start > v_end [deceleration] 169 | #case D1: no limit is reached 170 | def test_not_max_vel_nor_max_acc_deceleration_negative_motion(): 171 | check_fit_traj_segment(0.0, -2.0, -1.5, -0.5, p_max, v_max, a_max, j_max) 172 | 173 | ##case D2: vel_limit is reached 174 | def test_max_vel_but_not_max_acc_deceleration_negative_motion(): 175 | check_fit_traj_segment(0.0, -5.0, -2.5, -2.0, p_max, v_max, a_max, j_max) 176 | 177 | ##case D3: acc_limit is reached 178 | def test_max_acc_but_not_max_vel_deceleration_negative_motion(): 179 | check_fit_traj_segment(0.0, -3.0, -2.5, -0.5, p_max, v_max, a_max, j_max) 180 | 181 | ##case D4: vel_limit and acc_limit are reached 182 | def test_max_vel_and_max_acc_deceleration_negative_motion(): 183 | check_fit_traj_segment(0.0, -5.0, -2.5, -0.5, p_max, v_max, a_max, j_max) 184 | 185 | 186 | 187 | 188 | ############################################################################################### 189 | ################ complex motion: v_start*v_end <0, +ve and -ve parts ########################## 190 | ############################################################################################### 191 | 192 | 193 | ############ CASE X: positive dominant motion: pos_diff > 0 194 | ### starting from +ve to -ve 195 | def test_X1_complex_positive_dominant_motion(): 196 | check_fit_traj_segment(0.0, 8.0, 1.5, -1.0, p_max, v_max, a_max, j_max) 197 | ### starting from -ve to +ve 198 | def test_X2_complex_positive_dominant_motion(): 199 | check_fit_traj_segment(0.0, 5.0, -1.5, +1.0, p_max, v_max, a_max, j_max) 200 | 201 | 202 | #############CASE Y: negative dominant motion: pos_diff < 0 203 | ### starting from +ve to -ve 204 | def test_Y1_complex_positive_dominant_motion(): 205 | check_fit_traj_segment(0.0, -8.0, 1.5, -1.0, p_max, v_max, a_max, j_max) 206 | ### starting from -ve to +ve 207 | def test_Y2_complex_positive_dominant_motion(): 208 | check_fit_traj_segment(0.0, -5.0, -1.5, +1.0, p_max, v_max, a_max, j_max) 209 | 210 | 211 | 212 | 213 | 214 | 215 | ################################################################################################ 216 | ################# cases where exception should be raised by the function ####################### 217 | ################################################################################################ 218 | def test_exception_vel_motion_opposite_to_pos_motion_1(): 219 | with nose.tools.assert_raises_regexp(ValueError, "non feasible case: vel_motion opposite to pos_motion"): 220 | position, velocity, jerk, acceleration = traj.fit_traj_segment(0.0, 5.0, -1.5, -1.0, p_max, v_max, a_max, j_max) 221 | 222 | def test_exception_vel_motion_opposite_to_pos_motion_2(): 223 | with nose.tools.assert_raises_regexp(ValueError, "non feasible case: vel_motion opposite to pos_motion"): 224 | position, velocity, jerk, acceleration = traj.fit_traj_segment(0.0, -5.0, 1.5, 1.0, p_max, v_max, a_max, j_max) 225 | 226 | 227 | def test_exception_violate_min_pos_to_vf(): 228 | with nose.tools.assert_raises_regexp(ValueError, "non feasible case: violate min_pos_to_vf"): 229 | position, velocity, jerk, acceleration = traj.fit_traj_segment(4.0, 4.4, 0.2, 2.5, p_max, v_max, a_max, j_max) 230 | 231 | def test_exception_violate_p_max(): 232 | with nose.tools.assert_raises_regexp(ValueError, "non feasible case: violate p_max"): 233 | position, velocity, jerk, acceleration = traj.fit_traj_segment(9.0, 8.0, 2.5, -3.0, p_max, v_max, a_max, j_max) 234 | 235 | 236 | 237 | --------------------------------------------------------------------------------