├── descartes_core ├── rosdoc.yaml ├── CHANGELOG.rst ├── src │ └── trajectory_id.cpp ├── mainpage.dox ├── package.xml ├── CMakeLists.txt └── include │ └── descartes_core │ ├── trajectory_timing_constraint.h │ ├── path_planner_base.h │ ├── utils.h │ ├── trajectory_id.h │ ├── robot_model.h │ └── trajectory_pt.h ├── descartes_moveit ├── rosdoc.yaml ├── CHANGELOG.rst ├── mainpage.dox ├── moveit_adapter_plugins.xml ├── package.xml ├── src │ ├── plugin_init.cpp │ └── ikfast_moveit_state_adapter.cpp ├── CMakeLists.txt └── include │ └── descartes_moveit │ ├── utils.h │ ├── ikfast_moveit_state_adapter.h │ ├── seed_search.h │ └── moveit_state_adapter.h ├── descartes_planner ├── rosdoc.yaml ├── CHANGELOG.rst ├── descartes_planner_plugins.xml ├── src │ ├── plugin_init.cpp │ ├── ladder_graph_dag_search.cpp │ └── dense_planner.cpp ├── mainpage.dox ├── package.xml ├── include │ └── descartes_planner │ │ ├── ladder_graph_dag_search.h │ │ ├── dense_planner.h │ │ ├── planning_graph.h │ │ ├── sparse_planner.h │ │ ├── planning_graph_edge_policy.h │ │ └── ladder_graph.h └── CMakeLists.txt ├── descartes_trajectory ├── rosdoc.yaml ├── CHANGELOG.rst ├── test │ ├── utest.cpp │ └── trajectory_pt.cpp ├── package.xml ├── mainpage.dox ├── CMakeLists.txt ├── include │ └── descartes_trajectory │ │ ├── axial_symmetric_pt.h │ │ └── joint_trajectory_pt.h └── src │ ├── axial_symmetric_pt.cpp │ └── joint_trajectory_pt.cpp ├── descartes ├── CMakeLists.txt └── package.xml ├── .gitignore ├── descartes_tests ├── test │ ├── moveit │ │ ├── resources │ │ │ └── kuka_kr210 │ │ │ │ ├── kinematics.yaml │ │ │ │ ├── joint_limits.yaml │ │ │ │ ├── kuka_kr210.srdf │ │ │ │ └── kr210l150.urdf │ │ ├── launch │ │ │ ├── utest.launch │ │ │ └── moveit_state_adapter.launch │ │ ├── utest.cpp │ │ └── moveit_state_adapter_test.cpp │ ├── planner │ │ ├── utest.cpp │ │ ├── dense_planner.cpp │ │ ├── sparse_planner.cpp │ │ ├── utils │ │ │ ├── trajectory_maker.h │ │ │ └── trajectory_maker.cpp │ │ ├── planner_tests.h │ │ └── planning_graph_tests.cpp │ └── trajectory │ │ ├── utest.cpp │ │ ├── joint_trajectory_pt.cpp │ │ ├── cartesian_robot_test.cpp │ │ ├── robot_model_test.hpp │ │ ├── axial_symmetric_pt.cpp │ │ └── cart_trajectory_pt.cpp ├── package.xml ├── CMakeLists.txt ├── include │ └── descartes_tests │ │ └── cartesian_robot.h └── src │ └── cartesian_robot.cpp ├── .travis.yml ├── descartes_utilities ├── package.xml ├── CMakeLists.txt ├── include │ └── descartes_utilities │ │ └── ros_conversions.h └── src │ └── ros_conversions.cpp └── README.md /descartes_core/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | exclude_symbols: pretty_print* 3 | -------------------------------------------------------------------------------- /descartes_moveit/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | exclude_symbols: pretty_print* 3 | -------------------------------------------------------------------------------- /descartes_planner/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | exclude_symbols: pretty_print* 3 | -------------------------------------------------------------------------------- /descartes_trajectory/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | exclude_symbols: pretty_print* 3 | -------------------------------------------------------------------------------- /descartes/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(descartes) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() -------------------------------------------------------------------------------- /descartes_moveit/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package descartes_moveit 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.1 (2015-04-01) 6 | ------------------ 7 | * Alpha Release 8 | * Contributors: Jonathan Meyer, Shaun Edwards, gavanderhoorn, jrgnicho, ros-devel 9 | -------------------------------------------------------------------------------- /descartes_core/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package descartes_core 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.1 (2015-04-01) 6 | ------------------ 7 | * Alpha Release 8 | * Contributors: Jonathan Meyer, Purser Sturgeon II, Shaun Edwards, gavanderhoorn, jrgnicho, ros, ros-devel 9 | -------------------------------------------------------------------------------- /descartes_planner/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package descartes_planner 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.1 (2015-04-01) 6 | ------------------ 7 | * Alpha Release 8 | * Contributors: Jonathan Meyer, Shaun Edwards, gavanderhoorn, jrgnicho, ratnesh, ros-devel 9 | -------------------------------------------------------------------------------- /descartes_trajectory/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package descartes_trajectory 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.1 (2015-04-01) 6 | ------------------ 7 | * Alpha Release 8 | * Contributors: Jonathan Meyer, Shaun Edwards, gavanderhoorn, jrgnicho, ros-devel 9 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Compiled Dynamic libraries 8 | *.so 9 | *.dylib 10 | *.dll 11 | 12 | # Compiled Static libraries 13 | *.lai 14 | *.la 15 | *.a 16 | *.lib 17 | 18 | # Executables 19 | *.exe 20 | *.out 21 | *.app 22 | 23 | # eclipse 24 | .cproject 25 | .project 26 | 27 | # build 28 | devel/ 29 | 30 | # temp files 31 | *~ 32 | 33 | -------------------------------------------------------------------------------- /descartes_tests/test/moveit/resources/kuka_kr210/kinematics.yaml: -------------------------------------------------------------------------------- 1 | manipulator: 2 | ## IKFast plugin not returning "closest" solution 3 | ## kinematics_solver: kuka_kr210_manipulator_kinematics/IKFastKinematicsPlugin 4 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 5 | kinematics_solver_search_resolution: 0.01 6 | kinematics_solver_timeout: 0.01 7 | kinematics_solver_attempts: 10 8 | -------------------------------------------------------------------------------- /descartes_moveit/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b descartes_moveit provides a reference implementation of a descartes_core::RobotModel using MoveIt! to perform the math for a generic robot. 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | - descartes_moveit::MoveitStateAdapter : This class provides a reference implementation of a descartes_core::RobotModel using Moveit! to perform the underlying calculations. Not thread safe. 14 | 15 | */ 16 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: trusty 3 | language: generic 4 | compiler: 5 | - gcc 6 | notifications: 7 | email: 8 | on_failure: always 9 | env: 10 | matrix: 11 | - USE_DEB=true 12 | ROS_DISTRO="melodic" 13 | ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu 14 | - USE_DEB=true 15 | ROS_DISTRO="melodic" 16 | ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu 17 | install: 18 | - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config 19 | script: 20 | - source .ci_config/travis.sh 21 | -------------------------------------------------------------------------------- /descartes_moveit/moveit_adapter_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This is a robot model adapter for descartes 5 | 6 | 7 | This is a robot model adapter for descartes when using a MoveIt-IKFast plugin 8 | 9 | 10 | -------------------------------------------------------------------------------- /descartes_planner/descartes_planner_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | This is a planner for dense trajectories 6 | 7 | 9 | This is a planner that samples from a dense trajectory in order to plan a sparse trajectory and then 10 | interpolates through the rest of the dense points 11 | 12 | 13 | -------------------------------------------------------------------------------- /descartes_tests/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | descartes_tests 4 | 0.1.0 5 | 6 | A package dedicated to unit tests for the Descartes cartesian path 7 | planning library. 8 | 9 | 10 | Jonathan Meyer 11 | Apache 2.0 12 | 13 | gtest 14 | rostest 15 | rosunit 16 | moveit_kinematics 17 | 18 | catkin 19 | descartes_moveit 20 | descartes_planner 21 | descartes_trajectory 22 | eigen_conversions 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /descartes_tests/test/moveit/launch/utest.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /descartes_trajectory/test/utest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include 20 | 21 | int main(int argc, char **argv) 22 | { 23 | testing::InitGoogleTest(&argc, argv); 24 | return RUN_ALL_TESTS(); 25 | } 26 | -------------------------------------------------------------------------------- /descartes_tests/test/planner/utest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include 20 | 21 | int main(int argc, char **argv) 22 | { 23 | testing::InitGoogleTest(&argc, argv); 24 | return RUN_ALL_TESTS(); 25 | } 26 | -------------------------------------------------------------------------------- /descartes_tests/test/trajectory/utest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include 20 | 21 | int main(int argc, char **argv) 22 | { 23 | testing::InitGoogleTest(&argc, argv); 24 | return RUN_ALL_TESTS(); 25 | } 26 | -------------------------------------------------------------------------------- /descartes_utilities/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | descartes_utilities 4 | 0.0.1 5 | 6 | This package contains helper routines for working with the Descartes motion planning library 7 | that ease practical use, but do fit cleanly into the core library. This includes conversions 8 | to ROS trajectories, and similar operations. 9 | 10 | 11 | Jonathan Meyer 12 | 13 | Apache 2.0 14 | 15 | catkin 16 | 17 | descartes_core 18 | descartes_trajectory 19 | trajectory_msgs 20 | 21 | descartes_core 22 | descartes_trajectory 23 | trajectory_msgs 24 | 25 | -------------------------------------------------------------------------------- /descartes_tests/test/planner/dense_planner.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include "planner_tests.h" 20 | #include 21 | 22 | INSTANTIATE_TYPED_TEST_CASE_P(DensePlannerTest, PathPlannerTest, descartes_planner::DensePlanner); 23 | -------------------------------------------------------------------------------- /descartes_tests/test/planner/sparse_planner.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include "planner_tests.h" 20 | #include 21 | 22 | INSTANTIATE_TYPED_TEST_CASE_P(SparsePlannerTest, PathPlannerTest, descartes_planner::SparsePlanner); -------------------------------------------------------------------------------- /descartes_core/src/trajectory_id.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include "descartes_core/trajectory_id.h" 20 | 21 | using namespace descartes_core; 22 | uint64_t detail::IdGenerator::counter_(1); 23 | boost::mutex detail::IdGenerator::counter_mutex_; 24 | -------------------------------------------------------------------------------- /descartes_tests/test/moveit/resources/kuka_kr210/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | joint_limits: 2 | joint_4: 3 | has_velocity_limits: true 4 | max_velocity: 3.124139447 5 | has_acceleration_limits: false 6 | max_acceleration: 0 7 | joint_1: 8 | has_velocity_limits: true 9 | max_velocity: 2.146755039 10 | has_acceleration_limits: false 11 | max_acceleration: 0 12 | joint_3: 13 | has_velocity_limits: true 14 | max_velocity: 1.954768816 15 | has_acceleration_limits: false 16 | max_acceleration: 0 17 | joint_2: 18 | has_velocity_limits: true 19 | max_velocity: 2.007128695 20 | has_acceleration_limits: false 21 | max_acceleration: 0 22 | joint_6: 23 | has_velocity_limits: true 24 | max_velocity: 3.822271167 25 | has_acceleration_limits: false 26 | max_acceleration: 0 27 | joint_5: 28 | has_velocity_limits: true 29 | max_velocity: 3.001966396 30 | has_acceleration_limits: false 31 | max_acceleration: 0 -------------------------------------------------------------------------------- /descartes_tests/test/trajectory/joint_trajectory_pt.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include "descartes_trajectory/joint_trajectory_pt.h" 20 | #include 21 | 22 | using namespace descartes_core; 23 | using namespace descartes_trajectory; 24 | 25 | TEST(JointTrajPt, construction) 26 | { 27 | JointTrajectoryPt def(); 28 | } 29 | -------------------------------------------------------------------------------- /descartes_core/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b descartes_core defines the abstract interfaces for the core descartes concepts of robot models, trajectory points, and path planners. 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | The main APIs of are largely captured in the following interface classes: 15 | - descartes_core::TrajectoryPt : Defines the interface for a descartes trajectory point. Emodies the logic of how to search for valid joint solutions. 16 | 17 | - descartes_core::RobotModel : Defines the interface for a descartes robot model. These models embody the kinematics of the robot. They are used to do forward and inverse kinematics and are also responsible for checking the validity of positions and movements. 18 | 19 | - descartes_core::PathPlannerBase : Defines the interface to a descartes path planner. Planners are responsible for transforming a sequence of trajectory points into a meaningful sequence of joint positions. 20 | 21 | */ 22 | -------------------------------------------------------------------------------- /descartes_trajectory/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | descartes_trajectory 4 | 0.0.1 5 | The descartes_trajectory package 6 | 7 | Jorge Nicho 8 | Jonathan Meyer 9 | Shaun Edwards 10 | 11 | Apache2.0 12 | Jorge Nicho 13 | 14 | catkin 15 | descartes_core 16 | moveit_core 17 | roscpp 18 | cmake_modules 19 | eigen 20 | 21 | descartes_core 22 | moveit_core 23 | roscpp 24 | 25 | rosunit 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Descartes 2 | ========= 3 | [![Build Status](https://travis-ci.com/ros-industrial-consortium/descartes.svg?branch=melodic-devel)](https://travis-ci.com/ros-industrial-consortium/descartes) 4 | 5 | ROS-Industrial Special Project: Cartesian Path Planner 6 | 7 | [René Descartes](http://en.wikipedia.org/wiki/Ren%C3%A9_Descartes) _René Descartes (French: [ʁəne dekaʁt]; Latinized: Renatus Cartesius; adjectival form: "Cartesian"; 31 March 1596 – 11 February 1650) was a French philosopher, mathematician and writer who spent most of his life in the Dutch Republic. He has been dubbed The Father of Modern Philosophy, and much subsequent Western philosophy is a response to his writings, which are studied closely to this day._ 8 | 9 | Further information on the Decartes project can be found on the ROS [wiki](http://wiki.ros.org/descartes). Those who are interested in contributing should look at the open [issues](https://github.com/ros-industrial-consortium/descartes/issues) and then email the [ROS-Industrial developer group](mailto:swri-ros-pkg-dev@googlegroups.com) before performing any work. 10 | -------------------------------------------------------------------------------- /descartes_moveit/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | descartes_moveit 4 | 0.0.1 5 | Moveit wrapper functions for descartes base types 6 | 7 | Jorge Nicho 8 | Jonathan Meyer 9 | Shaun Edwards 10 | 11 | Apache2 12 | 13 | Shaun Edwards 14 | 15 | catkin 16 | 17 | rosconsole_bridge 18 | moveit_core 19 | tf 20 | cmake_modules 21 | descartes_core 22 | descartes_trajectory 23 | moveit_ros_planning 24 | pluginlib 25 | eigen 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /descartes_planner/src/plugin_init.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | #include 19 | #include 20 | #include 21 | 22 | PLUGINLIB_EXPORT_CLASS(descartes_planner::DensePlanner, descartes_core::PathPlannerBase) 23 | PLUGINLIB_EXPORT_CLASS(descartes_planner::SparsePlanner, descartes_core::PathPlannerBase) 24 | -------------------------------------------------------------------------------- /descartes_moveit/src/plugin_init.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | PLUGINLIB_EXPORT_CLASS(descartes_moveit::MoveitStateAdapter, descartes_core::RobotModel) 24 | PLUGINLIB_EXPORT_CLASS(descartes_moveit::IkFastMoveitStateAdapter, descartes_core::RobotModel) 25 | -------------------------------------------------------------------------------- /descartes/package.xml: -------------------------------------------------------------------------------- 1 | 2 | descartes 3 | 0.0.0 4 | Descartes is a ROS-Industrial project for performing path-planning on under-defined Cartesian trajectories. More details regarding motivation can be found in rep-I0003.rst. 5 | 6 | Shaun Edwards 7 | Jorge Nicho 8 | Jonathan Meyer 9 | Apache2.0 10 | http://wiki.ros.org/descartes_moveit 11 | Ratnesh Madaan 12 | 13 | catkin 14 | descartes_core 15 | descartes_moveit 16 | descartes_planner 17 | descartes_trajectory 18 | descartes_utilities 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /descartes_utilities/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | project(descartes_utilities) 3 | 4 | add_compile_options(-std=c++11 -Wall -Wextra) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | descartes_core 8 | descartes_trajectory 9 | trajectory_msgs 10 | ) 11 | 12 | catkin_package( 13 | INCLUDE_DIRS include 14 | LIBRARIES ${PROJECT_NAME} 15 | CATKIN_DEPENDS descartes_core descartes_trajectory trajectory_msgs 16 | ) 17 | 18 | include_directories( 19 | include 20 | ${catkin_INCLUDE_DIRS} 21 | ) 22 | 23 | add_library(${PROJECT_NAME} 24 | src/ros_conversions.cpp 25 | ) 26 | 27 | add_dependencies(${PROJECT_NAME} 28 | ${catkin_EXPORTED_TARGETS} 29 | ) 30 | 31 | target_link_libraries(${PROJECT_NAME} 32 | ${catkin_LIBRARIES} 33 | ) 34 | 35 | install(TARGETS ${PROJECT_NAME} 36 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 37 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 38 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 39 | ) 40 | 41 | install(DIRECTORY include/${PROJECT_NAME}/ 42 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 43 | FILES_MATCHING PATTERN "*.h" 44 | ) 45 | -------------------------------------------------------------------------------- /descartes_trajectory/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b descartes_trajectory provides reference implementations of descartes_core::TrajectoryPt for common use cases. 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | The reference implementations are: 14 | - descartes_trajectory::CartTrajectoryPt : A trajectory point representing a 6D cartesian pose, possibly with some tolerances. Points in cartesian space are sampled and then expanded into (possibly many) joint solutions using descartes_core::RobotModel. 15 | - descartes_trajectory::JointTrajectoryPt : A trajectory point representing the specification of the position of all active joints on the robot, optionally with tolerances. 16 | - descartes_trajectory::AxialSymmetricPt : A specialization of descartes_trajectory::CartTrajectoryPt where all tolerances are zero except for a given rotation axis which is allowed to rotate freely. A user may specify a nominal pose for the end-effector and an axis (X_AXIS, Y_AXIS, or Z_AXIS) representing the axis of the nominal pose about which to search for solutions. 17 | 18 | */ 19 | -------------------------------------------------------------------------------- /descartes_tests/test/moveit/utest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | int main(int argc, char **argv) 24 | { 25 | testing::InitGoogleTest(&argc, argv); 26 | ros::init(argc, argv, "utest"); 27 | boost::thread ros_thread(boost::bind(&ros::spin)); 28 | 29 | int res = RUN_ALL_TESTS(); 30 | ros_thread.interrupt(); 31 | ros_thread.join(); 32 | 33 | return res; 34 | } 35 | -------------------------------------------------------------------------------- /descartes_planner/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b descartes_planner provides reference implementations of descartes_core::PathPlannerBase. 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | This package offers the following path planners: 14 | - descartes_planner::DensePlanner : DensePlanner takes a brute force approach to finding a sequence of joint solutions that defines an "optimum" path through a sequence of input descartes_core::TrajectoryPt's. Optimum is defined as the path with minimal joint changes. It works by expanding all of the valid inverse kinematic solutions for every trajectory point passed in. Edges are added between every possible pair of solutions from contigous user trajectory points. Dijkstra's algorithm is then used to find a path with minimal joint motion through the graph. 15 | 16 | - descartes_planner::SparsePlanner : SparsePlanner attempts to optimize the process used by DensePlanner by not expanding every point. Instead a subset of the points are expanded and the planner attempts to use joint interpolation to find a satisfactory solution to the not-expanded points (thereby reducing the number of IK solutions required). 17 | 18 | */ 19 | -------------------------------------------------------------------------------- /descartes_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | descartes_planner 4 | 0.0.1 5 | The descartes_planner package 6 | 7 | Jorge Nicho 8 | Jonathan Meyer 9 | Shaun Edwards 10 | 11 | Apache2.0 12 | 13 | Jorge Nicho 14 | 15 | catkin 16 | 17 | descartes_core 18 | descartes_trajectory 19 | boost 20 | roscpp 21 | pluginlib 22 | cmake_modules 23 | eigen 24 | 25 | descartes_core 26 | descartes_trajectory 27 | boost 28 | roscpp 29 | pluginlib 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /descartes_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | descartes_core 4 | 0.0.1 5 | The descartes_core package creates joint trajectories for trajectory plans. Trajectory plans are typically underdefined paths through space that allow for kinematic/dynamic tolerances, such as unspecified tool roll. 6 | 7 | Jorge Nicho 8 | Jonathan Meyer 9 | Shaun Edwards 10 | 11 | Apache2.0 12 | https://github.com/industrial-moveit/descartes_core 13 | 14 | Dan Solomon 15 | 16 | catkin 17 | 18 | boost 19 | moveit_core 20 | roscpp 21 | cmake_modules 22 | eigen 23 | 24 | boost 25 | moveit_core 26 | roscpp 27 | 28 | rosunit 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /descartes_tests/test/trajectory/cartesian_robot_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include "descartes_tests/cartesian_robot.h" 20 | #include "robot_model_test.hpp" 21 | 22 | using namespace descartes_core; 23 | 24 | using testing::Types; 25 | 26 | namespace descartes_tests 27 | { 28 | template <> 29 | RobotModelPtr CreateRobotModel() 30 | { 31 | return RobotModelPtr(new CartesianRobot()); 32 | } 33 | 34 | template 35 | class CartesianRobotModelTest : public descartes_tests::RobotModelTest 36 | { 37 | }; 38 | 39 | INSTANTIATE_TYPED_TEST_CASE_P(CartesianRobotModelTest, RobotModelTest, CartesianRobot); 40 | 41 | } // descartes_tests 42 | -------------------------------------------------------------------------------- /descartes_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | project(descartes_core) 3 | 4 | add_compile_options(-std=c++11 -Wall -Wextra) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | cmake_modules 8 | moveit_core 9 | roscpp 10 | ) 11 | 12 | find_package(Boost REQUIRED) 13 | find_package(Eigen3 REQUIRED) 14 | 15 | # Eigen 3.2 (Wily) only provides EIGEN3_INCLUDE_DIR, not EIGEN3_INCLUDE_DIRS 16 | if(NOT EIGEN3_INCLUDE_DIRS) 17 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 18 | endif() 19 | 20 | catkin_package( 21 | INCLUDE_DIRS 22 | include 23 | LIBRARIES 24 | ${PROJECT_NAME} 25 | CATKIN_DEPENDS 26 | moveit_core 27 | roscpp 28 | DEPENDS 29 | Boost 30 | EIGEN3 31 | ) 32 | 33 | ########### 34 | ## Build ## 35 | ########### 36 | include_directories(include 37 | ${catkin_INCLUDE_DIRS} 38 | ${Boost_INCLUDE_DIRS} 39 | ${EIGEN3_INCLUDE_DIRS} 40 | ) 41 | 42 | add_library(${PROJECT_NAME} 43 | src/trajectory_id.cpp 44 | ) 45 | 46 | target_link_libraries(${PROJECT_NAME} 47 | ${catkin_LIBRARIES} 48 | ${EIGEN3_LIBRARIES} 49 | ) 50 | 51 | ############# 52 | ## Install ## 53 | ############# 54 | 55 | install( 56 | TARGETS ${PROJECT_NAME} 57 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 58 | 59 | install( 60 | DIRECTORY include/${PROJECT_NAME}/ 61 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 62 | -------------------------------------------------------------------------------- /descartes_tests/test/moveit/launch/moveit_state_adapter.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /descartes_tests/test/planner/utils/trajectory_maker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef DESCARTES_TEST_TRAJECTORY_MAKER_H 20 | #define DESCARTES_TEST_TRAJECTORY_MAKER_H 21 | 22 | #include "descartes_trajectory/cart_trajectory_pt.h" 23 | #include "descartes_core/utils.h" 24 | 25 | namespace descartes_tests 26 | { 27 | // Makes a linear trajectory with a given tool speed 28 | std::vector makeConstantVelocityTrajectory(const Eigen::Vector3d& start, 29 | const Eigen::Vector3d& stop, 30 | double tool_vel, size_t n_steps); 31 | 32 | // Make a pattern that moves in a line but that oscillates along the 33 | // y axis as it moves 34 | std::vector makeZigZagTrajectory(double x_start, double x_stop, double y_amplitude, 35 | double tool_vel, size_t n_steps); 36 | } 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /descartes_moveit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | project(descartes_moveit) 3 | 4 | add_compile_options(-std=c++11 -Wall -Wextra) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | cmake_modules 8 | descartes_core 9 | descartes_trajectory 10 | moveit_core 11 | moveit_ros_planning 12 | pluginlib 13 | rosconsole_bridge 14 | tf 15 | ) 16 | 17 | find_package(Boost REQUIRED) 18 | find_package(Eigen3 REQUIRED) 19 | 20 | # Eigen 3.2 (Wily) only provides EIGEN3_INCLUDE_DIR, not EIGEN3_INCLUDE_DIRS 21 | if(NOT EIGEN3_INCLUDE_DIRS) 22 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 23 | endif() 24 | 25 | catkin_package( 26 | INCLUDE_DIRS 27 | include 28 | LIBRARIES 29 | ${PROJECT_NAME} 30 | CATKIN_DEPENDS 31 | cmake_modules 32 | descartes_core 33 | descartes_trajectory 34 | moveit_core 35 | rosconsole_bridge 36 | tf 37 | DEPENDS 38 | Boost 39 | EIGEN3 40 | ) 41 | 42 | ########### 43 | ## Build ## 44 | ########### 45 | include_directories(include 46 | ${catkin_INCLUDE_DIRS} 47 | ${Boost_INCLUDE_DIRS} 48 | ${EIGEN3_INCLUDE_DIRS} 49 | ) 50 | 51 | ## Descartes MoveIt lib 52 | add_library(${PROJECT_NAME} 53 | src/ikfast_moveit_state_adapter.cpp 54 | src/moveit_state_adapter.cpp 55 | src/plugin_init.cpp 56 | src/seed_search.cpp 57 | ) 58 | target_link_libraries(${PROJECT_NAME} 59 | ${catkin_LIBRARIES} 60 | ) 61 | 62 | ############# 63 | ## Install ## 64 | ############# 65 | 66 | install( 67 | TARGETS ${PROJECT_NAME} 68 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 69 | 70 | install( 71 | FILES moveit_adapter_plugins.xml 72 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 73 | 74 | install( 75 | DIRECTORY include/${PROJECT_NAME}/ 76 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 77 | -------------------------------------------------------------------------------- /descartes_moveit/include/descartes_moveit/utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * utils.h 3 | * 4 | * Created on: Feb 1, 2019 5 | * Author: Jorge Nicho 6 | */ 7 | /* 8 | * Software License Agreement (Apache License) 9 | * 10 | * Copyright (c) 2019, Jorge Nicho 11 | * 12 | * Licensed under the Apache License, Version 2.0 (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License at 15 | * 16 | * http://www.apache.org/licenses/LICENSE-2.0 17 | * 18 | * Unless required by applicable law or agreed to in writing, software 19 | * distributed under the License is distributed on an "AS IS" BASIS, 20 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 21 | * See the License for the specific language governing permissions and 22 | * limitations under the License. 23 | */ 24 | 25 | #ifndef INCLUDE_DESCARTES_MOVEIT_UTILS_H_ 26 | #define INCLUDE_DESCARTES_MOVEIT_UTILS_H_ 27 | 28 | #include 29 | 30 | namespace descartes_moveit 31 | { 32 | 33 | /** 34 | * @brief utility function that converts an Eigen Affine Transform type to an Isometry one. 35 | * @param t The Affine transform 36 | * @return An Isometry transform 37 | */ 38 | template 39 | static Eigen::Transform toIsometry(const Eigen::Transform& t) 40 | { 41 | Eigen::Transform o; 42 | o.translation() = t.translation(); 43 | o.linear() = t.rotation(); 44 | return std::move(o); 45 | } 46 | 47 | /** 48 | * @brief workaround to allow compatibility with previous versions of MoveIt! that used Affine3 as the 49 | * preferred transform type. 50 | * @param t The Affine transform 51 | * @return An Isometry transform 52 | */ 53 | template 54 | static Eigen::Transform toIsometry(const Eigen::Transform& t) 55 | { 56 | return std::move(Eigen::Transform(t)); 57 | } 58 | 59 | } 60 | 61 | 62 | 63 | #endif /* INCLUDE_DESCARTES_MOVEIT_UTILS_H_ */ 64 | -------------------------------------------------------------------------------- /descartes_planner/include/descartes_planner/ladder_graph_dag_search.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2016, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef DESCARTES_LADDER_GRAPH_DAG_SEARCH_H 20 | #define DESCARTES_LADDER_GRAPH_DAG_SEARCH_H 21 | 22 | #include "descartes_planner/ladder_graph.h" 23 | 24 | namespace descartes_planner 25 | { 26 | 27 | // Directed Acyclic graph search 28 | class DAGSearch 29 | { 30 | public: 31 | using predecessor_t = unsigned; 32 | using size_type = std::size_t; 33 | 34 | explicit DAGSearch(const LadderGraph& graph); 35 | 36 | double run(); 37 | 38 | std::vector shortestPath() const; 39 | 40 | private: 41 | const LadderGraph& graph_; 42 | 43 | struct SolutionRung 44 | { 45 | std::vector distance; 46 | std::vector predecessor; 47 | }; 48 | 49 | inline double& distance(size_type rung, size_type index) noexcept 50 | { 51 | return solution_[rung].distance[index]; 52 | } 53 | 54 | inline predecessor_t& predecessor(size_type rung, size_type index) noexcept 55 | { 56 | return solution_[rung].predecessor[index]; 57 | } 58 | 59 | inline const predecessor_t& predecessor(size_type rung, size_type index) const noexcept 60 | { 61 | return solution_[rung].predecessor[index]; 62 | } 63 | 64 | std::vector solution_; 65 | }; 66 | } // descartes_planner 67 | #endif 68 | -------------------------------------------------------------------------------- /descartes_trajectory/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | project(descartes_trajectory) 3 | 4 | add_compile_options(-std=c++11 -Wall -Wextra) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | cmake_modules 8 | descartes_core 9 | moveit_core 10 | roscpp 11 | ) 12 | 13 | find_package(Boost REQUIRED) 14 | find_package(Eigen3 REQUIRED) 15 | 16 | # Eigen 3.2 (Wily) only provides EIGEN3_INCLUDE_DIR, not EIGEN3_INCLUDE_DIRS 17 | if(NOT EIGEN3_INCLUDE_DIRS) 18 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 19 | endif() 20 | 21 | ################################### 22 | ## catkin specific configuration ## 23 | ################################### 24 | catkin_package( 25 | INCLUDE_DIRS 26 | include 27 | LIBRARIES 28 | ${PROJECT_NAME} 29 | CATKIN_DEPENDS 30 | descartes_core 31 | moveit_core 32 | roscpp 33 | DEPENDS 34 | Boost 35 | EIGEN3 36 | ) 37 | 38 | ########### 39 | ## Build ## 40 | ########### 41 | 42 | include_directories(include 43 | ${catkin_INCLUDE_DIRS} 44 | ${Boost_INCLUDE_DIRS} 45 | ${EIGEN3_INCLUDE_DIRS} 46 | ) 47 | 48 | add_library(${PROJECT_NAME} 49 | src/axial_symmetric_pt.cpp 50 | src/cart_trajectory_pt.cpp 51 | src/joint_trajectory_pt.cpp 52 | ) 53 | 54 | target_link_libraries(${PROJECT_NAME} 55 | ${catkin_LIBRARIES} 56 | ) 57 | 58 | 59 | ############# 60 | ## Install ## 61 | ############# 62 | 63 | install( 64 | TARGETS ${PROJECT_NAME} 65 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 66 | 67 | install( 68 | DIRECTORY include/${PROJECT_NAME}/ 69 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 70 | 71 | 72 | ############# 73 | ## Testing ## 74 | ############# 75 | 76 | if(CATKIN_ENABLE_TESTING) 77 | set(UTEST_SRC_FILES 78 | test/utest.cpp 79 | test/trajectory_pt.cpp 80 | ) 81 | catkin_add_gtest(${PROJECT_NAME}_utest ${UTEST_SRC_FILES}) 82 | target_compile_definitions(${PROJECT_NAME}_utest PUBLIC GTEST_USE_OWN_TR1_TUPLE=0) 83 | target_link_libraries(${PROJECT_NAME}_utest ${PROJECT_NAME}) 84 | endif() 85 | -------------------------------------------------------------------------------- /descartes_utilities/include/descartes_utilities/ros_conversions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2016, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | /* 19 | * ros_conversions.h 20 | * 21 | * Created on: Feb 28, 2016 22 | * Author: Jonathan Meyer 23 | */ 24 | 25 | #ifndef DESCARTES_ROS_CONVERSIONS_H 26 | #define DESCARTES_ROS_CONVERSIONS_H 27 | 28 | #include 29 | #include 30 | 31 | namespace descartes_utilities 32 | { 33 | /** 34 | * @brief Converts a sequence of Descartes joint trajectory points to ROS trajectory points. 35 | * Copies timing if specified, and sets vel/acc/effort fields to zeros. 36 | * @param model Descartes robot model associated with the Descartes joint trajectory 37 | * @param joint_traj Sequence of 'joint trajectory points' as returned by Dense/Sparse planner 38 | * @param default_joint_vel If a point, does not have timing specified, this value (in rads/s) 39 | * is used to calculate a 'default' time. Must be > 0 & less than 100. 40 | * @param out Buffer in which to store the resulting ROS trajectory. Only overwritten on success. 41 | * @return True if the conversion succeeded. False otherwise. 42 | */ 43 | bool toRosJointPoints(const descartes_core::RobotModel& model, 44 | const std::vector& joint_traj, double default_joint_vel, 45 | std::vector& out); 46 | } 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /descartes_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | project(descartes_planner) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | cmake_modules 8 | descartes_core 9 | descartes_trajectory 10 | pluginlib 11 | roscpp 12 | ) 13 | 14 | find_package(Boost REQUIRED) 15 | find_package(Eigen3 REQUIRED) 16 | 17 | # Let's try to use open-mp parallization if we can 18 | find_package(OpenMP) 19 | if (OPENMP_FOUND) 20 | message(STATUS "Descartes will use OpenMP") 21 | set(OpenMP_FLAGS ${OpenMP_CXX_FLAGS}) 22 | endif() 23 | 24 | # Eigen 3.2 (Wily) only provides EIGEN3_INCLUDE_DIR, not EIGEN3_INCLUDE_DIRS 25 | if(NOT EIGEN3_INCLUDE_DIRS) 26 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 27 | endif() 28 | 29 | ################################### 30 | ## catkin specific configuration ## 31 | ################################### 32 | catkin_package( 33 | INCLUDE_DIRS 34 | include 35 | LIBRARIES 36 | ${PROJECT_NAME} 37 | CATKIN_DEPENDS 38 | descartes_core 39 | descartes_trajectory 40 | roscpp 41 | DEPENDS 42 | Boost 43 | EIGEN3 44 | ) 45 | 46 | ########### 47 | ## Build ## 48 | ########### 49 | include_directories(include 50 | ${catkin_INCLUDE_DIRS} 51 | ${Boost_INCLUDE_DIRS} 52 | ${EIGEN3_INCLUDE_DIRS} 53 | ) 54 | 55 | ## DescartesTrajectoryPt lib 56 | add_library(${PROJECT_NAME} 57 | src/dense_planner.cpp 58 | src/ladder_graph_dag_search.cpp 59 | src/planning_graph.cpp 60 | src/plugin_init.cpp 61 | src/sparse_planner.cpp 62 | ) 63 | 64 | target_link_libraries(${PROJECT_NAME} 65 | ${catkin_LIBRARIES} 66 | ${OpenMP_FLAGS} 67 | ) 68 | 69 | target_compile_options(${PROJECT_NAME} PRIVATE ${OpenMP_FLAGS}) 70 | 71 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 72 | 73 | ############# 74 | ## Install ## 75 | ############# 76 | install( 77 | TARGETS ${PROJECT_NAME} 78 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 79 | 80 | install( 81 | FILES ${PROJECT_NAME}_plugins.xml 82 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 83 | 84 | install( 85 | DIRECTORY include/${PROJECT_NAME}/ 86 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 87 | -------------------------------------------------------------------------------- /descartes_tests/test/moveit/moveit_state_adapter_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include "descartes_moveit/moveit_state_adapter.h" 20 | #include "moveit/robot_model_loader/robot_model_loader.h" 21 | #include 22 | #include "../trajectory/robot_model_test.hpp" 23 | 24 | using namespace descartes_moveit; 25 | using namespace descartes_trajectory; 26 | using namespace descartes_core; 27 | 28 | using testing::Types; 29 | 30 | namespace descartes_tests 31 | { 32 | // This variable must be global in order for the test to pass. 33 | // Destructing the robot model results in a boost mutex exception: 34 | // --- 35 | // /usr/include/boost/thread/pthread/pthread_mutex_scoped_lock.hpp:26: 36 | // boost::pthread::pthread_mutex_scoped_lock::pthread_mutex_scoped_lock(pthread_mutex_t*): 37 | // Assertion `!pthread_mutex_lock(m)' failed. 38 | // --- 39 | 40 | robot_model_loader::RobotModelLoaderPtr robot_; 41 | 42 | template <> 43 | RobotModelPtr CreateRobotModel() 44 | { 45 | ROS_INFO_STREAM("Construction descartes robot model"); 46 | descartes_core::RobotModelPtr descartes_model_; 47 | descartes_model_ = descartes_core::RobotModelPtr(new descartes_moveit::MoveitStateAdapter()); 48 | EXPECT_TRUE(descartes_model_->initialize("robot_description", "manipulator", "base_link", "tool0")); 49 | ROS_INFO_STREAM("Descartes robot model constructed"); 50 | 51 | descartes_model_->setCheckCollisions(true); 52 | EXPECT_TRUE(descartes_model_->getCheckCollisions()); 53 | ROS_INFO_STREAM("Descartes robot enabled collision checks"); 54 | 55 | return descartes_model_; 56 | } 57 | 58 | template 59 | class MoveitRobotModelTest : public descartes_tests::RobotModelTest 60 | { 61 | }; 62 | 63 | INSTANTIATE_TYPED_TEST_CASE_P(MoveitRobotModelTest, RobotModelTest, MoveitStateAdapter); 64 | 65 | } // descartes_moveit_test 66 | -------------------------------------------------------------------------------- /descartes_tests/test/moveit/resources/kuka_kr210/kuka_kr210.srdf: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /descartes_tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(descartes_tests) 3 | 4 | add_compile_options(-std=c++11 -Wall -Wextra) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | descartes_moveit 8 | descartes_planner 9 | descartes_trajectory 10 | eigen_conversions 11 | ) 12 | 13 | catkin_package( 14 | INCLUDE_DIRS 15 | include 16 | LIBRARIES 17 | ${PROJECT_NAME} 18 | CATKIN_DEPENDS 19 | descartes_moveit 20 | descartes_planner 21 | descartes_trajectory 22 | eigen_conversions 23 | ) 24 | 25 | include_directories( 26 | include 27 | ${catkin_INCLUDE_DIRS} 28 | ) 29 | 30 | add_library(${PROJECT_NAME} 31 | src/cartesian_robot.cpp 32 | ) 33 | 34 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 35 | 36 | target_link_libraries(${PROJECT_NAME} 37 | ${catkin_LIBRARIES} 38 | ) 39 | 40 | install(TARGETS ${PROJECT_NAME} 41 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 42 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 43 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 44 | ) 45 | 46 | install(DIRECTORY include/${PROJECT_NAME}/ 47 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 48 | FILES_MATCHING PATTERN "*.h" 49 | ) 50 | 51 | if(CATKIN_ENABLE_TESTING) 52 | # Trajectory Unit Tests 53 | catkin_add_gtest(${PROJECT_NAME}_trajectory_utest 54 | test/trajectory/utest.cpp 55 | test/trajectory/axial_symmetric_pt.cpp 56 | test/trajectory/cart_trajectory_pt.cpp 57 | test/trajectory/joint_trajectory_pt.cpp 58 | test/trajectory/cartesian_robot_test.cpp 59 | ) 60 | target_compile_definitions(${PROJECT_NAME}_trajectory_utest PUBLIC GTEST_USE_OWN_TR1_TUPLE=0) 61 | target_link_libraries(${PROJECT_NAME}_trajectory_utest ${PROJECT_NAME}) 62 | 63 | # Planner unit tests 64 | catkin_add_gtest(${PROJECT_NAME}_planner_utest 65 | test/planner/utest.cpp 66 | test/planner/dense_planner.cpp 67 | test/planner/sparse_planner.cpp 68 | test/planner/planning_graph_tests.cpp 69 | test/planner/utils/trajectory_maker.cpp 70 | ) 71 | target_compile_definitions(${PROJECT_NAME}_planner_utest PUBLIC GTEST_USE_OWN_TR1_TUPLE=0) 72 | target_link_libraries(${PROJECT_NAME}_planner_utest ${PROJECT_NAME}) 73 | 74 | # Moveit adapter unit tests 75 | find_package(rostest REQUIRED) 76 | add_rostest_gtest(${PROJECT_NAME}_moveit_utest 77 | test/moveit/launch/utest.launch 78 | test/moveit/utest.cpp 79 | test/moveit/moveit_state_adapter_test.cpp 80 | ) 81 | target_compile_definitions(${PROJECT_NAME}_moveit_utest PUBLIC GTEST_USE_OWN_TR1_TUPLE=0) 82 | target_link_libraries(${PROJECT_NAME}_moveit_utest ${PROJECT_NAME}) 83 | 84 | endif() 85 | -------------------------------------------------------------------------------- /descartes_moveit/include/descartes_moveit/ikfast_moveit_state_adapter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2016, Jonathan Meyer 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef IKFAST_MOVEIT_STATE_ADAPTER_H 20 | #define IKFAST_MOVEIT_STATE_ADAPTER_H 21 | 22 | #include "descartes_moveit/moveit_state_adapter.h" 23 | 24 | namespace descartes_moveit 25 | { 26 | class IkFastMoveitStateAdapter : public descartes_moveit::MoveitStateAdapter 27 | { 28 | public: 29 | virtual ~IkFastMoveitStateAdapter() 30 | { 31 | } 32 | 33 | virtual bool initialize(const std::string& robot_description, const std::string& group_name, 34 | const std::string& world_frame, const std::string& tcp_frame); 35 | 36 | virtual bool getAllIK(const Eigen::Isometry3d& pose, std::vector >& joint_poses) const; 37 | 38 | virtual bool getIK(const Eigen::Isometry3d& pose, const std::vector& seed_state, 39 | std::vector& joint_pose) const; 40 | 41 | virtual bool getFK(const std::vector& joint_pose, Eigen::Isometry3d& pose) const; 42 | 43 | /** 44 | * @brief Sets the internal state of the robot model to the argument. For the IKFast impl, 45 | * it also recomputes the transformations to/from the IKFast reference frames. 46 | */ 47 | void setState(const moveit::core::RobotState& state); 48 | 49 | protected: 50 | bool computeIKFastTransforms(); 51 | 52 | /** 53 | * The IKFast implementation commonly solves between 'base_link' of a robot 54 | * and 'tool0'. We will commonly want to take advantage of an additional 55 | * fixed transformation from the robot flange, 'tool0', to some user defined 56 | * tool. This prevents the user from having to manually adjust tool poses to 57 | * account for this. 58 | */ 59 | descartes_core::Frame tool0_to_tip_; 60 | 61 | /** 62 | * Likewise this parameter is used to accomodate transformations between the base 63 | * of the IKFast solver and the base of the MoveIt move group. 64 | */ 65 | descartes_core::Frame world_to_base_; 66 | }; 67 | 68 | } // end namespace 'descartes_moveit' 69 | #endif 70 | -------------------------------------------------------------------------------- /descartes_tests/include/descartes_tests/cartesian_robot.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef CARTESIAN_ROBOT_H_ 20 | #define CARTESIAN_ROBOT_H_ 21 | 22 | #include "descartes_core/robot_model.h" 23 | 24 | namespace descartes_tests 25 | { 26 | /**@brief Cartesian Robot used for test purposes. This is a simple robot with simple kinematics. Each 27 | * joint corresponds to a cartesian direction (i.e. x, y, R, P, Y) (don't ask me how this is built, it 28 | * just works). 29 | */ 30 | class CartesianRobot : public descartes_core::RobotModel 31 | { 32 | public: 33 | CartesianRobot(); 34 | 35 | CartesianRobot(double pos_range, double orient_range, 36 | const std::vector &joint_velocities = std::vector(6, 1.0)); 37 | 38 | virtual bool getIK(const Eigen::Isometry3d &pose, const std::vector &seed_state, 39 | std::vector &joint_pose) const; 40 | 41 | virtual bool getAllIK(const Eigen::Isometry3d &pose, std::vector > &joint_poses) const; 42 | 43 | virtual bool getFK(const std::vector &joint_pose, Eigen::Isometry3d &pose) const; 44 | 45 | virtual bool isValid(const std::vector &joint_pose) const; 46 | 47 | virtual bool isValid(const Eigen::Isometry3d &pose) const; 48 | 49 | virtual int getDOF() const; 50 | 51 | virtual bool initialize(const std::string &robot_description, const std::string &group_name, 52 | const std::string &world_frame, const std::string &tcp_frame); 53 | 54 | virtual bool isValidMove(const double* s, const double* f, double dt) const override; 55 | 56 | virtual std::vector getJointVelocityLimits() const override 57 | { 58 | return joint_velocities_; 59 | } 60 | 61 | bool setJointVelocities(const std::vector &new_joint_vels) 62 | { 63 | if (new_joint_vels.size() != joint_velocities_.size()) 64 | return false; 65 | joint_velocities_ = new_joint_vels; 66 | return true; 67 | } 68 | 69 | double pos_range_; 70 | double orient_range_; 71 | std::vector joint_velocities_; 72 | }; 73 | } 74 | 75 | #endif // CARTESIAN_ROBOT_H 76 | -------------------------------------------------------------------------------- /descartes_tests/test/planner/utils/trajectory_maker.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include "trajectory_maker.h" 20 | 21 | // Makes a linear trajectory with a given tool speed 22 | std::vector descartes_tests::makeConstantVelocityTrajectory( 23 | const Eigen::Vector3d& start, const Eigen::Vector3d& stop, double tool_vel, size_t n_steps) 24 | { 25 | Eigen::Vector3d delta = stop - start; 26 | Eigen::Vector3d step = delta / n_steps; 27 | double dt = delta.norm() / tool_vel; 28 | double time_step = dt / n_steps; 29 | // We know our dt, all points should have it 30 | descartes_core::TimingConstraint tm(time_step); 31 | 32 | std::vector result; 33 | for (size_t i = 0; i <= n_steps; ++i) 34 | { 35 | Eigen::Isometry3d pose; 36 | pose = Eigen::Translation3d(start + i * step); 37 | descartes_core::TrajectoryPtPtr pt(new descartes_trajectory::CartTrajectoryPt(pose, tm)); 38 | result.push_back(pt); 39 | } 40 | return result; 41 | } 42 | 43 | std::vector descartes_tests::makeZigZagTrajectory(double x_start, double x_stop, 44 | double y_amplitude, double tool_vel, 45 | size_t n_steps) 46 | { 47 | using namespace Eigen; 48 | Vector3d start(x_start, 0, 0); 49 | Vector3d stop(x_stop, 0, 0); 50 | 51 | Eigen::Vector3d delta = stop - start; 52 | Eigen::Vector3d step = delta / n_steps; 53 | double dt = delta.norm() / tool_vel; 54 | double time_step = dt / n_steps; 55 | // We know our dt, all points should have it 56 | descartes_core::TimingConstraint tm(time_step); 57 | 58 | std::vector result; 59 | for (size_t i = 0; i <= n_steps; ++i) 60 | { 61 | Eigen::Isometry3d pose; 62 | pose = Eigen::Translation3d(start + i * step); 63 | if (i & 1) 64 | { 65 | pose *= Translation3d(0, y_amplitude, 0); 66 | } 67 | else 68 | { 69 | pose *= Translation3d(0, -y_amplitude, 0); 70 | } 71 | descartes_core::TrajectoryPtPtr pt(new descartes_trajectory::CartTrajectoryPt(pose, tm)); 72 | result.push_back(pt); 73 | } 74 | return result; 75 | } 76 | -------------------------------------------------------------------------------- /descartes_planner/include/descartes_planner/dense_planner.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef DENSE_PLANNER_H_ 20 | #define DENSE_PLANNER_H_ 21 | 22 | #include 23 | #include 24 | 25 | namespace descartes_planner 26 | { 27 | class DensePlanner : public descartes_core::PathPlannerBase 28 | { 29 | public: 30 | DensePlanner(); 31 | 32 | virtual ~DensePlanner(); 33 | 34 | virtual bool initialize(descartes_core::RobotModelConstPtr model); 35 | virtual bool initialize(descartes_core::RobotModelConstPtr model, 36 | descartes_planner::CostFunction cost_function_callback); 37 | virtual bool setConfig(const descartes_core::PlannerConfig& config); 38 | virtual void getConfig(descartes_core::PlannerConfig& config) const; 39 | virtual bool planPath(const std::vector& traj); 40 | virtual bool getPath(std::vector& path) const; 41 | virtual bool addAfter(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr tp); 42 | virtual bool addBefore(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr tp); 43 | virtual bool remove(const descartes_core::TrajectoryPt::ID& ref_id); 44 | virtual bool modify(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr tp); 45 | virtual int getErrorCode() const; 46 | virtual bool getErrorMessage(int error_code, std::string& msg) const; 47 | 48 | // Helper functions meant to access the underlying graph structure 49 | 50 | const PlanningGraph& getPlanningGraph() const 51 | { 52 | return *planning_graph_; 53 | } 54 | 55 | protected: 56 | descartes_core::TrajectoryPt::ID getPrevious(const descartes_core::TrajectoryPt::ID& ref_id); 57 | descartes_core::TrajectoryPt::ID getNext(const descartes_core::TrajectoryPt::ID& ref_id); 58 | descartes_core::TrajectoryPtPtr get(const descartes_core::TrajectoryPt::ID& ref_id); 59 | bool updatePath(); 60 | 61 | protected: 62 | boost::shared_ptr planning_graph_; 63 | int error_code_; 64 | descartes_core::PlannerConfig config_; 65 | std::vector path_; 66 | std::map error_map_; 67 | }; 68 | 69 | } /* namespace descartes_core */ 70 | #endif /* DENSE_PLANNER_H_ */ 71 | -------------------------------------------------------------------------------- /descartes_core/include/descartes_core/trajectory_timing_constraint.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | /* 19 | * trajectory_pt_transition.h 20 | * 21 | * Created on: March 7, 2015 22 | * Author: Jonathan Meyer 23 | */ 24 | 25 | #ifndef TRAJECTORY_TIMING_CONSTRAINT_H 26 | #define TRAJECTORY_TIMING_CONSTRAINT_H 27 | 28 | #include 29 | 30 | namespace descartes_core 31 | { 32 | /** 33 | * @brief A window of time for this point to be achieved relative to a previous point or 34 | * the starting position. 35 | * 36 | * This struct defines a 'lower' and 'upper' bound for the desired time. If the upper bound 37 | * is zero or negative, it is considered unspecified and the behavior of your planner 38 | * or filter is considered undefined. 39 | * 40 | * All time-value units are in seconds. 41 | */ 42 | struct TimingConstraint 43 | { 44 | /** 45 | * @brief The default constructor creates an unspecified point 46 | */ 47 | TimingConstraint() : lower(0.0), upper(0.0) 48 | { 49 | } 50 | 51 | /** 52 | * @brief Constructs a timing constraint with a nominal time value (window width of zero) 53 | * @param nominal The desired time in seconds to achieve this point from the previous 54 | */ 55 | explicit TimingConstraint(double nominal) : lower(nominal), upper(nominal) 56 | { 57 | if (nominal < 0.0) 58 | { 59 | ROS_WARN_STREAM("Nominal time value must be greater than or equal to 0.0"); 60 | lower = 0.0; 61 | upper = 0.0; 62 | } 63 | } 64 | 65 | /** 66 | * @brief Constructs a timing constraint using the provided time window 67 | * @param lower The lower bound of the acceptable time window in seconds. 68 | * @param upper The upper bound of the acceptable time window in seconds. 69 | */ 70 | TimingConstraint(double lower, double upper) : lower(lower), upper(upper) 71 | { 72 | if (lower < 0.0) 73 | { 74 | ROS_WARN_STREAM("Lower time value must be greater than or equal to 0.0"); 75 | lower = 0.0; 76 | } 77 | 78 | if (upper < 0.0) 79 | { 80 | ROS_WARN_STREAM("Upper time value must be greater than or equal to 0.0"); 81 | upper = 0.0; 82 | } 83 | } 84 | 85 | /** 86 | * @brief Checks if the given timing constraint has been specified 87 | * @return true if the point is specified 88 | */ 89 | bool isSpecified() const 90 | { 91 | return upper > 0.0; 92 | } 93 | 94 | double lower, upper; 95 | }; 96 | 97 | } // end namespace descartes core 98 | 99 | #endif /* TRAJECTORY_TIMING_CONSTRAINT_H */ 100 | -------------------------------------------------------------------------------- /descartes_trajectory/include/descartes_trajectory/axial_symmetric_pt.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2015, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef AXIAL_SYMMETRIC_PT_H 20 | #define AXIAL_SYMMETRIC_PT_H 21 | 22 | #include 23 | 24 | namespace descartes_trajectory 25 | { 26 | /** 27 | * @brief Specialization of cartesian trajectory point. 28 | * Represents a cartesian point whose rotation 29 | * about a chosen axis is unconstrained. 30 | */ 31 | class AxialSymmetricPt : public descartes_trajectory::CartTrajectoryPt 32 | { 33 | public: 34 | /** 35 | * @brief Enum used to select the free axis of rotation for this point 36 | */ 37 | enum FreeAxis 38 | { 39 | X_AXIS, 40 | Y_AXIS, 41 | Z_AXIS 42 | }; 43 | 44 | /** 45 | * @brief This default constructor is exactly equivalent to CartTrajectoryPt's. Initializes all 46 | * frames to identity. 47 | */ 48 | AxialSymmetricPt(const descartes_core::TimingConstraint& timing = descartes_core::TimingConstraint()); 49 | 50 | /** 51 | * @brief Constructs a cartesian trajectory point that places the robot tip at the position 52 | * defined by the transform from the origin of the world. The transform is first a 53 | * translation by the vector (x,y,z) followed by an XYZ rotation 54 | * (moving axis) by (rx,ry,rz) respectively. 55 | * 56 | * @param x x component of translation part of transform 57 | * @param y y component of translation part of transform 58 | * @param z z component of translation part of transform 59 | * @param rx rotation about x axis of nominal pose 60 | * @param ry rotation about y axis of nominal pose 61 | * @param rz rotation about z axis of nominal pose 62 | * @param orient_increment (in radians, discretization of space [-Pi, Pi]) 63 | * @param axis The free-axis of the nominal pose of the tool 64 | */ 65 | AxialSymmetricPt(double x, double y, double z, double rx, double ry, double rz, double orient_increment, 66 | FreeAxis axis, const descartes_core::TimingConstraint& timing = descartes_core::TimingConstraint()); 67 | 68 | /** 69 | * @brief Similar to other constructor except that it takes an affine pose instead of 70 | * individual translation and rotation arguments. 71 | */ 72 | AxialSymmetricPt(const Eigen::Isometry3d& pose, double orient_increment, FreeAxis axis, 73 | const descartes_core::TimingConstraint& timing = descartes_core::TimingConstraint()); 74 | 75 | virtual descartes_core::TrajectoryPtPtr copy() const 76 | { 77 | return descartes_core::TrajectoryPtPtr(new AxialSymmetricPt(*this)); 78 | } 79 | }; 80 | 81 | } // descartes trajectory 82 | 83 | #endif // AXIAL_SYMMETRIC_PT_H 84 | -------------------------------------------------------------------------------- /descartes_planner/src/ladder_graph_dag_search.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | #include "descartes_planner/ladder_graph_dag_search.h" 19 | 20 | namespace descartes_planner 21 | { 22 | 23 | DAGSearch::DAGSearch(const LadderGraph &graph) 24 | : graph_(graph) 25 | { 26 | // On creating an object, let's allocate everything we need 27 | solution_.resize(graph.size()); 28 | 29 | for (size_t i = 0; i < graph.size(); ++i) 30 | { 31 | const auto n_vertices = graph.rungSize(i); 32 | solution_[i].distance.resize(n_vertices); 33 | solution_[i].predecessor.resize(n_vertices); 34 | } 35 | } 36 | 37 | double DAGSearch::run() 38 | { 39 | // Cost to the first rung should be set to zero 40 | std::fill(solution_.front().distance.begin(), solution_.front().distance.end(), 0.0); 41 | // Other rows initialize to zero 42 | for (size_type i = 1; i < solution_.size(); ++i) 43 | { 44 | std::fill(solution_[i].distance.begin(), solution_[i].distance.end(), std::numeric_limits::max()); 45 | } 46 | 47 | // Now we iterate over the graph in 'topological' order 48 | for (size_type rung = 0; rung < solution_.size() - 1; ++rung) 49 | { 50 | const auto n_vertices = graph_.rungSize(rung); 51 | const auto next_rung = rung + 1; 52 | // For each vertex in the out edge list 53 | for (size_t index = 0; index < n_vertices; ++index) 54 | { 55 | const auto u_cost = distance(rung, index); 56 | const auto& edges = graph_.getEdges(rung)[index]; 57 | // for each out edge 58 | for (const auto& edge : edges) 59 | { 60 | auto dv = u_cost + edge.cost; // new cost 61 | if (dv < distance(next_rung, edge.idx)) 62 | { 63 | distance(next_rung, edge.idx) = dv; 64 | predecessor(next_rung, edge.idx) = index; // the predecessor's rung is implied to be the current rung 65 | } 66 | } 67 | } // vertex for loop 68 | } // rung for loop 69 | 70 | return *std::min_element(solution_.back().distance.begin(), solution_.back().distance.end()); 71 | } 72 | 73 | std::vector DAGSearch::shortestPath() const 74 | { 75 | auto min_it = std::min_element(solution_.back().distance.begin(), solution_.back().distance.end()); 76 | auto min_idx = std::distance(solution_.back().distance.begin(), min_it); 77 | assert(min_idx >= 0); 78 | 79 | std::vector path (solution_.size()); 80 | 81 | size_type current_rung = path.size() - 1; 82 | size_type current_index = min_idx; 83 | 84 | for (unsigned i = 0; i < path.size(); ++i) 85 | { 86 | auto count = path.size() - 1 - i; 87 | assert(current_rung == count); 88 | path[count] = current_index; 89 | current_index = predecessor(current_rung, current_index); 90 | current_rung -= 1; 91 | } 92 | 93 | return path; 94 | } 95 | 96 | } // namespace descartes_planner 97 | -------------------------------------------------------------------------------- /descartes_moveit/include/descartes_moveit/seed_search.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2015, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | * 18 | */ 19 | 20 | /** 21 | * @file seed_search.h 22 | * @author Jonathan Meyer 23 | * @date April 2015 24 | * 25 | * @brief This file defines methods that can be used to generate seed values for iterative 26 | * numerical solvers used by Moveit for a generic robot. 27 | * 28 | * Users can call the findSeedStates() function with a robot state object, a 29 | * particular Moveit move group, and a series of joint pairs. These joint pairs 30 | * should define arm configurations such as elbow up or elbow down. For many 6 DOF 31 | * robots, joints 2 & 3 and joints 4 & 6 (starting counting at 1) will form elbow 32 | * and wrist configurations. 33 | */ 34 | 35 | #ifndef SEED_SEARCH_H 36 | #define SEED_SEARCH_H 37 | 38 | #include 39 | 40 | namespace descartes_moveit 41 | { 42 | namespace seed 43 | { 44 | /** 45 | * @brief Returns a sequence of seed states for iterative inverse kinematic solvers to use 46 | * when 'sampling' the solution space of a pose. These seeds are generated by 47 | * iterating through all possible joint permutations of each pair of joints passed 48 | * in by the user. 49 | * @param state Shared pointer to robot state used to perform FK/IK 50 | * @param group_name Name of the move group for which to generate seeds 51 | * @param tool_frame The name of the tool link in which to work with FK/IK 52 | * @param pairs A sequence of joint pairs used to generate the seed states. 53 | * @return A vector of seed states 54 | */ 55 | std::vector > findSeedStatesByPairs(moveit::core::RobotState& state, const std::string& group_name, 56 | const std::string& tool_frame, 57 | const std::vector >& pairs); 58 | 59 | /** 60 | * @brief findIndustrialSixDOFSeeds() is a specialization of findSeedStatesByPairs() 61 | * that searches for seed states over joints 2,3 and 4,6 (1 indexed). These 62 | * joints often form elbow and wrist configurations. 63 | */ 64 | inline std::vector > findIndustrialSixDOFSeeds(moveit::core::RobotState& state, 65 | const std::string& group_name, 66 | const std::string& tool_frame) 67 | { 68 | return findSeedStatesByPairs(state, group_name, tool_frame, { { 1, 2 }, { 3, 5 } }); 69 | } 70 | 71 | /** 72 | * @brief Uses moveit's underlying random function to find n random joint configurations 73 | * that satisfy model bounds. 74 | * @param state Shared pointer to robot state used to perform FK/IK 75 | * @param group_name Name of the move group for which to generate seeds 76 | * @param n Number of random seeds to create 77 | * @return n random valid positions of 'group_name' in moveit config defined by state 78 | */ 79 | std::vector > findRandomSeeds(moveit::core::RobotState& state, const std::string& group_name, 80 | unsigned n); 81 | 82 | } // end namespace seed 83 | } // end namespace descartes_moveit 84 | 85 | #endif // SEED_SEARCH_H 86 | -------------------------------------------------------------------------------- /descartes_tests/test/trajectory/robot_model_test.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef ROBOT_MODEL_TEST_HPP_ 20 | #define ROBOT_MODEL_TEST_HPP_ 21 | 22 | #include "descartes_core/robot_model.h" 23 | #include "ros/console.h" 24 | #include 25 | 26 | /** 27 | @brief: This file contains inerface test for the Descartes RobotModel. These 28 | tests can be executed on arbitrary types that inplment the RobotModel interface. 29 | For more info see: 30 | 31 | GTest Advanced Guide - Type-Parameterized Tests 32 | 33 | */ 34 | 35 | namespace descartes_tests 36 | { 37 | template 38 | descartes_core::RobotModelPtr CreateRobotModel(); 39 | 40 | template 41 | class RobotModelTest : public ::testing::Test 42 | { 43 | public: 44 | RobotModelTest() : model_(CreateRobotModel()) 45 | { 46 | ROS_INFO("Instantiated RobotModelTest fixture(base) (parameterized)"); 47 | } 48 | 49 | virtual void SetUp() 50 | { 51 | ROS_INFO("Setting up RobotModelTest fixture(base) (parameterized)"); 52 | ASSERT_TRUE(static_cast(this->model_)); 53 | } 54 | 55 | virtual void TearDown() 56 | { 57 | ROS_INFO("Tearing down RobotModelTest fixture(base) (parameterized)"); 58 | } 59 | 60 | virtual ~RobotModelTest() 61 | { 62 | ROS_INFO("Desctructing RobotModelTest fixture(base) (parameterized)"); 63 | } 64 | 65 | descartes_core::RobotModelPtr model_; 66 | }; 67 | 68 | using namespace descartes_core; 69 | 70 | TYPED_TEST_CASE_P(RobotModelTest); 71 | 72 | const double TF_EQ_TOL = 0.001; 73 | const double JOINT_EQ_TOL = 0.001; 74 | 75 | TYPED_TEST_P(RobotModelTest, construction) 76 | { 77 | ROS_INFO_STREAM("Robot model test construction"); 78 | } 79 | 80 | TYPED_TEST_P(RobotModelTest, getIK) 81 | { 82 | ROS_INFO_STREAM("Testing getIK"); 83 | std::vector fk_joint(6, 0.0); 84 | std::vector ik_joint; 85 | Eigen::Isometry3d ik_pose, fk_pose; 86 | EXPECT_TRUE(this->model_->getFK(fk_joint, ik_pose)); 87 | EXPECT_TRUE(this->model_->getIK(ik_pose, fk_joint, ik_joint)); 88 | // This doesn't always work, but it should. The IKFast solution doesn't 89 | // return the "closets" solution. Numeric IK does appear to do this. 90 | EXPECT_TRUE(this->model_->getFK(ik_joint, fk_pose)); 91 | EXPECT_TRUE(ik_pose.matrix().isApprox(fk_pose.matrix(), TF_EQ_TOL)); 92 | ROS_INFO_STREAM("getIK Test completed"); 93 | } 94 | 95 | TYPED_TEST_P(RobotModelTest, getAllIK) 96 | { 97 | ROS_INFO_STREAM("Testing getAllIK"); 98 | std::vector fk_joint(6, 0.5); 99 | std::vector > joint_poses; 100 | Eigen::Isometry3d ik_pose, fk_pose; 101 | 102 | EXPECT_TRUE(this->model_->getFK(fk_joint, ik_pose)); 103 | EXPECT_TRUE(this->model_->getAllIK(ik_pose, joint_poses)); 104 | ROS_INFO_STREAM("Get all IK returned " << joint_poses.size() << " solutions"); 105 | std::vector >::iterator it; 106 | for (it = joint_poses.begin(); it != joint_poses.end(); ++it) 107 | { 108 | EXPECT_TRUE(this->model_->getFK(*it, fk_pose)); 109 | EXPECT_TRUE(ik_pose.matrix().isApprox(fk_pose.matrix(), TF_EQ_TOL)); 110 | } 111 | } 112 | 113 | REGISTER_TYPED_TEST_CASE_P(RobotModelTest, construction, getIK, getAllIK); 114 | 115 | } // descartes_tests 116 | 117 | #endif // ROBOT_MODEL_TEST_HPP_ 118 | -------------------------------------------------------------------------------- /descartes_trajectory/src/axial_symmetric_pt.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | #include "descartes_trajectory/axial_symmetric_pt.h" 19 | 20 | using descartes_trajectory::TolerancedFrame; 21 | using descartes_trajectory::AxialSymmetricPt; 22 | using namespace descartes_core::utils; 23 | 24 | static TolerancedFrame makeUnconstrainedRotation(double x, double y, double z, double rx, double ry, double rz, 25 | AxialSymmetricPt::FreeAxis axis) 26 | { 27 | using namespace descartes_trajectory; 28 | 29 | Eigen::Isometry3d pose = toFrame(x, y, z, rx, ry, rz, EulerConventions::XYZ); 30 | PositionTolerance pos_tol = ToleranceBase::zeroTolerance(x, y, z); 31 | OrientationTolerance orient_tol = ToleranceBase::createSymmetric( 32 | ((axis == AxialSymmetricPt::X_AXIS) ? 0.0 : rx), ((axis == AxialSymmetricPt::Y_AXIS) ? 0.0 : ry), 33 | ((axis == AxialSymmetricPt::Z_AXIS) ? 0.0 : rz), ((axis == AxialSymmetricPt::X_AXIS) ? 2 * M_PI : 0.0), 34 | ((axis == AxialSymmetricPt::Y_AXIS) ? 2 * M_PI : 0.0), ((axis == AxialSymmetricPt::Z_AXIS) ? 2 * M_PI : 0.0)); 35 | return TolerancedFrame(pose, pos_tol, orient_tol); 36 | } 37 | 38 | static TolerancedFrame makeUnconstrainedRotation(const Eigen::Isometry3d& pose, AxialSymmetricPt::FreeAxis axis) 39 | { 40 | using namespace descartes_trajectory; 41 | 42 | Eigen::Vector3d rpy = pose.rotation().eulerAngles(0, 1, 2); 43 | double rx = rpy(0); 44 | double ry = rpy(1); 45 | double rz = rpy(2); 46 | double x = pose.translation()(0); 47 | double y = pose.translation()(1); 48 | double z = pose.translation()(2); 49 | 50 | PositionTolerance pos_tol = ToleranceBase::zeroTolerance(x, y, z); 51 | OrientationTolerance orient_tol = ToleranceBase::createSymmetric( 52 | ((axis == AxialSymmetricPt::X_AXIS) ? 0.0 : rx), ((axis == AxialSymmetricPt::Y_AXIS) ? 0.0 : ry), 53 | ((axis == AxialSymmetricPt::Z_AXIS) ? 0.0 : rz), ((axis == AxialSymmetricPt::X_AXIS) ? 2 * M_PI : 0.0), 54 | ((axis == AxialSymmetricPt::Y_AXIS) ? 2 * M_PI : 0.0), ((axis == AxialSymmetricPt::Z_AXIS) ? 2 * M_PI : 0.0)); 55 | return TolerancedFrame(pose, pos_tol, orient_tol); 56 | } 57 | 58 | namespace descartes_trajectory 59 | { 60 | AxialSymmetricPt::AxialSymmetricPt(const descartes_core::TimingConstraint& timing) : CartTrajectoryPt(timing) 61 | { 62 | } 63 | 64 | AxialSymmetricPt::AxialSymmetricPt(double x, double y, double z, double rx, double ry, double rz, 65 | double orient_increment, FreeAxis axis, 66 | const descartes_core::TimingConstraint& timing) 67 | : CartTrajectoryPt(makeUnconstrainedRotation(x, y, z, rx, ry, rz, axis), 68 | 0.0, // The position discretization 69 | orient_increment, // Orientation discretization (starting at -2Pi and marching to 2Pi) 70 | timing) 71 | { 72 | } 73 | 74 | AxialSymmetricPt::AxialSymmetricPt(const Eigen::Isometry3d& pose, double orient_increment, FreeAxis axis, 75 | const descartes_core::TimingConstraint& timing) 76 | : CartTrajectoryPt(makeUnconstrainedRotation(pose, axis), 77 | 0.0, // The position discretization 78 | orient_increment, // Orientation discretization (starting at -2Pi and marching to 2Pi) 79 | timing) 80 | { 81 | } 82 | 83 | } // end of namespace descartes_trajectory 84 | -------------------------------------------------------------------------------- /descartes_planner/include/descartes_planner/planning_graph.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | /* 19 | * Planning_graph.h 20 | * 21 | * Created on: Jun 5, 2014 22 | * Author: Dan Solomon 23 | * Author: Jonathan Meyer 24 | */ 25 | 26 | #ifndef PLANNING_GRAPH_H_ 27 | #define PLANNING_GRAPH_H_ 28 | 29 | #include 30 | #include "descartes_core/trajectory_pt.h" 31 | #include "descartes_trajectory/cart_trajectory_pt.h" 32 | #include "descartes_trajectory/joint_trajectory_pt.h" 33 | 34 | #include "descartes_planner/ladder_graph.h" 35 | 36 | namespace descartes_planner 37 | { 38 | 39 | typedef boost::function CostFunction; 40 | 41 | 42 | class PlanningGraph 43 | { 44 | public: 45 | PlanningGraph(descartes_core::RobotModelConstPtr model, CostFunction cost_function_callback = CostFunction{}); 46 | 47 | /** \brief Clear all previous graph data */ 48 | void clear() { graph_.clear(); } 49 | 50 | /** @brief initial population of graph trajectory elements 51 | * @param points list of trajectory points to be used to construct the graph 52 | * @return True if the graph was successfully created 53 | */ 54 | bool insertGraph(const std::vector& points); 55 | 56 | /** @brief adds a single trajectory point to the graph 57 | * @param point The new point to add to the graph 58 | * @return True if the point was successfully added 59 | */ 60 | bool addTrajectory(descartes_core::TrajectoryPtPtr point, descartes_core::TrajectoryPt::ID previous_id, 61 | descartes_core::TrajectoryPt::ID next_id); 62 | 63 | bool modifyTrajectory(descartes_core::TrajectoryPtPtr point); 64 | 65 | bool removeTrajectory(const descartes_core::TrajectoryPt::ID& point); 66 | 67 | bool getShortestPath(double &cost, std::list &path); 68 | 69 | const descartes_planner::LadderGraph& graph() const noexcept { return graph_; } 70 | 71 | descartes_core::RobotModelConstPtr getRobotModel() const { return robot_model_; } 72 | 73 | protected: 74 | descartes_planner::LadderGraph graph_; 75 | descartes_core::RobotModelConstPtr robot_model_; 76 | CostFunction custom_cost_function_; 77 | 78 | /** 79 | * @brief A pair indicating the validity of the edge, and if valid, the cost associated 80 | * with that edge 81 | */ 82 | bool calculateJointSolutions(const descartes_core::TrajectoryPtPtr* points, const std::size_t count, 83 | std::vector>>& poses) const; 84 | 85 | /** @brief (Re)create the actual graph nodes(vertices) from the list of joint solutions (vertices) */ 86 | bool populateGraphVertices(const std::vector &points, 87 | std::vector> &poses); 88 | 89 | void computeAndAssignEdges(const std::size_t start_idx, const std::size_t end_idx); 90 | 91 | 92 | template 93 | std::vector calculateEdgeWeights(EdgeBuilder&& builder, 94 | const std::vector &start_joints, 95 | const std::vector &end_joints, 96 | const size_t dof, 97 | bool& has_edges) const; 98 | 99 | }; 100 | 101 | } /* namespace descartes_planner */ 102 | 103 | #endif /* PLANNING_GRAPH_H_ */ 104 | -------------------------------------------------------------------------------- /descartes_tests/test/trajectory/axial_symmetric_pt.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2015, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include "descartes_trajectory/axial_symmetric_pt.h" 20 | #include "descartes_core/utils.h" 21 | #include "descartes_tests/cartesian_robot.h" 22 | #include 23 | 24 | using namespace descartes_trajectory; 25 | using namespace descartes_tests; 26 | 27 | // Does it have a default constructor 28 | TEST(AxialSymPt, construction) 29 | { 30 | AxialSymmetricPt def(); 31 | } 32 | 33 | TEST(AxialSymPt, discretization_count) 34 | { 35 | const double SEARCH_DISC = M_PI / 2.0; 36 | CartesianRobot robot(10.0, 2 * M_PI); 37 | 38 | Eigen::Isometry3d pose(Eigen::Isometry3d::Identity()); 39 | 40 | AxialSymmetricPt z_point(pose, SEARCH_DISC, AxialSymmetricPt::Z_AXIS); 41 | AxialSymmetricPt x_point(pose, SEARCH_DISC, AxialSymmetricPt::X_AXIS); 42 | AxialSymmetricPt y_point(pose, SEARCH_DISC, AxialSymmetricPt::Y_AXIS); 43 | 44 | EigenSTL::vector_Isometry3d solutions; 45 | std::vector > joint_solutions; 46 | 47 | const unsigned EXPECTED_POSES = (2.0 * M_PI / SEARCH_DISC) + 1; 48 | 49 | z_point.getCartesianPoses(robot, solutions); 50 | EXPECT_EQ(EXPECTED_POSES, solutions.size()); 51 | z_point.getJointPoses(robot, joint_solutions); 52 | EXPECT_EQ(EXPECTED_POSES, joint_solutions.size()); 53 | 54 | x_point.getCartesianPoses(robot, solutions); 55 | EXPECT_EQ(EXPECTED_POSES, solutions.size()); 56 | x_point.getJointPoses(robot, joint_solutions); 57 | EXPECT_EQ(EXPECTED_POSES, joint_solutions.size()); 58 | 59 | y_point.getCartesianPoses(robot, solutions); 60 | EXPECT_EQ(EXPECTED_POSES, solutions.size()); 61 | y_point.getJointPoses(robot, joint_solutions); 62 | EXPECT_EQ(EXPECTED_POSES, joint_solutions.size()); 63 | } 64 | 65 | bool approxEqual(double a, double b, double tol) 66 | { 67 | return std::fmod(std::fabs(a - b), M_PI) <= tol; 68 | } 69 | 70 | TEST(AxialSymPt, discretization_values) 71 | { 72 | const double SEARCH_DISC = M_PI / 2.0; 73 | CartesianRobot robot(10.0, 2 * M_PI); 74 | 75 | Eigen::Isometry3d pose(Eigen::Isometry3d::Identity()); 76 | 77 | AxialSymmetricPt z_point(pose, SEARCH_DISC, AxialSymmetricPt::Z_AXIS); 78 | AxialSymmetricPt x_point(pose, SEARCH_DISC, AxialSymmetricPt::X_AXIS); 79 | AxialSymmetricPt y_point(pose, SEARCH_DISC, AxialSymmetricPt::Y_AXIS); 80 | 81 | const double ANGLE_TOL = 0.001; 82 | // 83 | EigenSTL::vector_Isometry3d solutions; 84 | z_point.getCartesianPoses(robot, solutions); 85 | for (std::size_t i = 0; i < solutions.size(); ++i) 86 | { 87 | Eigen::Vector3d rpy = solutions[i].rotation().eulerAngles(0, 1, 2); 88 | EXPECT_TRUE(approxEqual(0.0, rpy(0), ANGLE_TOL)); 89 | EXPECT_TRUE(approxEqual(0.0, rpy(1), ANGLE_TOL)); 90 | EXPECT_TRUE(approxEqual(-M_PI + SEARCH_DISC * i, rpy(2), ANGLE_TOL)); 91 | } 92 | 93 | x_point.getCartesianPoses(robot, solutions); 94 | for (std::size_t i = 0; i < solutions.size(); ++i) 95 | { 96 | Eigen::Vector3d rpy = solutions[i].rotation().eulerAngles(0, 1, 2); 97 | EXPECT_TRUE(approxEqual(-M_PI + SEARCH_DISC * i, rpy(0), ANGLE_TOL)); 98 | EXPECT_TRUE(approxEqual(0.0, rpy(1), ANGLE_TOL)); 99 | EXPECT_TRUE(approxEqual(0.0, rpy(2), ANGLE_TOL)); 100 | } 101 | 102 | y_point.getCartesianPoses(robot, solutions); 103 | for (std::size_t i = 0; i < solutions.size(); ++i) 104 | { 105 | Eigen::Vector3d rpy = solutions[i].rotation().eulerAngles(0, 1, 2); 106 | EXPECT_TRUE(approxEqual(0.0, rpy(0), ANGLE_TOL)); 107 | EXPECT_TRUE(approxEqual(-M_PI + SEARCH_DISC * i, rpy(1), ANGLE_TOL)); 108 | EXPECT_TRUE(approxEqual(0.0, rpy(2), ANGLE_TOL)); 109 | } 110 | } 111 | -------------------------------------------------------------------------------- /descartes_trajectory/test/trajectory_pt.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include "descartes_core/trajectory_pt.h" 20 | #include "descartes_trajectory/cart_trajectory_pt.h" 21 | #include "descartes_trajectory/joint_trajectory_pt.h" 22 | #include "descartes_trajectory/axial_symmetric_pt.h" 23 | #include "ros/console.h" 24 | #include 25 | 26 | using namespace descartes_core; 27 | using namespace descartes_trajectory; 28 | 29 | // Helper function for testing timing data 30 | bool equal(const descartes_core::TimingConstraint& a, const descartes_core::TimingConstraint& b) 31 | { 32 | return std::abs(a.upper - b.upper) < 0.001; 33 | } 34 | 35 | // Factory methods for trajectory point construction 36 | template 37 | TrajectoryPt* CreateTrajectoryPt(); 38 | 39 | template <> 40 | TrajectoryPt* CreateTrajectoryPt() 41 | { 42 | return new CartTrajectoryPt(); 43 | } 44 | 45 | template <> 46 | TrajectoryPt* CreateTrajectoryPt() 47 | { 48 | return new JointTrajectoryPt(); 49 | } 50 | 51 | template <> 52 | TrajectoryPt* CreateTrajectoryPt() 53 | { 54 | return new AxialSymmetricPt(); 55 | } 56 | 57 | template 58 | class TrajectoryPtTest : public testing::Test 59 | { 60 | protected: 61 | TrajectoryPtTest() 62 | : lhs_(CreateTrajectoryPt()) 63 | , rhs_(CreateTrajectoryPt()) 64 | , lhs_copy_(CreateTrajectoryPt()) 65 | , lhs_clone_(CreateTrajectoryPt()) 66 | { 67 | lhs_->setTiming(descartes_core::TimingConstraint(10.0)); 68 | 69 | lhs_copy_ = lhs_->copy(); 70 | lhs_clone_ = lhs_->clone(); 71 | lhs_same_ = lhs_; 72 | } 73 | 74 | TrajectoryPtPtr lhs_; 75 | TrajectoryPtPtr rhs_; 76 | TrajectoryPtPtr lhs_copy_; 77 | TrajectoryPtPtr lhs_clone_; 78 | TrajectoryPtPtr lhs_same_; 79 | }; 80 | 81 | using testing::Types; 82 | 83 | // Add types of trajectory points here: 84 | typedef Types Implementations; 85 | 86 | TYPED_TEST_CASE(TrajectoryPtTest, Implementations); 87 | 88 | TYPED_TEST(TrajectoryPtTest, construction) 89 | { 90 | EXPECT_FALSE(this->lhs_->getID().is_nil()); 91 | EXPECT_FALSE(this->lhs_copy_->getID().is_nil()); 92 | EXPECT_FALSE(this->lhs_clone_->getID().is_nil()); 93 | EXPECT_FALSE(this->rhs_->getID().is_nil()); 94 | 95 | // Depending on construction method (declaration, copy, clone, same pointer), the 96 | // objects and specifically IDs equality should be defined as follow 97 | 98 | // TODO: Implement equality checks 99 | 100 | // Declared objects should always be different 101 | // EXPECT_NE(*(this->lhs_), *(this->rhs_)); 102 | EXPECT_NE(this->lhs_->getID(), this->rhs_->getID()); 103 | 104 | // Copied objects should always be the same 105 | // EXPECT_EQ(*(this->lhs_), *(this->lhs_copy_)); 106 | EXPECT_EQ(this->lhs_->getID(), this->lhs_copy_->getID()); 107 | 108 | // Cloned objects should have the same data (we can't test, but different ids) 109 | // EXPECT_NE(*(this->lhs_), *(this->lhs_clone_)); 110 | EXPECT_NE(this->lhs_->getID(), this->lhs_clone_->getID()); 111 | 112 | // Pointers to the same objects should be identical (like a copy, but no ambiguity) 113 | // EXPECT_EQ(*(this->lhs_), *(this->lhs_same_)); 114 | EXPECT_EQ(this->lhs_->getID(), this->lhs_same_->getID()); 115 | 116 | // Copied and cloned points should have the same timing information 117 | EXPECT_TRUE(equal(this->lhs_->getTiming(), this->lhs_copy_->getTiming())); 118 | EXPECT_TRUE(equal(this->lhs_->getTiming(), this->lhs_clone_->getTiming())); 119 | EXPECT_FALSE(equal(this->lhs_->getTiming(), this->rhs_->getTiming())); 120 | } 121 | -------------------------------------------------------------------------------- /descartes_tests/test/moveit/resources/kuka_kr210/kr210l150.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | -------------------------------------------------------------------------------- /descartes_core/include/descartes_core/path_planner_base.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef DESCARTES_CORE_PATH_PLANNER_BASE_H_ 20 | #define DESCARTES_CORE_PATH_PLANNER_BASE_H_ 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | namespace descartes_core 27 | { 28 | namespace PlannerErrors 29 | { 30 | enum PlannerError 31 | { 32 | OK = 1, 33 | IK_NOT_AVAILABLE = -1, 34 | FX_NOT_AVAILABLE = -2, 35 | SELF_COLLISION_FOUND = -3, 36 | ENVIRONMENT_COLLISION_FOUND = -4, 37 | PLANNING_TIMEOUT = -5, 38 | EMPTY_PATH = -6, 39 | SPEED_LIMIT_EXCEEDED = -7, 40 | ACCELERATION_LIMIT_EXCEEDED = -8, 41 | MAX_TRAJECTORY_SIZE_EXCEEDED = -9, 42 | UNINITIALIZED = -10, 43 | INVALID_ID = -11, 44 | INCOMPLETE_PATH = -12, 45 | INVALID_CONFIGURATION_PARAMETER = -13, 46 | UKNOWN = -99 47 | }; 48 | } 49 | typedef PlannerErrors::PlannerError PlannerError; 50 | 51 | typedef std::map PlannerConfig; 52 | 53 | DESCARTES_CLASS_FORWARD(PathPlannerBase); 54 | class PathPlannerBase 55 | { 56 | public: 57 | virtual ~PathPlannerBase() 58 | { 59 | } 60 | 61 | /** 62 | * @brief Plans a path for the given robot model and configuration parameters. 63 | * @param model robot model implementation for which to plan a path 64 | * @param config A map containing the parameter/value pairs. 65 | */ 66 | virtual bool initialize(RobotModelConstPtr model) = 0; 67 | 68 | /** 69 | * @brief Configure the planner's parameters. Should return 'true' when all the entries were properly parsed. 70 | * @param config A map containing the parameter/value pairs. 71 | */ 72 | virtual bool setConfig(const PlannerConfig& config) = 0; 73 | 74 | /** 75 | * @brief Get the current configuration parameters used by the planner 76 | * @param config A map containing the current parameter/value pairs. 77 | */ 78 | virtual void getConfig(PlannerConfig& config) const = 0; 79 | 80 | /** 81 | * @brief Generates a robot path from the trajectory. 82 | * @param traj the points used to plan the robot path 83 | * @error_code Integer flag which indicates the type of error encountered during planning. 84 | */ 85 | virtual bool planPath(const std::vector& traj) = 0; 86 | 87 | /** 88 | * @brief Returns the last robot path generated from the input trajectory 89 | * @param path Array that contains the points in the robot path 90 | */ 91 | virtual bool getPath(std::vector& path) const = 0; 92 | 93 | /** 94 | * @brief Add a point to the current path after the point with 'ref_id'. 95 | * @param ref_id ID of the reference point 96 | */ 97 | virtual bool addAfter(const TrajectoryPt::ID& ref_id, TrajectoryPtPtr tp) = 0; 98 | 99 | /** 100 | * @brief Add a point to the current path before the point with 'ref_id'. 101 | * @param ref_id ID of the reference point 102 | */ 103 | virtual bool addBefore(const TrajectoryPt::ID& ref_id, TrajectoryPtPtr tp) = 0; 104 | 105 | /** 106 | * @brief Removes the point with 'ref_id' from the path. 107 | * @param ref_id ID of the reference point 108 | */ 109 | virtual bool remove(const TrajectoryPt::ID& ref_id) = 0; 110 | 111 | /** 112 | * @brief Modifies the point with 'ref_id'. 113 | * @param ref_id ID of the reference point 114 | */ 115 | virtual bool modify(const TrajectoryPt::ID& ref_id, TrajectoryPtPtr tp) = 0; 116 | 117 | /** 118 | * @brief Returns the last error code. 119 | */ 120 | virtual int getErrorCode() const = 0; 121 | 122 | /** 123 | * @brief Gets the error message corresponding to the error code. 124 | * @param error_code Integer code from the PlannerError enumeration. 125 | */ 126 | virtual bool getErrorMessage(int error_code, std::string& msg) const = 0; 127 | 128 | protected: 129 | PathPlannerBase() 130 | { 131 | } 132 | }; 133 | } 134 | 135 | #endif /* DESCARTES_CORE_PATH_PLANNER_BASE_H_ */ 136 | -------------------------------------------------------------------------------- /descartes_core/include/descartes_core/utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef UTILS_H_ 20 | #define UTILS_H_ 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | /** \def DESCARTES_CLASS_FORWARD 27 | Macro that forward declares a class XXX, and also defines two shared ptrs with named XXXPtr and XXXConstPtr */ 28 | 29 | #define DESCARTES_CLASS_FORWARD(C) \ 30 | class C; \ 31 | typedef boost::shared_ptr C##Ptr; \ 32 | typedef boost::shared_ptr C##ConstPtr; 33 | 34 | namespace descartes_core 35 | { 36 | namespace utils 37 | { 38 | /** 39 | @brief Converts scalar translations and rotations to an Eigen Frame. This is achieved by chaining a 40 | translation with individual euler rotations in ZYX order (this is equivalent to fixed rotatins XYZ) 41 | http://en.wikipedia.org/wiki/Euler_angles#Conversion_between_intrinsic_and_extrinsic_rotations 42 | 43 | @param tx, ty, tz - translations in x, y, z respectively 44 | @param rx, ry, rz - rotations about x, y, z, respectively 45 | */ 46 | namespace EulerConventions 47 | { 48 | enum EulerConvention 49 | { 50 | XYZ = 0, 51 | ZYX, 52 | ZXZ 53 | }; 54 | } 55 | 56 | typedef EulerConventions::EulerConvention EulerConvention; 57 | 58 | // Use a function declaration so that we can add the 'unused' attribute, which prevents compiler warnings 59 | static Eigen::Isometry3d toFrame(double tx, double ty, double tz, double rx, double ry, double rz, 60 | int convention = int(EulerConventions::ZYX)) __attribute__((unused)); 61 | 62 | static Eigen::Isometry3d toFrame(double tx, double ty, double tz, double rx, double ry, double rz, int convention) 63 | { 64 | Eigen::Isometry3d rtn; 65 | 66 | switch (convention) 67 | { 68 | case EulerConventions::XYZ: 69 | rtn = Eigen::Translation3d(tx, ty, tz) * Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()) * 70 | Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()); 71 | break; 72 | 73 | case EulerConventions::ZYX: 74 | rtn = Eigen::Translation3d(tx, ty, tz) * Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()) * 75 | Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()); 76 | break; 77 | 78 | case EulerConventions::ZXZ: 79 | rtn = Eigen::Translation3d(tx, ty, tz) * Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()) * 80 | Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()); 81 | break; 82 | 83 | default: 84 | CONSOLE_BRIDGE_logError("Invalid euler convention entry %i", convention); 85 | break; 86 | } 87 | 88 | return rtn; 89 | } 90 | 91 | /** 92 | * Compares two vectors for equality (within +/- tolerance). abs(lhs - rhs) > tol 93 | * @param lhs 94 | * @param rhs 95 | * @param tol +/- tolerance for floating point equality 96 | */ 97 | 98 | // Use a function declaration so that we can add the 'unused' attribute, which prevents compiler warnings 99 | static bool equal(const std::vector &lhs, const std::vector &rhs, const double tol) 100 | __attribute__((unused)); 101 | 102 | static bool equal(const std::vector &lhs, const std::vector &rhs, const double tol) 103 | { 104 | bool rtn = false; 105 | if (lhs.size() == rhs.size()) 106 | { 107 | rtn = true; 108 | for (size_t ii = 0; ii < lhs.size(); ++ii) 109 | { 110 | if (std::fabs(lhs[ii] - rhs[ii]) > tol) 111 | { 112 | rtn = false; 113 | break; 114 | } 115 | } 116 | } 117 | else 118 | { 119 | rtn = false; 120 | } 121 | return rtn; 122 | } 123 | 124 | } // utils 125 | 126 | } // descartes_core 127 | 128 | #endif /* UTILS_H_ */ 129 | -------------------------------------------------------------------------------- /descartes_planner/include/descartes_planner/sparse_planner.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | /* 19 | * sparse_planner.h 20 | * 21 | * Created on: Dec 17, 2014 22 | * Author: ros developer 23 | */ 24 | 25 | #ifndef SPARSE_PLANNER_H_ 26 | #define SPARSE_PLANNER_H_ 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | namespace descartes_planner 33 | { 34 | class SparsePlanner : public descartes_core::PathPlannerBase 35 | { 36 | public: 37 | typedef std::vector > 38 | SolutionArray; 39 | 40 | public: 41 | SparsePlanner(descartes_core::RobotModelConstPtr model, double sampling = 0.1f); 42 | SparsePlanner(); 43 | virtual ~SparsePlanner(); 44 | 45 | virtual bool initialize(descartes_core::RobotModelConstPtr model); 46 | virtual bool initialize(descartes_core::RobotModelConstPtr model, 47 | descartes_planner::CostFunction cost_function_callback); 48 | virtual bool setConfig(const descartes_core::PlannerConfig& config); 49 | virtual void getConfig(descartes_core::PlannerConfig& config) const; 50 | virtual bool planPath(const std::vector& traj); 51 | virtual bool addAfter(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr cp); 52 | virtual bool addBefore(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr cp); 53 | virtual bool modify(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr cp); 54 | virtual bool remove(const descartes_core::TrajectoryPt::ID& ref_id); 55 | virtual bool getPath(std::vector& path) const; 56 | virtual int getErrorCode() const; 57 | virtual bool getErrorMessage(int error_code, std::string& msg) const; 58 | 59 | void setSampling(double sampling); 60 | const std::map& getSolution(); 61 | bool getSolutionJointPoint(const descartes_trajectory::CartTrajectoryPt::ID& cart_id, 62 | descartes_trajectory::JointTrajectoryPt& j); 63 | 64 | protected: 65 | bool plan(); 66 | bool interpolateJointPose(const std::vector& start, const std::vector& end, double t, 67 | std::vector& interp); 68 | int interpolateSparseTrajectory(const SolutionArray& sparse_solution, int& sparse_index, int& point_pos); 69 | void sampleTrajectory(double sampling, const std::vector& dense_trajectory_array, 70 | std::vector& sparse_trajectory_array); 71 | 72 | int getDensePointIndex(const descartes_core::TrajectoryPt::ID& ref_id); 73 | int getSparsePointIndex(const descartes_core::TrajectoryPt::ID& ref_id); 74 | int findNearestSparsePointIndex(const descartes_core::TrajectoryPt::ID& ref_id, bool skip_equal = true); 75 | bool isInSparseTrajectory(const descartes_core::TrajectoryPt::ID& ref_id); 76 | bool checkJointChanges(const std::vector& s1, const std::vector& s2, const double& max_change); 77 | 78 | bool getOrderedSparseArray(std::vector& sparse_array); 79 | bool getSparseSolutionArray(SolutionArray& sparse_solution_array); 80 | 81 | protected: 82 | enum class InterpolationResult : int 83 | { 84 | ERROR = -1, 85 | REPLAN, 86 | SUCCESS 87 | }; 88 | 89 | double sampling_; 90 | int error_code_; 91 | std::map error_map_; 92 | descartes_core::PlannerConfig config_; 93 | boost::shared_ptr planning_graph_; 94 | std::vector cart_points_; 95 | SolutionArray sparse_solution_array_; 96 | std::map joint_points_map_; 97 | std::vector timing_cache_; 98 | }; 99 | 100 | } /* namespace descartes_planner */ 101 | #endif /* SPARSE_PLANNER_H_ */ 102 | -------------------------------------------------------------------------------- /descartes_core/include/descartes_core/trajectory_id.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef TRAJECTORY_ID_H 20 | #define TRAJECTORY_ID_H 21 | 22 | #include 23 | 24 | #include 25 | 26 | namespace descartes_core 27 | { 28 | namespace detail 29 | { 30 | /** 31 | * @brief Unimplemented base for IdGenerator. Users should specialize 32 | * this struct for the base ID type. It represents a concept 33 | * that defines the following types: 34 | * 1. value_type typedef representing the type of unique state object 35 | * 2. value_type make_nil() function that returns a nil sentinel state object 36 | * 3. value_type make_id() function that returns a unique state object 37 | * 4. bool is_nil(value_type) function that tests if an object is the sentinel 38 | */ 39 | template 40 | struct IdGenerator; 41 | 42 | /** 43 | * @brief This specialization of the id generator uses a 64 bit unsigned integer 44 | * for the unique 'state'. Zero is reserved as a special value 45 | */ 46 | template <> 47 | struct IdGenerator 48 | { 49 | typedef uint64_t value_type; 50 | 51 | static value_type make_nil() 52 | { 53 | return 0; 54 | } 55 | 56 | static value_type make_id() 57 | { 58 | boost::unique_lock scoped_lock(counter_mutex_); 59 | return counter_++; 60 | } 61 | 62 | static bool is_nil(value_type id) 63 | { 64 | return id == 0; 65 | } 66 | 67 | private: 68 | // Initialized to 1 69 | static value_type counter_; 70 | static boost::mutex counter_mutex_; 71 | }; 72 | } 73 | 74 | /** 75 | * @brief TrajectoryID_ represents a unique id to be associated with a TrajectoryPt 76 | * 77 | */ 78 | template 79 | class TrajectoryID_ 80 | { 81 | public: 82 | typedef T value_type; 83 | 84 | /** 85 | * @brief Constructor for generating a trajectory id using the given state object 86 | */ 87 | TrajectoryID_(value_type id) : id_(id) 88 | { 89 | } 90 | 91 | /** 92 | * @brief Constructor for generating a trajectory with a default id. Here we default 93 | * the points to nil. Default constructor is needed for STL containers. 94 | */ 95 | TrajectoryID_() : id_(detail::IdGenerator::make_nil()) 96 | { 97 | } 98 | 99 | /** 100 | * @brief Tests if this ID is nil 101 | */ 102 | bool is_nil() const 103 | { 104 | return detail::IdGenerator::is_nil(id_); 105 | } 106 | 107 | /** 108 | * @brief Retrieves the value of the underlying id state object 109 | */ 110 | value_type value() const 111 | { 112 | return id_; 113 | } 114 | 115 | /** 116 | * @brief Factory function to generate a trajectory ID with a unique state object 117 | */ 118 | static TrajectoryID_ make_id() 119 | { 120 | return TrajectoryID_(detail::IdGenerator::make_id()); 121 | } 122 | 123 | /** 124 | * @brief Factory function to generate a trajectory ID with a nil state object 125 | */ 126 | static TrajectoryID_ make_nil() 127 | { 128 | return TrajectoryID_(detail::IdGenerator::make_nil()); 129 | } 130 | 131 | private: 132 | value_type id_; 133 | }; 134 | 135 | ////////////////////// 136 | // Helper Functions // 137 | ////////////////////// 138 | 139 | template 140 | inline bool operator==(TrajectoryID_ lhs, TrajectoryID_ rhs) 141 | { 142 | return lhs.value() == rhs.value(); 143 | } 144 | 145 | template 146 | inline bool operator!=(TrajectoryID_ lhs, TrajectoryID_ rhs) 147 | { 148 | return !(lhs == rhs); 149 | } 150 | 151 | template 152 | inline bool operator<(TrajectoryID_ lhs, TrajectoryID_ rhs) 153 | { 154 | return lhs.value() < rhs.value(); 155 | } 156 | 157 | template 158 | inline std::ostream& operator<<(std::ostream& os, TrajectoryID_ id) 159 | { 160 | os << "ID" << id.value(); 161 | return os; 162 | } 163 | 164 | typedef TrajectoryID_ TrajectoryID; 165 | 166 | } // end namespace descartes_core 167 | 168 | #endif 169 | -------------------------------------------------------------------------------- /descartes_utilities/src/ros_conversions.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2016, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | /* 19 | * ros_conversions.cpp 20 | * 21 | * Created on: Feb 28, 2016 22 | * Author: Jonathan Meyer 23 | */ 24 | 25 | #include "descartes_utilities/ros_conversions.h" 26 | #include 27 | #include 28 | 29 | /** 30 | * @brief Given two sets of joint values representing two robot joint poses, this function computes the 31 | * minimum amount of time required to interpolate between the points assuming that each joint can 32 | * move no faster than a constant 'max_vel' speed. 33 | * @return The minimum time required to interpolate between poses. 34 | */ 35 | static double minTime(const std::vector& pose_a, const std::vector& pose_b, double max_vel) 36 | { 37 | std::vector diff; 38 | diff.reserve(pose_a.size()); 39 | 40 | // compute joint-wise minimum time required based on relative distance between joint positions 41 | // and the maximum allowable joint velocity 42 | std::transform(pose_a.begin(), pose_a.end(), pose_b.begin(), std::back_inserter(diff), [max_vel](double a, double b) 43 | { 44 | return std::abs(a - b) / max_vel; 45 | }); 46 | // The biggest time across all of the joints is the min time 47 | return *std::max_element(diff.begin(), diff.end()); 48 | } 49 | 50 | bool descartes_utilities::toRosJointPoints(const descartes_core::RobotModel& model, 51 | const std::vector& joint_traj, 52 | double default_joint_vel, 53 | std::vector& out) 54 | { 55 | if (default_joint_vel <= 0.0) 56 | { 57 | CONSOLE_BRIDGE_logError("%s: Invalid value for default joint velocity. Must be > 0 (radians/second)", __FUNCTION__); 58 | return false; 59 | } 60 | 61 | const static double max_default_joint_velocity = 100.0; // (radians / s); approx 1000 rpm 62 | if (default_joint_vel > max_default_joint_velocity) 63 | { 64 | CONSOLE_BRIDGE_logError("%s: Default joint velocity of %f exceeds assumed limit of %f.", __FUNCTION__, default_joint_vel, 65 | max_default_joint_velocity); 66 | return false; 67 | } 68 | 69 | ros::Duration from_start(0.0); 70 | std::vector ros_trajectory; 71 | ros_trajectory.reserve(joint_traj.size()); 72 | 73 | std::vector joint_point; 74 | std::vector dummy; 75 | 76 | for (std::size_t i = 0; i < joint_traj.size(); ++i) 77 | { 78 | if (!joint_traj[i]) 79 | { 80 | CONSOLE_BRIDGE_logError("%s: Input trajectory contained null pointer at index %lu", __FUNCTION__, static_cast(i)); 81 | return false; 82 | } 83 | 84 | descartes_core::TrajectoryPt& pt = *joint_traj[i]; 85 | 86 | if (!pt.getNominalJointPose(dummy, model, joint_point)) 87 | { 88 | CONSOLE_BRIDGE_logError("%s: Failed to extract joint positions from input trajectory at index %lu", __FUNCTION__, 89 | static_cast(i)); 90 | return false; 91 | } 92 | 93 | trajectory_msgs::JointTrajectoryPoint ros_pt; 94 | ros_pt.positions = joint_point; 95 | // Descartes has no internal representation of velocity, acceleration, or effort so we fill these field with zeros. 96 | ros_pt.velocities.resize(joint_point.size(), 0.0); 97 | ros_pt.accelerations.resize(joint_point.size(), 0.0); 98 | ros_pt.effort.resize(joint_point.size(), 0.0); 99 | 100 | if (pt.getTiming().isSpecified()) 101 | { 102 | from_start += ros::Duration(pt.getTiming().upper); 103 | } 104 | else 105 | { 106 | // If we have a previous point, compute dt based on default, max joint velocity 107 | // otherwise trajectory starts at current location (time offset == 0). 108 | double dt; 109 | if (i == 0) 110 | dt = 0.0; 111 | else 112 | dt = minTime(joint_point, ros_trajectory.back().positions, default_joint_vel); 113 | 114 | from_start += ros::Duration(dt); 115 | } 116 | 117 | ros_pt.time_from_start = from_start; 118 | ros_trajectory.push_back(ros_pt); 119 | } 120 | 121 | out = ros_trajectory; 122 | return true; 123 | } 124 | -------------------------------------------------------------------------------- /descartes_tests/src/cartesian_robot.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include "descartes_tests/cartesian_robot.h" 20 | #include "eigen_conversions/eigen_kdl.h" 21 | #include "ros/console.h" 22 | #include "ros/assert.h" 23 | 24 | namespace descartes_tests 25 | { 26 | const static int DOF = 6; 27 | 28 | static void displayRange(double pos_range, double orient_range) 29 | { 30 | double pos_limit = pos_range / 2.0; 31 | double orient_limit = orient_range / 2.0; 32 | ROS_INFO_STREAM("Creating Cartesian Robot with valid linear ranges from " 33 | << -pos_limit << " to " << pos_limit << ", and orientation from " << -orient_limit << " to " 34 | << orient_limit); 35 | } 36 | 37 | CartesianRobot::CartesianRobot() : pos_range_(2.0), orient_range_(M_PI_2), joint_velocities_(DOF, 1.0) 38 | { 39 | displayRange(pos_range_, orient_range_); 40 | } 41 | 42 | CartesianRobot::CartesianRobot(double pos_range, double orient_range, const std::vector &joint_velocities) 43 | : pos_range_(pos_range), orient_range_(orient_range), joint_velocities_(joint_velocities) 44 | { 45 | ROS_ASSERT(joint_velocities_.size() == DOF); 46 | displayRange(pos_range_, orient_range_); 47 | } 48 | 49 | bool CartesianRobot::initialize(const std::string &, const std::string &, 50 | const std::string &, const std::string &) 51 | { 52 | return true; 53 | } 54 | 55 | bool CartesianRobot::isValidMove(const double *s, const double *f, double dt) const 56 | { 57 | for (int i = 0; i < getDOF(); ++i) 58 | { 59 | if (std::abs(s[i] - f[i]) / dt > joint_velocities_[i]) 60 | return false; 61 | } 62 | return true; 63 | } 64 | 65 | bool CartesianRobot::getIK(const Eigen::Isometry3d &pose, const std::vector &, 66 | std::vector &joint_pose) const 67 | { 68 | bool rtn = false; 69 | KDL::Frame kdl_frame; 70 | tf::transformEigenToKDL(pose, kdl_frame); 71 | 72 | joint_pose.resize(DOF, 0.0); 73 | joint_pose[0] = kdl_frame.p.x(); 74 | joint_pose[1] = kdl_frame.p.y(); 75 | joint_pose[2] = kdl_frame.p.z(); 76 | kdl_frame.M.GetRPY(joint_pose[3], joint_pose[4], joint_pose[5]); 77 | 78 | if (isValid(joint_pose)) 79 | { 80 | rtn = true; 81 | } 82 | else 83 | { 84 | rtn = false; 85 | } 86 | return rtn; 87 | } 88 | 89 | bool CartesianRobot::getAllIK(const Eigen::Isometry3d &pose, std::vector > &joint_poses) const 90 | { 91 | std::vector empty; 92 | joint_poses.resize(1); 93 | return getIK(pose, empty, joint_poses[0]); 94 | } 95 | 96 | bool CartesianRobot::getFK(const std::vector &joint_pose, Eigen::Isometry3d &pose) const 97 | { 98 | bool rtn = false; 99 | 100 | if (isValid(joint_pose)) 101 | { 102 | pose = Eigen::Translation3d(joint_pose[0], joint_pose[1], joint_pose[2]) * 103 | Eigen::AngleAxisd(joint_pose[5], Eigen::Vector3d::UnitZ()) * 104 | Eigen::AngleAxisd(joint_pose[4], Eigen::Vector3d::UnitY()) * 105 | Eigen::AngleAxisd(joint_pose[3], Eigen::Vector3d::UnitX()); 106 | rtn = true; 107 | } 108 | else 109 | { 110 | rtn = false; 111 | } 112 | 113 | return rtn; 114 | } 115 | 116 | int CartesianRobot::getDOF() const 117 | { 118 | return DOF; 119 | } 120 | 121 | bool CartesianRobot::isValid(const std::vector &joint_pose) const 122 | { 123 | bool rtn = false; 124 | 125 | double pos_limit = pos_range_ / 2.0; 126 | double orient_limit = orient_range_ / 2.0; 127 | 128 | if (DOF == joint_pose.size()) 129 | { 130 | rtn = (fabs(joint_pose[0]) <= pos_limit && fabs(joint_pose[1]) <= pos_limit && fabs(joint_pose[2]) <= pos_limit && 131 | fabs(joint_pose[3]) <= orient_limit && fabs(joint_pose[4]) <= orient_limit && 132 | fabs(joint_pose[5]) <= orient_limit); 133 | } 134 | else 135 | { 136 | ROS_DEBUG_STREAM("Joint pose size: " << joint_pose.size() << "exceeds " << DOF); 137 | } 138 | 139 | return rtn; 140 | } 141 | 142 | bool CartesianRobot::isValid(const Eigen::Isometry3d &pose) const 143 | { 144 | bool rtn = false; 145 | double R, P, Y; 146 | KDL::Frame kdl_frame; 147 | tf::transformEigenToKDL(pose, kdl_frame); 148 | kdl_frame.M.GetRPY(R, P, Y); 149 | 150 | double pos_limit = pos_range_ / 2.0; 151 | double orient_limit = orient_range_ / 2.0; 152 | 153 | rtn = 154 | (fabs(kdl_frame.p.x()) <= pos_limit && fabs(kdl_frame.p.y()) <= pos_limit && fabs(kdl_frame.p.z()) <= pos_limit && 155 | fabs(R) <= orient_limit && fabs(P) <= orient_limit && fabs(Y) <= orient_limit); 156 | 157 | return rtn; 158 | } 159 | 160 | 161 | } 162 | -------------------------------------------------------------------------------- /descartes_tests/test/planner/planner_tests.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include "utils/trajectory_maker.h" 25 | 26 | // Google Test Framework for Path Planners 27 | 28 | template 29 | class PathPlannerTest : public testing::Test 30 | { 31 | protected: 32 | descartes_core::PathPlannerBasePtr makePlanner() 33 | { 34 | descartes_core::PathPlannerBasePtr planner(new T()); 35 | EXPECT_TRUE(planner->initialize(robot_)) << "Failed to initalize robot model"; 36 | return planner; 37 | } 38 | 39 | PathPlannerTest() 40 | : velocity_limits_(6, 1.0), robot_(new descartes_tests::CartesianRobot(5.0, 0.001, velocity_limits_)) 41 | { 42 | ros::Time::init(); 43 | } 44 | 45 | std::vector velocity_limits_; 46 | descartes_core::RobotModelConstPtr robot_; 47 | }; 48 | 49 | TYPED_TEST_CASE_P(PathPlannerTest); 50 | 51 | TYPED_TEST_P(PathPlannerTest, construction) 52 | { 53 | using namespace descartes_core; 54 | 55 | PathPlannerBasePtr planner = this->makePlanner(); 56 | } 57 | 58 | TYPED_TEST_P(PathPlannerTest, basicConfigure) 59 | { 60 | descartes_core::PathPlannerBasePtr planner = this->makePlanner(); 61 | descartes_core::PlannerConfig config; 62 | planner->getConfig(config); 63 | EXPECT_TRUE(planner->setConfig(config)); 64 | } 65 | 66 | TYPED_TEST_P(PathPlannerTest, preservesTiming) 67 | { 68 | using namespace descartes_tests; 69 | using namespace descartes_core; 70 | 71 | PathPlannerBasePtr planner = this->makePlanner(); 72 | 73 | std::vector input, output; 74 | // Make input trajectory 75 | input = makeConstantVelocityTrajectory(Eigen::Vector3d(-1.0, 0, 0), // start position 76 | Eigen::Vector3d(1.0, 0, 0), // end position 77 | 0.9, // tool velocity 78 | 20); // samples 79 | // Double the dt of every pt to provide some variety 80 | double dt = input.front().get()->getTiming().upper; 81 | for (auto& pt : input) 82 | { 83 | pt.get()->setTiming(TimingConstraint(dt)); 84 | dt *= 1.5; 85 | } 86 | // // Solve 87 | ASSERT_TRUE(planner->planPath(input)); 88 | // Get the result 89 | ASSERT_TRUE(planner->getPath(output)); 90 | // Compare timing 91 | ASSERT_TRUE(input.size() == output.size()); 92 | for (size_t i = 0; i < input.size(); ++i) 93 | { 94 | double t1 = input[i].get()->getTiming().upper; 95 | double t2 = output[i].get()->getTiming().upper; 96 | EXPECT_DOUBLE_EQ(t1, t2) << "Input/output timing should correspond for same index: " << t1 << " " << t2; 97 | } 98 | } 99 | 100 | TYPED_TEST_P(PathPlannerTest, simpleVelocityCheck) 101 | { 102 | using namespace descartes_core; 103 | 104 | PathPlannerBasePtr planner = this->makePlanner(); 105 | 106 | std::vector input; 107 | input = descartes_tests::makeConstantVelocityTrajectory(Eigen::Vector3d(-1.0, 0, 0), // start position 108 | Eigen::Vector3d(1.0, 0, 0), // end position 109 | 0.9, // tool velocity (< 1.0 m/s limit) 110 | 10); // samples 111 | ASSERT_TRUE(!input.empty()); 112 | // The nominal trajectory (0.9 m/s) is less than max tool speed of 1.0 m/s 113 | EXPECT_TRUE(planner->planPath(input)); 114 | // Unconstraining a point should still succeed 115 | input.back().get()->setTiming(descartes_core::TimingConstraint()); 116 | EXPECT_TRUE(planner->planPath(input)); 117 | // Making a dt for a segment very small should induce failure 118 | input.back().get()->setTiming(descartes_core::TimingConstraint(0.001)); 119 | EXPECT_FALSE(planner->planPath(input)) << "Trajectory pt has very small dt; planner should fail for velocity out of " 120 | "bounds"; 121 | } 122 | 123 | TYPED_TEST_P(PathPlannerTest, zigzagTrajectory) 124 | { 125 | using namespace descartes_core; 126 | 127 | PathPlannerBasePtr planner = this->makePlanner(); 128 | 129 | std::vector input; 130 | input = descartes_tests::makeZigZagTrajectory(-1.0, // start position 131 | 1.0, // end position 132 | 0.5, 133 | 0.1, // tool velocity (< 1.0 m/s limit) 134 | 10); // samples 135 | ASSERT_TRUE(!input.empty()); 136 | // The nominal trajectory (0.9 m/s) is less than max tool speed of 1.0 m/s 137 | EXPECT_TRUE(planner->planPath(input)); 138 | } 139 | 140 | REGISTER_TYPED_TEST_CASE_P(PathPlannerTest, construction, basicConfigure, preservesTiming, simpleVelocityCheck, 141 | zigzagTrajectory); 142 | -------------------------------------------------------------------------------- /descartes_tests/test/planner/planning_graph_tests.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2015, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | static boost::shared_ptr makeTestRobot() 27 | { 28 | const auto max_joint_vel = 1.0; 29 | const auto dof = 6; 30 | return boost::shared_ptr( 31 | new descartes_tests::CartesianRobot(5.0, 0.001, std::vector(dof, max_joint_vel)) 32 | ); 33 | } 34 | 35 | static inline boost::shared_ptr makePoint(double v, double tm = 0.0) 36 | { 37 | return boost::make_shared(std::vector(6, v), 38 | descartes_core::TimingConstraint(tm)); 39 | } 40 | 41 | static std::vector twoPoints() 42 | { 43 | auto start_pt = makePoint(0.0); 44 | auto stop_pt = makePoint(1.0); 45 | 46 | std::vector ret; 47 | ret.push_back(start_pt); 48 | ret.push_back(stop_pt); 49 | return ret; 50 | } 51 | 52 | static descartes_core::TrajectoryPtPtr thirdTestPoint() 53 | { 54 | return makePoint(2.0); 55 | } 56 | 57 | static std::vector threePoints() 58 | { 59 | auto pt = thirdTestPoint(); 60 | auto vec = twoPoints(); 61 | vec.push_back(pt); 62 | return vec; 63 | } 64 | 65 | TEST(PlanningGraph, setup) 66 | { 67 | // Create robot 68 | auto robot = makeTestRobot(); 69 | // Create planner 70 | descartes_planner::PlanningGraph graph {robot}; 71 | // Add a trajectory 72 | auto points = threePoints(); 73 | ASSERT_TRUE(graph.insertGraph(points)); 74 | 75 | double cost; 76 | std::list out; 77 | EXPECT_TRUE(graph.getShortestPath(cost, out)); 78 | } 79 | 80 | TEST(PlanningGraph, custom_cost_fn) 81 | { 82 | // Create robot 83 | auto robot = makeTestRobot(); 84 | auto points = threePoints(); 85 | 86 | auto custom_cost_fn = [] (const double* a, const double* b) { 87 | double cost = 0.0; 88 | for (int i = 0; i < 6; ++i) cost += std::abs(a[i] - b[i]); 89 | return cost; 90 | }; 91 | 92 | // Create planner 93 | descartes_planner::PlanningGraph graph {robot, custom_cost_fn}; 94 | 95 | ASSERT_TRUE(graph.insertGraph(points)); 96 | double cost; 97 | std::list out; 98 | EXPECT_TRUE(graph.getShortestPath(cost, out)); 99 | } 100 | 101 | TEST(PlanningGraph, insert_then_add_point) 102 | { 103 | // Create robot 104 | auto robot = makeTestRobot(); 105 | auto points = twoPoints(); 106 | 107 | // Create planner 108 | descartes_planner::PlanningGraph graph {robot}; 109 | 110 | // initialize graph 111 | ASSERT_TRUE(graph.insertGraph(points)); 112 | 113 | auto new_pt = thirdTestPoint(); 114 | 115 | ASSERT_TRUE(graph.addTrajectory( new_pt, points.back()->getID(), descartes_core::TrajectoryPt::ID::make_nil() )); 116 | 117 | double cost; 118 | std::list out; 119 | ASSERT_TRUE(graph.getShortestPath(cost, out)); 120 | 121 | EXPECT_TRUE(out.size() == 3); 122 | EXPECT_TRUE(cost != 0.0); 123 | } 124 | 125 | TEST(PlanningGraph, insert_then_remove_point) 126 | { 127 | // Create robot 128 | auto robot = makeTestRobot(); 129 | auto points = threePoints(); 130 | 131 | // Create planner 132 | descartes_planner::PlanningGraph graph {robot}; 133 | 134 | // initialize graph 135 | ASSERT_TRUE(graph.insertGraph(points)); 136 | 137 | // Now remove the last point 138 | ASSERT_TRUE( graph.removeTrajectory(points.back()->getID()) ); 139 | 140 | double cost; 141 | std::list out; 142 | ASSERT_TRUE(graph.getShortestPath(cost, out)); 143 | 144 | EXPECT_TRUE(out.size() == 2); 145 | EXPECT_TRUE(cost != 0.0); 146 | } 147 | 148 | TEST(PlanningGraph, insert_then_modify_all_points) 149 | { 150 | // Create robot 151 | auto robot = makeTestRobot(); 152 | auto points = threePoints(); 153 | 154 | // Create planner 155 | descartes_planner::PlanningGraph graph {robot}; 156 | 157 | // initialize graph 158 | ASSERT_TRUE(graph.insertGraph(points)); 159 | 160 | // modify each of the points to mimic itself 161 | ASSERT_TRUE( graph.modifyTrajectory(points[0]) ); 162 | ASSERT_TRUE( graph.modifyTrajectory(points[1]) ); 163 | ASSERT_TRUE( graph.modifyTrajectory(points[2]) ); 164 | 165 | double cost; 166 | std::list out; 167 | ASSERT_TRUE(graph.getShortestPath(cost, out)); 168 | 169 | EXPECT_TRUE(out.size() == 3); 170 | EXPECT_TRUE(cost != 0.0); 171 | 172 | // modify a point to be invalid 173 | auto invalid_pt = makePoint(100.0, 1.0); // way out of range of the other points (0, 1, & 2) 174 | invalid_pt->setID(points[1]->getID()); // copy that points ID 175 | ASSERT_TRUE(invalid_pt->getTiming().isSpecified()); 176 | 177 | ASSERT_TRUE( graph.modifyTrajectory(invalid_pt) ); 178 | EXPECT_FALSE(graph.getShortestPath(cost, out)); 179 | } 180 | -------------------------------------------------------------------------------- /descartes_planner/include/descartes_planner/planning_graph_edge_policy.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef PLANNING_GRAPH_EDGE_POLICY_H 20 | #define PLANNING_GRAPH_EDGE_POLICY_H 21 | 22 | #include "descartes_planner/ladder_graph.h" 23 | #include 24 | 25 | namespace descartes_planner 26 | { 27 | 28 | struct DefaultEdgesWithTime 29 | { 30 | DefaultEdgesWithTime(const size_t n_start, 31 | const size_t n_end, 32 | const size_t dof, 33 | const double upper_tm, 34 | const std::vector& joint_vel_limits) 35 | : results_(n_start) 36 | , edge_scratch_(n_end) 37 | , max_dtheta_(dof) 38 | , delta_buffer_(dof) 39 | , dof_(dof) 40 | , count_(0) 41 | , has_edges_(false) 42 | { 43 | std::transform(joint_vel_limits.cbegin(), joint_vel_limits.cend(), max_dtheta_.begin(), [upper_tm] (double v) { 44 | return v * upper_tm; 45 | }); 46 | } 47 | 48 | inline void consider(const double* const start, const double* const stop, size_t index) noexcept 49 | { 50 | for (size_t i = 0; i < dof_; ++i) 51 | { 52 | delta_buffer_[i] = std::abs(start[i] - stop[i]); 53 | if (delta_buffer_[i] > max_dtheta_[i]) return; 54 | } 55 | 56 | auto cost = std::accumulate(delta_buffer_.cbegin(), delta_buffer_.cend(), 0.0); 57 | edge_scratch_[count_].cost = cost; 58 | edge_scratch_[count_].idx = static_cast(index); 59 | count_++; 60 | } 61 | 62 | inline void next(const size_t i) 63 | { 64 | results_[i].assign(edge_scratch_.cbegin(), edge_scratch_.cbegin() + count_); 65 | has_edges_ = has_edges_ || count_ > 0; 66 | count_ = 0; 67 | } 68 | 69 | inline std::vector& result() noexcept { return results_; } 70 | 71 | inline bool hasEdges() const noexcept { return has_edges_; } 72 | 73 | std::vector results_; 74 | LadderGraph::EdgeList edge_scratch_; // pre-allocated space to work in 75 | std::vector max_dtheta_; 76 | std::vector delta_buffer_; 77 | size_t dof_; 78 | unsigned count_; 79 | bool has_edges_; 80 | }; 81 | 82 | struct CustomEdgesWithTime : public DefaultEdgesWithTime 83 | { 84 | CustomEdgesWithTime(const size_t n_start, 85 | const size_t n_end, 86 | const size_t dof, 87 | const double upper_tm, 88 | const std::vector& joint_vel_limits, 89 | descartes_planner::CostFunction fn) 90 | : DefaultEdgesWithTime(n_start, n_end, dof, upper_tm, joint_vel_limits) 91 | , custom_cost_fn(fn) 92 | {} 93 | 94 | inline void consider(const double * const start, const double * const stop, const size_t index) noexcept 95 | { 96 | for (size_t i = 0; i < dof_; ++i) 97 | { 98 | delta_buffer_[i] = std::abs(start[i] - stop[i]); 99 | if (delta_buffer_[i] > max_dtheta_[i]) return; 100 | } 101 | 102 | double cost = custom_cost_fn(start, stop); 103 | edge_scratch_[count_++] = {cost, static_cast(index)}; 104 | } 105 | 106 | descartes_planner::CostFunction custom_cost_fn; // TODO: Header doesn't stand on its own 107 | }; 108 | 109 | struct DefaultEdgesWithoutTime 110 | { 111 | DefaultEdgesWithoutTime(const size_t n_start, 112 | const size_t n_end, 113 | const size_t dof) 114 | : results_(n_start) 115 | , dof_(dof) 116 | , count_(0) 117 | , layer_(0) 118 | { 119 | for (auto& edges : results_) 120 | { 121 | edges.resize(n_end); 122 | } 123 | } 124 | 125 | inline bool hasEdges() const { return true; } 126 | 127 | // no-op 128 | inline void next(const size_t) { layer_++; count_ = 0; } 129 | 130 | inline std::vector& result() noexcept { return results_; } 131 | 132 | inline void consider(const double* const start, const double* const stop, const size_t index) noexcept 133 | { 134 | double cost = 0.0; 135 | for (size_t i = 0; i < dof_; ++i) 136 | cost += std::abs(start[i] - stop[i]); 137 | 138 | results_[layer_][count_].cost = cost; 139 | results_[layer_][count_].idx = static_cast(index); 140 | count_++; 141 | } 142 | 143 | std::vector results_; 144 | size_t dof_; 145 | size_t count_; 146 | size_t layer_; 147 | }; 148 | 149 | struct CustomEdgesWithoutTime : public DefaultEdgesWithoutTime 150 | { 151 | CustomEdgesWithoutTime(const size_t n_start, 152 | const size_t n_end, 153 | const size_t dof, 154 | descartes_planner::CostFunction fn) 155 | : DefaultEdgesWithoutTime(n_start, n_end, dof), custom_cost_fn(fn) 156 | {} 157 | 158 | inline void consider(const double* const start, const double* const stop, const size_t index) noexcept 159 | { 160 | results_[layer_][count_].cost = custom_cost_fn(start, stop); 161 | results_[layer_][count_].idx = static_cast(index); 162 | count_++; 163 | } 164 | 165 | descartes_planner::CostFunction custom_cost_fn; // TODO: Header doesn't stand on its own 166 | }; 167 | 168 | } 169 | 170 | #endif // PLANNING_GRAPH_EDGE_POLICY_H 171 | -------------------------------------------------------------------------------- /descartes_moveit/include/descartes_moveit/moveit_state_adapter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Dan Solomon 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef MOVEIT_STATE_ADAPTER_H_ 20 | #define MOVEIT_STATE_ADAPTER_H_ 21 | 22 | #include "descartes_core/robot_model.h" 23 | #include "descartes_trajectory/cart_trajectory_pt.h" 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace descartes_moveit 30 | { 31 | /** 32 | * @brief MoveitStateAdapter adapts the MoveIt RobotState to the Descartes RobotModel interface 33 | */ 34 | class MoveitStateAdapter : public descartes_core::RobotModel 35 | { 36 | public: 37 | MoveitStateAdapter(); 38 | 39 | virtual ~MoveitStateAdapter() 40 | { 41 | } 42 | 43 | virtual bool initialize(const std::string &robot_description, const std::string &group_name, 44 | const std::string &world_frame, const std::string &tcp_frame); 45 | 46 | virtual bool initialize(robot_model::RobotModelConstPtr robot_model, const std::string &group_name, 47 | const std::string &world_frame, const std::string &tcp_frame); 48 | 49 | virtual bool getIK(const Eigen::Isometry3d &pose, const std::vector &seed_state, 50 | std::vector &joint_pose) const; 51 | 52 | virtual bool getAllIK(const Eigen::Isometry3d &pose, std::vector > &joint_poses) const; 53 | 54 | virtual bool getFK(const std::vector &joint_pose, Eigen::Isometry3d &pose) const; 55 | 56 | virtual bool isValid(const std::vector &joint_pose) const; 57 | 58 | virtual bool isValid(const Eigen::Isometry3d &pose) const; 59 | 60 | virtual int getDOF() const; 61 | 62 | virtual bool isValidMove(const double* from_joint_pose, const double* to_joint_pose, 63 | double dt) const; 64 | 65 | virtual std::vector getJointVelocityLimits() const override; 66 | /** 67 | * @brief Set the initial states used for iterative inverse kineamtics 68 | * @param seeds Vector of vector of doubles representing joint positions. 69 | * Be sure that it's sized correctly for the DOF. 70 | */ 71 | void setSeedStates(const std::vector > &seeds) 72 | { 73 | seed_states_ = seeds; 74 | } 75 | 76 | /** 77 | * @brief Retrieves the initial seed states used by iterative inverse kinematic solvers 78 | */ 79 | const std::vector > &getSeedStates() const 80 | { 81 | return seed_states_; 82 | } 83 | 84 | /** 85 | * @brief Returns the underlying moveit state object so it can be used to generate seeds 86 | */ 87 | moveit::core::RobotStatePtr getState() 88 | { 89 | return robot_state_; 90 | } 91 | 92 | /** 93 | * @brief Copies the internal state of 'state' into this model. Useful for initializing the 94 | * value of joints that are not part of the active move group. Should be called after 95 | * 'initialize()'. 96 | */ 97 | void setState(const moveit::core::RobotState &state); 98 | 99 | protected: 100 | /** 101 | * Gets IK solution (assumes robot state is pre-seeded) 102 | * @param pose Affine pose of TOOL in WOBJ frame 103 | * @param joint_pose Solution (if function successful). 104 | * @return 105 | */ 106 | bool getIK(const Eigen::Isometry3d &pose, std::vector &joint_pose) const; 107 | 108 | /** 109 | * TODO: Checks for collisions at this joint pose. The setCollisionCheck(true) must have been 110 | * called previously in order to enable collision checks, otherwise it will return false. 111 | * @param joint_pose the joint values at which check for collisions will be made 112 | */ 113 | bool isInCollision(const std::vector& joint_pose) const; 114 | 115 | /** 116 | * @brief Checks to see if the given joint_pose state is inside the bounds for the initialized 117 | * robot model's active joints. 118 | * @param joint_pose The pose to check; should be the same length as your groups active num active joints 119 | * @return true if the @e joint_pose is in bounds, false otherwise 120 | */ 121 | bool isInLimits(const std::vector& joint_pose) const; 122 | 123 | /** 124 | * Maximum joint velocities (rad/s) for each joint in the chain. Used for checking in 125 | * `isValidMove()` 126 | */ 127 | std::vector velocity_limits_; 128 | 129 | mutable moveit::core::RobotStatePtr robot_state_; 130 | 131 | planning_scene::PlanningScenePtr planning_scene_; 132 | 133 | robot_model::RobotModelConstPtr robot_model_ptr_; 134 | 135 | robot_model_loader::RobotModelLoaderPtr robot_model_loader_; 136 | 137 | const moveit::core::JointModelGroup *joint_group_; 138 | 139 | /** 140 | * @brief Vector of starting configurations for the numerical solver 141 | */ 142 | std::vector > seed_states_; 143 | 144 | /** 145 | * @brief Planning group name 146 | */ 147 | std::string group_name_; 148 | 149 | /** 150 | * @brief Tool frame name 151 | */ 152 | std::string tool_frame_; 153 | 154 | /** 155 | * @brief Work object/reference frame name 156 | */ 157 | std::string world_frame_; 158 | 159 | /** 160 | * @brief convenient transformation frame 161 | */ 162 | descartes_core::Frame world_to_root_; 163 | }; 164 | 165 | } // descartes_moveit 166 | 167 | #endif /* MOVEIT_STATE_ADAPTER_H_ */ 168 | -------------------------------------------------------------------------------- /descartes_trajectory/src/joint_trajectory_pt.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Dan Solomon 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | /* 19 | * joint_trajectory_pt.cpp 20 | * 21 | * Created on: Oct 3, 2014 22 | * Author: Dan Solomon 23 | */ 24 | 25 | #include 26 | #include "descartes_trajectory/joint_trajectory_pt.h" 27 | 28 | #define NOT_IMPLEMENTED_ERR(ret) \ 29 | CONSOLE_BRIDGE_logError("%s not implemented", __PRETTY_FUNCTION__); \ 30 | return ret; 31 | 32 | // Utility function to unpack joint bounds from a TolerancedJointValue struct 33 | // Note that this does not clear the existing vectors. 34 | static void unpackTolerancedJoints(const std::vector &tolerances, 35 | std::vector &lower, std::vector &nominal, std::vector &upper) 36 | { 37 | lower.reserve(tolerances.size()); 38 | nominal.reserve(tolerances.size()); 39 | upper.reserve(tolerances.size()); 40 | 41 | for (std::size_t i = 0; i < tolerances.size(); ++i) 42 | { 43 | lower.push_back(tolerances[i].lower); 44 | nominal.push_back(tolerances[i].nominal); 45 | upper.push_back(tolerances[i].upper); 46 | } 47 | } 48 | 49 | using namespace descartes_core; 50 | namespace descartes_trajectory 51 | { 52 | JointTrajectoryPt::JointTrajectoryPt(const descartes_core::TimingConstraint &timing) 53 | : descartes_core::TrajectoryPt(timing), tool_(Eigen::Isometry3d::Identity()), wobj_(Eigen::Isometry3d::Identity()) 54 | { 55 | } 56 | 57 | JointTrajectoryPt::JointTrajectoryPt(const std::vector &joints, const Frame &tool, 58 | const Frame &wobj, const descartes_core::TimingConstraint &timing) 59 | : descartes_core::TrajectoryPt(timing), tool_(tool), wobj_(wobj) 60 | { 61 | unpackTolerancedJoints(joints, lower_, nominal_, upper_); 62 | } 63 | 64 | JointTrajectoryPt::JointTrajectoryPt(const std::vector &joints, 65 | const descartes_core::TimingConstraint &timing) 66 | : descartes_core::TrajectoryPt(timing), tool_(Eigen::Isometry3d::Identity()), wobj_(Eigen::Isometry3d::Identity()) 67 | { 68 | unpackTolerancedJoints(joints, lower_, nominal_, upper_); 69 | } 70 | 71 | JointTrajectoryPt::JointTrajectoryPt(const std::vector &joints, const descartes_core::TimingConstraint &timing) 72 | : descartes_core::TrajectoryPt(timing) 73 | , nominal_(joints) 74 | , lower_(joints) 75 | , upper_(joints) 76 | , tool_(Eigen::Isometry3d::Identity()) 77 | , wobj_(Eigen::Isometry3d::Identity()) 78 | { 79 | } 80 | 81 | bool JointTrajectoryPt::getClosestCartPose(const std::vector &, const RobotModel &, 82 | Eigen::Isometry3d &) const 83 | { 84 | NOT_IMPLEMENTED_ERR(false) 85 | } 86 | 87 | bool JointTrajectoryPt::getNominalCartPose(const std::vector &, const RobotModel &model, 88 | Eigen::Isometry3d &pose) const 89 | { 90 | return model.getFK(nominal_, pose); 91 | } 92 | 93 | void JointTrajectoryPt::getCartesianPoses(const RobotModel &, EigenSTL::vector_Isometry3d &poses) const 94 | { 95 | poses.clear(); 96 | } 97 | 98 | bool JointTrajectoryPt::getClosestJointPose(const std::vector &seed_state, const RobotModel &model, 99 | std::vector &joint_pose) const 100 | { 101 | if (nominal_.empty()) 102 | { 103 | return false; 104 | } 105 | else 106 | { 107 | return getNominalJointPose(seed_state, model, joint_pose); 108 | } 109 | } 110 | 111 | bool JointTrajectoryPt::getNominalJointPose(const std::vector &, const RobotModel &, 112 | std::vector &joint_pose) const 113 | { 114 | joint_pose.assign(nominal_.begin(), nominal_.end()); 115 | return true; 116 | } 117 | 118 | void JointTrajectoryPt::getJointPoses(const RobotModel &model, std::vector > &joint_poses) const 119 | { 120 | std::vector empty_seed; 121 | joint_poses.resize(1); 122 | getNominalJointPose(empty_seed, model, joint_poses[0]); 123 | } 124 | 125 | bool JointTrajectoryPt::isValid(const RobotModel &model) const 126 | { 127 | return model.isValid(lower_) && model.isValid(upper_); 128 | } 129 | 130 | bool JointTrajectoryPt::setDiscretization(const std::vector &discretization) 131 | { 132 | if (discretization.size() != 1 || discretization.size() != nominal_.size()) 133 | { 134 | CONSOLE_BRIDGE_logError("discretization must be size 1 or same size as joint count."); 135 | return false; 136 | } 137 | 138 | if (discretization.size() == 1) 139 | { 140 | discretization_ = std::vector(nominal_.size(), discretization[0]); 141 | return true; 142 | } 143 | 144 | /* Do not copy discretization values until all values are confirmed */ 145 | for (size_t ii = 0; ii < discretization.size(); ++ii) 146 | { 147 | if (discretization[ii] < 0. || discretization[ii] > (upper_[ii] - lower_[ii])) 148 | { 149 | CONSOLE_BRIDGE_logError("discretization value out of range."); 150 | return false; 151 | } 152 | } 153 | 154 | discretization_ = discretization; 155 | 156 | return true; 157 | } 158 | 159 | void JointTrajectoryPt::setJoints(const std::vector &joints) 160 | { 161 | lower_.clear(); 162 | nominal_.clear(); 163 | upper_.clear(); 164 | unpackTolerancedJoints(joints, lower_, nominal_, upper_); 165 | } 166 | 167 | } /* namespace descartes_trajectory */ 168 | -------------------------------------------------------------------------------- /descartes_core/include/descartes_core/robot_model.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef ROBOT_KINEMATICS_H_ 20 | #define ROBOT_KINEMATICS_H_ 21 | 22 | // TODO: The include below picks up Eigen::Isometry3d, but there is probably a better way 23 | #include 24 | #include "descartes_core/utils.h" 25 | 26 | namespace descartes_core 27 | { 28 | DESCARTES_CLASS_FORWARD(RobotModel); 29 | 30 | /**@brief RobotModel defines the interface to a kinematics/dynamics functions. Implementations 31 | * of this class will be used in conjunction with TrajectoryPt objects to determine forward 32 | * and inverse kinematics 33 | * 34 | * All methods in this interface class assume a *FIXED* TOOL & WOBJ frame (see TrajectoryPt 35 | * for frame definitions). The methods for setting/getting these frames are not defined by 36 | * this class. Implementations of this interface should provide these either by construction 37 | * or getter/setter methods. 38 | */ 39 | class RobotModel 40 | { 41 | public: 42 | virtual ~RobotModel() 43 | { 44 | } 45 | 46 | /** 47 | * @brief Returns the joint pose closest to the seed pose for a desired affine pose 48 | * @param pose Affine pose of TOOL in WOBJ frame 49 | * @param seed_state Joint position seed (returned solution is "close" to the seed). 50 | * @param joint_pose Solution (if function successful). 51 | * @return True if successful 52 | */ 53 | virtual bool getIK(const Eigen::Isometry3d &pose, const std::vector &seed_state, 54 | std::vector &joint_pose) const = 0; 55 | 56 | /** 57 | * @brief Returns "all" the joint poses("distributed" in joint space) for a desired affine pose. 58 | * "All" is determined by each implementation (In the worst case, this means at least getIK). 59 | * "Distributed" is determined by each implementation. 60 | * @param pose Affine pose of TOOL in WOBJ frame 61 | * @param joint_poses Solution (if function successful). 62 | * @return True if successful 63 | */ 64 | virtual bool getAllIK(const Eigen::Isometry3d &pose, std::vector > &joint_poses) const = 0; 65 | 66 | /** 67 | * @brief Returns the affine pose 68 | * @param joint_pose Solution (if function successful). 69 | * @param pose Affine pose of TOOL in WOBJ frame 70 | * @return True if successful 71 | */ 72 | virtual bool getFK(const std::vector &joint_pose, Eigen::Isometry3d &pose) const = 0; 73 | 74 | /** 75 | * @brief Returns number of DOFs 76 | * @return Int 77 | */ 78 | virtual int getDOF() const = 0; 79 | 80 | /** 81 | * @brief Performs all necessary checks to determine joint pose is valid 82 | * @param joint_pose Pose to check 83 | * @return True if valid 84 | */ 85 | virtual bool isValid(const std::vector &joint_pose) const = 0; 86 | 87 | /** 88 | * @brief Performs all necessary checks to determine affine pose is valid 89 | * @param pose Affine pose of TOOL in WOBJ frame 90 | * @return True if valid 91 | */ 92 | virtual bool isValid(const Eigen::Isometry3d &pose) const = 0; 93 | 94 | /** 95 | * @brief Returns the joint velocity limits for each joint in the robot kinematic model 96 | * @return Sequence of joint velocity limits. Units are a function of the joint type (m/s 97 | * for linear joints; rad/s for rotational). size of vector == getDOF() 98 | */ 99 | virtual std::vector getJointVelocityLimits() const = 0; 100 | 101 | /** 102 | * @brief Initializes the robot model when it is instantiated as a moveit_core plugin. 103 | * @param robot_description name of the ros parameter containing the urdf description 104 | * @param group_name the manipulation group for all the robot links that are part of the same kinematic chain 105 | * @param world_frame name of the root link in the urdf 106 | * @param tcp_frame tool link attached to the robot. When it's not in 'group_name' then it should have 107 | * a fixed location relative to the last link in 'group_name'. 108 | */ 109 | virtual bool initialize(const std::string &robot_description, const std::string &group_name, 110 | const std::string &world_frame, const std::string &tcp_frame) = 0; 111 | 112 | /** 113 | * @brief Enables collision checks 114 | * @param check_collisions enables or disables collisions 115 | */ 116 | virtual void setCheckCollisions(bool check_collisions) 117 | { 118 | check_collisions_ = check_collisions; 119 | } 120 | 121 | /** 122 | * @brief Indicates if collision checks are enabled 123 | * @return Bool 124 | */ 125 | virtual bool getCheckCollisions() 126 | { 127 | return check_collisions_; 128 | } 129 | 130 | 131 | /** 132 | * @brief Performs necessary checks to see if the robot is capable of moving from the initial joint pose 133 | * to the final pose in dt seconds 134 | * @param from_joint_pose [description] 135 | * @param to_joint_pose [description] 136 | * @param dt [description] 137 | * @return [description] 138 | */ 139 | virtual bool isValidMove(const std::vector &from_joint_pose, const std::vector &to_joint_pose, 140 | double dt) const 141 | { 142 | return isValidMove(from_joint_pose.data(), to_joint_pose.data(), dt); 143 | } 144 | 145 | virtual bool isValidMove(const double* s, const double* f, double dt) const = 0; 146 | 147 | protected: 148 | RobotModel() : check_collisions_(false) 149 | { 150 | } 151 | 152 | bool check_collisions_; 153 | }; 154 | 155 | } // descartes_core 156 | 157 | #endif /* ROBOT_KINEMATICS_H_ */ 158 | -------------------------------------------------------------------------------- /descartes_moveit/src/ikfast_moveit_state_adapter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2016, Jonathan Meyer 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include "descartes_moveit/ikfast_moveit_state_adapter.h" 20 | 21 | #include 22 | #include 23 | 24 | const static std::string default_base_frame = "base_link"; 25 | const static std::string default_tool_frame = "tool0"; 26 | 27 | // Compute the 'joint distance' between two poses 28 | static double distance(const std::vector& a, const std::vector& b) 29 | { 30 | double cost = 0.0; 31 | for (size_t i = 0; i < a.size(); ++i) 32 | cost += std::abs(b[i] - a[i]); 33 | return cost; 34 | } 35 | 36 | // Compute the index of the closest joint pose in 'candidates' from 'target' 37 | static size_t closestJointPose(const std::vector& target, const std::vector>& candidates) 38 | { 39 | size_t closest = 0; // index into candidates 40 | double lowest_cost = std::numeric_limits::max(); 41 | for (size_t i = 0; i < candidates.size(); ++i) 42 | { 43 | assert(target.size() == candidates[i].size()); 44 | double c = distance(target, candidates[i]); 45 | if (c < lowest_cost) 46 | { 47 | closest = i; 48 | lowest_cost = c; 49 | } 50 | } 51 | return closest; 52 | } 53 | 54 | bool descartes_moveit::IkFastMoveitStateAdapter::initialize(const std::string& robot_description, 55 | const std::string& group_name, 56 | const std::string& world_frame, 57 | const std::string& tcp_frame) 58 | { 59 | if (!MoveitStateAdapter::initialize(robot_description, group_name, world_frame, tcp_frame)) 60 | { 61 | return false; 62 | } 63 | 64 | return computeIKFastTransforms(); 65 | } 66 | 67 | bool descartes_moveit::IkFastMoveitStateAdapter::getAllIK(const Eigen::Isometry3d& pose, 68 | std::vector>& joint_poses) const 69 | { 70 | joint_poses.clear(); 71 | const auto& solver = joint_group_->getSolverInstance(); 72 | 73 | // Transform input pose 74 | Eigen::Isometry3d tool_pose = world_to_base_.frame_inv * pose * tool0_to_tip_.frame; 75 | 76 | // convert to geometry_msgs ... 77 | geometry_msgs::Pose geometry_pose; 78 | tf::poseEigenToMsg(tool_pose, geometry_pose); 79 | std::vector poses = { geometry_pose }; 80 | 81 | std::vector dummy_seed(getDOF(), 0.0); 82 | std::vector> joint_results; 83 | kinematics::KinematicsResult result; 84 | kinematics::KinematicsQueryOptions options; // defaults are reasonable as of Indigo 85 | 86 | if (!solver->getPositionIK(poses, dummy_seed, joint_results, result, options)) 87 | { 88 | return false; 89 | } 90 | 91 | for (auto& sol : joint_results) 92 | { 93 | if (isValid(sol)) 94 | joint_poses.push_back(std::move(sol)); 95 | } 96 | 97 | return joint_poses.size() > 0; 98 | } 99 | 100 | bool descartes_moveit::IkFastMoveitStateAdapter::getIK(const Eigen::Isometry3d& pose, 101 | const std::vector& seed_state, 102 | std::vector& joint_pose) const 103 | { 104 | // Descartes Robot Model interface calls for 'closest' point to seed position 105 | std::vector> joint_poses; 106 | if (!getAllIK(pose, joint_poses)) 107 | return false; 108 | // Find closest joint pose; getAllIK() does isValid checks already 109 | joint_pose = joint_poses[closestJointPose(seed_state, joint_poses)]; 110 | return true; 111 | } 112 | 113 | bool descartes_moveit::IkFastMoveitStateAdapter::getFK(const std::vector& joint_pose, 114 | Eigen::Isometry3d& pose) const 115 | { 116 | const auto& solver = joint_group_->getSolverInstance(); 117 | 118 | std::vector tip_frame = { solver->getTipFrame() }; 119 | std::vector output; 120 | 121 | if (!isValid(joint_pose)) 122 | return false; 123 | 124 | if (!solver->getPositionFK(tip_frame, joint_pose, output)) 125 | return false; 126 | 127 | tf::poseMsgToEigen(output[0], pose); // pose in frame of IkFast base 128 | pose = world_to_base_.frame * pose * tool0_to_tip_.frame_inv; 129 | return true; 130 | } 131 | 132 | void descartes_moveit::IkFastMoveitStateAdapter::setState(const moveit::core::RobotState& state) 133 | { 134 | descartes_moveit::MoveitStateAdapter::setState(state); 135 | computeIKFastTransforms(); 136 | } 137 | 138 | bool descartes_moveit::IkFastMoveitStateAdapter::computeIKFastTransforms() 139 | { 140 | // look up the IKFast base and tool frame 141 | ros::NodeHandle nh; 142 | std::string ikfast_base_frame, ikfast_tool_frame; 143 | nh.param("ikfast_base_frame", ikfast_base_frame, default_base_frame); 144 | nh.param("ikfast_tool_frame", ikfast_tool_frame, default_tool_frame); 145 | 146 | if (!robot_state_->knowsFrameTransform(ikfast_base_frame)) 147 | { 148 | CONSOLE_BRIDGE_logError("IkFastMoveitStateAdapter: Cannot find transformation to frame '%s' in group '%s'.", 149 | ikfast_base_frame.c_str(), group_name_.c_str()); 150 | return false; 151 | } 152 | 153 | if (!robot_state_->knowsFrameTransform(ikfast_tool_frame)) 154 | { 155 | CONSOLE_BRIDGE_logError("IkFastMoveitStateAdapter: Cannot find transformation to frame '%s' in group '%s'.", 156 | ikfast_tool_frame.c_str(), group_name_.c_str()); 157 | return false; 158 | } 159 | 160 | // calculate frames 161 | tool0_to_tip_ = descartes_core::Frame(robot_state_->getFrameTransform(tool_frame_).inverse() * 162 | robot_state_->getFrameTransform(ikfast_tool_frame)); 163 | 164 | world_to_base_ = descartes_core::Frame(world_to_root_.frame * robot_state_->getFrameTransform(ikfast_base_frame)); 165 | 166 | CONSOLE_BRIDGE_logInform("IkFastMoveitStateAdapter: initialized with IKFast tool frame '%s' and base frame '%s'.", 167 | ikfast_tool_frame.c_str(), ikfast_base_frame.c_str()); 168 | return true; 169 | } 170 | -------------------------------------------------------------------------------- /descartes_tests/test/trajectory/cart_trajectory_pt.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include "descartes_trajectory/cart_trajectory_pt.h" 20 | #include "descartes_core/utils.h" 21 | #include "descartes_tests/cartesian_robot.h" 22 | #include 23 | 24 | using namespace descartes_trajectory; 25 | using namespace descartes_core; 26 | using namespace descartes_tests; 27 | 28 | TEST(CartTrajPt, construction) 29 | { 30 | CartTrajectoryPt def(); 31 | } 32 | 33 | TEST(CartTrajPt, zeroTolerance) 34 | { 35 | ROS_INFO_STREAM("Initializing zero tolerance cartesian point"); 36 | CartTrajectoryPt zero_tol_pos(TolerancedFrame(utils::toFrame(0, 0, 0, 0, 0, 0), 37 | ToleranceBase::zeroTolerance(0.0, 0.0, 0.0), 38 | ToleranceBase::zeroTolerance(0.0, 0.0, 0.0)), 39 | 0, 0); 40 | 41 | EigenSTL::vector_Isometry3d solutions; 42 | std::vector > joint_solutions; 43 | 44 | CartesianRobot robot; 45 | zero_tol_pos.getCartesianPoses(robot, solutions); 46 | EXPECT_EQ(solutions.size(), 1); 47 | zero_tol_pos.getJointPoses(robot, joint_solutions); 48 | EXPECT_EQ(joint_solutions.size(), 1); 49 | } 50 | 51 | TEST(CartTrajPt, closestJointPose) 52 | { 53 | const double POS_TOL = 0.5f; 54 | const double POS_INC = 0.2; 55 | const double ORIENT_TOL = 1.0; 56 | const double ORIENT_INC = 0.2; 57 | CartesianRobot robot(10, 4); 58 | 59 | // declaring pose values 60 | const double x = 4.0f; 61 | const double y = 5.0f; 62 | const double z = 2.0f; 63 | const double rx = 0.0f; 64 | const double ry = 0.0f; 65 | const double rz = M_PI / 4; 66 | std::vector joint_pose = { x, y, z, rx, ry, rz }; 67 | Eigen::Isometry3d frame_pose = descartes_core::utils::toFrame(x, y, z, rx, ry, rz); 68 | 69 | ROS_INFO_STREAM("Initializing tolerance cartesian point"); 70 | CartTrajectoryPt cart_point( 71 | TolerancedFrame(utils::toFrame(x, y, z, rx, ry, rz), 72 | ToleranceBase::createSymmetric(x, y, z, POS_TOL), 73 | ToleranceBase::createSymmetric(rx, ry, rz, ORIENT_TOL)), 74 | POS_INC, ORIENT_INC); 75 | 76 | ROS_INFO_STREAM("Testing getClosestJointPose(...)"); 77 | std::vector closest_joint_pose; 78 | EXPECT_TRUE(cart_point.getClosestJointPose(joint_pose, robot, closest_joint_pose)); 79 | 80 | ROS_INFO_STREAM("Testing equality between seed joint pose and closest joint pose"); 81 | EXPECT_EQ(joint_pose, closest_joint_pose); 82 | } 83 | 84 | TEST(CartTrajPt, getPoses) 85 | { 86 | const double POS_TOL = 2.0; 87 | const double POS_INC = 0.2; 88 | 89 | const double ORIENT_TOL = 2 * M_PI; 90 | const double ORIENT_INC = M_PI / 4; 91 | 92 | const double EPSILON = 0.001; 93 | 94 | const int NUM_SAMPLED_POS = pow((POS_TOL / POS_INC) + 1, 3.0); 95 | const int NUM_SAMPLED_ORIENT = pow((ORIENT_TOL / ORIENT_INC) + 1, 3.0); 96 | const int NUM_SAMPLED_BOTH = NUM_SAMPLED_POS * NUM_SAMPLED_ORIENT; 97 | 98 | ROS_INFO_STREAM("Expected samples, position: " << NUM_SAMPLED_POS << ", orientation: " << NUM_SAMPLED_ORIENT 99 | << ", both: " << NUM_SAMPLED_BOTH); 100 | 101 | ROS_INFO_STREAM("Initializing fuzzy position point"); 102 | CartTrajectoryPt fuzzy_pos( 103 | TolerancedFrame(utils::toFrame(0, 0, 0, 0, 0, 0), 104 | ToleranceBase::createSymmetric(0.0, 0.0, 0.0, POS_TOL + EPSILON), 105 | ToleranceBase::createSymmetric(0.0, 0.0, 0.0, 0.0)), 106 | POS_INC, ORIENT_INC); 107 | 108 | ROS_INFO_STREAM("Initializing fuzzy orientation point"); 109 | CartTrajectoryPt fuzzy_orient( 110 | TolerancedFrame(utils::toFrame(0, 0, 0, 0, 0, 0), 111 | ToleranceBase::createSymmetric(0.0, 0.0, 0.0, 0.0), 112 | ToleranceBase::createSymmetric(0.0, 0.0, 0.0, ORIENT_TOL + EPSILON)), 113 | POS_INC, ORIENT_INC); 114 | 115 | ROS_INFO_STREAM("Initializing fuzzy position/orientation point"); 116 | CartTrajectoryPt fuzzy_both( 117 | TolerancedFrame(utils::toFrame(0, 0, 0, 0, 0, 0), 118 | ToleranceBase::createSymmetric(0.0, 0.0, 0.0, POS_TOL + EPSILON), 119 | ToleranceBase::createSymmetric(0.0, 0.0, 0.0, ORIENT_TOL + EPSILON)), 120 | POS_INC, ORIENT_INC); 121 | 122 | EigenSTL::vector_Isometry3d solutions; 123 | std::vector > joint_solutions; 124 | 125 | ROS_INFO_STREAM("Testing fuzzy pos point"); 126 | CartesianRobot robot(POS_TOL + 2 * EPSILON, ORIENT_TOL + 2 * EPSILON); 127 | fuzzy_pos.getCartesianPoses(robot, solutions); 128 | EXPECT_EQ(solutions.size(), NUM_SAMPLED_POS); 129 | fuzzy_pos.getJointPoses(robot, joint_solutions); 130 | EXPECT_EQ(joint_solutions.size(), NUM_SAMPLED_POS); 131 | 132 | ROS_INFO_STREAM("Testing fuzzy orient point"); 133 | fuzzy_orient.getCartesianPoses(robot, solutions); 134 | EXPECT_EQ(solutions.size(), NUM_SAMPLED_ORIENT); 135 | fuzzy_orient.getJointPoses(robot, joint_solutions); 136 | EXPECT_EQ(joint_solutions.size(), NUM_SAMPLED_ORIENT); 137 | 138 | ROS_INFO_STREAM("Testing fuzzy both point"); 139 | fuzzy_both.getCartesianPoses(robot, solutions); 140 | EXPECT_EQ(solutions.size(), NUM_SAMPLED_BOTH); 141 | fuzzy_both.getJointPoses(robot, joint_solutions); 142 | EXPECT_EQ(joint_solutions.size(), NUM_SAMPLED_BOTH); 143 | 144 | // TODO: Add tests for cartesian points outside of the robot workspace. These 145 | // tests below seem to work, but predicting the number of samples isn't working. 146 | // const double WS_FRACTION = 0.5; //Reduces robot workspace 147 | // const int LIMIT_SAMPLED_POS = pow(((WS_FRACTION * POS_TOL)/POS_INC) + 1, 3.0); 148 | // const int LIMIT_SAMPLED_ORIENT = pow(((WS_FRACTION * ORIENT_TOL)/ORIENT_INC) + 1, 3.0); 149 | // const int LIMIT_SAMPLED_BOTH = LIMIT_SAMPLED_POS * LIMIT_SAMPLED_ORIENT; 150 | 151 | // CartesianRobot limited_robot(POS_TOL * WS_FRACTION + 2*EPSILON, ORIENT_TOL * WS_FRACTION + 2*EPSILON); 152 | // fuzzy_pos.getCartesianPoses(limited_robot, solutions); 153 | // EXPECT_EQ(solutions.size(), LIMIT_SAMPLED_POS); 154 | 155 | // fuzzy_orient.getCartesianPoses(limited_robot, solutions); 156 | // EXPECT_EQ(solutions.size(), LIMIT_SAMPLED_ORIENT); 157 | 158 | // fuzzy_both.getCartesianPoses(limited_robot, solutions); 159 | // EXPECT_EQ(solutions.size(), LIMIT_SAMPLED_BOTH); 160 | } 161 | -------------------------------------------------------------------------------- /descartes_planner/include/descartes_planner/ladder_graph.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2016, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef DESCARTES_LADDER_GRAPH_H 20 | #define DESCARTES_LADDER_GRAPH_H 21 | 22 | #include "descartes_core/trajectory_id.h" 23 | #include "descartes_core/trajectory_timing_constraint.h" 24 | 25 | namespace descartes_planner 26 | { 27 | 28 | struct __attribute__ ((__packed__)) Edge 29 | { 30 | double cost; // transition cost from vertex who owns this object to 'idx' in next rung 31 | unsigned idx; // from THIS rung to 'idx' into the NEXT rung 32 | }; 33 | 34 | struct Rung 35 | { 36 | using EdgeList = std::vector; 37 | 38 | descartes_core::TrajectoryID id; // corresponds to user's input ID 39 | descartes_core::TimingConstraint timing; // user input timing 40 | std::vector data; // joint values stored in one contiguous array 41 | std::vector edges; 42 | }; 43 | 44 | /** 45 | * @brief LadderGraph is an adjacency list based, directed graph structure with vertices 46 | * arranged into "rungs" which have connections only to vertices in the adjacent 47 | * rungs. Assumes a fixed DOF. 48 | */ 49 | class LadderGraph 50 | { 51 | public: 52 | using size_type = std::size_t; 53 | using EdgeList = Rung::EdgeList; 54 | 55 | /** 56 | * @brief LadderGraph 57 | * @param dof The number of joints that constitute a single 'DOF' 58 | */ 59 | explicit LadderGraph(size_type dof) noexcept 60 | : dof_(dof) 61 | { 62 | assert(dof != 0); 63 | } 64 | 65 | /** 66 | * @brief resize Resizes the internal ladder to have 'n_rung' rungs 67 | * @param n_rungs Number of individual rungs 68 | */ 69 | void resize(size_type n_rungs) 70 | { 71 | rungs_.resize(n_rungs); 72 | } 73 | 74 | Rung& getRung(size_type index) noexcept 75 | { 76 | return const_cast(static_cast(*this).getRung(index)); 77 | } 78 | 79 | const Rung& getRung(size_type index) const noexcept 80 | { 81 | assert(index < rungs_.size()); 82 | return rungs_[index]; 83 | } 84 | 85 | std::vector& getEdges(size_type index) noexcept // see p.23 Effective C++ (Scott Meyers) 86 | { 87 | return const_cast&>(static_cast(*this).getEdges(index)); 88 | } 89 | 90 | const std::vector& getEdges(size_type index) const noexcept 91 | { 92 | assert(index < rungs_.size()); 93 | return rungs_[index].edges; 94 | } 95 | 96 | size_type rungSize(size_type index) const noexcept 97 | { 98 | return getRung(index).data.size() / dof_; 99 | } 100 | 101 | /** 102 | * @brief numVertices Counts the total number of vertices in the graph 103 | */ 104 | size_type numVertices() const noexcept 105 | { 106 | size_type count = 0; // Add the size of each rung d 107 | for (const auto& rung : rungs_) count += (rung.data.size() / dof_); 108 | return count; 109 | } 110 | 111 | /** 112 | * @brief indexOf returns a pair describing whether the given ID is in the graph and if so, what 113 | * index it has. 114 | * @param id The ID to 115 | * @return std::pair(index, was_found) 116 | */ 117 | std::pair indexOf(descartes_core::TrajectoryID id) const noexcept 118 | { 119 | auto it = std::find_if(rungs_.cbegin(), rungs_.cend(), [id] (const Rung& r) { 120 | return id == r.id; 121 | }); 122 | if (it == rungs_.cend()) 123 | return {0u, false}; 124 | else 125 | return {static_cast(std::distance(rungs_.cbegin(), it)), true}; 126 | } 127 | 128 | /** 129 | * @brief isLast tests to see if a given index is the last one in the graph 130 | */ 131 | bool isLast(size_type index) const noexcept 132 | { 133 | return index + 1 == size(); 134 | } 135 | 136 | /** 137 | * @brief isFirst tests to see if given index is the first in the graph 138 | */ 139 | bool isFirst(size_type index) const noexcept 140 | { 141 | return index == 0; 142 | } 143 | 144 | /** 145 | * @brief vertex returns a pointer to the data that constitutes the Nth vertex in the Jth row 146 | * where N = index & J = rung 147 | */ 148 | const double* vertex(size_type rung, size_type index) const 149 | { 150 | return getRung(rung).data.data() + (dof_ * index); 151 | } 152 | 153 | /** 154 | * @brief The number of rungs 155 | */ 156 | size_type size() const noexcept 157 | { 158 | return rungs_.size(); 159 | } 160 | 161 | size_type dof() const noexcept 162 | { 163 | return dof_; 164 | } 165 | 166 | /** 167 | * @brief assign Consumes the given edge list and assigns it to the rung-index given by 'rung' 168 | */ 169 | void assignEdges(size_type rung, std::vector&& edges) // noexcept? 170 | { 171 | getEdges(rung) = std::move(edges); 172 | } 173 | 174 | /** 175 | * @brief assignRung Special helper function to assign a solution set associated with a Descartes point & 176 | * it's meta-info. Also resizes the associated edge list to the size of 'sols'. 177 | * @param sols All of the joint solutions for this point. 178 | */ 179 | void assignRung(size_type index, descartes_core::TrajectoryID id, descartes_core::TimingConstraint time, 180 | const std::vector>& sols) 181 | { 182 | Rung& r = getRung(index); 183 | r.id = id; 184 | r.timing = time; 185 | r.data.reserve(sols.size() * dof_); 186 | for (const auto& sol : sols) 187 | { 188 | r.data.insert(r.data.end(), sol.cbegin(), sol.cend()); 189 | } 190 | // Given this new vertex set, build an edge list for each 191 | getEdges(index).resize(r.data.size()); 192 | } 193 | 194 | void removeRung(size_type index) 195 | { 196 | rungs_.erase(std::next(rungs_.begin(), index)); 197 | } 198 | 199 | void clearVertices(size_type index) 200 | { 201 | rungs_[index].data.clear(); 202 | } 203 | 204 | void clearEdges(size_type index) 205 | { 206 | rungs_[index].edges.clear(); 207 | } 208 | 209 | /** 210 | * @brief insertRung Adds a new rung at the 'index'-th position. E.g., insertRung(0) will add a new 211 | * rung to the beginning of the graph and the previous 0th index is now at 1. 212 | */ 213 | void insertRung(size_type index) 214 | { 215 | rungs_.insert(std::next(rungs_.begin(), index), Rung() ); 216 | } 217 | 218 | /** 219 | * @brief Clears all existing rungs & associated data 220 | */ 221 | void clear() 222 | { 223 | rungs_.clear(); 224 | } 225 | 226 | private: 227 | const size_type dof_; 228 | std::vector rungs_; 229 | }; 230 | } // descartes_planner 231 | #endif 232 | -------------------------------------------------------------------------------- /descartes_trajectory/include/descartes_trajectory/joint_trajectory_pt.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Dan Solomon 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | /* 19 | * joint_trajectory_pt.h 20 | * 21 | * Created on: Oct 3, 2014 22 | * Author: Dan Solomon 23 | */ 24 | 25 | #ifndef JOINT_TRAJECTORY_PT_H_ 26 | #define JOINT_TRAJECTORY_PT_H_ 27 | 28 | #include 29 | #include "descartes_core/trajectory_pt.h" 30 | 31 | namespace descartes_trajectory 32 | { 33 | /** 34 | * @brief Structure to specify a valid joint range [lower, upper] with a nominal position given by 'nominal'. 35 | */ 36 | struct TolerancedJointValue 37 | { 38 | TolerancedJointValue(double nominal, double lower, double upper) : nominal(nominal), lower(lower), upper(upper) 39 | { 40 | } 41 | 42 | TolerancedJointValue(double nominal) : nominal(nominal), lower(nominal), upper(nominal) 43 | { 44 | } 45 | 46 | double range() const 47 | { 48 | return upper - lower; 49 | } 50 | 51 | double nominal, lower, upper; 52 | }; 53 | 54 | /**@brief Joint Trajectory Point used to describe a joint goal for a robot trajectory. 55 | * 56 | * Background: 57 | * The TOOL is something held by the robot. It is located relative to robot wrist/tool plate. 58 | * The WOBJ is something that exists in the world/global environment that is not held by robot. 59 | * 60 | * For a JointTrajectoryPt, the transform from wrist to tool, and base to workobject, are defined by fixed frames. 61 | * These transforms are important when calculating interpolation. 62 | * The joint position is specified as a nominal with upper/lower tolerances. 63 | * 64 | * The get*Pose() methods of JointTrajectoryPt try to set joint positions of a robot such that @e tool_ is coincident 65 | *with @e wobj_. 66 | */ 67 | class JointTrajectoryPt : public descartes_core::TrajectoryPt 68 | { 69 | public: 70 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // TODO is this needed when Frame already has it? 71 | public: 72 | /** 73 | @brief All frames initialized to Identity, joint 74 | values left empty 75 | */ 76 | JointTrajectoryPt(const descartes_core::TimingConstraint &timing = descartes_core::TimingConstraint()); 77 | 78 | /** 79 | @brief Full joint trajectory point constructor 80 | @param joints Fixed joint position with tolerance 81 | @param tool Transform from robot wrist to active tool pt. 82 | @param wobj Transform from world to active workobject pt. 83 | */ 84 | JointTrajectoryPt(const std::vector &joints, const descartes_core::Frame &tool, 85 | const descartes_core::Frame &wobj, 86 | const descartes_core::TimingConstraint &timing = descartes_core::TimingConstraint()); 87 | 88 | /** 89 | @brief Full joint trajectory point constructor 90 | @param joints Fixed joint position with tolerance 91 | */ 92 | JointTrajectoryPt(const std::vector &joints, 93 | const descartes_core::TimingConstraint &timing = descartes_core::TimingConstraint()); 94 | 95 | /** 96 | @brief Full joint trajectory point constructor 97 | @param joints Fixed joint position 98 | */ 99 | JointTrajectoryPt(const std::vector &joints, 100 | const descartes_core::TimingConstraint &timing = descartes_core::TimingConstraint()); 101 | 102 | virtual ~JointTrajectoryPt(){}; 103 | 104 | /**@name Getters for Cartesian pose(s) 105 | * @{ 106 | */ 107 | 108 | // TODO complete 109 | virtual bool getClosestCartPose(const std::vector &seed_state, const descartes_core::RobotModel &model, 110 | Eigen::Isometry3d &pose) const; 111 | 112 | // TODO complete 113 | virtual bool getNominalCartPose(const std::vector &seed_state, const descartes_core::RobotModel &model, 114 | Eigen::Isometry3d &pose) const; 115 | 116 | // TODO complete 117 | virtual void getCartesianPoses(const descartes_core::RobotModel &model, EigenSTL::vector_Isometry3d &poses) const; 118 | /** @} (end section) */ 119 | 120 | /**@name Getters for joint pose(s) 121 | * @{ 122 | */ 123 | 124 | // TODO complete 125 | virtual bool getClosestJointPose(const std::vector &seed_state, const descartes_core::RobotModel &model, 126 | std::vector &joint_pose) const; 127 | // TODO complete 128 | virtual bool getNominalJointPose(const std::vector &seed_state, const descartes_core::RobotModel &model, 129 | std::vector &joint_pose) const; 130 | 131 | // TODO complete 132 | virtual void getJointPoses(const descartes_core::RobotModel &model, 133 | std::vector > &joint_poses) const; 134 | /** @} (end section) */ 135 | 136 | // TODO complete 137 | virtual bool isValid(const descartes_core::RobotModel &model) const; 138 | 139 | // TODO complete 140 | /**@brief Set discretization. Each joint can have a different discretization. 141 | * @param discretization Vector of discretization values. If length=1, set all elements of discretization_ are set to 142 | * value. 143 | * @return True if vector is length 1 or length(joint_position_) and value[ii] are within 0-range(joint_position[ii]). 144 | */ 145 | virtual bool setDiscretization(const std::vector &discretization); 146 | 147 | virtual descartes_core::TrajectoryPtPtr copy() const 148 | { 149 | return descartes_core::TrajectoryPtPtr(new JointTrajectoryPt(*this)); 150 | } 151 | 152 | void setJoints(const std::vector &joints); 153 | 154 | inline void setTool(const descartes_core::Frame &tool) 155 | { 156 | tool_ = tool; 157 | } 158 | 159 | inline void setWobj(const descartes_core::Frame &wobj) 160 | { 161 | wobj_ = wobj; 162 | } 163 | /**@} (end Setters section) */ 164 | 165 | inline const std::vector &nominal() const 166 | { 167 | return nominal_; 168 | } 169 | 170 | inline const std::vector &upper() const 171 | { 172 | return upper_; 173 | } 174 | 175 | inline const std::vector &lower() const 176 | { 177 | return lower_; 178 | } 179 | 180 | protected: 181 | std::vector nominal_; 182 | std::vector lower_; 183 | std::vector upper_; 184 | std::vector discretization_; /**<@brief How finely to discretize each joint */ 185 | 186 | /** @name JointTrajectoryPt transforms. Used in get*CartPose() methods and for interpolation. 187 | * @{ 188 | */ 189 | descartes_core::Frame tool_; /**<@brief Transform from robot wrist to active tool pt. */ 190 | descartes_core::Frame wobj_; /**<@brief Transform from world to active workobject pt. */ 191 | /** @} (end section) */ 192 | }; 193 | 194 | } /* namespace descartes_trajectory */ 195 | 196 | #endif /* JOINT_TRAJECTORY_PT_H_ */ 197 | -------------------------------------------------------------------------------- /descartes_core/include/descartes_core/trajectory_pt.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | /* 19 | * trajectory_pt.h 20 | * 21 | * Created on: Jun 5, 2014 22 | * Author: Dan Solomon 23 | */ 24 | 25 | #ifndef TRAJECTORY_PT_H_ 26 | #define TRAJECTORY_PT_H_ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include "descartes_core/robot_model.h" 34 | #include "descartes_core/trajectory_id.h" 35 | #include "descartes_core/trajectory_timing_constraint.h" 36 | 37 | namespace descartes_core 38 | { 39 | /**@brief Frame is a wrapper for an affine frame transform. 40 | * Frame inverse can also be stored for increased speed in downstream calculations. 41 | */ 42 | struct Frame 43 | { 44 | Frame(){}; 45 | Frame(const Eigen::Isometry3d &a) : frame(a), frame_inv(a.inverse()){}; 46 | Frame(const Eigen::Affine3d& a) 47 | { 48 | Eigen::Isometry3d t; 49 | frame.translation() = a.translation(); 50 | frame.linear() = a.rotation(); 51 | frame_inv = frame.inverse(); 52 | } 53 | 54 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 55 | Eigen::Isometry3d frame; 56 | Eigen::Isometry3d frame_inv; 57 | 58 | static const Frame Identity() 59 | { 60 | return Frame(Eigen::Isometry3d::Identity()); 61 | } 62 | }; 63 | 64 | /**@brief A TrajectoryPt is the basis for a Trajectory describing the desired path a robot should execute. 65 | * The desired robot motion spans both Cartesian and Joint space, and so the TrajectoryPt must have capability 66 | * to report on both these properties. 67 | * 68 | * In practice, an application will create a series of process points, 69 | * and use these process points to create a Trajectory that can be solved for a robot path. 70 | * In order to implement this easily, each process point should keep track of the TrajectoryPt id, and 71 | * provide an interpolation method between points. 72 | */ 73 | DESCARTES_CLASS_FORWARD(TrajectoryPt); 74 | class TrajectoryPt 75 | { 76 | public: 77 | typedef TrajectoryID ID; 78 | 79 | TrajectoryPt(const TimingConstraint &timing) : id_(TrajectoryID::make_id()), timing_(timing) 80 | { 81 | } 82 | 83 | virtual ~TrajectoryPt() 84 | { 85 | } 86 | 87 | /**@name Getters for Cartesian pose(s) 88 | * References to "closest" position are decided by norm of joint-space distance. 89 | * @{ 90 | */ 91 | 92 | /**@brief Get single Cartesian pose associated with closest position of this point to seed_state. 93 | * (Pose of TOOL point expressed in WOBJ frame). 94 | * @param seed_state Joint_position seed. 95 | * @param kinematics Kinematics object used to calculate pose 96 | * @param pose If successful, affine pose of this state. 97 | * @return True if calculation successful. pose untouched if return false. 98 | */ 99 | virtual bool getClosestCartPose(const std::vector &seed_state, const RobotModel &kinematics, 100 | Eigen::Isometry3d &pose) const = 0; 101 | 102 | /**@brief Get single Cartesian pose associated with nominal of this point. 103 | * (Pose of TOOL point expressed in WOBJ frame). 104 | * @param seed_state Joint_position seed. 105 | * @param kinematics Kinematics object used to calculate pose 106 | * @param pose If successful, affine pose of this state. 107 | * @return True if calculation successful. pose untouched if return false. 108 | */ 109 | virtual bool getNominalCartPose(const std::vector &seed_state, const RobotModel &kinematics, 110 | Eigen::Isometry3d &pose) const = 0; 111 | 112 | /**@brief Get "all" Cartesian poses that satisfy this point. 113 | * @param kinematics Kinematics object used to calculate pose 114 | * @param poses Note: Number of poses returned may be subject to discretization used. 115 | */ 116 | virtual void getCartesianPoses(const RobotModel &kinematics, EigenSTL::vector_Isometry3d &poses) const = 0; 117 | /** @} (end section) */ 118 | 119 | /**@name Getters for joint pose(s) 120 | * References to "closest" position are decided by norm of joint-space distance. 121 | * @{ 122 | */ 123 | 124 | /**@brief Get single Joint pose closest to seed_state. 125 | * @param seed_state Joint_position seed. 126 | * @param model Robot mode object used to calculate pose 127 | * @param joint_pose Solution (if function successful). 128 | * @return True if calculation successful. joint_pose untouched if return is false. 129 | */ 130 | virtual bool getClosestJointPose(const std::vector &seed_state, const RobotModel &model, 131 | std::vector &joint_pose) const = 0; 132 | 133 | /**@brief Get single Joint pose closest to seed_state. 134 | * @param seed_state Joint_position seed. 135 | * @param model Robot model object used to calculate pose 136 | * @param seed_state RobotState used kinematic calculations and joint position seed. 137 | * @return True if calculation successful. joint_pose untouched if return is false. 138 | */ 139 | virtual bool getNominalJointPose(const std::vector &seed_state, const RobotModel &model, 140 | std::vector &joint_pose) const = 0; 141 | 142 | /**@brief Get "all" joint poses that satisfy this point. 143 | * @param model Robot model object used to calculate pose 144 | * @param joint_poses vector of solutions (if function successful). Note: # of solutions may be subject to 145 | * discretization used. 146 | */ 147 | virtual void getJointPoses(const RobotModel &model, std::vector > &joint_poses) const = 0; 148 | /** @} (end section) */ 149 | 150 | /**@brief Check if state satisfies trajectory point requirements. 151 | * @param model Robot model object used to determine validity 152 | */ 153 | virtual bool isValid(const RobotModel &model) const = 0; 154 | 155 | /**@brief Set discretization. Note: derived classes interpret and use discretization differently. 156 | * @param discretization Vector of discretization values. 157 | * @return True if vector is valid length/values. 158 | */ 159 | virtual bool setDiscretization(const std::vector &discretization) = 0; 160 | 161 | /**@name Getters/Setters for ID 162 | * @{ */ 163 | 164 | /**@brief Get ID associated with this point */ 165 | inline ID getID() const 166 | { 167 | return id_; 168 | } 169 | 170 | /**@brief Set ID for this point. 171 | * @param id Number to set id_ to. 172 | */ 173 | void setID(const ID &id) 174 | { 175 | id_ = id; 176 | } 177 | /** @} (end section) */ 178 | 179 | /** 180 | * @brief Makes a copy of the underlying trajectory point and returns a polymorphic handle to it 181 | * @return A copy, with the same ID, of the underlying point type 182 | */ 183 | virtual TrajectoryPtPtr copy() const = 0; 184 | 185 | /** 186 | * @brief Makes a copy of the underlying trajectory point, mutates the timing, and returns a 187 | * polymorphic handle to it. 188 | * @param tm The new timing value for the copied point 189 | * @return A copy, with the same ID and new timing, of the underlying point type 190 | */ 191 | virtual TrajectoryPtPtr copyAndSetTiming(const TimingConstraint &tm) const 192 | { 193 | TrajectoryPtPtr cp = copy(); 194 | cp->setTiming(tm); 195 | return cp; 196 | } 197 | 198 | /** 199 | * @brief Makes a clone of the underlying trajectory point and returns a polymorphic handle to it 200 | * @return A clone, with the same data but a unique ID, of the underlying point type 201 | */ 202 | virtual TrajectoryPtPtr clone() const 203 | { 204 | TrajectoryPtPtr cp = copy(); 205 | cp->setID(TrajectoryID::make_id()); 206 | return cp; 207 | } 208 | 209 | /** 210 | * @brief Makes a clone of the underlying trajectory point and returns a polymorphic handle to it with new timing 211 | * @param tm The new timing value for the copied point 212 | * @return A clone, with the same data but a unique ID, of the underlying point type with new timing 213 | */ 214 | virtual TrajectoryPtPtr cloneAndSetTiming(const TimingConstraint &tm) const 215 | { 216 | TrajectoryPtPtr cp = clone(); 217 | cp->setTiming(tm); 218 | return cp; 219 | } 220 | 221 | const TimingConstraint &getTiming() const 222 | { 223 | return timing_; 224 | } 225 | 226 | void setTiming(const TimingConstraint &timing) 227 | { 228 | timing_ = timing; 229 | } 230 | 231 | protected: 232 | ID id_; /**<@brief ID associated with this pt. Generally refers back to a process path defined elsewhere. */ 233 | TimingConstraint timing_; /**<@brief Information specifying acceptable timing from this point to the next. */ 234 | }; 235 | 236 | } /* namespace descartes_core */ 237 | 238 | #endif /* TRAJECTORY_PT_H_ */ 239 | -------------------------------------------------------------------------------- /descartes_planner/src/dense_planner.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | #include 19 | #include 20 | 21 | namespace descartes_planner 22 | { 23 | using namespace descartes_core; 24 | DensePlanner::DensePlanner() : planning_graph_(), error_code_(descartes_core::PlannerError::UNINITIALIZED) 25 | { 26 | error_map_ = { { PlannerError::OK, "OK" }, 27 | { PlannerError::EMPTY_PATH, "No path plan has been generated" }, 28 | { PlannerError::INVALID_ID, "ID is nil or isn't part of the path" }, 29 | { PlannerError::IK_NOT_AVAILABLE, "One or more ik solutions could not be found" }, 30 | { PlannerError::UNINITIALIZED, "Planner has not been initialized with a robot model" }, 31 | { PlannerError::INCOMPLETE_PATH, "Input trajectory and output path point cound differ" } }; 32 | } 33 | 34 | DensePlanner::~DensePlanner() 35 | { 36 | } 37 | 38 | bool DensePlanner::initialize(descartes_core::RobotModelConstPtr model) 39 | { 40 | planning_graph_ = 41 | boost::shared_ptr(new descartes_planner::PlanningGraph(std::move(model))); 42 | error_code_ = descartes_core::PlannerErrors::EMPTY_PATH; 43 | return true; 44 | } 45 | 46 | bool DensePlanner::initialize(descartes_core::RobotModelConstPtr model, 47 | descartes_planner::CostFunction cost_function_callback) 48 | { 49 | planning_graph_ = boost::shared_ptr( 50 | new descartes_planner::PlanningGraph(std::move(model), cost_function_callback)); 51 | error_code_ = descartes_core::PlannerErrors::EMPTY_PATH; 52 | return true; 53 | } 54 | 55 | bool DensePlanner::setConfig(const descartes_core::PlannerConfig& config) 56 | { 57 | config_ = config; 58 | config_.clear(); 59 | return true; 60 | } 61 | 62 | void DensePlanner::getConfig(descartes_core::PlannerConfig& config) const 63 | { 64 | config = config_; 65 | } 66 | 67 | descartes_core::TrajectoryPt::ID DensePlanner::getPrevious(const descartes_core::TrajectoryPt::ID& ref_id) 68 | { 69 | descartes_core::TrajectoryPt::ID id; 70 | auto predicate = [&ref_id](descartes_core::TrajectoryPtPtr p) 71 | { 72 | return ref_id == p->getID(); 73 | }; 74 | 75 | auto pos = std::find_if(path_.begin()++, path_.end(), predicate); 76 | if (pos == path_.end()) 77 | { 78 | id = descartes_core::TrajectoryID::make_nil(); 79 | } 80 | else 81 | { 82 | pos--; 83 | id = (*pos)->getID(); 84 | } 85 | 86 | return id; 87 | } 88 | 89 | bool DensePlanner::updatePath() 90 | { 91 | double c; 92 | std::list list; 93 | if (planning_graph_->getShortestPath(c, list)) 94 | { 95 | error_code_ = descartes_core::PlannerErrors::OK; 96 | for (auto&& p : list) 97 | { 98 | path_.push_back(boost::make_shared(std::move(p))); 99 | } 100 | return true; 101 | } 102 | else 103 | { 104 | error_code_ = descartes_core::PlannerErrors::UKNOWN; 105 | return false; 106 | } 107 | } 108 | 109 | descartes_core::TrajectoryPt::ID DensePlanner::getNext(const descartes_core::TrajectoryPt::ID& ref_id) 110 | { 111 | descartes_core::TrajectoryPt::ID id; 112 | auto predicate = [&ref_id](descartes_core::TrajectoryPtPtr p) 113 | { 114 | return ref_id == p->getID(); 115 | }; 116 | 117 | auto pos = std::find_if(path_.begin(), path_.end() - 2, predicate); 118 | if (pos == path_.end()) 119 | { 120 | id = descartes_core::TrajectoryID::make_nil(); 121 | } 122 | else 123 | { 124 | pos++; 125 | id = (*pos)->getID(); 126 | } 127 | return id; 128 | } 129 | 130 | descartes_core::TrajectoryPtPtr DensePlanner::get(const descartes_core::TrajectoryPt::ID& ref_id) 131 | { 132 | descartes_core::TrajectoryPtPtr p; 133 | auto predicate = [&ref_id](descartes_core::TrajectoryPtPtr p) 134 | { 135 | return ref_id == p->getID(); 136 | }; 137 | 138 | auto pos = std::find_if(path_.begin(), path_.end() - 2, predicate); 139 | if (pos == path_.end()) 140 | { 141 | p.reset(); 142 | } 143 | else 144 | { 145 | p = *pos; 146 | } 147 | return p; 148 | } 149 | 150 | bool DensePlanner::planPath(const std::vector& traj) 151 | { 152 | if (error_code_ == descartes_core::PlannerError::UNINITIALIZED) 153 | { 154 | ROS_ERROR_STREAM("Planner has not been initialized"); 155 | return false; 156 | } 157 | 158 | path_.clear(); 159 | error_code_ = descartes_core::PlannerError::EMPTY_PATH; 160 | 161 | if (planning_graph_->insertGraph(traj)) 162 | { 163 | updatePath(); 164 | } 165 | else 166 | { 167 | error_code_ = descartes_core::PlannerError::IK_NOT_AVAILABLE; 168 | } 169 | 170 | return descartes_core::PlannerError::OK == error_code_; 171 | } 172 | 173 | bool DensePlanner::getPath(std::vector& path) const 174 | { 175 | if (path_.empty()) 176 | return false; 177 | 178 | path.assign(path_.begin(), path_.end()); 179 | return error_code_ == descartes_core::PlannerError::OK; 180 | } 181 | 182 | bool DensePlanner::addAfter(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr tp) 183 | { 184 | if (path_.empty()) 185 | { 186 | return false; 187 | } 188 | 189 | descartes_core::TrajectoryPt::ID next_id = getNext(ref_id); 190 | if (!next_id.is_nil()) 191 | { 192 | if (planning_graph_->addTrajectory(tp, ref_id, next_id)) 193 | { 194 | if (updatePath()) 195 | { 196 | error_code_ = descartes_core::PlannerError::OK; 197 | } 198 | else 199 | { 200 | return false; 201 | } 202 | } 203 | else 204 | { 205 | error_code_ = descartes_core::PlannerErrors::IK_NOT_AVAILABLE; 206 | return false; 207 | } 208 | } 209 | else 210 | { 211 | error_code_ = descartes_core::PlannerError::INVALID_ID; 212 | return false; 213 | } 214 | 215 | return true; 216 | } 217 | 218 | bool DensePlanner::addBefore(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr tp) 219 | { 220 | if (path_.empty()) 221 | { 222 | return false; 223 | } 224 | 225 | descartes_core::TrajectoryPt::ID prev_id = getPrevious(ref_id); 226 | if (!prev_id.is_nil()) 227 | { 228 | if (planning_graph_->addTrajectory(tp, prev_id, ref_id)) 229 | { 230 | if (updatePath()) 231 | { 232 | error_code_ = descartes_core::PlannerError::OK; 233 | } 234 | else 235 | { 236 | return false; 237 | } 238 | } 239 | else 240 | { 241 | error_code_ = descartes_core::PlannerErrors::IK_NOT_AVAILABLE; 242 | return false; 243 | } 244 | } 245 | else 246 | { 247 | error_code_ = descartes_core::PlannerError::INVALID_ID; 248 | return false; 249 | } 250 | 251 | return true; 252 | } 253 | 254 | bool DensePlanner::remove(const descartes_core::TrajectoryPt::ID& ref_id) 255 | { 256 | if (path_.empty()) 257 | { 258 | return false; 259 | } 260 | 261 | descartes_core::TrajectoryPtPtr tp = get(ref_id); 262 | if (tp) 263 | { 264 | tp->setID(ref_id); 265 | if (planning_graph_->removeTrajectory(tp->getID())) // TODO: Clean up this extra copy & lookup 266 | { 267 | if (updatePath()) // TODO: Should we force an update here? What if the user wants to remove several points? 268 | { 269 | error_code_ = descartes_core::PlannerError::OK; 270 | } 271 | else 272 | { 273 | return false; 274 | } 275 | } 276 | else 277 | { 278 | error_code_ = descartes_core::PlannerErrors::IK_NOT_AVAILABLE; 279 | return false; 280 | } 281 | } 282 | else 283 | { 284 | error_code_ = descartes_core::PlannerError::INVALID_ID; 285 | return false; 286 | } 287 | 288 | return true; 289 | } 290 | 291 | bool DensePlanner::modify(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr tp) 292 | { 293 | if (path_.empty()) 294 | { 295 | return false; 296 | } 297 | 298 | if (!ref_id.is_nil()) 299 | { 300 | tp->setID(ref_id); 301 | if (planning_graph_->modifyTrajectory(tp)) 302 | { 303 | if (updatePath()) 304 | { 305 | error_code_ = descartes_core::PlannerError::OK; 306 | } 307 | else 308 | { 309 | return false; 310 | } 311 | } 312 | else 313 | { 314 | error_code_ = descartes_core::PlannerErrors::IK_NOT_AVAILABLE; 315 | return false; 316 | } 317 | } 318 | else 319 | { 320 | error_code_ = descartes_core::PlannerError::INVALID_ID; 321 | return false; 322 | } 323 | 324 | return true; 325 | } 326 | 327 | int DensePlanner::getErrorCode() const 328 | { 329 | return error_code_; 330 | } 331 | 332 | bool DensePlanner::getErrorMessage(int error_code, std::string& msg) const 333 | { 334 | std::map::const_iterator it = error_map_.find(error_code); 335 | 336 | if (it != error_map_.cend()) 337 | { 338 | msg = it->second; 339 | } 340 | else 341 | { 342 | return false; 343 | } 344 | return true; 345 | } 346 | 347 | } /* namespace descartes_core */ 348 | --------------------------------------------------------------------------------