├── kuka_kr120_with_torch ├── kuka_kr120_with_torch_moveit_config │ ├── .gitignore~ │ ├── .gitignore │ ├── launch │ │ ├── kr120_with_torch_moveit_sensor_manager.launch.xml │ │ ├── kr120_with_torch_moveit_controller_manager.launch.xml │ │ ├── planning_pipeline.launch.xml │ │ ├── fake_moveit_controller_manager.launch.xml │ │ ├── warehouse.launch │ │ ├── setup_assistant.launch │ │ ├── warehouse_settings.launch.xml │ │ ├── joystick_control.launch │ │ ├── sensor_manager.launch.xml │ │ ├── moveit_rviz.launch │ │ ├── default_warehouse_db.launch │ │ ├── run_benchmark_ompl.launch │ │ ├── ompl_planning_pipeline.launch.xml │ │ ├── planning_context.launch │ │ ├── trajectory_execution.launch.xml │ │ ├── demo.launch │ │ ├── descartes_pathplanning.launch │ │ ├── move_group.launch │ │ └── moveit.rviz │ ├── config │ │ ├── joint_names.yaml~ │ │ ├── joint_names.yaml │ │ ├── fake_controllers.yaml │ │ ├── kinematics.yaml │ │ ├── joint_limits.yaml │ │ ├── kr120_with_torch.srdf │ │ ├── kr120_with_torch.srdf~ │ │ └── ompl_planning.yaml │ ├── .setup_assistant │ ├── CMakeLists.txt │ └── package.xml ├── kuka_kr120_with_torch_support │ ├── meshes │ │ └── torch.stl │ ├── package.xml │ ├── urdf │ │ ├── kr120_with_torch.urdf.xacro │ │ └── kr120_with_torch.urdf │ └── CMakeLists.txt └── kuka_kr120_with_torch_ikfast │ ├── update_ikfast_plugin.sh │ ├── kr120_with_torch_manipulator_moveit_ikfast_plugin_description.xml │ ├── package.xml │ ├── CMakeLists.txt │ ├── include │ └── ikfast.h │ └── src │ └── kr120_with_torch_manipulator_ikfast_moveit_plugin.cpp ├── descartes_tutorials ├── meshes │ ├── table.stl │ └── profile.stl ├── launch │ ├── tutorial1.launch │ ├── motoman_sim.launch │ ├── tutorial2.launch │ └── kuka_sim.launch ├── package.xml ├── CMakeLists.txt └── src │ ├── tutorial1.cpp │ └── tutorial2.cpp ├── .gitignore ├── tutorial_utilities ├── package.xml ├── CMakeLists.txt ├── src │ ├── path_generation.cpp │ ├── collision_object_utils.cpp │ └── visualization.cpp └── include │ └── tutorial_utilities │ ├── path_generation.h │ ├── collision_object_utils.h │ └── visualization.h ├── worspace_structure.md ├── tutorial2_info.md ├── README.md └── LICENSE /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/.gitignore~: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/.gitignore: -------------------------------------------------------------------------------- 1 | default_warehouse_mongo_db 2 | -------------------------------------------------------------------------------- /descartes_tutorials/meshes/table.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JeroenDM/descartes_tutorials/HEAD/descartes_tutorials/meshes/table.stl -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/kr120_with_torch_moveit_sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/kr120_with_torch_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/config/joint_names.yaml~: -------------------------------------------------------------------------------- 1 | controller_joint_names: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] 2 | -------------------------------------------------------------------------------- /descartes_tutorials/launch/tutorial1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/config/joint_names.yaml: -------------------------------------------------------------------------------- 1 | controller_joint_names: [arm_joint_a1, arm_joint_a2, arm_joint_a3, arm_joint_a4, arm_joint_a5, arm_joint_a6] 2 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_support/meshes/torch.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JeroenDM/descartes_tutorials/HEAD/kuka_kr120_with_torch/kuka_kr120_with_torch_support/meshes/torch.stl -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_ikfast/update_ikfast_plugin.sh: -------------------------------------------------------------------------------- 1 | rosrun moveit_ikfast create_ikfast_moveit_plugin.py kr120_with_torch manipulator kr120_with_torch_ikfast /home/jeroen/ros/temp_ws/src/kr120_with_torch_ikfast/src/kr120_with_torch_manipulator_ikfast_solver.cpp -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/config/fake_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: fake_manipulator_controller 3 | joints: 4 | - arm_joint_a1 5 | - arm_joint_a2 6 | - arm_joint_a3 7 | - arm_joint_a4 8 | - arm_joint_a5 9 | - arm_joint_a6 -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | manipulator: 2 | kinematics_solver: kr120_with_torch_manipulator_kinematics/IKFastKinematicsPlugin 3 | kinematics_solver_attempts: 3 4 | kinematics_solver_search_resolution: 0.005 5 | kinematics_solver_timeout: 0.005 6 | -------------------------------------------------------------------------------- /descartes_tutorials/launch/motoman_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /descartes_tutorials/launch/tutorial2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | URDF: 3 | package: kr120_with_torch 4 | relative_path: urdf/kr120_with_torch.urdf 5 | SRDF: 6 | relative_path: config/kr120_with_torch.srdf 7 | CONFIG: 8 | author_name: jeroen 9 | author_email: jeroen.demaeyer@kuleuven.be 10 | generated_timestamp: 1490603342 -------------------------------------------------------------------------------- /descartes_tutorials/launch/kuka_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | 19 | # Compiled Static libraries 20 | *.lai 21 | *.la 22 | *.a 23 | *.lib 24 | 25 | # Executables 26 | *.exe 27 | *.out 28 | *.app 29 | 30 | # vscode settings 31 | settings.json 32 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(kr120_with_torch_moveit_config) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 9 | PATTERN "setup_assistant.launch" EXCLUDE) 10 | install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 11 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_ikfast/kr120_with_torch_manipulator_moveit_ikfast_plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | IKFast61 plugin for closed-form kinematics 5 | 6 | 7 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/fake_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_support/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kr120_with_torch 4 | 0.1.0 5 | 6 | The kr120_with_torch package, based on the package kuka_experimental from ros-i, but with a welding torch added. 7 | 8 | 9 | Jeroen De Maeyer 10 | 11 | BSD 12 | 13 | catkin 14 | 15 | 16 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/warehouse.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/setup_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/warehouse_settings.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/joystick_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/moveit_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/default_warehouse_db.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_ikfast/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kr120_with_torch_ikfast 4 | 0.1.0 5 | Autogenerated configuration package to use the ikfast plugin in moveit 6 | 7 | Jeroen De Maeyer 8 | 9 | BSD 10 | 11 | catkin 12 | 13 | 14 | 15 | 16 | 17 | moveit_core 18 | liblapack-dev 19 | roscpp 20 | tf_conversions 21 | pluginlib 22 | 23 | moveit_core 24 | liblapack-dev 25 | roscpp 26 | tf_conversions 27 | pluginlib 28 | 29 | 30 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_ikfast/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(kr120_with_torch_ikfast) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | moveit_core 6 | pluginlib 7 | roscpp 8 | tf_conversions 9 | ) 10 | 11 | include_directories(${catkin_INCLUDE_DIRS}) 12 | 13 | catkin_package( 14 | LIBRARIES 15 | CATKIN_DEPENDS 16 | moveit_core 17 | pluginlib 18 | roscpp 19 | tf_conversions 20 | ) 21 | 22 | include_directories(include) 23 | 24 | set(IKFAST_LIBRARY_NAME kr120_with_torch_manipulator_moveit_ikfast_plugin) 25 | 26 | find_package(LAPACK REQUIRED) 27 | 28 | add_library(${IKFAST_LIBRARY_NAME} src/kr120_with_torch_manipulator_ikfast_moveit_plugin.cpp) 29 | target_link_libraries(${IKFAST_LIBRARY_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${LAPACK_LIBRARIES}) 30 | 31 | install(TARGETS ${IKFAST_LIBRARY_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 32 | 33 | install( 34 | FILES 35 | kr120_with_torch_manipulator_moveit_ikfast_plugin_description.xml 36 | DESTINATION 37 | ${CATKIN_PACKAGE_SHARE_DESTINATION} 38 | ) -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/run_benchmark_ompl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/ompl_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /tutorial_utilities/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | tutorial_utilities 4 | 0.1.0 5 | Utilitie functions to make the code in descartes_tutorials more managable. 6 | 7 | jeroen 8 | http://wiki.ros.org/descartes/ 9 | Bart Moyaers 10 | Jeroen De Maeyer 11 | 12 | Apache 2.0 13 | 14 | catkin 15 | 16 | descartes_core 17 | descartes_trajectory 18 | trajectory_msgs 19 | moveit_ros_planning_interface 20 | std_msgs 21 | moveit_msgs 22 | 23 | descartes_core 24 | descartes_trajectory 25 | trajectory_msgs 26 | moveit_ros_planning_interface 27 | std_msgs 28 | moveit_msgs 29 | 30 | 31 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kr120_with_torch_moveit_config 4 | 0.3.0 5 | 6 | An automatically generated package with all the configuration and launch files for using the kr120_with_torch with the MoveIt! Motion Planning Framework 7 | 8 | 9 | Jeroen De Maeyer 10 | 11 | BSD 12 | 13 | http://moveit.ros.org/ 14 | https://github.com/ros-planning/moveit/issues 15 | https://github.com/ros-planning/moveit 16 | 17 | catkin 18 | 19 | moveit_ros_move_group 20 | moveit_kinematics 21 | moveit_planners_ompl 22 | moveit_ros_visualization 23 | joint_state_publisher 24 | robot_state_publisher 25 | xacro 26 | kr120_with_torch 27 | kr120_with_torch 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 3 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 4 | joint_limits: 5 | arm_joint_a1: 6 | has_velocity_limits: true 7 | max_velocity: 2.72271363311 8 | has_acceleration_limits: false 9 | max_acceleration: 0 10 | arm_joint_a2: 11 | has_velocity_limits: true 12 | max_velocity: 2.72271363311 13 | has_acceleration_limits: false 14 | max_acceleration: 0 15 | arm_joint_a3: 16 | has_velocity_limits: true 17 | max_velocity: 2.72271363311 18 | has_acceleration_limits: false 19 | max_acceleration: 0 20 | arm_joint_a4: 21 | has_velocity_limits: true 22 | max_velocity: 5.75958653158 23 | has_acceleration_limits: false 24 | max_acceleration: 0 25 | arm_joint_a5: 26 | has_velocity_limits: true 27 | max_velocity: 5.75958653158 28 | has_acceleration_limits: false 29 | max_acceleration: 0 30 | arm_joint_a6: 31 | has_velocity_limits: true 32 | max_velocity: 10.7337748998 33 | has_acceleration_limits: false 34 | max_acceleration: 0 -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/planning_context.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 | -------------------------------------------------------------------------------- /tutorial_utilities/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(tutorial_utilities) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | add_definitions(-std=c++11) 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | descartes_trajectory 9 | ) 10 | 11 | catkin_package( 12 | INCLUDE_DIRS 13 | include 14 | LIBRARIES 15 | tutorial_utilities 16 | CATKIN_DEPENDS 17 | descartes_core 18 | descartes_trajectory 19 | trajectory_msgs 20 | moveit_msgs 21 | std_msgs 22 | moveit_ros_planning_interface 23 | DEPENDS 24 | eigen 25 | ) 26 | 27 | include_directories( 28 | include 29 | ${catkin_INCLUDE_DIRS} 30 | ${Eigen_INCLUDE_DIRS} 31 | ) 32 | 33 | add_library(tutorial_utilities 34 | src/path_generation.cpp 35 | src/collision_object_utils.cpp 36 | src/visualization.cpp 37 | ) 38 | 39 | add_dependencies(tutorial_utilities 40 | ${catkin_EXPORTED_TARGETS} 41 | ) 42 | 43 | #target_link_libraries(${PROJECT_NAME} 44 | # tutorial_utilities_lib 45 | # ${catkin_LIBRARIES} 46 | #) 47 | 48 | install(TARGETS ${PROJECT_NAME} 49 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 50 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 51 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 52 | ) 53 | 54 | install(DIRECTORY include/${PROJECT_NAME}/ 55 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 56 | FILES_MATCHING PATTERN "*.h" 57 | ) -------------------------------------------------------------------------------- /descartes_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | descartes_tutorials 4 | 0.1.0 5 | First excercise of the Descartes tutorials 6 | 7 | Jonathan Meyer 8 | 9 | Apache 2.0 10 | 11 | http://wiki.ros.org/descartes/ 12 | 13 | Jonathan Meyer 14 | 15 | catkin 16 | 17 | descartes_core 18 | descartes_moveit 19 | descartes_planner 20 | descartes_trajectory 21 | trajectory_msgs 22 | tutorial_utilities 23 | 24 | motoman_sia20d_support 25 | motoman_sia20d_moveit_config 26 | kuka_kr120_support 27 | kr120_with_torch_ikfast 28 | industrial_robot_simulator 29 | descartes_core 30 | descartes_moveit 31 | descartes_planner 32 | descartes_trajectory 33 | trajectory_msgs 34 | tutorial_utilities 35 | 36 | 37 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/trajectory_execution.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /worspace_structure.md: -------------------------------------------------------------------------------- 1 | # Workspace organization 2 | 3 | Because we have to fix dependency issues and stuff, I put together an overview of the workspace layout I'm currently using for developing and testing the new tutorial. 4 | Everything is in one (catkin) workspace, because using multiple workspaces caused problems. In the src folder of the workspaces I put all the git repos that containing the packages. This results in the following file structure: 5 | With the provided links, it should be possible to recreate this workspace. 6 | 7 | descartes_ws/ 8 | - build/ 9 | - devel/ 10 | - src/ 11 | - [descartes/](https://github.com/JeroenDM/descartes/tree/indigo-devel) 12 | - ... 13 | - [descartes_tutorials/](https://github.com/JeroenDM/descartes_tutorials) 14 | - descartes_tutorials/ 15 | - tutorial_utitilies/ 16 | -include/ 17 | - visualization.h 18 | - path_generation.h 19 | - collision_object_utils.h 20 | - .gitignore 21 | - LICENSE 22 | - [kuka_kr120_robot/](https://github.com/JeroenDM/kuka_kr120_robot) 23 | - kuka_kr120_with_torch/ 24 | - kuka_kr120_with_torch_ikfast/ 25 | - kuka_kr120_with_torch_moveit_config/ 26 | - [motoman_robot/](https://github.com/ros-industrial/motoman) 27 | - motoman_sia20d_support/ 28 | - motoman_sia20d_moveit_config/ 29 | - [kuka_experimental](https://github.com/ros-industrial/kuka_experimental) 30 | - [industrial_core](https://github.com/ros-industrial/industrial_core) 31 | - CMakeLists.txt 32 | -------------------------------------------------------------------------------- /tutorial_utilities/src/path_generation.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 | * path_generation.cpp 20 | * 21 | * Created on: Feb 28, 2016 22 | * Author: Bart and Jeroen 23 | */ 24 | 25 | #include 26 | #include 27 | 28 | 29 | namespace tutorial_utilities 30 | { 31 | std::vector line( Eigen::Affine3d start_pose, Eigen::Affine3d end_pose, int steps) 32 | { 33 | Eigen::Vector3d translation_vector; 34 | translation_vector = end_pose.translation() - start_pose.translation(); 35 | translation_vector = translation_vector / steps; 36 | Eigen::Translation translate(translation_vector); 37 | 38 | std::vector poses; 39 | poses.push_back(start_pose); 40 | for(int i = 0; i < (steps - 1); ++i) 41 | { 42 | poses.push_back(translate * poses.back()); 43 | } 44 | return poses; 45 | } 46 | } -------------------------------------------------------------------------------- /tutorial_utilities/include/tutorial_utilities/path_generation.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 | * path_generation.h 20 | * 21 | * Created on: Feb 28, 2016 22 | * Author: Bart and Jeroen 23 | */ 24 | 25 | #ifndef DESCARTES_PATH_GENERATION_H 26 | #define DESCARTES_PATH_GENERATION_H 27 | 28 | #include 29 | #include 30 | 31 | 32 | namespace tutorial_utilities 33 | { 34 | /** 35 | * Get a vector of poses on a straight line between two points. 36 | * @param startPose Transformation matrix of the starting pose 37 | * @param endPose Transformation matrix of the end pose 38 | * @param steps Number of trajectory points on straight line 39 | * @return Vector containing the points of the line trajectory 40 | */ 41 | std::vector line( Eigen::Affine3d start_pose, Eigen::Affine3d en_pPose, int steps); 42 | } 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /tutorial2_info.md: -------------------------------------------------------------------------------- 1 | # Descartes welding example 2 | We implemented an example of path planning for a welding robot. We started from the tutorial1.cpp file and added new code and used a different robot. The original code is written [Bart's repository](https://github.com/Bart123456/lasrobot_ws). It was in serious need of cleaning and refactoring, therefore we started from scratch in this repository, which is a fork of the original code. 3 | 4 | For our first contribution, we avoided changes things in the descartes core package. However, to include collision avoidance we had to make a small change to the moveit_state_adapter and the robot_model. 5 | 6 | ## What we want to show in the tutorial 7 | - Visualization of a trajectory. 8 | - Adding collision objects. (Should this be in a seperate cpp executable, same launch file?) 9 | - To little trajectory points -> collision not detected (9 points for this example). 10 | - (custom cost function) 11 | 12 | **Important:** for the collision objects part, some lines of code are added in the descartes source code which can be found [here](https://github.com/JeroenDM/descartes/commit/4f4311c969dcf1ad64539679cac35a5e7c5060a2). 13 | Originaly many of the parameters could be changed in the launch file. For simplicity this is left out in this tutorial at the moment. 14 | 15 | # Detailed planning of functions to add 16 | ## visualization 17 | - createMarker 18 | - createVisualFrame 19 | - addVisualFrame 20 | 21 | ## path_generation 22 | - line: generate a straight line of eigen poses all with the same orientation 23 | - circle: generate a circle of eigen poses around a fiven pose with a given radius 24 | 25 | ## collision_object_utils 26 | - createCollisionObject 27 | - createObjectColor 28 | -------------------------------------------------------------------------------- /descartes_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(descartes_tutorials) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | descartes_core 6 | descartes_moveit 7 | descartes_trajectory 8 | descartes_planner 9 | tutorial_utilities REQUIRED 10 | trajectory_msgs 11 | moveit_ros_planning_interface 12 | ) 13 | 14 | catkin_package() 15 | 16 | ########### 17 | ## Build ## 18 | ########### 19 | include_directories( 20 | ${catkin_INCLUDE_DIRS} 21 | ${tutorial_utilities_INCLUDE_DIRS} 22 | ) 23 | 24 | add_executable(${PROJECT_NAME}_tutorial1 src/tutorial1.cpp) 25 | add_executable(${PROJECT_NAME}_tutorial2 src/tutorial2.cpp) 26 | 27 | target_link_libraries(${PROJECT_NAME}_tutorial1 28 | ${catkin_LIBRARIES} 29 | ) 30 | target_link_libraries(${PROJECT_NAME}_tutorial2 31 | ${catkin_LIBRARIES} 32 | ) 33 | 34 | set_target_properties(${PROJECT_NAME}_tutorial1 35 | PROPERTIES OUTPUT_NAME tutorial1 36 | PREFIX "") 37 | set_target_properties(${PROJECT_NAME}_tutorial2 38 | PROPERTIES OUTPUT_NAME tutorial2 39 | PREFIX "") 40 | 41 | 42 | ############# 43 | ## Install ## 44 | ############# 45 | 46 | install(TARGETS ${PROJECT_NAME}_tutorial1 47 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 48 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 49 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 50 | ) 51 | 52 | install(TARGETS ${PROJECT_NAME}_tutorial2 53 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 54 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 55 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 56 | ) 57 | 58 | install(DIRECTORY include/${PROJECT_NAME}/ 59 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 60 | FILES_MATCHING PATTERN "*.h" 61 | ) 62 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_support/urdf/kr120_with_torch.urdf.xacro: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /tutorial_utilities/include/tutorial_utilities/collision_object_utils.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 | * collision_object_utils.h 20 | * 21 | * Created on: Feb 28, 2016 22 | * Author: Bart and Jeroen 23 | */ 24 | 25 | #ifndef DESCARTES_COLLISION_OBJECT_UTILS_H 26 | #define DESCARTES_COLLISION_OBJECT_UTILS_H 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | namespace tutorial_utilities 36 | { 37 | Eigen::Quaternion eulerToQuat(double rotX, double rotY, double rotZ); 38 | 39 | /** 40 | * Function to test library compilation 41 | */ 42 | void testCollisionUtils(); 43 | 44 | moveit_msgs::CollisionObject makeCollisionObject(std::string filepath, Eigen::Vector3d scale, std::string ID, Eigen::Affine3d pose); 45 | 46 | std_msgs::ColorRGBA makeColor(double r, double g, double b, double a); 47 | 48 | moveit_msgs::ObjectColor makeObjectColor(std::string id, std_msgs::ColorRGBA color); 49 | 50 | moveit_msgs::ObjectColor makeObjectColor(std::string id, double r, double g, double b, double a); 51 | } 52 | 53 | #endif -------------------------------------------------------------------------------- /tutorial_utilities/include/tutorial_utilities/visualization.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 | * visualization.h 20 | * 21 | * Created on: May 1, 2017 22 | * Author: Bart and Jeroen 23 | */ 24 | 25 | #ifndef __VISUALIZATION_H_INCLUDED__ 26 | #define __VISUALIZATION_H_INCLUDED__ 27 | 28 | //Include visualization markers for RViz 29 | #include 30 | #include 31 | //Include Eigen geometry header for rotations 32 | #include 33 | // Includes the descartes trajectory type we will be using 34 | #include 35 | 36 | namespace tutorial_utilities { 37 | //Function for constructing quaternion starting from Euler rotations XYZ 38 | Eigen::Quaternion eulerToQuat(double rotX, double rotY, double rotZ); 39 | 40 | visualization_msgs::Marker createMarker(double transX, double transY, double transZ, double rotX, double rotY, double rotZ, 41 | descartes_trajectory::AxialSymmetricPt::FreeAxis axis); 42 | 43 | void createVisualFrame(Eigen::Affine3d pose, std::vector & markervec); 44 | 45 | visualization_msgs::MarkerArray createMarkerArray(std::vector poses); 46 | } 47 | 48 | #endif -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | [/move_group/fake_controller_joint_states] 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 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/descartes_pathplanning.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/config/kr120_with_torch.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 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/config/kr120_with_torch.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 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/move_group.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 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 | 56 | 57 | 58 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/config/ompl_planning.yaml: -------------------------------------------------------------------------------- 1 | planner_configs: 2 | SBLkConfigDefault: 3 | type: geometric::SBL 4 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 5 | ESTkConfigDefault: 6 | type: geometric::EST 7 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() 8 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 9 | LBKPIECEkConfigDefault: 10 | type: geometric::LBKPIECE 11 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 12 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 13 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 14 | BKPIECEkConfigDefault: 15 | type: geometric::BKPIECE 16 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 17 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 18 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 19 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 20 | KPIECEkConfigDefault: 21 | type: geometric::KPIECE 22 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 23 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 24 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] 25 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 26 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 27 | RRTkConfigDefault: 28 | type: geometric::RRT 29 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 30 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 31 | RRTConnectkConfigDefault: 32 | type: geometric::RRTConnect 33 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 34 | RRTstarkConfigDefault: 35 | type: geometric::RRTstar 36 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 37 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 38 | delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 39 | TRRTkConfigDefault: 40 | type: geometric::TRRT 41 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 42 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 43 | max_states_failed: 10 # when to start increasing temp. default: 10 44 | temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 45 | min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 46 | init_temperature: 10e-6 # initial temperature. default: 10e-6 47 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 48 | frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 49 | k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() 50 | PRMkConfigDefault: 51 | type: geometric::PRM 52 | max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 53 | PRMstarkConfigDefault: 54 | type: geometric::PRMstar 55 | manipulator: 56 | planner_configs: 57 | - SBLkConfigDefault 58 | - ESTkConfigDefault 59 | - LBKPIECEkConfigDefault 60 | - BKPIECEkConfigDefault 61 | - KPIECEkConfigDefault 62 | - RRTkConfigDefault 63 | - RRTConnectkConfigDefault 64 | - RRTstarkConfigDefault 65 | - TRRTkConfigDefault 66 | - PRMkConfigDefault 67 | - PRMstarkConfigDefault -------------------------------------------------------------------------------- /tutorial_utilities/src/collision_object_utils.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 | * collision_object_utils.cpp 20 | * 21 | * Created on: Feb 28, 2016 22 | * Author: Bart and Jeroen 23 | */ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace tutorial_utilities 33 | { 34 | Eigen::Quaternion eulerToQuat(double rotX, double rotY, double rotZ) 35 | { 36 | Eigen::Matrix3d m; 37 | m = Eigen::AngleAxisd(rotX, Eigen::Vector3d::UnitX()) 38 | * Eigen::AngleAxisd(rotY, Eigen::Vector3d::UnitY()) 39 | * Eigen::AngleAxisd(rotZ, Eigen::Vector3d::UnitZ()); 40 | 41 | Eigen::AngleAxis aa; 42 | aa = Eigen::AngleAxisd(m); 43 | 44 | Eigen::Quaternion quat; 45 | quat = Eigen::Quaternion(aa); 46 | 47 | return quat; 48 | } 49 | 50 | void testCollisionUtils() 51 | { 52 | ROS_INFO("============== Collision object utils library working !! ==============="); 53 | } 54 | 55 | moveit_msgs::CollisionObject makeCollisionObject(std::string filepath, Eigen::Vector3d scale, std::string ID, Eigen::Affine3d pose) 56 | { 57 | moveit_msgs::CollisionObject co; 58 | 59 | //ROS_INFO("Loading mesh"); 60 | shapes::Mesh* m = shapes::createMeshFromResource(filepath, scale); 61 | //ROS_INFO("Mesh loaded"); 62 | 63 | shape_msgs::Mesh mesh; 64 | shapes::ShapeMsg mesh_msg; 65 | shapes::constructMsgFromShape(m, mesh_msg); 66 | mesh = boost::get(mesh_msg); 67 | 68 | Eigen::Vector3d translations; 69 | translations = pose.translation(); 70 | Eigen::Vector3d rotationsXYZ; 71 | rotationsXYZ = pose.rotation().eulerAngles(0,1,2); 72 | Eigen::Quaternion quat; 73 | quat = eulerToQuat(rotationsXYZ[0], rotationsXYZ[1], rotationsXYZ[2]); 74 | 75 | co.header.frame_id = "base_link"; 76 | co.id = ID; 77 | co.meshes.resize(1); 78 | co.mesh_poses.resize(1); 79 | co.meshes[0] = mesh; 80 | co.mesh_poses[0].position.x = translations[0]; 81 | co.mesh_poses[0].position.y = translations[1]; 82 | co.mesh_poses[0].position.z = translations[2]; 83 | co.mesh_poses[0].orientation.w= quat.w(); 84 | co.mesh_poses[0].orientation.x= quat.x(); 85 | co.mesh_poses[0].orientation.y= quat.y(); 86 | co.mesh_poses[0].orientation.z= quat.z(); 87 | 88 | co.operation = co.ADD; 89 | 90 | return co; 91 | } 92 | 93 | std_msgs::ColorRGBA makeColor(double r, double g, double b, double a) 94 | { 95 | std_msgs::ColorRGBA color; 96 | color.r = r; 97 | color.g = g; 98 | color.b = b; 99 | color.a = a; 100 | return color; 101 | } 102 | 103 | moveit_msgs::ObjectColor makeObjectColor(std::string id, std_msgs::ColorRGBA color) 104 | { 105 | moveit_msgs::ObjectColor oc; 106 | oc.id = id; 107 | oc.color = color; 108 | return oc; 109 | } 110 | 111 | moveit_msgs::ObjectColor makeObjectColor(std::string id, double r, double g, double b, double a) 112 | { 113 | moveit_msgs::ObjectColor oc; 114 | oc.color = makeColor(r, g, b, a); 115 | oc.id = id; 116 | return oc; 117 | } 118 | 119 | } -------------------------------------------------------------------------------- /tutorial_utilities/src/visualization.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 | * visualization.h 20 | * 21 | * Created on: May 1, 2017 22 | * Author: Bart and Jeroen 23 | */ 24 | 25 | #include 26 | 27 | namespace tutorial_utilities 28 | { 29 | //Function for constructing quaternion starting from Euler rotations XYZ 30 | /*Eigen::Quaternion eulerToQuat(double rotX, double rotY, double rotZ) 31 | { 32 | Eigen::Matrix3d m; 33 | m = Eigen::AngleAxisd(rotX, Eigen::Vector3d::UnitX()) 34 | * Eigen::AngleAxisd(rotY, Eigen::Vector3d::UnitY()) 35 | * Eigen::AngleAxisd(rotZ, Eigen::Vector3d::UnitZ()); 36 | 37 | Eigen::AngleAxis aa; 38 | aa = Eigen::AngleAxisd(m); 39 | 40 | Eigen::Quaternion quat; 41 | quat = Eigen::Quaternion(aa); 42 | return quat; 43 | }*/ 44 | 45 | visualization_msgs::Marker createMarker(double transX, double transY, double transZ, double rotX, double rotY, double rotZ, 46 | descartes_trajectory::AxialSymmetricPt::FreeAxis axis = 47 | descartes_trajectory::AxialSymmetricPt::X_AXIS) 48 | { 49 | static int count; 50 | visualization_msgs::Marker marker; 51 | marker.header.frame_id = "base_link"; 52 | marker.header.stamp = ros::Time(); 53 | marker.ns = "my_namespace"; 54 | marker.id = count; 55 | marker.type = visualization_msgs::Marker::ARROW; 56 | marker.action = visualization_msgs::Marker::ADD; 57 | marker.lifetime = ros::Duration(0); 58 | 59 | marker.pose.position.x = transX; 60 | marker.pose.position.y = transY; 61 | marker.pose.position.z = transZ; 62 | 63 | //To calculate the quaternion values we first define an AngleAxis object using Euler rotations, then convert it 64 | Eigen::Quaternion quat; 65 | if(axis == descartes_trajectory::AxialSymmetricPt::X_AXIS) 66 | { 67 | quat = tutorial_utilities::eulerToQuat(rotX, rotY, rotZ); 68 | marker.color.r = 1.0; 69 | marker.color.g = 0.0; 70 | marker.color.b = 0.0; 71 | } else if(axis == descartes_trajectory::AxialSymmetricPt::Y_AXIS) 72 | { 73 | quat = tutorial_utilities::eulerToQuat(rotX, rotY, rotZ) * tutorial_utilities::eulerToQuat(0, 0, M_PI / 2); 74 | marker.color.r = 0.0; 75 | marker.color.g = 1.0; 76 | marker.color.b = 0.0; 77 | } else if(axis == descartes_trajectory::AxialSymmetricPt::Z_AXIS) 78 | { 79 | quat = tutorial_utilities::eulerToQuat(rotX, rotY, rotZ) * tutorial_utilities::eulerToQuat(0, 3 * (M_PI / 2), 0); 80 | marker.color.r = 0.0; 81 | marker.color.g = 0.0; 82 | marker.color.b = 1.0; 83 | } 84 | 85 | marker.pose.orientation.x = quat.x(); 86 | marker.pose.orientation.y = quat.y(); 87 | marker.pose.orientation.z = quat.z(); 88 | marker.pose.orientation.w = quat.w(); 89 | marker.scale.x = 0.02; 90 | marker.scale.y = 0.002; 91 | marker.scale.z = 0.002; 92 | marker.color.a = 1.0; //Alpha 93 | 94 | count++; 95 | return marker; 96 | } 97 | 98 | void createVisualFrame(Eigen::Affine3d pose, std::vector & markervec) 99 | { 100 | Eigen::Vector3d translations; 101 | translations = pose.translation(); 102 | Eigen::Vector3d rotationsXYZ; 103 | rotationsXYZ = pose.rotation().eulerAngles(0,1,2); 104 | 105 | markervec.push_back(createMarker(translations[0], translations[1], translations[2], 106 | rotationsXYZ[0], rotationsXYZ[1], rotationsXYZ[2], descartes_trajectory::AxialSymmetricPt::X_AXIS)); 107 | markervec.push_back(createMarker(translations[0], translations[1], translations[2], 108 | rotationsXYZ[0], rotationsXYZ[1], rotationsXYZ[2], descartes_trajectory::AxialSymmetricPt::Y_AXIS)); 109 | markervec.push_back(createMarker(translations[0], translations[1], translations[2], 110 | rotationsXYZ[0], rotationsXYZ[1], rotationsXYZ[2], descartes_trajectory::AxialSymmetricPt::Z_AXIS)); 111 | } 112 | 113 | visualization_msgs::MarkerArray createMarkerArray(std::vector poses) 114 | { 115 | std::vector markerVector; 116 | int vectorSize; 117 | vectorSize = poses.size(); 118 | 119 | //Generate markers for every pose, put them in markerVector 120 | for(int i = 0; i < vectorSize; i++) 121 | { 122 | createVisualFrame(poses[i], markerVector); 123 | } 124 | 125 | //Copy vector to array so we can return it as a MarkerArray type. 126 | int size = markerVector.size(); 127 | visualization_msgs::MarkerArray ma; 128 | ma.markers.resize(size); 129 | for(int i = 0;i < size;i++) 130 | { 131 | ma.markers[i] = markerVector[i]; 132 | } 133 | return ma; 134 | } 135 | } //namespace tutorial_utilities -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # descartes_tutorials 2 | 3 | ## Introduction 4 | In this fork of the descartes_tutorials repo, we are working on a second tutorial. 5 | This tutorial will discuss: adding collision objects, visualizing trajectory points and why a planned path can still contain collisions. 6 | 7 | 8 | The instructions below are also written in [a tutorial on the ros wiki](http://wiki.ros.org/descartes/Tutorials/Robot%20Welding%20With%20Descartes). 9 | 10 | In an aditional branch, [new_cost_function](https://github.com/JeroenDM/descartes_tutorials/tree/new_cost_function), the use of an alternative cost function for the graph search is demonstrated. It is important to notice that also for the [modified descartes package](https://github.com/JeroenDM/descartes), the branch has to be changed to [new_cost_function](https://github.com/JeroenDM/descartes/tree/new_cost_function). 11 | 12 | ## Installation 13 | 14 | ### Quickstart with existing workspace 15 | 16 | In your workspace src folder, add three repositories. 17 | ``` 18 | cd ~/catkin_ws/src 19 | git clone https://github.com/JeroenDM/descartes.git 20 | git clone https://github.com/ros-industrial/kuka_experimental.git 21 | git clone https://github.com/JeroenDM/descartes_tutorials.git 22 | ``` 23 | 24 | Now go to the workspace root folder, update dependencies, build the new packages and source them. 25 | ``` 26 | cd ~/catkin_ws 27 | rosdep install -r -y --from-paths src --ignore-src 28 | catkin_make 29 | source devel/setup.bash 30 | ``` 31 | 32 | Now you should be able to launch the tutorial files "kuka_sim.launch" and "tutorial2.launch" in the following folder: 33 | ``` 34 | catkin_ws/src/descartes_tutorials/descartes_tutorials/launch 35 | ``` 36 | To be able to run the simulation you have to open a first command terminal (don't forget to source the workspace by running `source devel/setup.bash`) and run the "kuka_sim.launch" file. 37 | 38 | ``` 39 | roslaunch descartes_tutorials kuka_sim.launch 40 | ``` 41 | 42 | Then, in a second terminal while leaving the first one running, launch "tutorial2.launch". (Don't forget to source the workspace again, as shown in the first line below.) 43 | 44 | ``` 45 | source devel/setup.bash 46 | roslaunch descartes_tutorials tutorial2.launch 47 | ``` 48 | 49 | ### Longer explanation 50 | 51 | I assume you have installed [ros indigo](http://wiki.ros.org/indigo), runnig on ubuntu 14.04 LTS. 52 | Unfortunately this package cannot (yet) be installed just using apt-get. 53 | Some of the packages are not realeased and have to be installed from source. If you don't have a github account, create a github account. (You can also download zip files, but using ros without github is like peper without salt.) 54 | 55 | First, if you don't have a catkin workspace, you can [create one]http://wiki.ros.org/catkin/Tutorials/create_a_workspace) by executing the commands in the grey boxes in the terminal. 56 | 57 | Install catkin tools and rosdep used for building packages from source. 58 | ``` 59 | sudo apt-get install python-catkin-tools python-rosdep 60 | ``` 61 | Now you can create the workspace (which is just a directory with some files in it). 62 | ``` 63 | mkdir -p ~/catkin_ws/src 64 | cd ~/catkin_ws/src 65 | ``` 66 | and add the standard CMakeList.txt file in the src folder by running: 67 | ``` 68 | catkin_init_workspace 69 | ``` 70 | In the src folder you can now add the packages you want to build by done by cloning the github repositories in the src folder. Or downoad and unzip the repositories in this folder if you don't want to use a github account. 71 | 72 | We start with a [modified version of descartes](https://github.com/JeroenDM/descartes) which adds the possiblity for collision detection with objects other than the robot itself, nog present in the [indigo version of descartes](https://github.com/ros-industrial-consortium/descartes/tree/indigo-devel). 73 | Stil in the `~catkin_ws/src` directory, clone the package. 74 | ``` 75 | git clone https://github.com/JeroenDM/descartes.git 76 | ``` 77 | The two other packages we want to add manually are [kuka_experimental](https://github.com/ros-industrial/kuka_experimental) for some cad files of robots and of course [descartes_tutorials](https://github.com/JeroenDM/descartes_tutorials) for the tutorial itself. 78 | ``` 79 | git clone https://github.com/ros-industrial/kuka_experimental.git 80 | git clone https://github.com/JeroenDM/descartes_tutorials.git 81 | ``` 82 | Now we have all the necessary files for these packages, we can build them. This is done in the workspace root directory `~/catkin_ws`. Catkin will look in the `src` directory for packages to build. 83 | ``` 84 | cd ~/catkin_ws 85 | ``` 86 | Before we can build, however, ros has to check whether we need some other packages on which these one depend. (Spoiler alert, they depend on a lot of other stuff.) For this purpose we do: 87 | ``` 88 | rosdep install -r -y --from-paths src --ignore-src 89 | ``` 90 | Options `-y` stands for answer yes to all, `r` for continue installing despite errors and the remaining stuff for where to look and so on. More details [here](http://docs.ros.org/independent/api/rosdep/html/commands.html). If something goes wrong here, running `apt-get update` and then trying again can be a solution. If it worked, we can build the packages! 91 | ``` 92 | catkin_make 93 | ``` 94 | Et voilà, you succesfully installed packages from source....or noting is working and you almost threw you computer out of the window. In that case I would suggest hanging on to that computer and [creating an issue](https://help.github.com/articles/creating-an-issue/). 95 | 96 | Now before you can launch the tutorial, you have to source a setup file in your terminal, so ros knows where to look for the custom build packages. This file was automaticly created by the `catkin_make` command in the devel directory. 97 | ``` 98 | source devel/setup.bash 99 | ``` 100 | Now the instructions in the paragraph below should work. 101 | 102 | ## How to launch? 103 | 104 | We added two new launch files, "kuka_sim.launc" and "tutorial2.launch". 105 | The first will launch the robot model in rviz for this tutorial. 106 | The second will execute the tutorial source code. 107 | 108 | Open a first command terminal (don't forget to source the workspace by running `source devel/setup.bash`) and run the "kuka_sim.launch" file. 109 | 110 | ``` 111 | roslaunch descartes_tutorials kuka_sim.launch 112 | ``` 113 | 114 | Then, in a second terminal while leaving the first one running, launch "tutorial2.launch". (Don't forget to source the workspace again, as shown in the first line below.) 115 | 116 | ``` 117 | source devel/setup.bash 118 | roslaunch descartes_tutorials tutorial2.launch 119 | ``` 120 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_support/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(kr120_with_torch) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | # add_definitions(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a run_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if you package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES kr120_with_torch 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | # include_directories(include) 115 | 116 | ## Declare a C++ library 117 | # add_library(${PROJECT_NAME} 118 | # src/${PROJECT_NAME}/kr120_with_torch.cpp 119 | # ) 120 | 121 | ## Add cmake target dependencies of the library 122 | ## as an example, code may need to be generated before libraries 123 | ## either from message generation or dynamic reconfigure 124 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 125 | 126 | ## Declare a C++ executable 127 | ## With catkin_make all packages are built within a single CMake context 128 | ## The recommended prefix ensures that target names across packages don't collide 129 | # add_executable(${PROJECT_NAME}_node src/kr120_with_torch_node.cpp) 130 | 131 | ## Rename C++ executable without prefix 132 | ## The above recommended prefix causes long target names, the following renames the 133 | ## target back to the shorter version for ease of user use 134 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 135 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 136 | 137 | ## Add cmake target dependencies of the executable 138 | ## same as for the library above 139 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 140 | 141 | ## Specify libraries to link a library or executable target against 142 | # target_link_libraries(${PROJECT_NAME}_node 143 | # ${catkin_LIBRARIES} 144 | # ) 145 | 146 | ############# 147 | ## Install ## 148 | ############# 149 | 150 | # all install targets should use catkin DESTINATION variables 151 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 152 | 153 | ## Mark executable scripts (Python etc.) for installation 154 | ## in contrast to setup.py, you can choose the destination 155 | # install(PROGRAMS 156 | # scripts/my_python_script 157 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 158 | # ) 159 | 160 | ## Mark executables and/or libraries for installation 161 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 162 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 163 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 164 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark cpp header files for installation 168 | # install(DIRECTORY include/${PROJECT_NAME}/ 169 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 170 | # FILES_MATCHING PATTERN "*.h" 171 | # PATTERN ".svn" EXCLUDE 172 | # ) 173 | 174 | ## Mark other files for installation (e.g. launch and bag files, etc.) 175 | # install(FILES 176 | # # myfile1 177 | # # myfile2 178 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 179 | # ) 180 | 181 | ############# 182 | ## Testing ## 183 | ############# 184 | 185 | ## Add gtest based cpp test target and link libraries 186 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_kr120_with_torch.cpp) 187 | # if(TARGET ${PROJECT_NAME}-test) 188 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 189 | # endif() 190 | 191 | ## Add folders to be run by python nosetests 192 | # catkin_add_nosetests(test) 193 | -------------------------------------------------------------------------------- /descartes_tutorials/src/tutorial1.cpp: -------------------------------------------------------------------------------- 1 | // Core ros functionality like ros::init and spin 2 | #include 3 | // ROS Trajectory Action server definition 4 | #include 5 | // Means by which we communicate with above action-server 6 | #include 7 | 8 | // Includes the descartes robot model we will be using 9 | #include 10 | // Includes the descartes trajectory type we will be using 11 | #include 12 | #include 13 | // Includes the planner we will be using 14 | #include 15 | 16 | typedef std::vector TrajectoryVec; 17 | typedef TrajectoryVec::const_iterator TrajectoryIter; 18 | 19 | /** 20 | * Generates an completely defined (zero-tolerance) cartesian point from a pose 21 | */ 22 | descartes_core::TrajectoryPtPtr makeCartesianPoint(const Eigen::Affine3d& pose); 23 | /** 24 | * Generates a cartesian point with free rotation about the Z axis of the EFF frame 25 | */ 26 | descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(const Eigen::Affine3d& pose); 27 | 28 | /** 29 | * Translates a descartes trajectory to a ROS joint trajectory 30 | */ 31 | trajectory_msgs::JointTrajectory 32 | toROSJointTrajectory(const TrajectoryVec& trajectory, const descartes_core::RobotModel& model, 33 | const std::vector& joint_names, double time_delay); 34 | 35 | /** 36 | * Sends a ROS trajectory to the robot controller 37 | */ 38 | bool executeTrajectory(const trajectory_msgs::JointTrajectory& trajectory); 39 | 40 | int main(int argc, char** argv) 41 | { 42 | // Initialize ROS 43 | ros::init(argc, argv, "descartes_tutorial"); 44 | ros::NodeHandle nh; 45 | 46 | // Required for communication with moveit components 47 | ros::AsyncSpinner spinner (1); 48 | spinner.start(); 49 | 50 | // 1. Define sequence of points 51 | TrajectoryVec points; 52 | for (unsigned int i = 0; i < 10; ++i) 53 | { 54 | Eigen::Affine3d pose; 55 | pose = Eigen::Translation3d(0.0, 0.0, 1.0 + 0.05 * i); 56 | descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose); 57 | points.push_back(pt); 58 | } 59 | 60 | for (unsigned int i = 0; i < 5; ++i) 61 | { 62 | Eigen::Affine3d pose; 63 | pose = Eigen::Translation3d(0.0, 0.04 * i, 1.3); 64 | descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose); 65 | points.push_back(pt); 66 | } 67 | 68 | // 2. Create a robot model and initialize it 69 | descartes_core::RobotModelPtr model (new descartes_moveit::MoveitStateAdapter); 70 | 71 | // Name of description on parameter server. Typically just "robot_description". 72 | const std::string robot_description = "robot_description"; 73 | 74 | // name of the kinematic group you defined when running MoveitSetupAssistant 75 | const std::string group_name = "manipulator"; 76 | 77 | // Name of frame in which you are expressing poses. Typically "world_frame" or "base_link". 78 | const std::string world_frame = "base_link"; 79 | 80 | // tool center point frame (name of link associated with tool) 81 | const std::string tcp_frame = "tool0"; 82 | 83 | if (!model->initialize(robot_description, group_name, world_frame, tcp_frame)) 84 | { 85 | ROS_INFO("Could not initialize robot model"); 86 | return -1; 87 | } 88 | 89 | // 3. Create a planner and initialize it with our robot model 90 | descartes_planner::DensePlanner planner; 91 | planner.initialize(model); 92 | 93 | // 4. Feed the trajectory to the planner 94 | if (!planner.planPath(points)) 95 | { 96 | ROS_ERROR("Could not solve for a valid path"); 97 | return -2; 98 | } 99 | 100 | TrajectoryVec result; 101 | if (!planner.getPath(result)) 102 | { 103 | ROS_ERROR("Could not retrieve path"); 104 | return -3; 105 | } 106 | 107 | // 5. Translate the result into a type that ROS understands 108 | // Get Joint Names 109 | std::vector names; 110 | nh.getParam("controller_joint_names", names); 111 | // Generate a ROS joint trajectory with the result path, robot model, given joint names, 112 | // a certain time delta between each trajectory point 113 | trajectory_msgs::JointTrajectory joint_solution = toROSJointTrajectory(result, *model, names, 1.0); 114 | 115 | // 6. Send the ROS trajectory to the robot for execution 116 | if (!executeTrajectory(joint_solution)) 117 | { 118 | ROS_ERROR("Could not execute trajectory!"); 119 | return -4; 120 | } 121 | 122 | // Wait till user kills the process (Control-C) 123 | ROS_INFO("Done!"); 124 | return 0; 125 | } 126 | 127 | descartes_core::TrajectoryPtPtr makeCartesianPoint(const Eigen::Affine3d& pose) 128 | { 129 | using namespace descartes_core; 130 | using namespace descartes_trajectory; 131 | 132 | return TrajectoryPtPtr( new CartTrajectoryPt( TolerancedFrame(pose)) ); 133 | } 134 | 135 | descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(const Eigen::Affine3d& pose) 136 | { 137 | using namespace descartes_core; 138 | using namespace descartes_trajectory; 139 | return TrajectoryPtPtr( new AxialSymmetricPt(pose, M_PI/2.0-0.0001, AxialSymmetricPt::Z_AXIS) ); 140 | } 141 | 142 | trajectory_msgs::JointTrajectory 143 | toROSJointTrajectory(const TrajectoryVec& trajectory, 144 | const descartes_core::RobotModel& model, 145 | const std::vector& joint_names, 146 | double time_delay) 147 | { 148 | // Fill out information about our trajectory 149 | trajectory_msgs::JointTrajectory result; 150 | result.header.stamp = ros::Time::now(); 151 | result.header.frame_id = "world_frame"; 152 | result.joint_names = joint_names; 153 | 154 | // For keeping track of time-so-far in the trajectory 155 | double time_offset = 0.0; 156 | // Loop through the trajectory 157 | for (TrajectoryIter it = trajectory.begin(); it != trajectory.end(); ++it) 158 | { 159 | // Find nominal joint solution at this point 160 | std::vector joints; 161 | it->get()->getNominalJointPose(std::vector(), model, joints); 162 | 163 | // Fill out a ROS trajectory point 164 | trajectory_msgs::JointTrajectoryPoint pt; 165 | pt.positions = joints; 166 | // velocity, acceleration, and effort are given dummy values 167 | // we'll let the controller figure them out 168 | pt.velocities.resize(joints.size(), 0.0); 169 | pt.accelerations.resize(joints.size(), 0.0); 170 | pt.effort.resize(joints.size(), 0.0); 171 | // set the time into the trajectory 172 | pt.time_from_start = ros::Duration(time_offset); 173 | // increment time 174 | time_offset += time_delay; 175 | 176 | result.points.push_back(pt); 177 | } 178 | 179 | return result; 180 | } 181 | 182 | bool executeTrajectory(const trajectory_msgs::JointTrajectory& trajectory) 183 | { 184 | // Create a Follow Joint Trajectory Action Client 185 | actionlib::SimpleActionClient ac("joint_trajectory_action", true); 186 | if (!ac.waitForServer(ros::Duration(2.0))) 187 | { 188 | ROS_ERROR("Could not connect to action server"); 189 | return false; 190 | } 191 | 192 | control_msgs::FollowJointTrajectoryGoal goal; 193 | goal.trajectory = trajectory; 194 | goal.goal_time_tolerance = ros::Duration(1.0); 195 | 196 | ac.sendGoal(goal); 197 | 198 | if (ac.waitForResult(goal.trajectory.points[goal.trajectory.points.size()-1].time_from_start + ros::Duration(5))) 199 | { 200 | ROS_INFO("Action server reported successful execution"); 201 | return true; 202 | } else { 203 | ROS_WARN("Action server could not execute trajectory"); 204 | return false; 205 | } 206 | } -------------------------------------------------------------------------------- /descartes_tutorials/meshes/profile.stl: -------------------------------------------------------------------------------- 1 | solid OpenSCAD_Model 2 | facet normal 2.22045e-16 -4.93038e-32 -1 3 | outer loop 4 | vertex 0 0 0 5 | vertex 3.33067e-14 150 0 6 | vertex 400 -8.88178e-14 8.88178e-14 7 | endloop 8 | endfacet 9 | facet normal 2.22045e-16 -4.93038e-32 -1 10 | outer loop 11 | vertex 400 -8.88178e-14 8.88178e-14 12 | vertex 3.33067e-14 150 0 13 | vertex 400 150 8.88178e-14 14 | endloop 15 | endfacet 16 | facet normal -1 2.22045e-16 -2.22045e-16 17 | outer loop 18 | vertex 3.15303e-14 150 8 19 | vertex 3.33067e-14 150 0 20 | vertex 1.77636e-15 16 8 21 | endloop 22 | endfacet 23 | facet normal -1 2.22045e-16 -2.22045e-16 24 | outer loop 25 | vertex 0 0 0 26 | vertex -3.33067e-14 0 150 27 | vertex -1.77636e-15 8 16 28 | endloop 29 | endfacet 30 | facet normal -1 2.22045e-16 -2.22045e-16 31 | outer loop 32 | vertex 0 0 0 33 | vertex -1.77636e-15 8 16 34 | vertex 1.77636e-15 16 8 35 | endloop 36 | endfacet 37 | facet normal -1 2.22045e-16 -2.22045e-16 38 | outer loop 39 | vertex 0 0 0 40 | vertex 1.77636e-15 16 8 41 | vertex 3.33067e-14 150 0 42 | endloop 43 | endfacet 44 | facet normal -1 2.22045e-16 -2.22045e-16 45 | outer loop 46 | vertex -1.77636e-15 8 16 47 | vertex -3.33067e-14 0 150 48 | vertex -3.15303e-14 8 150 49 | endloop 50 | endfacet 51 | facet normal -2.22045e-16 -1 -4.93038e-32 52 | outer loop 53 | vertex -3.33067e-14 0 150 54 | vertex 0 0 0 55 | vertex 400 -8.88178e-14 8.88178e-14 56 | endloop 57 | endfacet 58 | facet normal -2.22045e-16 -1 -4.93038e-32 59 | outer loop 60 | vertex 400 -8.88178e-14 150 61 | vertex -3.33067e-14 0 150 62 | vertex 400 -8.88178e-14 8.88178e-14 63 | endloop 64 | endfacet 65 | facet normal 1 -2.22045e-16 2.22045e-16 66 | outer loop 67 | vertex 400 16 8 68 | vertex 400 -8.88178e-14 8.88178e-14 69 | vertex 400 150 8.88178e-14 70 | endloop 71 | endfacet 72 | facet normal 1 -2.22045e-16 2.22045e-16 73 | outer loop 74 | vertex 400 8 16 75 | vertex 400 -8.88178e-14 8.88178e-14 76 | vertex 400 16 8 77 | endloop 78 | endfacet 79 | facet normal 1 -2.22045e-16 2.22045e-16 80 | outer loop 81 | vertex 400 -8.88178e-14 150 82 | vertex 400 -8.88178e-14 8.88178e-14 83 | vertex 400 8 16 84 | endloop 85 | endfacet 86 | facet normal 1 -2.22045e-16 2.22045e-16 87 | outer loop 88 | vertex 400 150 8 89 | vertex 400 16 8 90 | vertex 400 150 8.88178e-14 91 | endloop 92 | endfacet 93 | facet normal 1 -2.22045e-16 2.22045e-16 94 | outer loop 95 | vertex 400 8 16 96 | vertex 400 8 150 97 | vertex 400 -8.88178e-14 150 98 | endloop 99 | endfacet 100 | facet normal 2.22045e-16 1 4.93038e-32 101 | outer loop 102 | vertex 3.33067e-14 150 0 103 | vertex 3.15303e-14 150 8 104 | vertex 400 150 8.88178e-14 105 | endloop 106 | endfacet 107 | facet normal 2.22045e-16 1 4.93038e-32 108 | outer loop 109 | vertex 400 150 8.88178e-14 110 | vertex 3.15303e-14 150 8 111 | vertex 150 150 8 112 | endloop 113 | endfacet 114 | facet normal 2.22045e-16 1 4.93038e-32 115 | outer loop 116 | vertex 165 150 8 117 | vertex 150 150 8 118 | vertex 150 150 83 119 | endloop 120 | endfacet 121 | facet normal 2.22045e-16 1 4.93038e-32 122 | outer loop 123 | vertex 400 150 8.88178e-14 124 | vertex 150 150 8 125 | vertex 165 150 8 126 | endloop 127 | endfacet 128 | facet normal 2.22045e-16 1 4.93038e-32 129 | outer loop 130 | vertex 400 150 8 131 | vertex 400 150 8.88178e-14 132 | vertex 165 150 8 133 | endloop 134 | endfacet 135 | facet normal 2.22045e-16 1 4.93038e-32 136 | outer loop 137 | vertex 165 150 8 138 | vertex 150 150 83 139 | vertex 165 150 83 140 | endloop 141 | endfacet 142 | facet normal -2.22045e-16 4.93038e-32 1 143 | outer loop 144 | vertex 165 75 8 145 | vertex 1.77636e-15 16 8 146 | vertex 400 16 8 147 | endloop 148 | endfacet 149 | facet normal -2.22045e-16 4.93038e-32 1 150 | outer loop 151 | vertex 150 75 8 152 | vertex 150 150 8 153 | vertex 3.15303e-14 150 8 154 | endloop 155 | endfacet 156 | facet normal -2.22045e-16 4.93038e-32 1 157 | outer loop 158 | vertex 400 150 8 159 | vertex 165 150 8 160 | vertex 400 16 8 161 | endloop 162 | endfacet 163 | facet normal -2.22045e-16 4.93038e-32 1 164 | outer loop 165 | vertex 150 75 8 166 | vertex 1.77636e-15 16 8 167 | vertex 165 75 8 168 | endloop 169 | endfacet 170 | facet normal -2.22045e-16 4.93038e-32 1 171 | outer loop 172 | vertex 3.15303e-14 150 8 173 | vertex 1.77636e-15 16 8 174 | vertex 150 75 8 175 | endloop 176 | endfacet 177 | facet normal -2.22045e-16 4.93038e-32 1 178 | outer loop 179 | vertex 165 150 8 180 | vertex 165 75 8 181 | vertex 400 16 8 182 | endloop 183 | endfacet 184 | facet normal 0 0.707107 0.707107 185 | outer loop 186 | vertex -1.77636e-15 8 16 187 | vertex 400 8 16 188 | vertex 1.77636e-15 16 8 189 | endloop 190 | endfacet 191 | facet normal 0 0.707107 0.707107 192 | outer loop 193 | vertex 1.77636e-15 16 8 194 | vertex 400 8 16 195 | vertex 400 16 8 196 | endloop 197 | endfacet 198 | facet normal 2.22045e-16 1 4.93038e-32 199 | outer loop 200 | vertex -1.77636e-15 8 16 201 | vertex -3.15303e-14 8 150 202 | vertex 400 8 16 203 | endloop 204 | endfacet 205 | facet normal 2.22045e-16 1 4.93038e-32 206 | outer loop 207 | vertex 400 8 16 208 | vertex -3.15303e-14 8 150 209 | vertex 400 8 150 210 | endloop 211 | endfacet 212 | facet normal -2.22045e-16 4.93038e-32 1 213 | outer loop 214 | vertex -3.15303e-14 8 150 215 | vertex -3.33067e-14 0 150 216 | vertex 400 -8.88178e-14 150 217 | endloop 218 | endfacet 219 | facet normal -2.22045e-16 4.93038e-32 1 220 | outer loop 221 | vertex 400 8 150 222 | vertex -3.15303e-14 8 150 223 | vertex 400 -8.88178e-14 150 224 | endloop 225 | endfacet 226 | facet normal 1 -2.22045e-16 2.22045e-16 227 | outer loop 228 | vertex 165 75 83 229 | vertex 165 75 8 230 | vertex 165 150 8 231 | endloop 232 | endfacet 233 | facet normal 1 -2.22045e-16 2.22045e-16 234 | outer loop 235 | vertex 165 150 83 236 | vertex 165 75 83 237 | vertex 165 150 8 238 | endloop 239 | endfacet 240 | facet normal -2.22045e-16 4.93038e-32 1 241 | outer loop 242 | vertex 150 150 83 243 | vertex 150 75 83 244 | vertex 165 75 83 245 | endloop 246 | endfacet 247 | facet normal -2.22045e-16 4.93038e-32 1 248 | outer loop 249 | vertex 165 150 83 250 | vertex 150 150 83 251 | vertex 165 75 83 252 | endloop 253 | endfacet 254 | facet normal -1 2.22045e-16 -2.22045e-16 255 | outer loop 256 | vertex 150 75 8 257 | vertex 150 75 83 258 | vertex 150 150 8 259 | endloop 260 | endfacet 261 | facet normal -1 2.22045e-16 -2.22045e-16 262 | outer loop 263 | vertex 150 150 8 264 | vertex 150 75 83 265 | vertex 150 150 83 266 | endloop 267 | endfacet 268 | facet normal -2.22045e-16 -1 -4.93038e-32 269 | outer loop 270 | vertex 150 75 83 271 | vertex 150 75 8 272 | vertex 165 75 8 273 | endloop 274 | endfacet 275 | facet normal -2.22045e-16 -1 -4.93038e-32 276 | outer loop 277 | vertex 165 75 83 278 | vertex 150 75 83 279 | vertex 165 75 8 280 | endloop 281 | endfacet 282 | endsolid OpenSCAD_Model 283 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_moveit_config/launch/moveit.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 84 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /MotionPlanning1 8 | - /Axes1 9 | - /Axes2 10 | Splitter Ratio: 0.74256 11 | Tree Height: 525 12 | - Class: rviz/Help 13 | Name: Help 14 | - Class: rviz/Views 15 | Expanded: 16 | - /Current View1 17 | Name: Views 18 | Splitter Ratio: 0.5 19 | Visualization Manager: 20 | Class: "" 21 | Displays: 22 | - Alpha: 0.5 23 | Cell Size: 1 24 | Class: rviz/Grid 25 | Color: 160; 160; 164 26 | Enabled: true 27 | Line Style: 28 | Line Width: 0.03 29 | Value: Lines 30 | Name: Grid 31 | Normal Cell Count: 0 32 | Offset: 33 | X: 0 34 | Y: 0 35 | Z: 0 36 | Plane: XY 37 | Plane Cell Count: 10 38 | Reference Frame: 39 | Value: true 40 | - Class: moveit_rviz_plugin/MotionPlanning 41 | Enabled: true 42 | Move Group Namespace: "" 43 | MoveIt_Goal_Tolerance: 0 44 | MoveIt_Planning_Attempts: 10 45 | MoveIt_Planning_Time: 5 46 | MoveIt_Use_Constraint_Aware_IK: true 47 | MoveIt_Warehouse_Host: 127.0.0.1 48 | MoveIt_Warehouse_Port: 33829 49 | MoveIt_Workspace: 50 | Center: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Size: 55 | X: 2 56 | Y: 2 57 | Z: 2 58 | Name: MotionPlanning 59 | Planned Path: 60 | Color Enabled: false 61 | Interrupt Display: false 62 | Links: 63 | All Links Enabled: true 64 | Expand Joint Details: false 65 | Expand Link Details: false 66 | Expand Tree: false 67 | Link Tree Style: Links in Alphabetic Order 68 | arm_base: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | arm_base_link: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | Value: true 77 | arm_link_1: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | Value: true 82 | arm_link_2: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | Value: true 87 | arm_link_3: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | Value: true 92 | arm_link_4: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | Value: true 97 | arm_link_5: 98 | Alpha: 1 99 | Show Axes: false 100 | Show Trail: false 101 | Value: true 102 | arm_link_6: 103 | Alpha: 1 104 | Show Axes: false 105 | Show Trail: false 106 | Value: true 107 | arm_tool0: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | base_link: 112 | Alpha: 1 113 | Show Axes: false 114 | Show Trail: false 115 | Value: true 116 | tool_tip: 117 | Alpha: 1 118 | Show Axes: false 119 | Show Trail: false 120 | welding_torch: 121 | Alpha: 1 122 | Show Axes: false 123 | Show Trail: false 124 | Value: true 125 | Loop Animation: true 126 | Robot Alpha: 0.5 127 | Robot Color: 150; 50; 150 128 | Show Robot Collision: false 129 | Show Robot Visual: true 130 | Show Trail: false 131 | State Display Time: 0.05 s 132 | Trajectory Topic: move_group/display_planned_path 133 | Planning Metrics: 134 | Payload: 1 135 | Show Joint Torques: false 136 | Show Manipulability: false 137 | Show Manipulability Index: false 138 | Show Weight Limit: false 139 | TextHeight: 0.08 140 | Planning Request: 141 | Colliding Link Color: 255; 0; 0 142 | Goal State Alpha: 1 143 | Goal State Color: 250; 128; 0 144 | Interactive Marker Size: 0 145 | Joint Violation Color: 255; 0; 255 146 | Planning Group: manipulator 147 | Query Goal State: false 148 | Query Start State: false 149 | Show Workspace: false 150 | Start State Alpha: 1 151 | Start State Color: 0; 255; 0 152 | Planning Scene Topic: move_group/monitored_planning_scene 153 | Robot Description: robot_description 154 | Scene Geometry: 155 | Scene Alpha: 1 156 | Scene Color: 50; 230; 50 157 | Scene Display Time: 0.2 158 | Show Scene Geometry: true 159 | Voxel Coloring: Z-Axis 160 | Voxel Rendering: Occupied Voxels 161 | Scene Robot: 162 | Attached Body Color: 150; 50; 150 163 | Links: 164 | All Links Enabled: true 165 | Expand Joint Details: false 166 | Expand Link Details: false 167 | Expand Tree: false 168 | Link Tree Style: Links in Alphabetic Order 169 | arm_base: 170 | Alpha: 1 171 | Show Axes: false 172 | Show Trail: false 173 | arm_base_link: 174 | Alpha: 1 175 | Show Axes: false 176 | Show Trail: false 177 | Value: true 178 | arm_link_1: 179 | Alpha: 1 180 | Show Axes: false 181 | Show Trail: false 182 | Value: true 183 | arm_link_2: 184 | Alpha: 1 185 | Show Axes: false 186 | Show Trail: false 187 | Value: true 188 | arm_link_3: 189 | Alpha: 1 190 | Show Axes: false 191 | Show Trail: false 192 | Value: true 193 | arm_link_4: 194 | Alpha: 1 195 | Show Axes: false 196 | Show Trail: false 197 | Value: true 198 | arm_link_5: 199 | Alpha: 1 200 | Show Axes: false 201 | Show Trail: false 202 | Value: true 203 | arm_link_6: 204 | Alpha: 1 205 | Show Axes: false 206 | Show Trail: false 207 | Value: true 208 | arm_tool0: 209 | Alpha: 1 210 | Show Axes: false 211 | Show Trail: false 212 | base_link: 213 | Alpha: 1 214 | Show Axes: false 215 | Show Trail: false 216 | Value: true 217 | tool_tip: 218 | Alpha: 1 219 | Show Axes: false 220 | Show Trail: false 221 | welding_torch: 222 | Alpha: 1 223 | Show Axes: false 224 | Show Trail: false 225 | Value: true 226 | Robot Alpha: 0.5 227 | Show Robot Collision: false 228 | Show Robot Visual: true 229 | Value: true 230 | - Class: rviz/MarkerArray 231 | Enabled: true 232 | Marker Topic: visualization_marker_array 233 | Name: MarkerArray 234 | Namespaces: 235 | {} 236 | Queue Size: 100 237 | Value: true 238 | - Class: rviz/Axes 239 | Enabled: true 240 | Length: 0.5 241 | Name: Axes 242 | Radius: 0.1 243 | Reference Frame: 244 | Value: true 245 | - Class: rviz/Axes 246 | Enabled: true 247 | Length: 0.2 248 | Name: Axes 249 | Radius: 0.02 250 | Reference Frame: tool_tip 251 | Value: true 252 | Enabled: true 253 | Global Options: 254 | Background Color: 48; 48; 48 255 | Fixed Frame: base_link 256 | Frame Rate: 30 257 | Name: root 258 | Tools: 259 | - Class: rviz/Interact 260 | Hide Inactive Objects: true 261 | - Class: rviz/MoveCamera 262 | - Class: rviz/Select 263 | Value: true 264 | Views: 265 | Current: 266 | Class: rviz/XYOrbit 267 | Distance: 2.37589 268 | Enable Stereo Rendering: 269 | Stereo Eye Separation: 0.06 270 | Stereo Focal Distance: 1 271 | Swap Stereo Eyes: false 272 | Value: false 273 | Focal Point: 274 | X: 0.113567 275 | Y: 0.10592 276 | Z: 2.23518e-07 277 | Name: Current View 278 | Near Clip Distance: 0.01 279 | Pitch: 0.194798 280 | Target Frame: base_link 281 | Value: XYOrbit (rviz) 282 | Yaw: 0.836767 283 | Saved: ~ 284 | Window Geometry: 285 | Displays: 286 | collapsed: false 287 | Height: 744 288 | Help: 289 | collapsed: false 290 | Hide Left Dock: false 291 | Hide Right Dock: false 292 | Motion Planning: 293 | collapsed: false 294 | QMainWindow State: 000000ff00000000fd0000000100000000000002a2000002a2fc0200000005fb000000100044006900730070006c0061007900730100000028000002a2000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e0067000000010b000001cc000001cc00ffffff0000026d000002a200000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 295 | Views: 296 | collapsed: false 297 | Width: 1301 298 | X: 55 299 | Y: 15 300 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_support/urdf/kr120_with_torch.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 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "{}" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright {yyyy} {name of copyright owner} 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | 203 | -------------------------------------------------------------------------------- /descartes_tutorials/src/tutorial2.cpp: -------------------------------------------------------------------------------- 1 | // Core ros functionality like ros::init and spin 2 | #include 3 | // ROS Trajectory Action server definition 4 | #include 5 | // Means by which we communicate with above action-server 6 | #include 7 | 8 | // Includes the descartes robot model we will be using 9 | #include 10 | // Includes the descartes trajectory type we will be using 11 | #include 12 | #include 13 | // Includes the planner we will be using 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | // Include the visualisation message that will be used to 21 | // visualize the trajectory points in RViz 22 | #include 23 | 24 | typedef std::vector TrajectoryVec; 25 | typedef TrajectoryVec::const_iterator TrajectoryIter; 26 | 27 | /** 28 | * Generates an completely defined (zero-tolerance) cartesian point from a pose 29 | */ 30 | descartes_core::TrajectoryPtPtr makeCartesianPoint(const Eigen::Affine3d &pose); 31 | /** 32 | * Generates a cartesian point with free rotation about the Z axis of the EFF frame 33 | */ 34 | descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(const Eigen::Affine3d &pose); 35 | descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(Eigen::Affine3d pose, 36 | double rxTolerance, double ryTolerance, double rzTolerance); 37 | 38 | /** 39 | * Translates a descartes trajectory to a ROS joint trajectory 40 | */ 41 | trajectory_msgs::JointTrajectory 42 | toROSJointTrajectory(const TrajectoryVec &trajectory, const descartes_core::RobotModel &model, 43 | const std::vector &joint_names, double time_delay); 44 | 45 | /** 46 | * Sends a ROS trajectory to the robot controller 47 | */ 48 | bool executeTrajectory(const trajectory_msgs::JointTrajectory &trajectory); 49 | 50 | /** 51 | * Waits for a subscriber to subscribe to a publisher 52 | */ 53 | bool waitForSubscribers(ros::Publisher &pub, ros::Duration timeout); 54 | 55 | /** 56 | * Add the welding object (l-profile) to the planning scene. 57 | * This is put in a function to keep the tutorial more readable. 58 | */ 59 | void addWeldingObject(moveit_msgs::PlanningScene &planningScene); 60 | 61 | /** 62 | * Add the welding table to the planning scene. 63 | * This is put in a function to keep the tutorial more readable. 64 | */ 65 | void addTable(moveit_msgs::PlanningScene &planningScene); 66 | 67 | /********************** 68 | ** MAIN LOOP 69 | **********************/ 70 | int main(int argc, char **argv) 71 | { 72 | // Initialize ROS 73 | ros::init(argc, argv, "descartes_tutorial"); 74 | ros::NodeHandle nh; 75 | 76 | // Required for communication with moveit components 77 | ros::AsyncSpinner spinner(1); 78 | spinner.start(); 79 | 80 | ros::Rate loop_rate(10); 81 | 82 | // 0. Add objects to planning scene 83 | moveit_msgs::PlanningScene planning_scene; 84 | 85 | addTable(planning_scene); 86 | addWeldingObject(planning_scene); 87 | 88 | ros::Publisher scene_diff_publisher; 89 | scene_diff_publisher = nh.advertise("planning_scene", 1); 90 | 91 | planning_scene.is_diff = true; 92 | 93 | ROS_INFO("Waiting for planning_scene subscriber."); 94 | if (waitForSubscribers(scene_diff_publisher, ros::Duration(2.0))) 95 | { 96 | scene_diff_publisher.publish(planning_scene); 97 | ros::spinOnce(); 98 | loop_rate.sleep(); 99 | ROS_INFO("Object added to the world."); 100 | } 101 | else 102 | { 103 | ROS_ERROR("No subscribers connected, collision object not added"); 104 | } 105 | 106 | // 1. Define sequence of points 107 | double x, y, z, rx, ry, rz; 108 | x = 2.0 - 0.025; 109 | y = 0.0; 110 | z = 0.008 + 0.025; 111 | rx = 0.0; 112 | ry = (M_PI / 2) + M_PI / 4; 113 | rz = 0.0; 114 | TrajectoryVec points; 115 | int N_points = 30; 116 | 117 | std::vector poses; 118 | Eigen::Affine3d startPose; 119 | Eigen::Affine3d endPose; 120 | startPose = descartes_core::utils::toFrame(x, y, z, rx, ry, rz, descartes_core::utils::EulerConventions::XYZ); 121 | endPose = descartes_core::utils::toFrame(x, y + 0.4, z, rx, ry, rz, descartes_core::utils::EulerConventions::XYZ); 122 | poses = tutorial_utilities::line(startPose, endPose, N_points); 123 | 124 | double rx_tol, ry_tol, rz_tol; 125 | rx_tol = 0.0; 126 | ry_tol = M_PI_4; 127 | rz_tol = M_PI; 128 | 129 | for (unsigned int i = 0; i < N_points; ++i) 130 | { 131 | descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(poses[i], rx_tol, ry_tol, rz_tol); 132 | points.push_back(pt); 133 | } 134 | 135 | // Visualize the trajectory points in RViz 136 | // Transform the generated poses into a markerArray message that can be visualized by RViz 137 | visualization_msgs::MarkerArray ma; 138 | ma = tutorial_utilities::createMarkerArray(poses); 139 | // Start the publisher for the Rviz Markers 140 | ros::Publisher vis_pub = nh.advertise("visualization_marker_array", 1); 141 | 142 | // Wait for subscriber and publish the markerArray once the subscriber is found. 143 | ROS_INFO("Waiting for marker subscribers."); 144 | if (waitForSubscribers(vis_pub, ros::Duration(2.0))) 145 | { 146 | ROS_INFO("Subscriber found, publishing markers."); 147 | vis_pub.publish(ma); 148 | ros::spinOnce(); 149 | loop_rate.sleep(); 150 | } 151 | else 152 | { 153 | ROS_ERROR("No subscribers connected, markers not published"); 154 | } 155 | 156 | // 2. Create a robot model and initialize it 157 | descartes_core::RobotModelPtr model(new descartes_moveit::IkFastMoveitStateAdapter); 158 | 159 | //Enable collision checking 160 | model->setCheckCollisions(true); 161 | 162 | // Name of description on parameter server. Typically just "robot_description". 163 | const std::string robot_description = "robot_description"; 164 | 165 | // name of the kinematic group you defined when running MoveitSetupAssistant 166 | const std::string group_name = "manipulator"; 167 | 168 | // Name of frame in which you are expressing poses. Typically "world_frame" or "base_link". 169 | const std::string world_frame = "base_link"; 170 | 171 | // tool center point frame (name of link associated with tool) 172 | // this is also updated in the launch file of the robot 173 | const std::string tcp_frame = "tool_tip"; 174 | 175 | if (!model->initialize(robot_description, group_name, world_frame, tcp_frame)) 176 | { 177 | ROS_INFO("Could not initialize robot model"); 178 | return -1; 179 | } 180 | 181 | // 3. Create a planner and initialize it with our robot model 182 | descartes_planner::DensePlanner planner; 183 | planner.initialize(model); 184 | 185 | // 4. Feed the trajectory to the planner 186 | if (!planner.planPath(points)) 187 | { 188 | ROS_ERROR("Could not solve for a valid path"); 189 | return -2; 190 | } 191 | 192 | TrajectoryVec result; 193 | if (!planner.getPath(result)) 194 | { 195 | ROS_ERROR("Could not retrieve path"); 196 | return -3; 197 | } 198 | 199 | // 5. Translate the result into a type that ROS understands 200 | // Get Joint Names 201 | std::vector names; 202 | nh.getParam("controller_joint_names", names); 203 | // Generate a ROS joint trajectory with the result path, robot model, given joint names, 204 | // a certain time delta between each trajectory point 205 | trajectory_msgs::JointTrajectory joint_solution = toROSJointTrajectory(result, *model, names, 1.0); 206 | 207 | // 6. Send the ROS trajectory to the robot for execution 208 | if (!executeTrajectory(joint_solution)) 209 | { 210 | ROS_ERROR("Could not execute trajectory!"); 211 | return -4; 212 | } 213 | 214 | // Wait till user kills the process (Control-C) 215 | ROS_INFO("Done!"); 216 | return 0; 217 | } 218 | 219 | descartes_core::TrajectoryPtPtr makeCartesianPoint(const Eigen::Affine3d &pose) 220 | { 221 | using namespace descartes_core; 222 | using namespace descartes_trajectory; 223 | 224 | return TrajectoryPtPtr(new CartTrajectoryPt(TolerancedFrame(pose))); 225 | } 226 | 227 | descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(const Eigen::Affine3d &pose) 228 | { 229 | using namespace descartes_core; 230 | using namespace descartes_trajectory; 231 | return TrajectoryPtPtr(new AxialSymmetricPt(pose, M_PI / 2.0 - 0.0001, AxialSymmetricPt::Z_AXIS)); 232 | } 233 | 234 | descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(Eigen::Affine3d pose, 235 | double rxTolerance, double ryTolerance, double rzTolerance) 236 | { 237 | using namespace descartes_core; 238 | using namespace descartes_trajectory; 239 | 240 | double rotStepSize = 0.1; //M_PI/180; 241 | 242 | Eigen::Vector3d translations; 243 | translations = pose.translation(); 244 | Eigen::Vector3d eulerXYZ; 245 | eulerXYZ = pose.rotation().eulerAngles(0, 1, 2); 246 | 247 | PositionTolerance p; 248 | p = ToleranceBase::zeroTolerance(translations(0), translations(1), translations(2)); 249 | OrientationTolerance o; 250 | o = ToleranceBase::createSymmetric(eulerXYZ(0), eulerXYZ(1), eulerXYZ(2), rxTolerance, ryTolerance, rzTolerance); 251 | return TrajectoryPtPtr(new CartTrajectoryPt(TolerancedFrame(pose, p, o), 0.0, rotStepSize)); 252 | } 253 | 254 | trajectory_msgs::JointTrajectory 255 | toROSJointTrajectory(const TrajectoryVec &trajectory, 256 | const descartes_core::RobotModel &model, 257 | const std::vector &joint_names, 258 | double time_delay) 259 | { 260 | // Fill out information about our trajectory 261 | trajectory_msgs::JointTrajectory result; 262 | result.header.stamp = ros::Time::now(); 263 | result.header.frame_id = "world_frame"; 264 | result.joint_names = joint_names; 265 | 266 | // For keeping track of time-so-far in the trajectory 267 | double time_offset = 0.0; 268 | // Loop through the trajectory 269 | for (TrajectoryIter it = trajectory.begin(); it != trajectory.end(); ++it) 270 | { 271 | // Find nominal joint solution at this point 272 | std::vector joints; 273 | it->get()->getNominalJointPose(std::vector(), model, joints); 274 | 275 | // Fill out a ROS trajectory point 276 | trajectory_msgs::JointTrajectoryPoint pt; 277 | pt.positions = joints; 278 | // velocity, acceleration, and effort are given dummy values 279 | // we'll let the controller figure them out 280 | pt.velocities.resize(joints.size(), 0.0); 281 | pt.accelerations.resize(joints.size(), 0.0); 282 | pt.effort.resize(joints.size(), 0.0); 283 | // set the time into the trajectory 284 | pt.time_from_start = ros::Duration(time_offset); 285 | // increment time 286 | time_offset += time_delay; 287 | 288 | result.points.push_back(pt); 289 | } 290 | 291 | return result; 292 | } 293 | 294 | bool executeTrajectory(const trajectory_msgs::JointTrajectory &trajectory) 295 | { 296 | // Create a Follow Joint Trajectory Action Client 297 | actionlib::SimpleActionClient ac("joint_trajectory_action", true); 298 | if (!ac.waitForServer(ros::Duration(2.0))) 299 | { 300 | ROS_ERROR("Could not connect to action server"); 301 | return false; 302 | } 303 | 304 | control_msgs::FollowJointTrajectoryGoal goal; 305 | goal.trajectory = trajectory; 306 | goal.goal_time_tolerance = ros::Duration(1.0); 307 | 308 | ac.sendGoal(goal); 309 | 310 | if (ac.waitForResult(goal.trajectory.points[goal.trajectory.points.size() - 1].time_from_start + ros::Duration(5))) 311 | { 312 | ROS_INFO("Action server reported successful execution"); 313 | return true; 314 | } 315 | else 316 | { 317 | ROS_WARN("Action server could not execute trajectory"); 318 | return false; 319 | } 320 | } 321 | 322 | bool waitForSubscribers(ros::Publisher &pub, ros::Duration timeout) 323 | { 324 | if (pub.getNumSubscribers() > 0) 325 | return true; 326 | ros::Time start = ros::Time::now(); 327 | ros::Rate waitTime(0.5); 328 | while (ros::Time::now() - start < timeout) 329 | { 330 | waitTime.sleep(); 331 | if (pub.getNumSubscribers() > 0) 332 | break; 333 | } 334 | return pub.getNumSubscribers() > 0; 335 | } 336 | 337 | void addWeldingObject(moveit_msgs::PlanningScene &scene) 338 | { 339 | Eigen::Vector3d scale(0.001, 0.001, 0.001); 340 | Eigen::Affine3d pose; 341 | pose = descartes_core::utils::toFrame(2.0, 0.0, 0.012, 0.0, 0.0, M_PI_2, descartes_core::utils::EulerConventions::XYZ); 342 | //ros::package::getPath('descartes_tutorials') 343 | scene.world.collision_objects.push_back( 344 | tutorial_utilities::makeCollisionObject("package://descartes_tutorials/meshes/profile.stl", scale, "Profile", pose)); 345 | scene.object_colors.push_back(tutorial_utilities::makeObjectColor("Profile", 0.5, 0.5, 0.5, 1.0)); 346 | } 347 | 348 | void addTable(moveit_msgs::PlanningScene &scene) 349 | { 350 | Eigen::Vector3d scale(1.0, 1.0, 1.0); 351 | Eigen::Affine3d pose; 352 | pose = descartes_core::utils::toFrame(1.5, -0.6, 0.0, 0.0, 0.0, 0.0, descartes_core::utils::EulerConventions::XYZ); 353 | scene.world.collision_objects.push_back( 354 | tutorial_utilities::makeCollisionObject("package://descartes_tutorials/meshes/table.stl", scale, "Table", pose)); 355 | scene.object_colors.push_back(tutorial_utilities::makeObjectColor("Table", 0.2, 0.2, 0.2, 1.0)); 356 | } -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_ikfast/include/ikfast.h: -------------------------------------------------------------------------------- 1 | // -*- coding: utf-8 -*- 2 | // Copyright (C) 2012 Rosen Diankov 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /** \brief Header file for all ikfast c++ files/shared objects. 15 | 16 | The ikfast inverse kinematics compiler is part of OpenRAVE. 17 | 18 | The file is divided into two sections: 19 | - Common - the abstract classes section that all ikfast share regardless of their settings 20 | - Library Specific - the library-specific definitions, which depends on the precision/settings that the library was compiled with 21 | 22 | The defines are as follows, they are also used for the ikfast C++ class: 23 | 24 | - IKFAST_HEADER_COMMON - common classes 25 | - IKFAST_HAS_LIBRARY - if defined, will include library-specific functions. by default this is off 26 | - IKFAST_CLIBRARY - Define this linking statically or dynamically to get correct visibility. 27 | - IKFAST_NO_MAIN - Remove the ``main`` function, usually used with IKFAST_CLIBRARY 28 | - IKFAST_ASSERT - Define in order to get a custom assert called when NaNs, divides by zero, and other invalid conditions are detected. 29 | - IKFAST_REAL - Use to force a custom real number type for IkReal. 30 | - IKFAST_NAMESPACE - Enclose all functions and classes in this namespace, the ``main`` function is excluded. 31 | 32 | */ 33 | #include 34 | #include 35 | #include 36 | 37 | #ifndef IKFAST_HEADER_COMMON 38 | #define IKFAST_HEADER_COMMON 39 | 40 | /// should be the same as ikfast.__version__ 41 | #define IKFAST_VERSION 61 42 | 43 | namespace ikfast { 44 | 45 | /// \brief holds the solution for a single dof 46 | template 47 | class IkSingleDOFSolutionBase 48 | { 49 | public: 50 | IkSingleDOFSolutionBase() : fmul(0), foffset(0), freeind(-1), maxsolutions(1) { 51 | indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1; 52 | } 53 | T fmul, foffset; ///< joint value is fmul*sol[freeind]+foffset 54 | signed char freeind; ///< if >= 0, mimics another joint 55 | unsigned char jointtype; ///< joint type, 0x01 is revolute, 0x11 is slider 56 | unsigned char maxsolutions; ///< max possible indices, 0 if controlled by free index or a free joint itself 57 | unsigned char indices[5]; ///< unique index of the solution used to keep track on what part it came from. sometimes a solution can be repeated for different indices. store at least another repeated root 58 | }; 59 | 60 | /// \brief The discrete solutions are returned in this structure. 61 | /// 62 | /// Sometimes the joint axes of the robot can align allowing an infinite number of solutions. 63 | /// Stores all these solutions in the form of free variables that the user has to set when querying the solution. Its prototype is: 64 | template 65 | class IkSolutionBase 66 | { 67 | public: 68 | virtual ~IkSolutionBase() { 69 | } 70 | /// \brief gets a concrete solution 71 | /// 72 | /// \param[out] solution the result 73 | /// \param[in] freevalues values for the free parameters \se GetFree 74 | virtual void GetSolution(T* solution, const T* freevalues) const = 0; 75 | 76 | /// \brief std::vector version of \ref GetSolution 77 | virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const { 78 | solution.resize(GetDOF()); 79 | GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); 80 | } 81 | 82 | /// \brief Gets the indices of the configuration space that have to be preset before a full solution can be returned 83 | /// 84 | /// \return vector of indices indicating the free parameters 85 | virtual const std::vector& GetFree() const = 0; 86 | 87 | /// \brief the dof of the solution 88 | virtual const int GetDOF() const = 0; 89 | }; 90 | 91 | /// \brief manages all the solutions 92 | template 93 | class IkSolutionListBase 94 | { 95 | public: 96 | virtual ~IkSolutionListBase() { 97 | } 98 | 99 | /// \brief add one solution and return its index for later retrieval 100 | /// 101 | /// \param vinfos Solution data for each degree of freedom of the manipulator 102 | /// \param vfree If the solution represents an infinite space, holds free parameters of the solution that users can freely set. 103 | virtual size_t AddSolution(const std::vector >& vinfos, const std::vector& vfree) = 0; 104 | 105 | /// \brief returns the solution pointer 106 | virtual const IkSolutionBase& GetSolution(size_t index) const = 0; 107 | 108 | /// \brief returns the number of solutions stored 109 | virtual size_t GetNumSolutions() const = 0; 110 | 111 | /// \brief clears all current solutions, note that any memory addresses returned from \ref GetSolution will be invalidated. 112 | virtual void Clear() = 0; 113 | }; 114 | 115 | /// \brief holds function pointers for all the exported functions of ikfast 116 | template 117 | class IkFastFunctions 118 | { 119 | public: 120 | IkFastFunctions() : _ComputeIk(NULL), _ComputeFk(NULL), _GetNumFreeParameters(NULL), _GetFreeParameters(NULL), _GetNumJoints(NULL), _GetIkRealSize(NULL), _GetIkFastVersion(NULL), _GetIkType(NULL), _GetKinematicsHash(NULL) { 121 | } 122 | virtual ~IkFastFunctions() { 123 | } 124 | typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase&); 125 | ComputeIkFn _ComputeIk; 126 | typedef void (*ComputeFkFn)(const T*, T*, T*); 127 | ComputeFkFn _ComputeFk; 128 | typedef int (*GetNumFreeParametersFn)(); 129 | GetNumFreeParametersFn _GetNumFreeParameters; 130 | typedef int* (*GetFreeParametersFn)(); 131 | GetFreeParametersFn _GetFreeParameters; 132 | typedef int (*GetNumJointsFn)(); 133 | GetNumJointsFn _GetNumJoints; 134 | typedef int (*GetIkRealSizeFn)(); 135 | GetIkRealSizeFn _GetIkRealSize; 136 | typedef const char* (*GetIkFastVersionFn)(); 137 | GetIkFastVersionFn _GetIkFastVersion; 138 | typedef int (*GetIkTypeFn)(); 139 | GetIkTypeFn _GetIkType; 140 | typedef const char* (*GetKinematicsHashFn)(); 141 | GetKinematicsHashFn _GetKinematicsHash; 142 | }; 143 | 144 | // Implementations of the abstract classes, user doesn't need to use them 145 | 146 | /// \brief Default implementation of \ref IkSolutionBase 147 | template 148 | class IkSolution : public IkSolutionBase 149 | { 150 | public: 151 | IkSolution(const std::vector >& vinfos, const std::vector& vfree) { 152 | _vbasesol = vinfos; 153 | _vfree = vfree; 154 | } 155 | 156 | virtual void GetSolution(T* solution, const T* freevalues) const { 157 | for(std::size_t i = 0; i < _vbasesol.size(); ++i) { 158 | if( _vbasesol[i].freeind < 0 ) 159 | solution[i] = _vbasesol[i].foffset; 160 | else { 161 | solution[i] = freevalues[_vbasesol[i].freeind]*_vbasesol[i].fmul + _vbasesol[i].foffset; 162 | if( solution[i] > T(3.14159265358979) ) { 163 | solution[i] -= T(6.28318530717959); 164 | } 165 | else if( solution[i] < T(-3.14159265358979) ) { 166 | solution[i] += T(6.28318530717959); 167 | } 168 | } 169 | } 170 | } 171 | 172 | virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const { 173 | solution.resize(GetDOF()); 174 | GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); 175 | } 176 | 177 | virtual const std::vector& GetFree() const { 178 | return _vfree; 179 | } 180 | virtual const int GetDOF() const { 181 | return static_cast(_vbasesol.size()); 182 | } 183 | 184 | virtual void Validate() const { 185 | for(size_t i = 0; i < _vbasesol.size(); ++i) { 186 | if( _vbasesol[i].maxsolutions == (unsigned char)-1) { 187 | throw std::runtime_error("max solutions for joint not initialized"); 188 | } 189 | if( _vbasesol[i].maxsolutions > 0 ) { 190 | if( _vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions ) { 191 | throw std::runtime_error("index >= max solutions for joint"); 192 | } 193 | if( _vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions ) { 194 | throw std::runtime_error("2nd index >= max solutions for joint"); 195 | } 196 | } 197 | } 198 | } 199 | 200 | virtual void GetSolutionIndices(std::vector& v) const { 201 | v.resize(0); 202 | v.push_back(0); 203 | for(int i = (int)_vbasesol.size()-1; i >= 0; --i) { 204 | if( _vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1 ) { 205 | for(size_t j = 0; j < v.size(); ++j) { 206 | v[j] *= _vbasesol[i].maxsolutions; 207 | } 208 | size_t orgsize=v.size(); 209 | if( _vbasesol[i].indices[1] != (unsigned char)-1 ) { 210 | for(size_t j = 0; j < orgsize; ++j) { 211 | v.push_back(v[j]+_vbasesol[i].indices[1]); 212 | } 213 | } 214 | if( _vbasesol[i].indices[0] != (unsigned char)-1 ) { 215 | for(size_t j = 0; j < orgsize; ++j) { 216 | v[j] += _vbasesol[i].indices[0]; 217 | } 218 | } 219 | } 220 | } 221 | } 222 | 223 | std::vector< IkSingleDOFSolutionBase > _vbasesol; ///< solution and their offsets if joints are mimiced 224 | std::vector _vfree; 225 | }; 226 | 227 | /// \brief Default implementation of \ref IkSolutionListBase 228 | template 229 | class IkSolutionList : public IkSolutionListBase 230 | { 231 | public: 232 | virtual size_t AddSolution(const std::vector >& vinfos, const std::vector& vfree) 233 | { 234 | size_t index = _listsolutions.size(); 235 | _listsolutions.push_back(IkSolution(vinfos,vfree)); 236 | return index; 237 | } 238 | 239 | virtual const IkSolutionBase& GetSolution(size_t index) const 240 | { 241 | if( index >= _listsolutions.size() ) { 242 | throw std::runtime_error("GetSolution index is invalid"); 243 | } 244 | typename std::list< IkSolution >::const_iterator it = _listsolutions.begin(); 245 | std::advance(it,index); 246 | return *it; 247 | } 248 | 249 | virtual size_t GetNumSolutions() const { 250 | return _listsolutions.size(); 251 | } 252 | 253 | virtual void Clear() { 254 | _listsolutions.clear(); 255 | } 256 | 257 | protected: 258 | std::list< IkSolution > _listsolutions; 259 | }; 260 | 261 | } 262 | 263 | #endif // OPENRAVE_IKFAST_HEADER 264 | 265 | // The following code is dependent on the C++ library linking with. 266 | #ifdef IKFAST_HAS_LIBRARY 267 | 268 | // defined when creating a shared object/dll 269 | #ifdef IKFAST_CLIBRARY 270 | #ifdef _MSC_VER 271 | #define IKFAST_API extern "C" __declspec(dllexport) 272 | #else 273 | #define IKFAST_API extern "C" 274 | #endif 275 | #else 276 | #define IKFAST_API 277 | #endif 278 | 279 | #ifdef IKFAST_NAMESPACE 280 | namespace IKFAST_NAMESPACE { 281 | #endif 282 | 283 | #ifdef IKFAST_REAL 284 | typedef IKFAST_REAL IkReal; 285 | #else 286 | typedef double IkReal; 287 | #endif 288 | 289 | /** \brief Computes all IK solutions given a end effector coordinates and the free joints. 290 | 291 | - ``eetrans`` - 3 translation values. For iktype **TranslationXYOrientation3D**, the z-axis is the orientation. 292 | - ``eerot`` 293 | - For **Transform6D** it is 9 values for the 3x3 rotation matrix. 294 | - For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target direction. 295 | - For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D** the first value represents the angle. 296 | - For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end effector coordinate system. 297 | */ 298 | IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, ikfast::IkSolutionListBase& solutions); 299 | 300 | /// \brief Computes the end effector coordinates given the joint values. This function is used to double check ik. 301 | IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot); 302 | 303 | /// \brief returns the number of free parameters users has to set apriori 304 | IKFAST_API int GetNumFreeParameters(); 305 | 306 | /// \brief the indices of the free parameters indexed by the chain joints 307 | IKFAST_API int* GetFreeParameters(); 308 | 309 | /// \brief the total number of indices of the chain 310 | IKFAST_API int GetNumJoints(); 311 | 312 | /// \brief the size in bytes of the configured number type 313 | IKFAST_API int GetIkRealSize(); 314 | 315 | /// \brief the ikfast version used to generate this file 316 | IKFAST_API const char* GetIkFastVersion(); 317 | 318 | /// \brief the ik type ID 319 | IKFAST_API int GetIkType(); 320 | 321 | /// \brief a hash of all the chain values used for double checking that the correct IK is used. 322 | IKFAST_API const char* GetKinematicsHash(); 323 | 324 | #ifdef IKFAST_NAMESPACE 325 | } 326 | #endif 327 | 328 | #endif // IKFAST_HAS_LIBRARY 329 | -------------------------------------------------------------------------------- /kuka_kr120_with_torch/kuka_kr120_with_torch_ikfast/src/kr120_with_torch_manipulator_ikfast_moveit_plugin.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2013, Dave Coleman, CU Boulder; Jeremy Zoss, SwRI; David Butterworth, KAIST; Mathias Lüdtke, Fraunhofer IPA 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in the 16 | * documentation and/or other materials provided with the distribution. 17 | * * Neither the name of the all of the author's companies nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | *********************************************************************/ 34 | 35 | /* 36 | * IKFast Plugin Template for moveit 37 | * 38 | * AUTO-GENERATED by create_ikfast_moveit_plugin.py in arm_kinematics_tools 39 | * You should run create_ikfast_moveit_plugin.py to generate your own plugin. 40 | * 41 | * Creates a kinematics plugin using the output of IKFast from OpenRAVE. 42 | * This plugin and the move_group node can be used as a general 43 | * kinematics service, from within the moveit planning environment, or in 44 | * your own ROS node. 45 | * 46 | */ 47 | 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | // Need a floating point tolerance when checking joint limits, in case the joint starts at limit 54 | const double LIMIT_TOLERANCE = .0000001; 55 | /// \brief Search modes for searchPositionIK(), see there 56 | enum SEARCH_MODE { OPTIMIZE_FREE_JOINT=1, OPTIMIZE_MAX_JOINT=2 }; 57 | 58 | namespace ikfast_kinematics_plugin 59 | { 60 | 61 | #define IKFAST_NO_MAIN // Don't include main() from IKFast 62 | 63 | /// \brief The types of inverse kinematics parameterizations supported. 64 | /// 65 | /// The minimum degree of freedoms required is set in the upper 4 bits of each type. 66 | /// The number of values used to represent the parameterization ( >= dof ) is the next 4 bits. 67 | /// The lower bits contain a unique id of the type. 68 | enum IkParameterizationType { 69 | IKP_None=0, 70 | IKP_Transform6D=0x67000001, ///< end effector reaches desired 6D transformation 71 | IKP_Rotation3D=0x34000002, ///< end effector reaches desired 3D rotation 72 | IKP_Translation3D=0x33000003, ///< end effector origin reaches desired 3D translation 73 | IKP_Direction3D=0x23000004, ///< direction on end effector coordinate system reaches desired direction 74 | IKP_Ray4D=0x46000005, ///< ray on end effector coordinate system reaches desired global ray 75 | IKP_Lookat3D=0x23000006, ///< direction on end effector coordinate system points to desired 3D position 76 | IKP_TranslationDirection5D=0x56000007, ///< end effector origin and direction reaches desired 3D translation and direction. Can be thought of as Ray IK where the origin of the ray must coincide. 77 | IKP_TranslationXY2D=0x22000008, ///< 2D translation along XY plane 78 | IKP_TranslationXYOrientation3D=0x33000009, ///< 2D translation along XY plane and 1D rotation around Z axis. The offset of the rotation is measured starting at +X, so at +X is it 0, at +Y it is pi/2. 79 | IKP_TranslationLocalGlobal6D=0x3600000a, ///< local point on end effector origin reaches desired 3D global point 80 | 81 | IKP_TranslationXAxisAngle4D=0x4400000b, ///< end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with x-axis like a cone, angle is from 0-pi. Axes defined in the manipulator base link's coordinate system) 82 | IKP_TranslationYAxisAngle4D=0x4400000c, ///< end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with y-axis like a cone, angle is from 0-pi. Axes defined in the manipulator base link's coordinate system) 83 | IKP_TranslationZAxisAngle4D=0x4400000d, ///< end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with z-axis like a cone, angle is from 0-pi. Axes are defined in the manipulator base link's coordinate system. 84 | 85 | IKP_TranslationXAxisAngleZNorm4D=0x4400000e, ///< end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to z-axis and be rotated at a certain angle starting from the x-axis (defined in the manipulator base link's coordinate system) 86 | IKP_TranslationYAxisAngleXNorm4D=0x4400000f, ///< end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to x-axis and be rotated at a certain angle starting from the y-axis (defined in the manipulator base link's coordinate system) 87 | IKP_TranslationZAxisAngleYNorm4D=0x44000010, ///< end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to y-axis and be rotated at a certain angle starting from the z-axis (defined in the manipulator base link's coordinate system) 88 | 89 | IKP_NumberOfParameterizations=16, ///< number of parameterizations (does not count IKP_None) 90 | 91 | IKP_VelocityDataBit = 0x00008000, ///< bit is set if the data represents the time-derivate velocity of an IkParameterization 92 | IKP_Transform6DVelocity = IKP_Transform6D|IKP_VelocityDataBit, 93 | IKP_Rotation3DVelocity = IKP_Rotation3D|IKP_VelocityDataBit, 94 | IKP_Translation3DVelocity = IKP_Translation3D|IKP_VelocityDataBit, 95 | IKP_Direction3DVelocity = IKP_Direction3D|IKP_VelocityDataBit, 96 | IKP_Ray4DVelocity = IKP_Ray4D|IKP_VelocityDataBit, 97 | IKP_Lookat3DVelocity = IKP_Lookat3D|IKP_VelocityDataBit, 98 | IKP_TranslationDirection5DVelocity = IKP_TranslationDirection5D|IKP_VelocityDataBit, 99 | IKP_TranslationXY2DVelocity = IKP_TranslationXY2D|IKP_VelocityDataBit, 100 | IKP_TranslationXYOrientation3DVelocity = IKP_TranslationXYOrientation3D|IKP_VelocityDataBit, 101 | IKP_TranslationLocalGlobal6DVelocity = IKP_TranslationLocalGlobal6D|IKP_VelocityDataBit, 102 | IKP_TranslationXAxisAngle4DVelocity = IKP_TranslationXAxisAngle4D|IKP_VelocityDataBit, 103 | IKP_TranslationYAxisAngle4DVelocity = IKP_TranslationYAxisAngle4D|IKP_VelocityDataBit, 104 | IKP_TranslationZAxisAngle4DVelocity = IKP_TranslationZAxisAngle4D|IKP_VelocityDataBit, 105 | IKP_TranslationXAxisAngleZNorm4DVelocity = IKP_TranslationXAxisAngleZNorm4D|IKP_VelocityDataBit, 106 | IKP_TranslationYAxisAngleXNorm4DVelocity = IKP_TranslationYAxisAngleXNorm4D|IKP_VelocityDataBit, 107 | IKP_TranslationZAxisAngleYNorm4DVelocity = IKP_TranslationZAxisAngleYNorm4D|IKP_VelocityDataBit, 108 | 109 | IKP_UniqueIdMask = 0x0000ffff, ///< the mask for the unique ids 110 | IKP_CustomDataBit = 0x00010000, ///< bit is set if the ikparameterization contains custom data, this is only used when serializing the ik parameterizations 111 | }; 112 | 113 | // Code generated by IKFast56/61 114 | #include "kr120_with_torch_manipulator_ikfast_solver.cpp" 115 | 116 | class IKFastKinematicsPlugin : public kinematics::KinematicsBase 117 | { 118 | std::vector joint_names_; 119 | std::vector joint_min_vector_; 120 | std::vector joint_max_vector_; 121 | std::vector joint_has_limits_vector_; 122 | std::vector link_names_; 123 | size_t num_joints_; 124 | std::vector free_params_; 125 | bool active_; // Internal variable that indicates whether solvers are configured and ready 126 | 127 | const std::vector& getJointNames() const { return joint_names_; } 128 | const std::vector& getLinkNames() const { return link_names_; } 129 | 130 | public: 131 | 132 | /** @class 133 | * @brief Interface for an IKFast kinematics plugin 134 | */ 135 | IKFastKinematicsPlugin(): 136 | active_(false) 137 | { 138 | srand( time(NULL) ); 139 | supported_methods_.push_back(kinematics::DiscretizationMethods::NO_DISCRETIZATION); 140 | supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_DISCRETIZED); 141 | supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_RANDOM_SAMPLED); 142 | } 143 | 144 | /** 145 | * @brief Given a desired pose of the end-effector, compute the joint angles to reach it 146 | * @param ik_pose the desired pose of the link 147 | * @param ik_seed_state an initial guess solution for the inverse kinematics 148 | * @param solution the solution vector 149 | * @param error_code an error code that encodes the reason for failure or success 150 | * @return True if a valid solution was found, false otherwise 151 | */ 152 | 153 | // Returns the first IK solution that is within joint limits, this is called by get_ik() service 154 | bool getPositionIK(const geometry_msgs::Pose &ik_pose, 155 | const std::vector &ik_seed_state, 156 | std::vector &solution, 157 | moveit_msgs::MoveItErrorCodes &error_code, 158 | const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; 159 | 160 | /** 161 | * @brief Given a desired pose of the end-effector, compute the set joint angles solutions that are able to reach it. 162 | * 163 | * This is a default implementation that returns only one solution and so its result is equivalent to calling 164 | * 'getPositionIK(...)' with a zero initialized seed. 165 | * 166 | * @param ik_poses The desired pose of each tip link 167 | * @param ik_seed_state an initial guess solution for the inverse kinematics 168 | * @param solutions A vector of vectors where each entry is a valid joint solution 169 | * @param result A struct that reports the results of the query 170 | * @param options An option struct which contains the type of redundancy discretization used. This default 171 | * implementation only supports the KinmaticSearches::NO_DISCRETIZATION method; requesting any 172 | * other will result in failure. 173 | * @return True if a valid set of solutions was found, false otherwise. 174 | */ 175 | bool getPositionIK(const std::vector &ik_poses, 176 | const std::vector &ik_seed_state, 177 | std::vector< std::vector >& solutions, 178 | kinematics::KinematicsResult& result, 179 | const kinematics::KinematicsQueryOptions &options) const; 180 | 181 | /** 182 | * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. 183 | * This particular method is intended for "searching" for a solutions by stepping through the redundancy 184 | * (or other numerical routines). 185 | * @param ik_pose the desired pose of the link 186 | * @param ik_seed_state an initial guess solution for the inverse kinematics 187 | * @return True if a valid solution was found, false otherwise 188 | */ 189 | bool searchPositionIK(const geometry_msgs::Pose &ik_pose, 190 | const std::vector &ik_seed_state, 191 | double timeout, 192 | std::vector &solution, 193 | moveit_msgs::MoveItErrorCodes &error_code, 194 | const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; 195 | 196 | /** 197 | * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. 198 | * This particular method is intended for "searching" for a solutions by stepping through the redundancy 199 | * (or other numerical routines). 200 | * @param ik_pose the desired pose of the link 201 | * @param ik_seed_state an initial guess solution for the inverse kinematics 202 | * @param the distance that the redundancy can be from the current position 203 | * @return True if a valid solution was found, false otherwise 204 | */ 205 | bool searchPositionIK(const geometry_msgs::Pose &ik_pose, 206 | const std::vector &ik_seed_state, 207 | double timeout, 208 | const std::vector &consistency_limits, 209 | std::vector &solution, 210 | moveit_msgs::MoveItErrorCodes &error_code, 211 | const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; 212 | 213 | /** 214 | * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. 215 | * This particular method is intended for "searching" for a solutions by stepping through the redundancy 216 | * (or other numerical routines). 217 | * @param ik_pose the desired pose of the link 218 | * @param ik_seed_state an initial guess solution for the inverse kinematics 219 | * @return True if a valid solution was found, false otherwise 220 | */ 221 | bool searchPositionIK(const geometry_msgs::Pose &ik_pose, 222 | const std::vector &ik_seed_state, 223 | double timeout, 224 | std::vector &solution, 225 | const IKCallbackFn &solution_callback, 226 | moveit_msgs::MoveItErrorCodes &error_code, 227 | const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; 228 | 229 | /** 230 | * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. 231 | * This particular method is intended for "searching" for a solutions by stepping through the redundancy 232 | * (or other numerical routines). The consistency_limit specifies that only certain redundancy positions 233 | * around those specified in the seed state are admissible and need to be searched. 234 | * @param ik_pose the desired pose of the link 235 | * @param ik_seed_state an initial guess solution for the inverse kinematics 236 | * @param consistency_limit the distance that the redundancy can be from the current position 237 | * @return True if a valid solution was found, false otherwise 238 | */ 239 | bool searchPositionIK(const geometry_msgs::Pose &ik_pose, 240 | const std::vector &ik_seed_state, 241 | double timeout, 242 | const std::vector &consistency_limits, 243 | std::vector &solution, 244 | const IKCallbackFn &solution_callback, 245 | moveit_msgs::MoveItErrorCodes &error_code, 246 | const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; 247 | 248 | /** 249 | * @brief Given a set of joint angles and a set of links, compute their pose 250 | * 251 | * @param link_names A set of links for which FK needs to be computed 252 | * @param joint_angles The state for which FK is being computed 253 | * @param poses The resultant set of poses (in the frame returned by getBaseFrame()) 254 | * @return True if a valid solution was found, false otherwise 255 | */ 256 | bool getPositionFK(const std::vector &link_names, 257 | const std::vector &joint_angles, 258 | std::vector &poses) const; 259 | 260 | 261 | /** 262 | * @brief Sets the discretization value for the redundant joint. 263 | * 264 | * Since this ikfast implementation allows for one redundant joint then only the first entry will be in the discretization map will be used. 265 | * Calling this method replaces previous discretization settings. 266 | * 267 | * @param discretization a map of joint indices and discretization value pairs. 268 | */ 269 | void setSearchDiscretization(const std::map& discretization); 270 | 271 | /** 272 | * @brief Overrides the default method to prevent changing the redundant joints 273 | */ 274 | bool setRedundantJoints(const std::vector &redundant_joint_indices); 275 | 276 | 277 | private: 278 | 279 | bool initialize(const std::string &robot_description, 280 | const std::string& group_name, 281 | const std::string& base_name, 282 | const std::string& tip_name, 283 | double search_discretization); 284 | 285 | /** 286 | * @brief Calls the IK solver from IKFast 287 | * @return The number of solutions found 288 | */ 289 | int solve(KDL::Frame &pose_frame, const std::vector &vfree, IkSolutionList &solutions) const; 290 | 291 | /** 292 | * @brief Gets a specific solution from the set 293 | */ 294 | void getSolution(const IkSolutionList &solutions, int i, std::vector& solution) const; 295 | 296 | double harmonize(const std::vector &ik_seed_state, std::vector &solution) const; 297 | //void getOrderedSolutions(const std::vector &ik_seed_state, std::vector >& solslist); 298 | void getClosestSolution(const IkSolutionList &solutions, const std::vector &ik_seed_state, std::vector &solution) const; 299 | void fillFreeParams(int count, int *array); 300 | bool getCount(int &count, const int &max_count, const int &min_count) const; 301 | 302 | /** 303 | * @brief samples the designated redundant joint using the chosen discretization method 304 | * @param method An enumeration flag indicating the discretization method to be used 305 | * @param sampled_joint_vals Sampled joint values for the redundant joint 306 | * @return True if sampling succeeded. 307 | */ 308 | bool sampleRedundantJoint(kinematics::DiscretizationMethod method, std::vector& sampled_joint_vals) const; 309 | 310 | }; // end class 311 | 312 | bool IKFastKinematicsPlugin::initialize(const std::string &robot_description, 313 | const std::string& group_name, 314 | const std::string& base_name, 315 | const std::string& tip_name, 316 | double search_discretization) 317 | { 318 | setValues(robot_description, group_name, base_name, tip_name, search_discretization); 319 | 320 | ros::NodeHandle node_handle("~/"+group_name); 321 | 322 | std::string robot; 323 | node_handle.param("robot",robot,std::string()); 324 | 325 | // IKFast56/61 326 | fillFreeParams( GetNumFreeParameters(), GetFreeParameters() ); 327 | num_joints_ = GetNumJoints(); 328 | 329 | if(free_params_.size() > 1) 330 | { 331 | ROS_FATAL("Only one free joint parameter supported!"); 332 | return false; 333 | } 334 | else if(free_params_.size() == 1) 335 | { 336 | redundant_joint_indices_.clear(); 337 | redundant_joint_indices_.push_back(free_params_[0]); 338 | KinematicsBase::setSearchDiscretization(DEFAULT_SEARCH_DISCRETIZATION); 339 | } 340 | 341 | urdf::Model robot_model; 342 | std::string xml_string; 343 | 344 | std::string urdf_xml,full_urdf_xml; 345 | node_handle.param("urdf_xml",urdf_xml,robot_description); 346 | node_handle.searchParam(urdf_xml,full_urdf_xml); 347 | 348 | ROS_DEBUG_NAMED("ikfast","Reading xml file from parameter server"); 349 | if (!node_handle.getParam(full_urdf_xml, xml_string)) 350 | { 351 | ROS_FATAL_NAMED("ikfast","Could not load the xml from parameter server: %s", urdf_xml.c_str()); 352 | return false; 353 | } 354 | 355 | node_handle.param(full_urdf_xml,xml_string,std::string()); 356 | robot_model.initString(xml_string); 357 | 358 | ROS_DEBUG_STREAM_NAMED("ikfast","Reading joints and links from URDF"); 359 | 360 | boost::shared_ptr link = boost::const_pointer_cast(robot_model.getLink(getTipFrame())); 361 | while(link->name != base_frame_ && joint_names_.size() <= num_joints_) 362 | { 363 | ROS_DEBUG_NAMED("ikfast","Link %s",link->name.c_str()); 364 | link_names_.push_back(link->name); 365 | boost::shared_ptr joint = link->parent_joint; 366 | if(joint) 367 | { 368 | if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) 369 | { 370 | ROS_DEBUG_STREAM_NAMED("ikfast","Adding joint " << joint->name ); 371 | 372 | joint_names_.push_back(joint->name); 373 | float lower, upper; 374 | int hasLimits; 375 | if ( joint->type != urdf::Joint::CONTINUOUS ) 376 | { 377 | if(joint->safety) 378 | { 379 | lower = joint->safety->soft_lower_limit; 380 | upper = joint->safety->soft_upper_limit; 381 | } else { 382 | lower = joint->limits->lower; 383 | upper = joint->limits->upper; 384 | } 385 | hasLimits = 1; 386 | } 387 | else 388 | { 389 | lower = -M_PI; 390 | upper = M_PI; 391 | hasLimits = 0; 392 | } 393 | if(hasLimits) 394 | { 395 | joint_has_limits_vector_.push_back(true); 396 | joint_min_vector_.push_back(lower); 397 | joint_max_vector_.push_back(upper); 398 | } 399 | else 400 | { 401 | joint_has_limits_vector_.push_back(false); 402 | joint_min_vector_.push_back(-M_PI); 403 | joint_max_vector_.push_back(M_PI); 404 | } 405 | } 406 | } else 407 | { 408 | ROS_WARN_NAMED("ikfast","no joint corresponding to %s",link->name.c_str()); 409 | } 410 | link = link->getParent(); 411 | } 412 | 413 | if(joint_names_.size() != num_joints_) 414 | { 415 | ROS_FATAL_STREAM_NAMED("ikfast","Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has " << num_joints_); 416 | return false; 417 | } 418 | 419 | std::reverse(link_names_.begin(),link_names_.end()); 420 | std::reverse(joint_names_.begin(),joint_names_.end()); 421 | std::reverse(joint_min_vector_.begin(),joint_min_vector_.end()); 422 | std::reverse(joint_max_vector_.begin(),joint_max_vector_.end()); 423 | std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end()); 424 | 425 | for(size_t i=0; i & discretization) 433 | { 434 | 435 | if(discretization.empty()) 436 | { 437 | ROS_ERROR("The 'discretization' map is empty"); 438 | return; 439 | } 440 | 441 | if(redundant_joint_indices_.empty()) 442 | { 443 | ROS_ERROR_STREAM("This group's solver doesn't support redundant joints"); 444 | return; 445 | } 446 | 447 | if(discretization.begin()->first != redundant_joint_indices_[0]) 448 | { 449 | std::string redundant_joint = joint_names_[free_params_[0]]; 450 | ROS_ERROR_STREAM("Attempted to discretize a non-redundant joint "<first<<", only joint '"<< 451 | redundant_joint<<"' with index " <second <= 0.0) 456 | { 457 | ROS_ERROR_STREAM("Discretization can not takes values that are <= 0"); 458 | return; 459 | } 460 | 461 | 462 | redundant_joint_discretization_.clear(); 463 | redundant_joint_discretization_[redundant_joint_indices_[0]] = discretization.begin()->second; 464 | } 465 | 466 | bool IKFastKinematicsPlugin::setRedundantJoints(const std::vector &redundant_joint_indices) 467 | { 468 | 469 | ROS_ERROR_STREAM("Changing the redundant joints isn't permitted by this group's solver "); 470 | return false; 471 | } 472 | 473 | int IKFastKinematicsPlugin::solve(KDL::Frame &pose_frame, const std::vector &vfree, IkSolutionList &solutions) const 474 | { 475 | // IKFast56/61 476 | solutions.Clear(); 477 | 478 | double trans[3]; 479 | trans[0] = pose_frame.p[0];//-.18; 480 | trans[1] = pose_frame.p[1]; 481 | trans[2] = pose_frame.p[2]; 482 | 483 | KDL::Rotation mult; 484 | KDL::Vector direction; 485 | 486 | switch (GetIkType()) 487 | { 488 | case IKP_Transform6D: 489 | case IKP_Translation3D: 490 | // For **Transform6D**, eerot is 9 values for the 3x3 rotation matrix. For **Translation3D**, these are ignored. 491 | 492 | mult = pose_frame.M; 493 | 494 | double vals[9]; 495 | vals[0] = mult(0,0); 496 | vals[1] = mult(0,1); 497 | vals[2] = mult(0,2); 498 | vals[3] = mult(1,0); 499 | vals[4] = mult(1,1); 500 | vals[5] = mult(1,2); 501 | vals[6] = mult(2,0); 502 | vals[7] = mult(2,1); 503 | vals[8] = mult(2,2); 504 | 505 | // IKFast56/61 506 | ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions); 507 | return solutions.GetNumSolutions(); 508 | 509 | case IKP_Direction3D: 510 | case IKP_Ray4D: 511 | case IKP_TranslationDirection5D: 512 | // For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target direction. 513 | 514 | direction = pose_frame.M * KDL::Vector(0, 0, 1); 515 | ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : NULL, solutions); 516 | return solutions.GetNumSolutions(); 517 | 518 | case IKP_TranslationXAxisAngle4D: 519 | case IKP_TranslationYAxisAngle4D: 520 | case IKP_TranslationZAxisAngle4D: 521 | // For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D**, the first value represents the angle. 522 | ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet."); 523 | return 0; 524 | 525 | case IKP_TranslationLocalGlobal6D: 526 | // For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end effector coordinate system. 527 | ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet."); 528 | return 0; 529 | 530 | case IKP_Rotation3D: 531 | case IKP_Lookat3D: 532 | case IKP_TranslationXY2D: 533 | case IKP_TranslationXYOrientation3D: 534 | case IKP_TranslationXAxisAngleZNorm4D: 535 | case IKP_TranslationYAxisAngleXNorm4D: 536 | case IKP_TranslationZAxisAngleYNorm4D: 537 | ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet."); 538 | return 0; 539 | 540 | default: 541 | ROS_ERROR_NAMED("ikfast", "Unknown IkParameterizationType! Was the solver generated with an incompatible version of Openrave?"); 542 | return 0; 543 | } 544 | } 545 | 546 | void IKFastKinematicsPlugin::getSolution(const IkSolutionList &solutions, int i, std::vector& solution) const 547 | { 548 | solution.clear(); 549 | solution.resize(num_joints_); 550 | 551 | // IKFast56/61 552 | const IkSolutionBase& sol = solutions.GetSolution(i); 553 | std::vector vsolfree( sol.GetFree().size() ); 554 | sol.GetSolution(&solution[0],vsolfree.size()>0?&vsolfree[0]:NULL); 555 | 556 | // std::cout << "solution " << i << ":" ; 557 | // for(int j=0;j &ik_seed_state, std::vector &solution) const 565 | { 566 | double dist_sqr = 0; 567 | std::vector ss = ik_seed_state; 568 | for(size_t i=0; i< ik_seed_state.size(); ++i) 569 | { 570 | while(ss[i] > 2*M_PI) { 571 | ss[i] -= 2*M_PI; 572 | } 573 | while(ss[i] < 2*M_PI) { 574 | ss[i] += 2*M_PI; 575 | } 576 | while(solution[i] > 2*M_PI) { 577 | solution[i] -= 2*M_PI; 578 | } 579 | while(solution[i] < 2*M_PI) { 580 | solution[i] += 2*M_PI; 581 | } 582 | dist_sqr += fabs(ik_seed_state[i] - solution[i]); 583 | } 584 | return dist_sqr; 585 | } 586 | 587 | // void IKFastKinematicsPlugin::getOrderedSolutions(const std::vector &ik_seed_state, 588 | // std::vector >& solslist) 589 | // { 590 | // std::vector 591 | // double mindist = 0; 592 | // int minindex = -1; 593 | // std::vector sol; 594 | // for(size_t i=0;i &ik_seed_state, 724 | double timeout, 725 | std::vector &solution, 726 | moveit_msgs::MoveItErrorCodes &error_code, 727 | const kinematics::KinematicsQueryOptions &options) const 728 | { 729 | const IKCallbackFn solution_callback = 0; 730 | std::vector consistency_limits; 731 | 732 | return searchPositionIK(ik_pose, 733 | ik_seed_state, 734 | timeout, 735 | consistency_limits, 736 | solution, 737 | solution_callback, 738 | error_code, 739 | options); 740 | } 741 | 742 | bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, 743 | const std::vector &ik_seed_state, 744 | double timeout, 745 | const std::vector &consistency_limits, 746 | std::vector &solution, 747 | moveit_msgs::MoveItErrorCodes &error_code, 748 | const kinematics::KinematicsQueryOptions &options) const 749 | { 750 | const IKCallbackFn solution_callback = 0; 751 | return searchPositionIK(ik_pose, 752 | ik_seed_state, 753 | timeout, 754 | consistency_limits, 755 | solution, 756 | solution_callback, 757 | error_code, 758 | options); 759 | } 760 | 761 | bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, 762 | const std::vector &ik_seed_state, 763 | double timeout, 764 | std::vector &solution, 765 | const IKCallbackFn &solution_callback, 766 | moveit_msgs::MoveItErrorCodes &error_code, 767 | const kinematics::KinematicsQueryOptions &options) const 768 | { 769 | std::vector consistency_limits; 770 | return searchPositionIK(ik_pose, 771 | ik_seed_state, 772 | timeout, 773 | consistency_limits, 774 | solution, 775 | solution_callback, 776 | error_code, 777 | options); 778 | } 779 | 780 | bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, 781 | const std::vector &ik_seed_state, 782 | double timeout, 783 | const std::vector &consistency_limits, 784 | std::vector &solution, 785 | const IKCallbackFn &solution_callback, 786 | moveit_msgs::MoveItErrorCodes &error_code, 787 | const kinematics::KinematicsQueryOptions &options) const 788 | { 789 | ROS_DEBUG_STREAM_NAMED("ikfast","searchPositionIK"); 790 | 791 | /// search_mode is currently fixed during code generation 792 | SEARCH_MODE search_mode = OPTIMIZE_MAX_JOINT; 793 | 794 | // Check if there are no redundant joints 795 | if(free_params_.size()==0) 796 | { 797 | ROS_DEBUG_STREAM_NAMED("ikfast","No need to search since no free params/redundant joints"); 798 | 799 | // Find first IK solution, within joint limits 800 | if(!getPositionIK(ik_pose, ik_seed_state, solution, error_code)) 801 | { 802 | ROS_DEBUG_STREAM_NAMED("ikfast","No solution whatsoever"); 803 | error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; 804 | return false; 805 | } 806 | 807 | // check for collisions if a callback is provided 808 | if( !solution_callback.empty() ) 809 | { 810 | solution_callback(ik_pose, solution, error_code); 811 | if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) 812 | { 813 | ROS_DEBUG_STREAM_NAMED("ikfast","Solution passes callback"); 814 | return true; 815 | } 816 | else 817 | { 818 | ROS_DEBUG_STREAM_NAMED("ikfast","Solution has error code " << error_code); 819 | return false; 820 | } 821 | } 822 | else 823 | { 824 | return true; // no collision check callback provided 825 | } 826 | } 827 | 828 | // ------------------------------------------------------------------------------------------------- 829 | // Error Checking 830 | if(!active_) 831 | { 832 | ROS_ERROR_STREAM_NAMED("ikfast","Kinematics not active"); 833 | error_code.val = error_code.NO_IK_SOLUTION; 834 | return false; 835 | } 836 | 837 | if(ik_seed_state.size() != num_joints_) 838 | { 839 | ROS_ERROR_STREAM_NAMED("ikfast","Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size()); 840 | error_code.val = error_code.NO_IK_SOLUTION; 841 | return false; 842 | } 843 | 844 | if(!consistency_limits.empty() && consistency_limits.size() != num_joints_) 845 | { 846 | ROS_ERROR_STREAM_NAMED("ikfast","Consistency limits be empty or must have size " << num_joints_ << " instead of size " << consistency_limits.size()); 847 | error_code.val = error_code.NO_IK_SOLUTION; 848 | return false; 849 | } 850 | 851 | 852 | // ------------------------------------------------------------------------------------------------- 853 | // Initialize 854 | 855 | KDL::Frame frame; 856 | tf::poseMsgToKDL(ik_pose,frame); 857 | 858 | std::vector vfree(free_params_.size()); 859 | 860 | ros::Time maxTime = ros::Time::now() + ros::Duration(timeout); 861 | int counter = 0; 862 | 863 | double initial_guess = ik_seed_state[free_params_[0]]; 864 | vfree[0] = initial_guess; 865 | 866 | // ------------------------------------------------------------------------------------------------- 867 | // Handle consitency limits if needed 868 | int num_positive_increments; 869 | int num_negative_increments; 870 | 871 | if(!consistency_limits.empty()) 872 | { 873 | // moveit replaced consistency_limit (scalar) w/ consistency_limits (vector) 874 | // Assume [0]th free_params element for now. Probably wrong. 875 | double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limits[free_params_[0]]); 876 | double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limits[free_params_[0]]); 877 | 878 | num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_); 879 | num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_); 880 | } 881 | else // no consitency limits provided 882 | { 883 | num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_; 884 | num_negative_increments = (initial_guess-joint_min_vector_[free_params_[0]])/search_discretization_; 885 | } 886 | 887 | // ------------------------------------------------------------------------------------------------- 888 | // Begin searching 889 | 890 | ROS_DEBUG_STREAM_NAMED("ikfast","Free param is " << free_params_[0] << " initial guess is " << initial_guess << ", # positive increments: " << num_positive_increments << ", # negative increments: " << num_negative_increments); 891 | if ((search_mode & OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000) 892 | ROS_WARN_STREAM_ONCE_NAMED("ikfast", "Large search space, consider increasing the search discretization"); 893 | 894 | double best_costs = -1.0; 895 | std::vector best_solution; 896 | int nattempts = 0, nvalid = 0; 897 | 898 | while(true) 899 | { 900 | IkSolutionList solutions; 901 | int numsol = solve(frame,vfree, solutions); 902 | 903 | ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast"); 904 | 905 | //ROS_INFO("%f",vfree[0]); 906 | 907 | if( numsol > 0 ) 908 | { 909 | for(int s = 0; s < numsol; ++s) 910 | { 911 | nattempts++; 912 | std::vector sol; 913 | getSolution(solutions,s,sol); 914 | 915 | bool obeys_limits = true; 916 | for(unsigned int i = 0; i < sol.size(); i++) 917 | { 918 | if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) 919 | { 920 | obeys_limits = false; 921 | break; 922 | } 923 | //ROS_INFO_STREAM_NAMED("ikfast","Num " << i << " value " << sol[i] << " has limits " << joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]); 924 | } 925 | if(obeys_limits) 926 | { 927 | getSolution(solutions,s,solution); 928 | 929 | // This solution is within joint limits, now check if in collision (if callback provided) 930 | if(!solution_callback.empty()) 931 | { 932 | solution_callback(ik_pose, solution, error_code); 933 | } 934 | else 935 | { 936 | error_code.val = error_code.SUCCESS; 937 | } 938 | 939 | if(error_code.val == error_code.SUCCESS) 940 | { 941 | nvalid++; 942 | if (search_mode & OPTIMIZE_MAX_JOINT) 943 | { 944 | // Costs for solution: Largest joint motion 945 | double costs = 0.0; 946 | for(unsigned int i = 0; i < solution.size(); i++) 947 | { 948 | double d = fabs(ik_seed_state[i] - solution[i]); 949 | if (d > costs) 950 | costs = d; 951 | } 952 | if (costs < best_costs || best_costs == -1.0) 953 | { 954 | best_costs = costs; 955 | best_solution = solution; 956 | } 957 | } 958 | else 959 | // Return first feasible solution 960 | return true; 961 | } 962 | } 963 | } 964 | } 965 | 966 | if(!getCount(counter, num_positive_increments, -num_negative_increments)) 967 | { 968 | // Everything searched 969 | error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; 970 | break; 971 | } 972 | 973 | vfree[0] = initial_guess+search_discretization_*counter; 974 | //ROS_DEBUG_STREAM_NAMED("ikfast","Attempt " << counter << " with 0th free joint having value " << vfree[0]); 975 | } 976 | 977 | ROS_DEBUG_STREAM_NAMED("ikfast", "Valid solutions: " << nvalid << "/" << nattempts); 978 | 979 | if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0) 980 | { 981 | solution = best_solution; 982 | error_code.val = error_code.SUCCESS; 983 | return true; 984 | } 985 | 986 | // No solution found 987 | error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; 988 | return false; 989 | } 990 | 991 | // Used when there are no redundant joints - aka no free params 992 | bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose, 993 | const std::vector &ik_seed_state, 994 | std::vector &solution, 995 | moveit_msgs::MoveItErrorCodes &error_code, 996 | const kinematics::KinematicsQueryOptions &options) const 997 | { 998 | ROS_DEBUG_STREAM_NAMED("ikfast","getPositionIK"); 999 | 1000 | if(!active_) 1001 | { 1002 | ROS_ERROR("kinematics not active"); 1003 | return false; 1004 | } 1005 | 1006 | std::vector vfree(free_params_.size()); 1007 | for(std::size_t i = 0; i < free_params_.size(); ++i) 1008 | { 1009 | int p = free_params_[i]; 1010 | ROS_ERROR("%u is %f",p,ik_seed_state[p]); // DTC 1011 | vfree[i] = ik_seed_state[p]; 1012 | } 1013 | 1014 | KDL::Frame frame; 1015 | tf::poseMsgToKDL(ik_pose,frame); 1016 | 1017 | IkSolutionList solutions; 1018 | int numsol = solve(frame,vfree,solutions); 1019 | 1020 | ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast"); 1021 | 1022 | if(numsol) 1023 | { 1024 | for(int s = 0; s < numsol; ++s) 1025 | { 1026 | std::vector sol; 1027 | getSolution(solutions,s,sol); 1028 | ROS_DEBUG_NAMED("ikfast","Sol %d: %e %e %e %e %e %e", s, sol[0], sol[1], sol[2], sol[3], sol[4], sol[5]); 1029 | 1030 | bool obeys_limits = true; 1031 | for(unsigned int i = 0; i < sol.size(); i++) 1032 | { 1033 | // Add tolerance to limit check 1034 | if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-LIMIT_TOLERANCE)) || 1035 | (sol[i] > (joint_max_vector_[i]+LIMIT_TOLERANCE)) ) ) 1036 | { 1037 | // One element of solution is not within limits 1038 | obeys_limits = false; 1039 | ROS_DEBUG_STREAM_NAMED("ikfast","Not in limits! " << i << " value " << sol[i] << " has limit: " << joint_has_limits_vector_[i] << " being " << joint_min_vector_[i] << " to " << joint_max_vector_[i]); 1040 | break; 1041 | } 1042 | } 1043 | if(obeys_limits) 1044 | { 1045 | // All elements of solution obey limits 1046 | getSolution(solutions,s,solution); 1047 | error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; 1048 | return true; 1049 | } 1050 | } 1051 | } 1052 | else 1053 | { 1054 | ROS_DEBUG_STREAM_NAMED("ikfast","No IK solution"); 1055 | } 1056 | 1057 | error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; 1058 | return false; 1059 | } 1060 | 1061 | bool IKFastKinematicsPlugin::getPositionIK(const std::vector &ik_poses, 1062 | const std::vector &ik_seed_state, 1063 | std::vector< std::vector >& solutions, 1064 | kinematics::KinematicsResult& result, 1065 | const kinematics::KinematicsQueryOptions &options) const 1066 | { 1067 | ROS_DEBUG_STREAM_NAMED("ikfast","getPositionIK with multiple solutions"); 1068 | 1069 | if(!active_) 1070 | { 1071 | ROS_ERROR("kinematics not active"); 1072 | result.kinematic_error = kinematics::KinematicErrors::SOLVER_NOT_ACTIVE; 1073 | return false; 1074 | } 1075 | 1076 | if(ik_poses.empty()) 1077 | { 1078 | ROS_ERROR("ik_poses is empty"); 1079 | result.kinematic_error = kinematics::KinematicErrors::EMPTY_TIP_POSES; 1080 | return false; 1081 | } 1082 | 1083 | if(ik_poses.size() > 1) 1084 | { 1085 | ROS_ERROR("ik_poses contains multiple entries, only one is allowed"); 1086 | result.kinematic_error = kinematics::KinematicErrors::MULTIPLE_TIPS_NOT_SUPPORTED; 1087 | return false; 1088 | } 1089 | 1090 | if(ik_seed_state.size() < num_joints_) 1091 | { 1092 | ROS_ERROR_STREAM("ik_seed_state only has "< > solution_set; 1101 | IkSolutionList ik_solutions; 1102 | std::vector vfree; 1103 | int numsol = 0; 1104 | std::vector sampled_joint_vals; 1105 | if(!redundant_joint_indices_.empty()) 1106 | { 1107 | // initializing from seed 1108 | sampled_joint_vals.push_back(ik_seed_state[redundant_joint_indices_[0]]); 1109 | 1110 | // checking joint limits when using no discretization 1111 | if(options.discretization_method == kinematics::DiscretizationMethods::NO_DISCRETIZATION && 1112 | joint_has_limits_vector_[redundant_joint_indices_.front()]) 1113 | { 1114 | double joint_min = joint_min_vector_[redundant_joint_indices_.front()]; 1115 | double joint_max = joint_max_vector_[redundant_joint_indices_.front()]; 1116 | 1117 | double jv = sampled_joint_vals[0]; 1118 | if(!( (jv > (joint_min - LIMIT_TOLERANCE)) && (jv < (joint_max + LIMIT_TOLERANCE)) )) 1119 | { 1120 | result.kinematic_error = kinematics::KinematicErrors::IK_SEED_OUTSIDE_LIMITS; 1121 | ROS_ERROR_STREAM("ik seed is out of bounds"); 1122 | return false; 1123 | } 1124 | 1125 | } 1126 | 1127 | 1128 | // computing all solutions sets for each sampled value of the redundant joint 1129 | if(!sampleRedundantJoint(options.discretization_method,sampled_joint_vals)) 1130 | { 1131 | result.kinematic_error = kinematics::KinematicErrors::UNSUPORTED_DISCRETIZATION_REQUESTED; 1132 | return false; 1133 | } 1134 | 1135 | for(unsigned int i = 0; i < sampled_joint_vals.size(); i++) 1136 | { 1137 | vfree.clear(); 1138 | vfree.push_back(sampled_joint_vals[i]); 1139 | numsol += solve(frame,vfree,ik_solutions); 1140 | solution_set.push_back(ik_solutions); 1141 | } 1142 | } 1143 | else 1144 | { 1145 | // computing for single solution set 1146 | numsol = solve(frame,vfree,ik_solutions); 1147 | solution_set.push_back(ik_solutions); 1148 | } 1149 | 1150 | ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast"); 1151 | bool solutions_found = false; 1152 | if( numsol > 0 ) 1153 | { 1154 | /* 1155 | Iterating through all solution sets and storing those that do not exceed joint limits. 1156 | */ 1157 | for(unsigned int r = 0; r < solution_set.size() ; r++) 1158 | { 1159 | 1160 | ik_solutions = solution_set[r]; 1161 | numsol = ik_solutions.GetNumSolutions(); 1162 | for(int s = 0; s < numsol; ++s) 1163 | { 1164 | std::vector sol; 1165 | getSolution(ik_solutions,s,sol); 1166 | 1167 | bool obeys_limits = true; 1168 | for(unsigned int i = 0; i < sol.size(); i++) 1169 | { 1170 | // Add tolerance to limit check 1171 | if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-LIMIT_TOLERANCE)) || 1172 | (sol[i] > (joint_max_vector_[i]+LIMIT_TOLERANCE)) ) ) 1173 | { 1174 | // One element of solution is not within limits 1175 | obeys_limits = false; 1176 | ROS_DEBUG_STREAM_NAMED("ikfast","Not in limits! " << i << " value " << sol[i] << " has limit: " << joint_has_limits_vector_[i] << " being " << joint_min_vector_[i] << " to " << joint_max_vector_[i]); 1177 | break; 1178 | } 1179 | } 1180 | if(obeys_limits) 1181 | { 1182 | // All elements of solution obey limits 1183 | solutions_found = true; 1184 | solutions.push_back(sol); 1185 | } 1186 | } 1187 | } 1188 | 1189 | if(solutions_found) 1190 | { 1191 | result.kinematic_error = kinematics::KinematicErrors::OK; 1192 | return true; 1193 | } 1194 | } 1195 | else 1196 | { 1197 | ROS_DEBUG_STREAM_NAMED("ikfast","No IK solution"); 1198 | } 1199 | 1200 | result.kinematic_error = kinematics::KinematicErrors::NO_SOLUTION; 1201 | return false; 1202 | } 1203 | 1204 | bool IKFastKinematicsPlugin::sampleRedundantJoint(kinematics::DiscretizationMethod method, std::vector& sampled_joint_vals) const 1205 | { 1206 | double joint_min = -M_PI; 1207 | double joint_max = M_PI; 1208 | int index = redundant_joint_indices_.front(); 1209 | double joint_dscrt = redundant_joint_discretization_.at(index); 1210 | 1211 | if(joint_has_limits_vector_[redundant_joint_indices_.front()]) 1212 | { 1213 | joint_min = joint_min_vector_[index]; 1214 | joint_max = joint_max_vector_[index]; 1215 | } 1216 | 1217 | 1218 | switch(method) 1219 | { 1220 | case kinematics::DiscretizationMethods::ALL_DISCRETIZED: 1221 | { 1222 | int steps = std::ceil((joint_max - joint_min)/joint_dscrt); 1223 | for(unsigned int i = 0; i < steps;i++) 1224 | { 1225 | sampled_joint_vals.push_back(joint_min + joint_dscrt*i); 1226 | } 1227 | sampled_joint_vals.push_back(joint_max); 1228 | } 1229 | break; 1230 | case kinematics::DiscretizationMethods::ALL_RANDOM_SAMPLED: 1231 | { 1232 | int steps = std::ceil((joint_max - joint_min)/joint_dscrt); 1233 | steps = steps > 0 ? steps : 1; 1234 | double diff = joint_max - joint_min; 1235 | for(int i = 0; i < steps; i++) 1236 | { 1237 | sampled_joint_vals.push_back( ((diff*std::rand())/(static_cast(RAND_MAX))) + joint_min ); 1238 | } 1239 | } 1240 | 1241 | break; 1242 | case kinematics::DiscretizationMethods::NO_DISCRETIZATION: 1243 | 1244 | break; 1245 | default: 1246 | ROS_ERROR_STREAM("Discretization method "< 1258 | PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase); 1259 | --------------------------------------------------------------------------------