├── .gitignore ├── omnid_supplementary_packages └── tf2_armadillo │ ├── .idea │ ├── .gitignore │ ├── tf2_armadillo.iml │ ├── vcs.xml │ ├── misc.xml │ └── modules.xml │ ├── cmake-build-debug │ └── CMakeFiles │ │ ├── clion-environment.txt │ │ ├── cmake.check_cache │ │ ├── 3.15.3 │ │ ├── CompilerIdC │ │ │ └── a.out │ │ ├── CompilerIdCXX │ │ │ └── a.out │ │ ├── CMakeDetermineCompilerABI_C.bin │ │ ├── CMakeDetermineCompilerABI_CXX.bin │ │ ├── CMakeSystem.cmake │ │ ├── CMakeCCompiler.cmake │ │ └── CMakeCXXCompiler.cmake │ │ └── clion-log.txt │ ├── package.xml │ ├── README.md │ ├── CMakeLists.txt │ ├── include │ └── tf2_armadillo │ │ └── tf2_armadillo.h │ └── test │ └── test_tf2_armadillo.cpp ├── omnid_moveit_config ├── launch │ ├── delta-robot-RUU_moveit_sensor_manager.launch.xml │ ├── planning_pipeline.launch.xml │ ├── ros_controllers.launch │ ├── fake_moveit_controller_manager.launch.xml │ ├── warehouse.launch │ ├── delta-robot-RUU_moveit_controller_manager.launch.xml │ ├── setup_assistant.launch │ ├── moveit_rviz.launch │ ├── joystick_control.launch │ ├── warehouse_settings.launch.xml │ ├── default_warehouse_db.launch │ ├── sensor_manager.launch.xml │ ├── run_benchmark_ompl.launch │ ├── gazebo.launch │ ├── chomp_planning_pipeline.launch.xml │ ├── ompl_planning_pipeline.launch.xml │ ├── planning_context.launch │ ├── trajectory_execution.launch.xml │ ├── demo.launch │ ├── demo_gazebo.launch │ └── move_group.launch ├── CMakeLists.txt ├── .setup_assistant ├── config │ ├── sensors_3d.yaml │ ├── chomp_planning.yaml │ ├── kinematics.yaml │ ├── omnid_controllers.yaml │ ├── ros_controllers.yaml │ ├── fake_controllers.yaml │ └── joint_limits.yaml └── package.xml ├── omnid ├── scripts │ ├── spring_test │ │ ├── __pycache__ │ │ │ └── torsion_spring.cpython-38.pyc │ │ ├── spring_test_simulator │ │ └── torsion_spring.py │ ├── disk.py │ ├── torsion_spring_omnid.py │ ├── omnid_simulator │ └── omnid_model.py ├── urdf │ ├── update_urdf.bash │ ├── disk.urdf │ ├── disk.urdf.xacro │ └── spring │ │ ├── spring.urdf │ │ └── spring.urdf.xacro ├── cfg │ └── omnid.cfg ├── package.xml ├── CMakeLists.txt ├── README.md ├── launch │ ├── omnid.launch │ ├── test_urdf.launch │ └── moveit_omnid.launch └── params │ └── params.yaml ├── omnid_move_group_interface ├── src │ └── omnid_group_ik.cpp ├── package.xml ├── CMakeLists.txt └── include │ └── omnid_move_group_interface │ └── omnid_move_group_interface.hpp ├── docker_setup ├── ros_settings.bash ├── omnid_docs.repos ├── Dockerfile └── dockint ├── omnid_kinematics_plugin ├── omnid_kinematics_plugin.xml ├── package.xml └── CMakeLists.txt ├── omnid_planning_adapter_plugin ├── omnid_planning_adapter_plugin.xml ├── package.xml ├── CMakeLists.txt ├── src │ └── omnid_planning_adapter_plugin.cpp └── include │ └── omnid_planning_adapter_plugin │ └── omnid_planning_adapter_plugin.h └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | .idea/ 2 | -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/.idea/.gitignore: -------------------------------------------------------------------------------- 1 | # Default ignored files 2 | /workspace.xml -------------------------------------------------------------------------------- /omnid_moveit_config/launch/delta-robot-RUU_moveit_sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/cmake-build-debug/CMakeFiles/clion-environment.txt: -------------------------------------------------------------------------------- 1 | ToolSet: 1.0 (local)Options: 2 | 3 | Options: -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/.idea/tf2_armadillo.iml: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/cmake-build-debug/CMakeFiles/cmake.check_cache: -------------------------------------------------------------------------------- 1 | # This file is generated by cmake for dependency checking of the CMakeCache.txt file 2 | -------------------------------------------------------------------------------- /omnid/scripts/spring_test/__pycache__/torsion_spring.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RicoJia/Omnid_Project/HEAD/omnid/scripts/spring_test/__pycache__/torsion_spring.cpython-38.pyc -------------------------------------------------------------------------------- /omnid_move_group_interface/src/omnid_group_ik.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "../include/omnid_move_group_interface/omnid_move_group_interface.hpp" 3 | //#include "omnid_move_group_interface/omnid_group_ik.hpp" 4 | 5 | 6 | -------------------------------------------------------------------------------- /docker_setup/ros_settings.bash: -------------------------------------------------------------------------------- 1 | # set up history backward search. 2 | #bind -f /home/ricojia/.inputrc 3 | #Source this in your docker terminal 4 | export ROS_MASTER_URI=http://localhost:11311 5 | export ROS_HOSTNAME=localhost 6 | 7 | -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/cmake-build-debug/CMakeFiles/3.15.3/CompilerIdC/a.out: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RicoJia/Omnid_Project/HEAD/omnid_supplementary_packages/tf2_armadillo/cmake-build-debug/CMakeFiles/3.15.3/CompilerIdC/a.out -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/cmake-build-debug/CMakeFiles/3.15.3/CompilerIdCXX/a.out: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RicoJia/Omnid_Project/HEAD/omnid_supplementary_packages/tf2_armadillo/cmake-build-debug/CMakeFiles/3.15.3/CompilerIdCXX/a.out -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/cmake-build-debug/CMakeFiles/3.15.3/CMakeDetermineCompilerABI_C.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RicoJia/Omnid_Project/HEAD/omnid_supplementary_packages/tf2_armadillo/cmake-build-debug/CMakeFiles/3.15.3/CMakeDetermineCompilerABI_C.bin -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/cmake-build-debug/CMakeFiles/3.15.3/CMakeDetermineCompilerABI_CXX.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RicoJia/Omnid_Project/HEAD/omnid_supplementary_packages/tf2_armadillo/cmake-build-debug/CMakeFiles/3.15.3/CMakeDetermineCompilerABI_CXX.bin -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /omnid_kinematics_plugin/omnid_kinematics_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Kinematics Plugin for the Omnid Delta Arm Project 4 | 5 | -------------------------------------------------------------------------------- /omnid_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1.3) 2 | project(omnid_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 | -------------------------------------------------------------------------------- /omnid/urdf/update_urdf.bash: -------------------------------------------------------------------------------- 1 | rosrun xacro xacro --inorder -o delta_robot_pybullet.urdf delta_robot_pybullet.urdf.xacro R_base:=0.18 h_base:=0.009525 M_base:=2.5 R_platform:=0.062 h_platform:=0.01 M_platform:=0.5 L_lower:=0.200 L_upper:=0.368 base_offset:=0.046375 2 | rosrun xacro xacro --inorder -o disk.urdf disk.urdf.xacro 3 | 4 | 5 | #gz sdf -p delta_robot.urdf > delta_robot.sdf 6 | #rm delta_robot.urdf 7 | -------------------------------------------------------------------------------- /omnid_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | URDF: 3 | package: omnid 4 | relative_path: urdf/delta_robot_group_moveit.urdf.xacro 5 | xacro_args: "" 6 | SRDF: 7 | relative_path: config/delta-robot-RUU.srdf 8 | CONFIG: 9 | author_name: Rico Ruotong Jia 10 | author_email: ruotongjia2020@u.northwestern.edu 11 | generated_timestamp: 1605304150 -------------------------------------------------------------------------------- /omnid_moveit_config/launch/planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /omnid/cfg/omnid.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | PACKAGE='omnid' 4 | import roslib 5 | roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator() 10 | 11 | gen.add("torque_k", double_t, 0, "k for torque", 5, 0.0, 10.0) 12 | # gen.add("joint_angle", double_t, 0, "joint angle", 0.0, -3.14, 3.14) 13 | 14 | exit(gen.generate(PACKAGE, "omnid_simulator", "omnid")) 15 | -------------------------------------------------------------------------------- /omnid_moveit_config/config/sensors_3d.yaml: -------------------------------------------------------------------------------- 1 | # The name of this file shouldn't be changed, or else the Setup Assistant won't detect it 2 | sensors: 3 | - filtered_cloud_topic: filtered_cloud 4 | max_range: 5.0 5 | max_update_rate: 1.0 6 | padding_offset: 0.1 7 | padding_scale: 1.0 8 | point_cloud_topic: /head_mount_kinect/depth_registered/points 9 | point_subsample: 1 10 | sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater -------------------------------------------------------------------------------- /omnid_moveit_config/launch/ros_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /omnid_moveit_config/launch/fake_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /omnid_planning_adapter_plugin/omnid_planning_adapter_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Planning adapter for post-processing planning results for the Omnid Delta Arm Project 4 | 5 | -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/cmake-build-debug/CMakeFiles/3.15.3/CMakeSystem.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_HOST_SYSTEM "Linux-5.4.0-53-generic") 2 | set(CMAKE_HOST_SYSTEM_NAME "Linux") 3 | set(CMAKE_HOST_SYSTEM_VERSION "5.4.0-53-generic") 4 | set(CMAKE_HOST_SYSTEM_PROCESSOR "x86_64") 5 | 6 | 7 | 8 | set(CMAKE_SYSTEM "Linux-5.4.0-53-generic") 9 | set(CMAKE_SYSTEM_NAME "Linux") 10 | set(CMAKE_SYSTEM_VERSION "5.4.0-53-generic") 11 | set(CMAKE_SYSTEM_PROCESSOR "x86_64") 12 | 13 | set(CMAKE_CROSSCOMPILING "FALSE") 14 | 15 | set(CMAKE_SYSTEM_LOADED 1) 16 | -------------------------------------------------------------------------------- /omnid_moveit_config/config/chomp_planning.yaml: -------------------------------------------------------------------------------- 1 | planning_time_limit: 10.0 2 | max_iterations: 200 3 | max_iterations_after_collision_free: 5 4 | smoothness_cost_weight: 0.1 5 | obstacle_cost_weight: 1.0 6 | learning_rate: 0.01 7 | smoothness_cost_velocity: 0.0 8 | smoothness_cost_acceleration: 1.0 9 | smoothness_cost_jerk: 0.0 10 | ridge_factor: 0.01 11 | use_pseudo_inverse: false 12 | pseudo_inverse_ridge_factor: 1e-4 13 | joint_update_limit: 0.1 14 | collision_clearence: 0.2 15 | collision_threshold: 0.07 16 | use_stochastic_descent: true 17 | enable_failure_recovery: true 18 | max_recovery_attempts: 5 -------------------------------------------------------------------------------- /omnid_moveit_config/launch/warehouse.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /omnid_moveit_config/launch/delta-robot-RUU_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /omnid_moveit_config/launch/setup_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /omnid_moveit_config/launch/moveit_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /omnid/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | omnid 4 | 0.1.0 5 | The omnid package 6 | Rico Ruotong Jia 7 | BSD 8 | catkin 9 | geometry_msgs 10 | rospy 11 | geometry_msgs 12 | rospy 13 | geometry_msgs 14 | rospy 15 | dynamic_reconfigure 16 | 17 | -------------------------------------------------------------------------------- /omnid_moveit_config/launch/joystick_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /omnid_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | end_effector_arm_1: 2 | kinematics_solver: omnid_kinematics/OmnidKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.005 4 | kinematics_solver_timeout: 0.005 5 | end_effector_arm_2: 6 | kinematics_solver: omnid_kinematics/OmnidKinematicsPlugin 7 | kinematics_solver_search_resolution: 0.005 8 | kinematics_solver_timeout: 0.005 9 | end_effector_arm_3: 10 | kinematics_solver: omnid_kinematics/OmnidKinematicsPlugin 11 | kinematics_solver_search_resolution: 0.005 12 | kinematics_solver_timeout: 0.005 13 | object_platform_arm: 14 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 15 | kinematics_solver_search_resolution: 0.005 16 | kinematics_solver_timeout: 0.005 -------------------------------------------------------------------------------- /omnid_moveit_config/launch/warehouse_settings.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | tf2_armadillo 4 | 0.1.0 5 | tf2_armadillo 6 | Rico Ruotong Jia 7 | Rico Ruotong Jia 8 | 9 | BSD 10 | 11 | catkin 12 | roscpp 13 | tf2 14 | roscpp 15 | tf2 16 | roscpp 17 | tf2 18 | geometry_msgs 19 | rosunit 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /omnid_moveit_config/launch/default_warehouse_db.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /omnid_move_group_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | omnid_move_group_interface 4 | 0.1.0 5 | The move_group interface for controlling a group of delta arms. Launch this node alongside moveit_config. 6 | Rico Ruotong Jia 7 | BSD 8 | moveit_core 9 | moveit_ros_planning 10 | moveit_ros_planning_interface 11 | visualization_msgs 12 | catkin 13 | roscpp 14 | roscpp 15 | roscpp 16 | tf2_armadillo 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /omnid_moveit_config/launch/sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /omnid_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 | -------------------------------------------------------------------------------- /omnid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(omnid) 3 | 4 | add_compile_options(-std=c++11) 5 | find_package(catkin REQUIRED COMPONENTS 6 | geometry_msgs 7 | rospy 8 | dynamic_reconfigure 9 | ) 10 | generate_dynamic_reconfigure_options( 11 | cfg/omnid.cfg 12 | ) 13 | catkin_package( 14 | # INCLUDE_DIRS include 15 | LIBRARIES omnid 16 | CATKIN_DEPENDS geometry_msgs rospy 17 | DEPENDS system_lib 18 | ) 19 | 20 | include_directories( 21 | # include 22 | ${catkin_INCLUDE_DIRS} 23 | ) 24 | install(PROGRAMS 25 | scripts/omnid_simulator 26 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 27 | ) 28 | install(DIRECTORY include/${PROJECT_NAME}/ 29 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 30 | FILES_MATCHING PATTERN "*.h" 31 | PATTERN ".svn" EXCLUDE 32 | ) 33 | install(FILES 34 | launch/omnid.launch 35 | # myfile2 36 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 37 | ) 38 | 39 | -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/README.md: -------------------------------------------------------------------------------- 1 | # tf2_armadillo 2 | 3 | **Author: Rico Ruotong Jia (ruotongjia2020@u.northwestern.edu)** 4 | 5 | This is an interface tool between ROS ```tf2``` and ```geometry_msgs``` messages and Armadillo matrices. 6 | 7 | So far this is a minimal implementation, as there are more message types that need to be implemented. 8 | 9 | ### Conversions and Transforms 10 | 1. toMsg from ```arma::Matrix::fixed<4,4>``` to 11 | - ```geometry_msgs::Transform``` 12 | - ```tf2::Transform``` 13 | 2. fromMsg to ```arma::Matrix::fixed<4,4>``` from 14 | - ```geometry_msgs::Transform``` 15 | - ```tf2::Transform``` 16 | 3. doTransform will to be implemented as needed 17 | 18 | ### Usage 19 | Clone this repository into a catkin workspace, then use the rosdep install tool to automatically download its dependencies. 20 | Depending on your current version of ROS, use: 21 | ``` 22 | rosdep install --from-paths src --ignore-src --rosdistro noetic 23 | ``` 24 | -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(tf2_armadillo) 3 | add_compile_options(-std=c++11) 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | tf2 geometry_msgs 7 | ) 8 | find_package(Armadillo REQUIRED) 9 | 10 | include_directories( 11 | include 12 | ${catkin_INCLUDE_DIRS} 13 | ${ARMADILLO_INCLUDE_DIRS} 14 | ) 15 | 16 | catkin_package( 17 | INCLUDE_DIRS include 18 | CATKIN_DEPENDS roscpp tf2 geometry_msgs 19 | DEPENDS systemlib 20 | ) 21 | 22 | install(DIRECTORY include/${PROJECT_NAME}/ 23 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 24 | # FILES_MATCHING PATTERN "*.h" 25 | ) 26 | 27 | # Add gtest based cpp test target and link libraries 28 | if(CATKIN_ENABLE_TESTING) 29 | catkin_add_gtest(${PROJECT_NAME}-test test/test_tf2_armadillo.cpp) 30 | target_link_libraries(${PROJECT_NAME}-test ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${ARMADILLO_LIBRARIES}) 31 | endif() 32 | 33 | ## Add folders to be run by python nosetests 34 | # catkin_add_nosetests(test) 35 | -------------------------------------------------------------------------------- /omnid_moveit_config/launch/gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /omnid_moveit_config/launch/chomp_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /docker_setup/omnid_docs.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | cmakeme: 3 | type: git 4 | url: https://github.com/omnid/cmakeme.git 5 | version: master 6 | nuhal: 7 | type: git 8 | url: https://github.com/omnid/nuhal.git 9 | version: master 10 | omnid_core: 11 | type: git 12 | url: https://github.com/omnid/omnid_core.git 13 | version: master 14 | omnid_docs: 15 | type: git 16 | url: https://github.com/omnid/omnid_docs.git 17 | version: master 18 | omnid_firmware: 19 | type: git 20 | url: https://github.com/omnid/omnid_firmware.git 21 | version: master 22 | omnid_lib: 23 | type: git 24 | url: https://github.com/omnid/omnid_lib.git 25 | version: master 26 | omnid_setup: 27 | type: git 28 | url: https://github.com/omnid/omnid_setup.git 29 | version: master 30 | omnid_tools: 31 | type: git 32 | url: https://github.com/omnid/omnid_tools.git 33 | version: master 34 | tiva_cmake: 35 | type: git 36 | url: https://github.com/omnid/tiva_cmake.git 37 | version: master 38 | tivaboot: 39 | type: git 40 | url: https://github.com/omnid/tivaboot.git 41 | version: master 42 | rosnu: 43 | type: git 44 | url: https://github.com/omnid/rosnu.git 45 | version: master 46 | -------------------------------------------------------------------------------- /omnid_moveit_config/launch/ompl_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /omnid_planning_adapter_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | omnid_planning_adapter_plugin 4 | 0.1.0 5 | Planning adapter for post-processing planning results for the Omnid Delta Arm Project 6 | 7 | Rico Ruotong Jia 8 | BSD 9 | 10 | catkin 11 | moveit_core 12 | moveit_ros_planning 13 | pluginlib 14 | roscpp 15 | moveit_core 16 | moveit_ros_planning 17 | pluginlib 18 | roscpp 19 | moveit_core 20 | moveit_ros_planning 21 | pluginlib 22 | roscpp 23 | omnid_core_all 24 | omnid_move_group_interface 25 | cmakeme 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /omnid_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 | 26 | -------------------------------------------------------------------------------- /omnid_moveit_config/config/omnid_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: all_arms_controller 3 | action_ns: follow_joint_trajectory 4 | type: FollowJointTrajectory 5 | default: true 6 | joints: 7 | - object_platform/x 8 | - object_platform/y 9 | - object_platform/z 10 | - object_platform/roll 11 | - object_platform/pitch 12 | - object_platform/yaw 13 | - robot_1/theta_1 14 | - robot_1/beta_1 15 | - robot_1/gamma_1 16 | - robot_1/theta_2 17 | - robot_1/beta_2 18 | - robot_1/gamma_2 19 | - robot_1/theta_3 20 | - robot_1/beta_3 21 | - robot_1/gamma_3 22 | - robot_1/x 23 | - robot_1/y 24 | - robot_1/z 25 | - robot_2/theta_1 26 | - robot_2/beta_1 27 | - robot_2/gamma_1 28 | - robot_2/theta_2 29 | - robot_2/beta_2 30 | - robot_2/gamma_2 31 | - robot_2/theta_3 32 | - robot_2/beta_3 33 | - robot_2/gamma_3 34 | - robot_2/x 35 | - robot_2/y 36 | - robot_2/z 37 | - robot_3/theta_1 38 | - robot_3/beta_1 39 | - robot_3/gamma_1 40 | - robot_3/theta_2 41 | - robot_3/beta_2 42 | - robot_3/gamma_2 43 | - robot_3/theta_3 44 | - robot_3/beta_3 45 | - robot_3/gamma_3 46 | - robot_3/x 47 | - robot_3/y 48 | - robot_3/z -------------------------------------------------------------------------------- /omnid/README.md: -------------------------------------------------------------------------------- 1 | # Omnid Project 2 | 3 | Author: Rico Ruotong Jia 4 | 5 | ### Topics and Actions 6 | #### Subscribed Topics 7 | 1. The Omnid simulator provides an interface with Moveit! for motion planning. That works on ROS actions. Note that an external user can publish on 8 | the 'goal' topic to control the robot too. 9 | - Action Topics: 10 | ``` 11 | /omnid/follow_joint_trajectory/cancel 12 | /omnid/follow_joint_trajectory/feedback 13 | /omnid/follow_joint_trajectory/goal 14 | /omnid/follow_joint_trajectory/result 15 | /omnid/follow_joint_trajectory/status 16 | ``` 17 | 2. Also, there is a test mode: set ```test_mode_on``` to true in ```params.yaml```, then you can control 18 | each joint by publishing on ```/joint_states_control``` 19 | 20 | #### Published Topics 21 | 1. ```/joint_states``` lists all joint states for motion planning. 22 | ### Modes of Operation 23 | #### Joint Control Test 24 | Direct joint level control, see [Subscribed Topics](#Subscribed-Topics) for how to enable it. 25 | #### After Spring Joint Test 26 | Direct control on the after spring joint (theta). 27 | 28 | ### Additional Packages 29 | These packages are optional to install, and they are not essential to the operation of 30 | the delta arm. However, based on your need, you might consider using them: 31 | 1. ros-noetic-joint-state-publisher-gui 32 | -------------------------------------------------------------------------------- /omnid/launch/omnid.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /omnid/urdf/disk.urdf: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /omnid_move_group_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(omnid_move_group_interface) 3 | 4 | add_compile_options(-std=c++17) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | moveit_ros_planning 9 | moveit_core 10 | moveit_ros_planning_interface 11 | visualization_msgs 12 | tf2_armadillo 13 | ) 14 | find_package(Armadillo REQUIRED) 15 | 16 | catkin_package( 17 | # INCLUDE_DIRS include 18 | LIBRARIES ${PROJECT_NAME} 19 | # CATKIN_DEPENDS roscpp 20 | # DEPENDS system_lib Eigen3 21 | ) 22 | 23 | include_directories( 24 | include 25 | ${catkin_INCLUDE_DIRS} 26 | ${ARMADILLO_INCLUDE_DIRS} 27 | ${BLAS_INCLUDE_DIRS} 28 | ${LAPACK_INCLUDE_DIRS} 29 | ) 30 | 31 | ## Declare a C++ library 32 | add_library(armadillo INTERFACE IMPORTED) 33 | set_property(TARGET armadillo PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${ARMADILLO_INCLUDE_DIRS}) 34 | 35 | add_executable(${PROJECT_NAME}_node src/omnid_move_group_interface_node.cpp) 36 | 37 | target_link_libraries(${PROJECT_NAME}_node 38 | ${catkin_LIBRARIES} 39 | ${Boost_LIBRARIES} 40 | ${ARMA_LIBS} 41 | ) 42 | 43 | install(TARGETS ${PROJECT_NAME}_node 44 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 45 | ) 46 | 47 | # Mark cpp header files for installation 48 | install(DIRECTORY include/${PROJECT_NAME}/ 49 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 50 | FILES_MATCHING PATTERN "*.hpp" 51 | ) 52 | -------------------------------------------------------------------------------- /omnid_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 | 22 | 23 | -------------------------------------------------------------------------------- /omnid_kinematics_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | omnid_kinematics_plugin 4 | 0.1.0 5 | The Moveit! kinematics plugin for a Delta Arm on an Omnid Robot 6 | 7 | Rico Ruotong Jia 8 | BSD 9 | 10 | catkin 11 | geometry_msgs 12 | moveit_core 13 | moveit_kinematics 14 | moveit_ros_planning 15 | pluginlib 16 | roscpp 17 | geometry_msgs 18 | moveit_core 19 | moveit_kinematics 20 | moveit_ros_planning 21 | pluginlib 22 | roscpp 23 | geometry_msgs 24 | moveit_core 25 | moveit_kinematics 26 | moveit_ros_planning 27 | pluginlib 28 | roscpp 29 | omnid_core_all 30 | cmakeme 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /omnid_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | omnid_moveit_config 4 | 0.3.0 5 | 6 | An automatically generated package with all the configuration and launch files for using the delta-robot-RUU with the MoveIt Motion Planning Framework 7 | 8 | Rico Ruotong Jia 9 | Rico Ruotong Jia 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_fake_controller_manager 21 | moveit_kinematics 22 | moveit_planners_ompl 23 | moveit_ros_visualization 24 | moveit_setup_assistant 25 | joint_state_publisher 26 | joint_state_publisher_gui 27 | robot_state_publisher 28 | tf2_ros 29 | xacro 30 | 31 | 32 | omnid 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/cmake-build-debug/CMakeFiles/clion-log.txt: -------------------------------------------------------------------------------- 1 | /home/ricojia/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/193.5662.56/bin/cmake/linux/bin/cmake -DCMAKE_BUILD_TYPE=Debug -G "CodeBlocks - Unix Makefiles" /home/ricojia/omnid_project/omnid_docker/omnid_docker_ws/src/omnid_rico/tf2_armadillo 2 | -- The C compiler identification is GNU 7.5.0 3 | -- The CXX compiler identification is GNU 7.5.0 4 | -- Check for working C compiler: /usr/bin/cc 5 | -- Check for working C compiler: /usr/bin/cc -- works 6 | -- Detecting C compiler ABI info 7 | -- Detecting C compiler ABI info - done 8 | -- Detecting C compile features 9 | -- Detecting C compile features - done 10 | -- Check for working CXX compiler: /usr/bin/c++ 11 | -- Check for working CXX compiler: /usr/bin/c++ -- works 12 | -- Detecting CXX compiler ABI info 13 | -- Detecting CXX compiler ABI info - done 14 | -- Detecting CXX compile features 15 | -- Detecting CXX compile features - done 16 | CMake Error at CMakeLists.txt:4 (find_package): 17 | By not providing "Findcatkin.cmake" in CMAKE_MODULE_PATH this project has 18 | asked CMake to find a package configuration file provided by "catkin", but 19 | CMake did not find one. 20 | 21 | Could not find a package configuration file provided by "catkin" with any 22 | of the following names: 23 | 24 | catkinConfig.cmake 25 | catkin-config.cmake 26 | 27 | Add the installation prefix of "catkin" to CMAKE_PREFIX_PATH or set 28 | "catkin_DIR" to a directory containing one of the above files. If "catkin" 29 | provides a separate development package or SDK, be sure it has been 30 | installed. 31 | 32 | 33 | -- Configuring incomplete, errors occurred! 34 | See also "/home/ricojia/omnid_project/omnid_docker/omnid_docker_ws/src/omnid_rico/tf2_armadillo/cmake-build-debug/CMakeFiles/CMakeOutput.log". 35 | -------------------------------------------------------------------------------- /omnid_planning_adapter_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(omnid_planning_adapter_plugin) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | moveit_core 6 | moveit_ros_planning 7 | pluginlib 8 | roscpp 9 | ) 10 | find_package(omnid_core) 11 | find_package(cmakeme) 12 | cmakeme_defaults(Debug) 13 | 14 | catkin_package( 15 | INCLUDE_DIRS include 16 | LIBRARIES omnid_planning_adapter_plugin 17 | CATKIN_DEPENDS 18 | moveit_core 19 | moveit_ros_planning 20 | pluginlib 21 | roscpp 22 | DEPENDS system_lib 23 | ) 24 | 25 | ########### 26 | ## Build ## 27 | ########### 28 | 29 | ## Specify additional locations of header files 30 | ## Your package locations should be listed before other locations 31 | include_directories( 32 | ${catkin_INCLUDE_DIRS} 33 | include 34 | ) 35 | 36 | ## Declare a C++ library 37 | add_library(${PROJECT_NAME} 38 | src/omnid_planning_adapter_plugin.cpp 39 | ) 40 | 41 | target_link_libraries(${PROJECT_NAME} 42 | PUBLIC 43 | ${catkin_LIBRARIES} 44 | omnid_core::omnid_core 45 | ) 46 | 47 | 48 | # cmakeme_install(${PROJECT_NAME} NAMESPACE ${PROJECT_NAME} DEPENDS omnid_core) 49 | 50 | ############# 51 | ## Install ## 52 | ############# 53 | 54 | ## Mark libraries for installation 55 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 56 | # install(TARGETS ${PROJECT_NAME} 57 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 58 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 59 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 60 | # ) 61 | 62 | # Mark cpp header files for installation 63 | install(DIRECTORY include/${PROJECT_NAME}/ 64 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 65 | FILES_MATCHING PATTERN "*.h" 66 | PATTERN ".svn" EXCLUDE 67 | ) 68 | -------------------------------------------------------------------------------- /omnid_moveit_config/config/ros_controllers.yaml: -------------------------------------------------------------------------------- 1 | # Simulation settings for using moveit_sim_controllers 2 | moveit_sim_hw_interface: 3 | joint_model_group: todo_group_name 4 | joint_model_group_pose: todo_state_name 5 | # Settings for ros_control_boilerplate control loop 6 | generic_hw_control_loop: 7 | loop_hz: 300 8 | cycle_time_error_threshold: 0.01 9 | # Settings for ros_control hardware interface 10 | hardware_interface: 11 | joints: 12 | - object_platform/x 13 | - object_platform/y 14 | - object_platform/z 15 | - object_platform/roll 16 | - object_platform/pitch 17 | - object_platform/yaw 18 | - robot_1/phi_1 19 | - robot_1/theta_1 20 | - robot_1/beta_1 21 | - robot_1/gamma_1 22 | - robot_1/phi_2 23 | - robot_1/theta_2 24 | - robot_1/beta_2 25 | - robot_1/gamma_2 26 | - robot_1/phi_3 27 | - robot_1/theta_3 28 | - robot_1/beta_3 29 | - robot_1/gamma_3 30 | - robot_1/x 31 | - robot_1/y 32 | - robot_1/z 33 | - robot_2/phi_1 34 | - robot_2/theta_1 35 | - robot_2/beta_1 36 | - robot_2/gamma_1 37 | - robot_2/phi_2 38 | - robot_2/theta_2 39 | - robot_2/beta_2 40 | - robot_2/gamma_2 41 | - robot_2/phi_3 42 | - robot_2/theta_3 43 | - robot_2/beta_3 44 | - robot_2/gamma_3 45 | - robot_2/x 46 | - robot_2/y 47 | - robot_2/z 48 | - robot_3/phi_1 49 | - robot_3/theta_1 50 | - robot_3/beta_1 51 | - robot_3/gamma_1 52 | - robot_3/phi_2 53 | - robot_3/theta_2 54 | - robot_3/beta_2 55 | - robot_3/gamma_2 56 | - robot_3/phi_3 57 | - robot_3/theta_3 58 | - robot_3/beta_3 59 | - robot_3/gamma_3 60 | - robot_3/x 61 | - robot_3/y 62 | - robot_3/z 63 | sim_control_mode: 1 # 0: position, 1: velocity 64 | # Publish all joint states 65 | # Creates the /joint_states topic necessary in ROS 66 | joint_state_controller: 67 | type: joint_state_controller/JointStateController 68 | publish_rate: 50 69 | controller_list: 70 | [] -------------------------------------------------------------------------------- /omnid_move_group_interface/include/omnid_move_group_interface/omnid_move_group_interface.hpp: -------------------------------------------------------------------------------- 1 | /// \file 2 | /// \brief Robot group level IK for calculating the pose of each robot and the end effector. The main setup is a group of omnid robots trying to carry an object 3 | /// We assume the object frame is placed at the centroid of the omnid robots bases. 4 | 5 | #ifndef OMNID_PROJECT_OMNID_GROUP_IK_HPP 6 | #define OMNID_PROJECT_OMNID_GROUP_IK_HPP 7 | #define ARMA_DONT_USE_WRAPPER 8 | 9 | #include 10 | 11 | typedef arma::Mat mat; 12 | 13 | namespace omnid_group_ik{ 14 | 15 | /// \brief High Level IK that update each robot's body frame end effector pose for a given object platfor pose in a reference frame 16 | /// \param in_pose - object platform pose in the world frame 17 | /// \param robot_to_world - transform from the robot body frame to the world frame. 18 | /// \param ref_to_robot = transform from the reference frame to robot 19 | /// \param out_pose - robot end effector pose (in the reference frame) to be updated. 20 | //TODO: take out the inverse 21 | void getRobotEefPose(const mat::fixed<4, 4> &in_pose, const mat::fixed<4, 4> &robot_to_world, 22 | const mat::fixed<4, 4> &ref_to_world, const mat::fixed<4, 4> &robot_to_world_inv, mat::fixed<4, 4> &out_pose) { 23 | // T_eff_ref = T_robot_world * T_in * T_ref_world * T_world_robot 24 | out_pose = (robot_to_world * in_pose); 25 | out_pose = out_pose * ref_to_world; 26 | // mat world_to_robot = arma::inv(robot_to_world); 27 | out_pose = out_pose * robot_to_world_inv; 28 | } 29 | /* 30 | void getRobotEefPose(const mat& in_pose, const mat &robot_to_world, 31 | const mat& ref_to_world, mat &out_pose) { 32 | // T_eff_ref = T_robot_world * T_in * T_ref_world * T_world_robot 33 | out_pose = (robot_to_world * in_pose); 34 | out_pose = out_pose * ref_to_world; 35 | mat world_to_robot = arma::inv(robot_to_world); 36 | out_pose = out_pose * world_to_robot; 37 | } 38 | */ 39 | 40 | 41 | } 42 | 43 | #endif //OMNID_PROJECT_OMNID_GROUP_IK_HPP 44 | -------------------------------------------------------------------------------- /omnid/launch/test_urdf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 17 | 28 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /omnid/scripts/disk.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import pybullet as p 4 | import numpy as np 5 | import rospy 6 | from sensor_msgs.msg import JointState 7 | 8 | class Disk: 9 | def __init__(self, urdf_path, spawn_position=[0.0, 0.0, 1.0], urdf_name = "disk.urdf"): 10 | urdf_full_path = urdf_path + "/" + urdf_name 11 | #pos will be [x,y,z], ori will be [roll, pitch, yaw] 12 | self.joint_names = rospy.get_param("omnid_simulator/object_platform_joint_info").keys() 13 | self.reset(urdf_full_path, spawn_position) 14 | self.object_thickness = rospy.get_param("object_thickness") #thickness of the end effector platform 15 | self.z_correction= rospy.get_param("base_offset") #The base was offseted in the URDF of the robot. Change this value if URDF has changed. 16 | 17 | def reset(self, urdf_full_path, spawn_position): 18 | self.model_unique_id = p.loadURDF(urdf_full_path, basePosition=spawn_position) 19 | self.spawn_position = spawn_position 20 | 21 | def getObjectInfo(self): 22 | """ return information such as spawn position, disk radius in a dictionary""" 23 | return {"spawn_position": self.spawn_position} 24 | 25 | def getEndEffectorLinkID(self): 26 | """ return the end effector link ID """ 27 | return -1 28 | 29 | def getJointStates(self): 30 | """ return the link states as a joint_state_msg""" 31 | joint_state_msg = JointState() 32 | joint_state_msg.name = self.joint_names 33 | pos, ori = p.getBasePositionAndOrientation(self.model_unique_id) 34 | pos = [pos[0] - self.spawn_position[0], pos[1] - self.spawn_position[1], pos[2] - self.z_correction] #x, y,z is defined as the relative pose to base. 35 | lin_vel, an_vel = p.getBaseVelocity(self.model_unique_id) 36 | rpy = p.getEulerFromQuaternion(ori) 37 | joint_state_msg.position += pos 38 | joint_state_msg.position += rpy 39 | joint_state_msg.velocity += lin_vel 40 | joint_state_msg.velocity += an_vel #an_vel is already Cartesian, but not sure if it's z,y,x 41 | joint_state_msg.effort += [0,0,0,0,0,0] 42 | #TODO 43 | joint_state_msg.name = ["x", "y", "z", "roll", "pitch", "yaw"] 44 | print("=====") 45 | print("pos", pos) 46 | print("joint_names: ", self.joint_names) 47 | print(joint_state_msg) 48 | return joint_state_msg 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /docker_setup/Dockerfile: -------------------------------------------------------------------------------- 1 | # This dockerfile sets up the build environment for the omnid project 2 | # Read through it to see what is needed to setup the environment 3 | 4 | # Use a docker image provided by ros 5 | FROM ros:noetic-ros-core-focal 6 | 7 | # ubuntu packages that are needed 8 | # Note: some of these packages are already covered when 9 | # doing a full desktop install of ubuntu/ros on the omnid computer 10 | RUN dpkg --add-architecture i386 \ 11 | && apt-get update -yq \ 12 | && apt-get install -yq --no-install-recommends \ 13 | build-essential \ 14 | curl \ 15 | dbus \ 16 | doxygen \ 17 | doxygen-latex \ 18 | git \ 19 | gcc-arm-none-eabi \ 20 | gdb-multiarch \ 21 | libarmadillo-dev \ 22 | liblapack-dev \ 23 | libc6:i386 \ 24 | libopenblas-dev \ 25 | libnewlib-arm-none-eabi \ 26 | libv4l-dev \ 27 | libstdc++-arm-none-eabi-newlib \ 28 | minicom \ 29 | openocd \ 30 | python3-pip \ 31 | python3-colcon-common-extensions \ 32 | python3-rosdep \ 33 | ros-noetic-camera-info-manager \ 34 | ros-noetic-catch-ros \ 35 | ros-noetic-diagnostic-updater \ 36 | ros-noetic-joint-state-controller \ 37 | ros-noetic-joint-state-publisher \ 38 | ros-noetic-pcl-conversions \ 39 | ros-noetic-pcl-ros \ 40 | ros-noetic-robot-state-publisher \ 41 | ros-noetic-rqt \ 42 | ros-noetic-rqt-common-plugins \ 43 | ros-noetic-rqt-console \ 44 | ros-noetic-rviz \ 45 | ros-noetic-self-test \ 46 | ros-noetic-usb-cam \ 47 | ros-noetic-xacro \ 48 | ros-noetic-moveit-simple-controller-manager \ 49 | ros-noetic-moveit \ 50 | v4l-utils \ 51 | && rm -rf /var/lib/apt/lists/* 52 | 53 | #install joint_state_publisher_gui 54 | RUN apt update \ 55 | && apt upgrade -y \ 56 | && apt install ros-noetic-joint-state-publisher-gui 57 | 58 | 59 | RUN pip3 install git+https://github.com/catkin/catkin_tools.git \ 60 | && pip3 install pybullet 61 | 62 | 63 | # install the ti-cgt-arm compiler (Optional as we have mainly been using gcc now) 64 | RUN curl -L http://software-dl.ti.com/codegen/esd/cgt_public_sw/TMS470/20.2.1.LTS/ti_cgt_tms470_20.2.1.LTS_linux_installer_x86.bin > /ti-cgt.run \ 65 | && chmod 755 /ti-cgt.run \ 66 | && /ti-cgt.run --unattendedmodeui none --mode unattended --prefix /opt/ti \ 67 | && rm /ti-cgt.run 68 | 69 | 70 | -------------------------------------------------------------------------------- /omnid/launch/moveit_omnid.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | [omnid/joint_states] 31 | 32 | 33 | 34 | [omnid/joint_states] 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/cmake-build-debug/CMakeFiles/3.15.3/CMakeCCompiler.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_C_COMPILER "/usr/bin/cc") 2 | set(CMAKE_C_COMPILER_ARG1 "") 3 | set(CMAKE_C_COMPILER_ID "GNU") 4 | set(CMAKE_C_COMPILER_VERSION "7.5.0") 5 | set(CMAKE_C_COMPILER_VERSION_INTERNAL "") 6 | set(CMAKE_C_COMPILER_WRAPPER "") 7 | set(CMAKE_C_STANDARD_COMPUTED_DEFAULT "11") 8 | set(CMAKE_C_COMPILE_FEATURES "c_std_90;c_function_prototypes;c_std_99;c_restrict;c_variadic_macros;c_std_11;c_static_assert") 9 | set(CMAKE_C90_COMPILE_FEATURES "c_std_90;c_function_prototypes") 10 | set(CMAKE_C99_COMPILE_FEATURES "c_std_99;c_restrict;c_variadic_macros") 11 | set(CMAKE_C11_COMPILE_FEATURES "c_std_11;c_static_assert") 12 | 13 | set(CMAKE_C_PLATFORM_ID "Linux") 14 | set(CMAKE_C_SIMULATE_ID "") 15 | set(CMAKE_C_COMPILER_FRONTEND_VARIANT "") 16 | set(CMAKE_C_SIMULATE_VERSION "") 17 | 18 | 19 | 20 | set(CMAKE_AR "/usr/bin/ar") 21 | set(CMAKE_C_COMPILER_AR "/usr/bin/gcc-ar-7") 22 | set(CMAKE_RANLIB "/usr/bin/ranlib") 23 | set(CMAKE_C_COMPILER_RANLIB "/usr/bin/gcc-ranlib-7") 24 | set(CMAKE_LINKER "/usr/bin/ld") 25 | set(CMAKE_MT "") 26 | set(CMAKE_COMPILER_IS_GNUCC 1) 27 | set(CMAKE_C_COMPILER_LOADED 1) 28 | set(CMAKE_C_COMPILER_WORKS TRUE) 29 | set(CMAKE_C_ABI_COMPILED TRUE) 30 | set(CMAKE_COMPILER_IS_MINGW ) 31 | set(CMAKE_COMPILER_IS_CYGWIN ) 32 | if(CMAKE_COMPILER_IS_CYGWIN) 33 | set(CYGWIN 1) 34 | set(UNIX 1) 35 | endif() 36 | 37 | set(CMAKE_C_COMPILER_ENV_VAR "CC") 38 | 39 | if(CMAKE_COMPILER_IS_MINGW) 40 | set(MINGW 1) 41 | endif() 42 | set(CMAKE_C_COMPILER_ID_RUN 1) 43 | set(CMAKE_C_SOURCE_FILE_EXTENSIONS c;m) 44 | set(CMAKE_C_IGNORE_EXTENSIONS h;H;o;O;obj;OBJ;def;DEF;rc;RC) 45 | set(CMAKE_C_LINKER_PREFERENCE 10) 46 | 47 | # Save compiler ABI information. 48 | set(CMAKE_C_SIZEOF_DATA_PTR "8") 49 | set(CMAKE_C_COMPILER_ABI "ELF") 50 | set(CMAKE_C_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 51 | 52 | if(CMAKE_C_SIZEOF_DATA_PTR) 53 | set(CMAKE_SIZEOF_VOID_P "${CMAKE_C_SIZEOF_DATA_PTR}") 54 | endif() 55 | 56 | if(CMAKE_C_COMPILER_ABI) 57 | set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_C_COMPILER_ABI}") 58 | endif() 59 | 60 | if(CMAKE_C_LIBRARY_ARCHITECTURE) 61 | set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 62 | endif() 63 | 64 | set(CMAKE_C_CL_SHOWINCLUDES_PREFIX "") 65 | if(CMAKE_C_CL_SHOWINCLUDES_PREFIX) 66 | set(CMAKE_CL_SHOWINCLUDES_PREFIX "${CMAKE_C_CL_SHOWINCLUDES_PREFIX}") 67 | endif() 68 | 69 | 70 | 71 | 72 | 73 | set(CMAKE_C_IMPLICIT_INCLUDE_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/7/include;/usr/local/include;/usr/lib/gcc/x86_64-linux-gnu/7/include-fixed;/usr/include/x86_64-linux-gnu;/usr/include") 74 | set(CMAKE_C_IMPLICIT_LINK_LIBRARIES "gcc;gcc_s;c;gcc;gcc_s") 75 | set(CMAKE_C_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/7;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") 76 | set(CMAKE_C_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") 77 | -------------------------------------------------------------------------------- /omnid_moveit_config/config/fake_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: fake_end_effector_1_controller 3 | joints: 4 | [] 5 | - name: fake_end_effector_2_controller 6 | joints: 7 | [] 8 | - name: fake_end_effector_3_controller 9 | joints: 10 | [] 11 | - name: fake_all_arms_controller 12 | joints: 13 | - object_platform/x 14 | - object_platform/y 15 | - object_platform/z 16 | - object_platform/roll 17 | - object_platform/pitch 18 | - object_platform/yaw 19 | - robot_1/theta_1 20 | - robot_1/beta_1 21 | - robot_1/gamma_1 22 | - robot_1/theta_2 23 | - robot_1/beta_2 24 | - robot_1/gamma_2 25 | - robot_1/theta_3 26 | - robot_1/beta_3 27 | - robot_1/gamma_3 28 | - robot_1/x 29 | - robot_1/y 30 | - robot_1/z 31 | - robot_2/theta_1 32 | - robot_2/beta_1 33 | - robot_2/gamma_1 34 | - robot_2/theta_2 35 | - robot_2/beta_2 36 | - robot_2/gamma_2 37 | - robot_2/theta_3 38 | - robot_2/beta_3 39 | - robot_2/gamma_3 40 | - robot_2/x 41 | - robot_2/y 42 | - robot_2/z 43 | - robot_3/theta_1 44 | - robot_3/beta_1 45 | - robot_3/gamma_1 46 | - robot_3/theta_2 47 | - robot_3/beta_2 48 | - robot_3/gamma_2 49 | - robot_3/theta_3 50 | - robot_3/beta_3 51 | - robot_3/gamma_3 52 | - robot_3/x 53 | - robot_3/y 54 | - robot_3/z 55 | - name: fake_end_effector_arm_1_controller 56 | joints: 57 | - robot_1/theta_1 58 | - robot_1/beta_1 59 | - robot_1/gamma_1 60 | - robot_1/theta_2 61 | - robot_1/beta_2 62 | - robot_1/gamma_2 63 | - robot_1/theta_3 64 | - robot_1/beta_3 65 | - robot_1/gamma_3 66 | - robot_1/x 67 | - robot_1/y 68 | - robot_1/z 69 | - name: fake_end_effector_arm_2_controller 70 | joints: 71 | - robot_2/theta_1 72 | - robot_2/beta_1 73 | - robot_2/gamma_1 74 | - robot_2/theta_2 75 | - robot_2/beta_2 76 | - robot_2/gamma_2 77 | - robot_2/theta_3 78 | - robot_2/beta_3 79 | - robot_2/gamma_3 80 | - robot_2/x 81 | - robot_2/y 82 | - robot_2/z 83 | - name: fake_end_effector_arm_3_controller 84 | joints: 85 | - robot_3/theta_1 86 | - robot_3/beta_1 87 | - robot_3/gamma_1 88 | - robot_3/theta_2 89 | - robot_3/beta_2 90 | - robot_3/gamma_2 91 | - robot_3/theta_3 92 | - robot_3/beta_3 93 | - robot_3/gamma_3 94 | - robot_3/x 95 | - robot_3/y 96 | - robot_3/z 97 | - name: fake_object_platform_eef_controller 98 | joints: 99 | [] 100 | - name: fake_object_platform_arm_controller 101 | joints: 102 | - object_platform/x 103 | - object_platform/y 104 | - object_platform/z 105 | - object_platform/roll 106 | - object_platform/pitch 107 | - object_platform/yaw 108 | initial: # Define initial robot poses. 109 | - group: fake_end_effector_arm_3_controller 110 | pose: home 111 | -------------------------------------------------------------------------------- /omnid_moveit_config/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | [move_group/fake_controller_joint_states] 36 | 37 | 38 | [move_group/fake_controller_joint_states] 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 | -------------------------------------------------------------------------------- /omnid_moveit_config/launch/demo_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 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 | [move_group/fake_controller_joint_states] 45 | [/joint_states] 46 | 47 | 48 | [move_group/fake_controller_joint_states] 49 | [/joint_states] 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 | -------------------------------------------------------------------------------- /omnid/urdf/disk.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 92 | 93 | 94 | -------------------------------------------------------------------------------- /omnid/scripts/spring_test/spring_test_simulator: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | ''' 4 | Simulate the torsional spring model and generate a plot for the associated states. 5 | The states are: 6 | 1. Coordinates of the spring joint(x,y) 7 | 2.angle between the spring and the parent link theta_p 8 | 3. angle between the spring and the child link, theta_c 9 | ''' 10 | 11 | import rospy 12 | import numpy as np 13 | import sys 14 | import pybullet as p 15 | import rospkg 16 | 17 | import numpy as np 18 | import pybullet_data 19 | from torsion_spring import Spring_Model 20 | import matplotlib as plt 21 | import matplotlib.pyplot as plt 22 | 23 | def plot_xy(tspan, dt, x,y, footnote): 24 | """A helper function to plot our data sets 25 | 26 | PARAMETERS 27 | ---------- 28 | x A 1d numpy array of x 29 | y A 1d numpy array of y 30 | footnote A python string for footnote 31 | """ 32 | # plot the data 33 | tvec = np.arange(start = min(tspan), stop = max(tspan),step=dt) 34 | plt.plot(tvec, x, label='x') 35 | plt.plot(tvec, y, label='y') 36 | plt.figtext(0.5, 0.01, footnote, ha="center") #foot note 37 | plt.axis('equal') 38 | plt.legend() 39 | plt.show() 40 | 41 | # ``` 42 | # - Make plots side by side 43 | # ``` 44 | # fig=plt.figure(figsize=(20,5), dpi= 100, facecolor='w', edgecolor='k') 45 | # plt.subplot(1,2,2) 46 | 47 | def plot_angles(tspan, dt, theta_c, theta_p, footnote): 48 | """A helper function to plot our data sets 49 | 50 | PARAMETERS 51 | ---------- 52 | x A 1d numpy array of x 53 | y A 1d numpy array of y 54 | footnote A python string for footnote 55 | """ 56 | # plot the data 57 | tvec = np.arange(start = min(tspan), stop = max(tspan),step=dt) 58 | if theta_c is not None: 59 | plt.plot(tvec, theta_c, label='child link angle') 60 | if theta_p is not None: 61 | plt.plot(tvec, theta_p, label='parentlink angle') 62 | plt.text(0.5, 0.01, footnote, ha="center") #foot note 63 | plt.axis('equal') 64 | plt.legend() 65 | plt.show() 66 | 67 | def main(unused_args): 68 | rospy.init_node("spring_test") 69 | c = p.connect(p.SHARED_MEMORY) 70 | if (c < 0): 71 | c = p.connect(p.GUI) 72 | p.setAdditionalSearchPath(pybullet_data.getDataPath()) 73 | 74 | p.resetSimulation() 75 | p.loadURDF("/plane.urdf" ) #loads from the root pybullet library 76 | p.setGravity(0, 0, 0.0) 77 | 78 | urdf_path = ((rospkg.RosPack()).get_path('omnid'))+'/urdf' 79 | print("urdf_path: ",urdf_path) 80 | p.setRealTimeSimulation(0) 81 | 82 | # global joint_values 83 | timeStep = 0.001 84 | p.setTimeStep(timeStep) 85 | r = rospy.Rate(1.0/timeStep) 86 | global spring_model 87 | initial_angle = 1.57 #radians 88 | torque_k=1 89 | joint_name="spring_joint" 90 | spring_model= Spring_Model(urdf_path, initial_angle, joint_name, torque_k) 91 | sim_time = 10.0 #second(s) 92 | #we need: base link x,y, orientation, joint angle 93 | child_joint_angle = np.zeros(int(sim_time/timeStep)) 94 | link_world_x = np.zeros(int(sim_time/timeStep)) 95 | link_world_y = np.zeros(int(sim_time/timeStep)) 96 | index = 0 97 | 98 | spring_turn = True 99 | while index < int(sim_time/timeStep) and p.isConnected() and not rospy.is_shutdown(): 100 | spring_model.apply_torque_spring() 101 | spring_model.non_zero_velocity_control() 102 | 103 | link_states = spring_model.get_link_state(link_index=0) 104 | child_joint_angle[index] = spring_model.get_joint_state(joint_name="spring_joint")["jointPosition"] 105 | link_world_x[index] = link_states["link_world_position"][0] 106 | link_world_y[index] = link_states["link_world_position"][1] 107 | p.stepSimulation() 108 | index+=1 109 | r.sleep() 110 | 111 | # plot_xy(tspan=[0,sim_time], dt=timeStep, x=link_world_x, y=link_world_y, footnote="position plot") 112 | plot_angles(tspan=[0, sim_time], dt=timeStep, theta_c=child_joint_angle, theta_p=None, footnote="joint angles") 113 | if __name__ == "__main__": 114 | try: 115 | main(0); 116 | except rospy.ROSInterruptException: 117 | pass 118 | -------------------------------------------------------------------------------- /omnid_moveit_config/launch/move_group.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 31 | 32 | 33 | 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 | -------------------------------------------------------------------------------- /omnid/scripts/torsion_spring_omnid.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import pybullet as p 4 | import numpy as np 5 | 6 | class Spring_Model: 7 | 8 | def __init__(self, model_unique_id, 9 | child_link_id, force_axis_c, moment_arm_c, force_pos_c, 10 | parent_link_id, force_axis_p, moment_arm_p, force_pos_p, 11 | primary_joint_id, secondary_joint_id, torque_k=0.1): 12 | """ 13 | In URDF, there might be two spring joints: one is attached to the end of the parent link (joint angle phi), one is attached to the beginning of the child link (joint angle theta). 14 | Phi is defined as the angle between the parent link (like a base)and a dummy spring link, theta is defined as the angle between the parent link and the child link (like an arm) 15 | :param model_unique_id: int, the model id of an omnid robot 16 | :param child_link_id: int, actual link spring force exerted on, like an arm. 17 | :param force_axis_c: array-like, unit vector of force in child link frame, when (phi - theta) is positive. [0,0,1] 18 | :param moment_arm_c: float, length of the child link origin to the spring 19 | :param parent_link_id: int, actual link spring force exerted on, like base_link 20 | :param force_axis_p: unit vector of force in parent link frame, when (phi - theta) is positive [0, 0, 1] 21 | :param moment_arm_p: float, length of the parent link origin to the spring 22 | :param primary_joint_id: joint connecting the base and the lower_arm_attached. Note that on Omnid, this is not an actuated joint, but a reference joint. 23 | :param secondary_joint_id: joint connecting the base and the lower arm, on Omnid this is an actuated joint. 24 | :param torque_k: torsion spring constant 25 | """ 26 | self.model_unique_id = model_unique_id 27 | self.child_link_id = child_link_id 28 | self.scaled_force_axis_c = 1.0/moment_arm_c * np.array(force_axis_c) 29 | self.parent_link_id = parent_link_id 30 | self.scaled_force_axis_p = 1.0/moment_arm_p * np.array(force_axis_p) 31 | self.primary_joint_id = primary_joint_id 32 | self.secondary_joint_id = secondary_joint_id 33 | self.force_pos_c = force_pos_c 34 | self.force_pos_p = force_pos_p 35 | self.torque_k = torque_k 36 | self.reset() 37 | 38 | def reset(self): 39 | #enable the actuated joint sensor on the secondary joint 40 | p.enableJointForceTorqueSensor(bodyUniqueId = self.model_unique_id, jointIndex = self.secondary_joint_id, enableSensor = True) 41 | 42 | def apply_spring_torque(self): 43 | """ 44 | The main API function other application should call to execute apply the corresponding torque. 45 | On omnid, the secondary joint (joint connecting the base and the lower arm) is the actuated joint 46 | """ 47 | #This is essential, as this disables the default motor imposed by bullet. These default motors prevent overspeeding, and functions as 48 | #friction drives. 49 | p.setJointMotorControl2(self.model_unique_id, self.secondary_joint_id, 50 | controlMode=p.VELOCITY_CONTROL, force=0) 51 | spring_angle = self.get_joint_angle() 52 | torque = spring_angle * self.torque_k 53 | 54 | #apply external force 55 | self.apply_external_force(force= torque * self.scaled_force_axis_c, link_index=self.child_link_id, position=self.force_pos_c) 56 | self.apply_external_force(force= torque * self.scaled_force_axis_p, link_index=self.parent_link_id, position=self.force_pos_p) 57 | # print("torque: ", torque) 58 | 59 | def get_joint_angle(self): 60 | """ 61 | :return: angle between phi and theta (primary_joint - secondary_joint) 62 | """ 63 | primary_joint_info = p.getJointState(bodyUniqueId = self.model_unique_id, jointIndex=self.primary_joint_id) 64 | secondary_joint_info = p.getJointState(bodyUniqueId = self.model_unique_id, jointIndex=self.secondary_joint_id) 65 | phi = primary_joint_info[0] 66 | theta = secondary_joint_info[0] 67 | # print("theta: ", theta, "phi: ", phi) 68 | return phi - theta 69 | 70 | def apply_external_force(self, force, link_index, position=[0,0,0]): 71 | """ 72 | Apply the specified external force on the specified position on the body / link. 73 | Args: 74 | link_index (int): unique link id. If -1, it will be the base. 75 | force (np.array[float[3]]): external force to be applied. 76 | position (np.array[float[3]], None): position on the link where the force is applied. See `flags` for 77 | coordinate systems. If None, it is the center of mass of the body (or the link if specified). 78 | frame (int): Specify the coordinate system of force/position: either `pybullet.WORL D_FRAME` (=2) for 79 | Cartesian world coordinates or `pybullet.LINK_FRAME` (=1) for local link coordinates. 80 | """ 81 | p.applyExternalForce( 82 | objectUniqueId=self.model_unique_id, linkIndex=link_index, 83 | forceObj=force, posObj=position, flags=p.LINK_FRAME) 84 | # print("link_id: ", link_index, " | force: ", force) 85 | -------------------------------------------------------------------------------- /omnid/params/params.yaml: -------------------------------------------------------------------------------- 1 | # This file has all the parameters needed for the omnid project 2 | 3 | #NOTE: Parameters shared by urdf, moveit plugins. 4 | object_clearance: 0.2 #the gap between the bottom of the object and the top of the robot end effector when the the object platform is horitontal 5 | object_thickness: 0.01 6 | object_inscribed_radius: 0.8 #the object is modelled as a cylinder in Moveit. 7 | object_mass: 50.0 #in kg 8 | base_positions: [[0.0, 0.0, 0.0], [0.5, 0.5, 0.0], [1.0, 0.0, 0.0]] #Cartesian coordinates of the each robot's base_link 9 | h_platform: 0.01 #delta_arm_end_effector_thickness 10 | base_offset: 0.046375 #how much the base should be lifted to bring to the ground. 11 | leg_num: 3 #total number of legs 12 | 13 | #High level parameters 14 | robot_names: ["robot_1", "robot_2", "robot_3"] #the first part of joint names in URDF. 15 | object_name: "object_platform" #also is the prefix of the object platform's joints in the urdf for moveit 16 | 17 | #omnid_move_group_interface & omnid_planning_adapter_plugin 18 | world_frame_name: "world" 19 | body_frame_name_prefix: "robot_" 20 | floating_world_frame_name: "/floating_world_0" #the name of the first "floating world" name of each robot. please include '/' if there is any. see each robot's URDF 21 | group_reference_frame_name: "object_platform_base" 22 | planning_group_prefix: "end_effector_arm_" 23 | object_platform_planning_group_name: "object_platform_arm" 24 | object_platform_eef_name: "object_platform_roll_link" 25 | eef_update_topic_name: "/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update" 26 | eef_update_frame_name: "EE:goal_object_platform_roll_link" 27 | robot_planning_group_names: ["end_effector_arm_1", "end_effector_arm_2", "end_effector_arm_3"] #each robot (not including the object platform arm) planning group's name in SRDF 28 | object_platform_name: "object_platform" 29 | 30 | #IK plugin 31 | theta_name: "theta" #see Matt's inverse kinematics pdf 32 | gamma_name: "gamma" #see Matt's inverse kinematics pdf 33 | beta_name: "beta" 34 | 35 | 36 | omnid_simulator: 37 | #Joint Position Controller parameters 38 | kp : 1 39 | kd : 0.6 40 | max_motor_force : 150000 41 | 42 | urdf_name: "delta_robot_pybullet.urdf" #file name of the URDF 43 | 44 | spring_parent_link_name : "lower_leg_prespring" #link connects to the pre-spring joint 45 | spring_child_link_name : "lower_leg" #links connects to the after-spring joint 46 | upper_leg_name: "upper_leg" #leg connects to the end-effector platform 47 | upper_leg_length: 0.368 #length of the upper leg, see URDF 48 | 49 | # We initialize our arm with identical pre_spring joint values (radians) 50 | pre_spring_joint_info: {"phi": 0.70} #name of the pre_spring_joint: joint_initial_value 51 | after_spring_joint_info: {"theta":1.50} #name of the after_spring_joint: joint_initial_value 52 | object_platform_joint_info: {"x": 0, "y": 0, "z": 0, "roll": 0, "pitch": 0, "yaw": 0} #PLEASE DO NOT CHANGE THESE NAMES AND THEIR ORDERING! joint names should match those of moveit urdf's object platform joints 53 | 54 | end_effector_initial_xyz: {"x":0.0, "y":0.0, "z":0.42} 55 | end_effector_radius: 0.062 #Delta Robot end effector radius 56 | end_effector_name: "platform_link" #end-effector platform name 57 | 58 | leg_pos_on_end_effector: {"upper_leg_1": 0.0, "upper_leg_2": rad(2.0*pi/3.0), "upper_leg_3": rad(4.0*pi/3.0)} #angular positions of upper legs on the platform 59 | 60 | #spring params - our spring applies "equivalent forces" on the lower arm and pre-spring links. Find where you want to apply these forces based on URDF. 61 | force_axis_c: [0.0, 0.0, 1.0] #positive unit vector of force on child link of after_spring_joint, when torque from the spring is positive 62 | lower_arm_length: 0.2 #length of the lower arm, right after the after_spring joint. 63 | force_pos_c: [0.1, 0.0, 0.0] #where we want to apply the equivalent spring force on the child link (positive direction is specified in force_axis_c) 64 | force_axis_p : [0.0, 0.0, 1.0] #positive unit vector of force on link of before_spring_joint, when torque from the spring is positive 65 | pre_spring_radius: 0.02 # radius of the pre-spring link 66 | force_pos_p: [0.0, 0.02, 0.0] #where we want to apply the equivalent spring force on link of before_spring_joint (positive direction is specified in force_axis_p) 67 | torque_k: 1000.0 #torsion spring constant 68 | 69 | #controller setup for Moveit! interface. Change this before modifying the Moveit! setup 70 | controller_namespace: "all_arms_controller" #action_ns of omnid_moveit_config's omnid_controllers.yaml 71 | 72 | #Test parameters. For production default value is false for every item here 73 | after_spring_actuated: true #By default, the pre-spring joints are actuated. However, that requires a PID controller. For testing purposes, we can apply control directly on after-spring joints. 74 | test_joint_control: true #enable direct joint level control on pre_spring joints (phi) 75 | test_with_end_effector_xyz: false #enable actuation on the end-effector x, y, z joints, which means other joint control will not be effective. Note that you have to set test_mode_on to start testing. 76 | test_without_spring: true -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/include/tf2_armadillo/tf2_armadillo.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ricojia on 11/16/20. 3 | // 4 | 5 | #ifndef OMNID_PROJECT_TF2_ARMADILLO_H 6 | #define OMNID_PROJECT_TF2_ARMADILLO_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace tf2{ 16 | typedef arma::Mat mat; 17 | 18 | /// \brief Quaternion to a SE(3) matrix with the corresponding rotation matrix and 0 translation 19 | /// \param x - x in quaternion 20 | /// \param y - y in quaternion 21 | /// \param z - z in quaternion 22 | /// \param w - w in quaternion 23 | /// \return 4x4 SE(3) matrix with the corresponding rotation matrix and 0 translation 24 | inline mat::fixed<4, 4> quatToRotation(const double x, const double y, const double z, const double w) { 25 | mat::fixed<4, 4> R = {{(1 - 2 * y*y - 2 * z*z), (2 * x*y - 2 * z*w), (2 * x*z + 2 * y*w), 0}, 26 | {(2 * x*y + 2 * z*w), (1 - 2 * x*x - 2 * z*z), (2 * y*z - 2 * x*w), 0}, 27 | {(2 * x*z - 2 * y*w) , (2 * y*z + 2 * x*w) , (1 - 2 * x*x - 2 * y*y), 0}, 28 | {0, 0, 0, 1}}; 29 | return R; 30 | } 31 | 32 | /// \brief Translation in the Cartesian Space to a SE(3) Matrix with the corresponding translation and identity rotation matrix 33 | /// \param x - Translation along the x axis 34 | /// \param y - Translation along the y axis 35 | /// \param z - Translation along the z axis 36 | /// \return 4x4 SE(3) matrix with the corresponding translation and identity rotation 37 | inline mat::fixed<4,4> xyzToTranslation(const double x, const double y, const double z){ 38 | mat::fixed<4, 4> T = {{1, 0, 0, x}, 39 | {0, 1, 0, y}, 40 | {0, 0, 1, z}, 41 | {0, 0, 0, 1}}; 42 | return T; 43 | } 44 | 45 | /// \brief Matrix3x3 to a SE(3) matrix with the corresponding rotation matrix and 0 translation 46 | /// \param R_tf2 Rotation matrix in Matrix3x3 47 | /// \return Euler angles expressed in tf2::Vector3 48 | inline mat::fixed<4,4> matrix3x3ToRotation(const Matrix3x3& R_tf2){ 49 | auto row_0 = R_tf2.getRow(0); 50 | auto row_1 = R_tf2.getRow(1); 51 | auto row_2 = R_tf2.getRow(2); 52 | mat::fixed<4, 4> R = {{row_0.x(), row_0.y(), row_0.z(), 0}, 53 | {row_1.x(), row_1.y(), row_1.z(), 0}, 54 | {row_2.x(), row_2.y(), row_2.z(), 0}, 55 | {0, 0, 0, 1}}; 56 | return R; 57 | } 58 | 59 | /// \brief Transform to Armadillo matrix for a tf2::Transform message 60 | /// \param tf Transform 61 | /// \param out output transform in mat::fixed<4, 4> 62 | /// \return SE(3) matrix for the transform 63 | inline void fromMsg (const Transform &msg, mat::fixed<4, 4>& out) { 64 | auto o = msg.getOrigin(); 65 | out = (xyzToTranslation(o.x(), o.y(), o.z())) * (matrix3x3ToRotation(msg.getBasis())); 66 | } 67 | 68 | /// \brief Transform to Armadillo matrix for a geometry_msgs::Transform message 69 | /// \param tf - in geometry_msgs::Transform 70 | /// \param out output transform in mat::fixed<4, 4> 71 | /// \return SE(3) matrix for the transform 72 | inline void fromMsg(const geometry_msgs::Transform& in, mat::fixed<4,4>& out){ 73 | auto p = in.translation; 74 | auto r = in.rotation; 75 | out = (xyzToTranslation(p.x, p.y, p.z)) * (quatToRotation(r.x, r.y, r.z, r.w)); 76 | } 77 | 78 | /// \brief Armadillo Matrix to tf2::Transform messages 79 | /// \param T - Transform 80 | /// \return Transform in tf2::Transform 81 | inline Transform toMsg (const mat::fixed<4, 4> T, Transform& TF){ 82 | Matrix3x3 R; 83 | R.setValue(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), T(2, 1), T(2, 2)); 84 | Vector3 p; 85 | p.setValue(T(0, 3), T(1, 3), T(2, 3)); 86 | 87 | TF.setBasis(R); 88 | TF.setOrigin(p); 89 | return TF; 90 | } 91 | 92 | /// \brief convert armadillo matrix to pose 93 | /// \param T - Transform 94 | /// \param pose - pose message to be outputed 95 | /// \return - the same pose message as in the parameters 96 | inline geometry_msgs::Pose toMsg (const mat::fixed<4, 4> T, geometry_msgs::Pose& pose){ 97 | Transform TF; 98 | toMsg(T, TF); 99 | toMsg(TF, pose); 100 | return pose; 101 | } 102 | 103 | /// \brief Armadillo Matrix to geometry_msgs::Transform 104 | /// \param T - Transform 105 | /// \return Transform in geometry_msgs::Transform 106 | inline geometry_msgs::Transform toMsg(const mat::fixed<4, 4> T){ 107 | Transform tf_tf2; 108 | tf_tf2 = toMsg(T, tf_tf2); 109 | geometry_msgs::Transform tf_geo = tf2::toMsg(tf_tf2); 110 | return tf_geo; 111 | } 112 | 113 | } 114 | #endif //OMNID_PROJECT_TF2_ARMADILLO_H 115 | -------------------------------------------------------------------------------- /omnid/urdf/spring/spring.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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Omnid Simulator and Moveit! Packages 2 | 3 | 4 | This is the main repository of an Omnid group simulator and its Moveit! motion planning pipeline. 5 | If you haven't done so, check out my [**blog post**](https://ricoruotongjia.medium.com/s-d65d8ffcc73d) for some higher level implementation detals. 6 | 7 | ![Hnet-image](https://user-images.githubusercontent.com/39393023/101986502-bfa2c600-3c53-11eb-8a7a-8cc360877151.gif) 8 | 9 | There are 3 Delta robots and one object platform in this project. 10 | ![1*c-ZS4OG450Jem4atBD1XIQ](https://user-images.githubusercontent.com/39393023/101987475-eebc3600-3c59-11eb-9791-72eb11737a46.png) 11 | 12 | The simulator includes: 13 | - Robot model, including URDF and PyBullet model 14 | - Joint control and joint information output 15 | - Torsion springs mounted on each robot as serial elastic actuators 16 | - Validation of torsion springs. 17 | 18 | ![Screenshot from 2020-12-11 18-48-34](https://user-images.githubusercontent.com/39393023/101987454-cc2a1d00-3c59-11eb-96fb-80ca5ef36e69.png) 19 | 20 | The motion planning pipeline includes 21 | - `omnid_moveit_config`: a properly configured Moveit! configuration package 22 | - Robot URDF model: URDF specifically for Moveit! 23 | - Plugins: 24 | - omnid_move_group_interface: updates robot poses when the object's interactive marker is clicked and dragged 25 | - omnid_kinematics_plugin: IK plugin for a single delta robot 26 | - omnid_planning_adapter_plugin: post-processing planner adapter that generates the right trajectory waypoints 27 | - Supplementary Packages: 28 | - control_msgs: this is the ROS [control_msgs package](https://wiki.ros.org/control_msgs), but as of Dec.2020 it is not 29 | available in Ubuntu 20.04 apt store yet. 30 | - tf2_armadillo: transformations for common Armadillo matrices and transform datatypes in ROS tf2 and geometry_msgs 31 | ![](https://media.giphy.com/media/6MiY7FDVTuBx0ZuSn1/giphy.gif) 32 | 33 | There's also some files for setting up the docker in `docker_setup` 34 | - docker file: docker image file 35 | - dockint: tool to build a docker container. Created by Dr. Matt Elwin 36 | - ros_settings.bash: settings to source when starting the docker file 37 | 38 | ### Build the Package 39 | The repo runs on Ubuntu Linux 20.04, ROS Noetic, rosdep, catkin_tools and several other prerequisites. 40 | As a universal way to setup the workspace, 41 | 42 | 1. Create workspace 43 | ``` shell script 44 | mkdir -p omnid/src 45 | cd omnid/src 46 | git clone https://github.com/RicoJia/Omnid_Project.git 47 | ``` 48 | 2. Pull Dependencies 49 | - ```cd Omnid_Project``` Go to the project directory 50 | - Install VCS tool, [see here](https://github.com/dirk-thomas/vcstool) 51 | - Use VCS tool to download all necessary packages. **If VCS tool does not work, try git clone all pacakges in ```docker_setup/omnid_docs.repos``` manually.** 52 | ```shell script 53 | vcs import < docker_setup/omnid_docs.repos 54 | ``` 55 | - ```cd ../..``` go to the root of the package 56 | 57 | 3. Build a docker container and start it(Please use this dockerfile as it contains the latest dependencies we need) 58 | - Build the image 59 | ``` 60 | cp src/Omnid_Project/docker_setup/dockint . 61 | cp src/Omnid_Project/docker_setup/Dockerfile . 62 | ./dockint from omnid $(pwd)/src/Omnid_Project/docker_setup 63 | ``` 64 | 65 | - Build a container. Thanks [Dr.Matt Elwin](https://robotics.northwestern.edu/people/profiles/faculty/elwin-matt.html) for sharing his dockint tool! 66 | ``` 67 | ./dockint start omnid $(pwd) 68 | ./dockint run omnid bash 69 | ``` 70 | 71 | 4. Build the package in the docker container 72 | - ```source /opt/ros/noetic/setup.bash``` this will setup some initial settings 73 | - ```catkin build``` build this package 74 | - ```source devel/setup.bash``` Source the workspace 75 | - ```source src/Omnid_Project/docker_setup/ros_settings.bash``` Source some correct ros settings that overwrite the previous ones 76 | - ```roslaunch omnid omnid.launch ``` Launch the project 77 | - Have fun planning! 78 | 79 | 5. Once done with the project 80 | - ```exit``` to exit the container 81 | - ```./dockint stop omnid``` to stop the container 82 | 83 | ### Basic Usage 84 | On Rviz, 85 | 1. Click and drag the marker embedded in the object platform to a desired pose 86 | - If we first start at unachievable poses, we can drag the marker anywhere we want until we reach the first achievable poses. 87 | - If we try to reach a valid pose to an unachievable pose, the marker will jump back to the closest valid pose 88 | 2. Hit plan to see the plan 89 | 3. Hit plan and execute to execute the plan 90 | - The simulator will return fail only if the action request from Moveit! is preempted. 91 | This is because Moveit! checks the proximity between the plan and the actual robot TF. Also it checks if the controller takes 92 | too long to execute a path. see `move_group/trajectory_execution/execution_duration_monitoring` 93 | 94 | ### External Resources 95 | 1. [**Notes for Developers**](https://docs.google.com/document/d/1iKX31U3NSdvEZ6RA3NsiCXwdtE4my4krcZCF_odcYEk/edit?usp=sharing) 96 | 2. [**Blog Post**](https://ricoruotongjia.medium.com/s-d65d8ffcc73d) 97 | 3. **Video Demonstration** 98 | [![Screenshot from 2020-12-13 14-03-54](https://user-images.githubusercontent.com/39393023/102022386-22778880-3d4c-11eb-8136-cbd5e864e6c6.png)](https://youtu.be/mwzxtJbuQ-Y) 99 | -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/cmake-build-debug/CMakeFiles/3.15.3/CMakeCXXCompiler.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_CXX_COMPILER "/usr/bin/c++") 2 | set(CMAKE_CXX_COMPILER_ARG1 "") 3 | set(CMAKE_CXX_COMPILER_ID "GNU") 4 | set(CMAKE_CXX_COMPILER_VERSION "7.5.0") 5 | set(CMAKE_CXX_COMPILER_VERSION_INTERNAL "") 6 | set(CMAKE_CXX_COMPILER_WRAPPER "") 7 | set(CMAKE_CXX_STANDARD_COMPUTED_DEFAULT "14") 8 | set(CMAKE_CXX_COMPILE_FEATURES "cxx_std_98;cxx_template_template_parameters;cxx_std_11;cxx_alias_templates;cxx_alignas;cxx_alignof;cxx_attributes;cxx_auto_type;cxx_constexpr;cxx_decltype;cxx_decltype_incomplete_return_types;cxx_default_function_template_args;cxx_defaulted_functions;cxx_defaulted_move_initializers;cxx_delegating_constructors;cxx_deleted_functions;cxx_enum_forward_declarations;cxx_explicit_conversions;cxx_extended_friend_declarations;cxx_extern_templates;cxx_final;cxx_func_identifier;cxx_generalized_initializers;cxx_inheriting_constructors;cxx_inline_namespaces;cxx_lambdas;cxx_local_type_template_args;cxx_long_long_type;cxx_noexcept;cxx_nonstatic_member_init;cxx_nullptr;cxx_override;cxx_range_for;cxx_raw_string_literals;cxx_reference_qualified_functions;cxx_right_angle_brackets;cxx_rvalue_references;cxx_sizeof_member;cxx_static_assert;cxx_strong_enums;cxx_thread_local;cxx_trailing_return_types;cxx_unicode_literals;cxx_uniform_initialization;cxx_unrestricted_unions;cxx_user_literals;cxx_variadic_macros;cxx_variadic_templates;cxx_std_14;cxx_aggregate_default_initializers;cxx_attribute_deprecated;cxx_binary_literals;cxx_contextual_conversions;cxx_decltype_auto;cxx_digit_separators;cxx_generic_lambdas;cxx_lambda_init_captures;cxx_relaxed_constexpr;cxx_return_type_deduction;cxx_variable_templates;cxx_std_17") 9 | set(CMAKE_CXX98_COMPILE_FEATURES "cxx_std_98;cxx_template_template_parameters") 10 | set(CMAKE_CXX11_COMPILE_FEATURES "cxx_std_11;cxx_alias_templates;cxx_alignas;cxx_alignof;cxx_attributes;cxx_auto_type;cxx_constexpr;cxx_decltype;cxx_decltype_incomplete_return_types;cxx_default_function_template_args;cxx_defaulted_functions;cxx_defaulted_move_initializers;cxx_delegating_constructors;cxx_deleted_functions;cxx_enum_forward_declarations;cxx_explicit_conversions;cxx_extended_friend_declarations;cxx_extern_templates;cxx_final;cxx_func_identifier;cxx_generalized_initializers;cxx_inheriting_constructors;cxx_inline_namespaces;cxx_lambdas;cxx_local_type_template_args;cxx_long_long_type;cxx_noexcept;cxx_nonstatic_member_init;cxx_nullptr;cxx_override;cxx_range_for;cxx_raw_string_literals;cxx_reference_qualified_functions;cxx_right_angle_brackets;cxx_rvalue_references;cxx_sizeof_member;cxx_static_assert;cxx_strong_enums;cxx_thread_local;cxx_trailing_return_types;cxx_unicode_literals;cxx_uniform_initialization;cxx_unrestricted_unions;cxx_user_literals;cxx_variadic_macros;cxx_variadic_templates") 11 | set(CMAKE_CXX14_COMPILE_FEATURES "cxx_std_14;cxx_aggregate_default_initializers;cxx_attribute_deprecated;cxx_binary_literals;cxx_contextual_conversions;cxx_decltype_auto;cxx_digit_separators;cxx_generic_lambdas;cxx_lambda_init_captures;cxx_relaxed_constexpr;cxx_return_type_deduction;cxx_variable_templates") 12 | set(CMAKE_CXX17_COMPILE_FEATURES "cxx_std_17") 13 | set(CMAKE_CXX20_COMPILE_FEATURES "") 14 | 15 | set(CMAKE_CXX_PLATFORM_ID "Linux") 16 | set(CMAKE_CXX_SIMULATE_ID "") 17 | set(CMAKE_CXX_COMPILER_FRONTEND_VARIANT "") 18 | set(CMAKE_CXX_SIMULATE_VERSION "") 19 | 20 | 21 | 22 | set(CMAKE_AR "/usr/bin/ar") 23 | set(CMAKE_CXX_COMPILER_AR "/usr/bin/gcc-ar-7") 24 | set(CMAKE_RANLIB "/usr/bin/ranlib") 25 | set(CMAKE_CXX_COMPILER_RANLIB "/usr/bin/gcc-ranlib-7") 26 | set(CMAKE_LINKER "/usr/bin/ld") 27 | set(CMAKE_MT "") 28 | set(CMAKE_COMPILER_IS_GNUCXX 1) 29 | set(CMAKE_CXX_COMPILER_LOADED 1) 30 | set(CMAKE_CXX_COMPILER_WORKS TRUE) 31 | set(CMAKE_CXX_ABI_COMPILED TRUE) 32 | set(CMAKE_COMPILER_IS_MINGW ) 33 | set(CMAKE_COMPILER_IS_CYGWIN ) 34 | if(CMAKE_COMPILER_IS_CYGWIN) 35 | set(CYGWIN 1) 36 | set(UNIX 1) 37 | endif() 38 | 39 | set(CMAKE_CXX_COMPILER_ENV_VAR "CXX") 40 | 41 | if(CMAKE_COMPILER_IS_MINGW) 42 | set(MINGW 1) 43 | endif() 44 | set(CMAKE_CXX_COMPILER_ID_RUN 1) 45 | set(CMAKE_CXX_IGNORE_EXTENSIONS inl;h;hpp;HPP;H;o;O;obj;OBJ;def;DEF;rc;RC) 46 | set(CMAKE_CXX_SOURCE_FILE_EXTENSIONS C;M;c++;cc;cpp;cxx;mm;CPP) 47 | set(CMAKE_CXX_LINKER_PREFERENCE 30) 48 | set(CMAKE_CXX_LINKER_PREFERENCE_PROPAGATES 1) 49 | 50 | # Save compiler ABI information. 51 | set(CMAKE_CXX_SIZEOF_DATA_PTR "8") 52 | set(CMAKE_CXX_COMPILER_ABI "ELF") 53 | set(CMAKE_CXX_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 54 | 55 | if(CMAKE_CXX_SIZEOF_DATA_PTR) 56 | set(CMAKE_SIZEOF_VOID_P "${CMAKE_CXX_SIZEOF_DATA_PTR}") 57 | endif() 58 | 59 | if(CMAKE_CXX_COMPILER_ABI) 60 | set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_CXX_COMPILER_ABI}") 61 | endif() 62 | 63 | if(CMAKE_CXX_LIBRARY_ARCHITECTURE) 64 | set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 65 | endif() 66 | 67 | set(CMAKE_CXX_CL_SHOWINCLUDES_PREFIX "") 68 | if(CMAKE_CXX_CL_SHOWINCLUDES_PREFIX) 69 | set(CMAKE_CL_SHOWINCLUDES_PREFIX "${CMAKE_CXX_CL_SHOWINCLUDES_PREFIX}") 70 | endif() 71 | 72 | 73 | 74 | 75 | 76 | set(CMAKE_CXX_IMPLICIT_INCLUDE_DIRECTORIES "/usr/include/c++/7;/usr/include/x86_64-linux-gnu/c++/7;/usr/include/c++/7/backward;/usr/lib/gcc/x86_64-linux-gnu/7/include;/usr/local/include;/usr/lib/gcc/x86_64-linux-gnu/7/include-fixed;/usr/include/x86_64-linux-gnu;/usr/include") 77 | set(CMAKE_CXX_IMPLICIT_LINK_LIBRARIES "stdc++;m;gcc_s;gcc;c;gcc_s;gcc") 78 | set(CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/7;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") 79 | set(CMAKE_CXX_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") 80 | -------------------------------------------------------------------------------- /omnid/urdf/spring/spring.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 | 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 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 107 | 108 | 116 | 117 | 120 | 121 | 129 | 130 | 133 | 134 | 142 | 143 | 146 | 147 | 148 | 149 | -------------------------------------------------------------------------------- /docker_setup/dockint: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | usage=\ 3 | "Usage: dockint from [PATH/URL] 4 | or: dockint start [homedir] 5 | or: dockint stop 6 | or: dockint run 7 | Work interactively with a docker container as if it's environment were on the host system. 8 | You work as your current user, all devices are available, and ports running in the container are shared with the host. 9 | 10 | 1. from [PATH/URL] : build a docker container starting with the provided image (e.g ubuntu:focal) 11 | If PATH/URL is specified, base_image will be built from the provided Dockerfile and context. 12 | 2. start [homedir] : start the docker container, mounting homedir as your home directory (this defaults to /home/user/image) 13 | 3. stop : stop the docker container 14 | 4. run : run the command specified by in the container corresponding to . 15 | For example, bash or zsh (which will be launched interactively). 16 | If the container has not been built in will be built. 17 | If the container has not been started it will be started with a default home directory of ~/dockerhome if it exists. 18 | The container follows the naming pattern ${base_image}_dockint, but can be directly specified by setting the container environment variable: 19 | container=containername dockint start base_image 20 | The image that is used is called ${base_image}_img but can be overrident by setting the image environment variable 21 | " 22 | 23 | dockerfile=\ 24 | 'ARG BASE=ubuntu:focal 25 | FROM $BASE 26 | # Make Xwindows work with the native x server and qt 27 | ENV DISPLAY=:0 28 | ENV QT_X11_NO_MITSHM=1 29 | # Get the user id from the host system 30 | # These args are passed at build time 31 | # and set up the docker container to have the same 32 | # users as the host system 33 | ARG UNAME=ricojia 34 | ARG UID=1000 35 | ARG GID=1000 36 | # create a normal user called "$USER" 37 | RUN groupadd -f -g $GID $UNAME 38 | RUN id -u $UNAME || useradd -m -u $UID -g $GID -s /bin/bash $UNAME 39 | # change to the desired user 40 | USER $UNAME 41 | WORKDIR /home/$UNAME' 42 | 43 | 44 | # determine if we need to use sudo when calling docker 45 | if groups $USER | grep -q docker; 46 | then 47 | DOCKER=docker 48 | else 49 | DOCKER="sudo docker" 50 | fi 51 | 52 | # set the base image name 53 | base_image=$2 54 | 55 | if [ -z "$base_image" ] 56 | then 57 | printf "$usage" 58 | exit 1 59 | fi 60 | 61 | if [ -z $image ] 62 | then 63 | image="${base_image}_img" 64 | fi 65 | 66 | cmd=$3 67 | 68 | # name of dockerfile to build(where applicable) 69 | dfile=$3 70 | #home directory to mount (where applicable) 71 | homedir=$3 72 | if [ -z $container ] 73 | then 74 | container="${base_image}_dockint" 75 | fi 76 | 77 | # build the docker image 78 | function docker_build_image { 79 | if [ ! -z $dfile ] 80 | then 81 | $DOCKER build --tag $base_image $dfile 82 | fi 83 | 84 | printf "$dockerfile" | $DOCKER build --build-arg UID=$(id -u) --build-arg GID=$(id -g) --build-arg UNAME=$USER --build-arg BASE=$base_image --tag "$image" - 85 | if [ $? -ne 0 ]; then 86 | echo "Failed to create docker image $image." 87 | exit 1 88 | fi 89 | } 90 | 91 | # start the docker container 92 | function docker_container_start { 93 | # run docker, giving it access to the specified devices and X windows 94 | # the cap-add and security-opt lets me use gdb to debug ros nodes 95 | # if $homedir does not exist, nothing will be mounted but the command will 96 | # still work 97 | xhost +local:root 98 | $DOCKER run -itd --rm --name "$container" \ 99 | --hostname "$HOST" \ 100 | -v $homedir:/home/$USER \ 101 | -v /tmp/.X11-unix/X0:/tmp/.X11-unix/X0 \ 102 | -v /etc/machine-id:/etc/machine-id \ 103 | -v /var/run/dbus:/var/run/dbus \ 104 | --device /dev/dri \ 105 | --cap-add=SYS_PTRACE \ 106 | --security-opt seccomp=unconfined \ 107 | --privileged \ 108 | --network=host \ 109 | -v /dev/:/dev \ 110 | -v $XDG_RUNTIME_DIR:$XDG_RUNTIME_DIR \ 111 | -e XDG_RUNTIME_DIR=$XDG_RUNTIME_DIR \ 112 | "$image" 113 | 114 | if [ $? -ne 0 ]; then 115 | echo "Failed to start docker container $container." 116 | exit 1 117 | fi 118 | # $DOCKER exec -ti "$container" sh -c "rosdep install --from-paths src --ignore-src -r -y" 119 | } 120 | 121 | 122 | function interactive_start { 123 | # check if the docker image has been created and if not create one 124 | if [ -z "$($DOCKER images -q $base_image)" ]; then 125 | docker_build_image 126 | fi 127 | 128 | # check if the docker image is running and if not start it 129 | if [ -z "$($DOCKER ps -q -f NAME=$container)" ]; then 130 | if [ -z $homedir ]; then 131 | homedir=~/$base_image 132 | fi 133 | if [ ! -d $homedir ]; then 134 | echo "The directory $homedir, to be mounted in the container at /home/$USER, does not exist." 135 | read -r -p "Would you like to create it [y/N]? (Otherwise /home/$USER will be mounted in the container) " response 136 | case "$response" in 137 | [yY]) 138 | mkdir -p $homedir 139 | ;; 140 | *) 141 | # docker creates non-existant directories automatically, so set this to empty to prevent that 142 | homedir="" 143 | esac 144 | fi 145 | docker_container_start 146 | fi 147 | } 148 | 149 | case $1 in 150 | from) 151 | docker_build_image 152 | exit 0 153 | ;; 154 | start) 155 | interactive_start 156 | exit 0 157 | ;; 158 | stop) 159 | $DOCKER stop "$container" 160 | exit $? 161 | ;; 162 | run) 163 | # issue a command in the container 164 | interactive_start 165 | shift 2 166 | $DOCKER exec -ti "$container" $@ 167 | ;; 168 | *) 169 | printf "$usage" 170 | exit 0 171 | ;; 172 | esac 173 | -------------------------------------------------------------------------------- /omnid_kinematics_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(omnid_kinematics_plugin) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-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 COMPONENTS 11 | geometry_msgs 12 | moveit_core 13 | moveit_kinematics 14 | moveit_ros_planning 15 | pluginlib 16 | roscpp 17 | ) 18 | 19 | find_package(omnid_core) 20 | find_package(cmakeme) 21 | cmakeme_defaults(Debug) 22 | ## System dependencies are found with CMake's conventions 23 | # find_package(Boost REQUIRED COMPONENTS system) 24 | 25 | 26 | ## Uncomment this if the package has a setup.py. This macro ensures 27 | ## modules and global scripts declared therein get installed 28 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 29 | # catkin_python_setup() 30 | 31 | ################################################ 32 | ## Declare ROS messages, services and actions ## 33 | ################################################ 34 | 35 | ## To declare and build messages, services or actions from within this 36 | ## package, follow these steps: 37 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 38 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 39 | ## * In the file package.xml: 40 | ## * add a build_depend tag for "message_generation" 41 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 42 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 43 | ## but can be declared for certainty nonetheless: 44 | ## * add a exec_depend tag for "message_runtime" 45 | ## * In this file (CMakeLists.txt): 46 | ## * add "message_generation" and every package in MSG_DEP_SET to 47 | ## find_package(catkin REQUIRED COMPONENTS ...) 48 | ## * add "message_runtime" and every package in MSG_DEP_SET to 49 | ## catkin_package(CATKIN_DEPENDS ...) 50 | ## * uncomment the add_*_files sections below as needed 51 | ## and list every .msg/.srv/.action file to be processed 52 | ## * uncomment the generate_messages entry below 53 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 54 | 55 | ## Generate messages in the 'msg' folder 56 | # add_message_files( 57 | # FILES 58 | # Message1.msg 59 | # Message2.msg 60 | # ) 61 | 62 | ## Generate services in the 'srv' folder 63 | # add_service_files( 64 | # FILES 65 | # Service1.srv 66 | # Service2.srv 67 | # ) 68 | 69 | ## Generate actions in the 'action' folder 70 | # add_action_files( 71 | # FILES 72 | # Action1.action 73 | # Action2.action 74 | # ) 75 | 76 | ## Generate added messages and services with any dependencies listed here 77 | # generate_messages( 78 | # DEPENDENCIES 79 | # geometry_msgs 80 | # ) 81 | 82 | ################################################ 83 | ## Declare ROS dynamic reconfigure parameters ## 84 | ################################################ 85 | 86 | ## To declare and build dynamic reconfigure parameters within this 87 | ## package, follow these steps: 88 | ## * In the file package.xml: 89 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 90 | ## * In this file (CMakeLists.txt): 91 | ## * add "dynamic_reconfigure" to 92 | ## find_package(catkin REQUIRED COMPONENTS ...) 93 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 94 | ## and list every .cfg file to be processed 95 | 96 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 97 | # generate_dynamic_reconfigure_options( 98 | # cfg/DynReconf1.cfg 99 | # cfg/DynReconf2.cfg 100 | # ) 101 | 102 | ################################### 103 | ## catkin specific configuration ## 104 | ################################### 105 | ## The catkin_package macro generates cmake config files for your package 106 | ## Declare things to be passed to dependent projects 107 | ## INCLUDE_DIRS: uncomment this if your package contains header files 108 | ## LIBRARIES: libraries you create in this project that dependent projects also need 109 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 110 | ## DEPENDS: system dependencies of this project that dependent projects also need 111 | catkin_package( 112 | INCLUDE_DIRS include 113 | CATKIN_DEPENDS geometry_msgs moveit_core moveit_kinematics moveit_ros_planning pluginlib roscpp 114 | DEPENDS system_lib 115 | ) 116 | 117 | ########### 118 | ## Build ## 119 | ########### 120 | 121 | ## Specify additional locations of header files 122 | ## Your package locations should be listed before other locations 123 | include_directories( 124 | include 125 | ${catkin_INCLUDE_DIRS} 126 | ) 127 | 128 | ## Declare a C++ library 129 | add_library(${PROJECT_NAME} 130 | src/omnid_kinematics_plugin.cpp 131 | ) 132 | 133 | ## Specify libraries to link a library or executable target against 134 | target_link_libraries(${PROJECT_NAME} 135 | PUBLIC 136 | ${catkin_LIBRARIES} 137 | omnid_core::omnid_core 138 | ) 139 | 140 | ## Mark executables for installation 141 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 142 | # install(TARGETS ${PROJECT_NAME}_node 143 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 144 | # ) 145 | 146 | ## Mark libraries for installation 147 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 148 | # install(TARGETS ${PROJECT_NAME} 149 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 150 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 151 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 152 | # ) 153 | 154 | ## Mark cpp header files for installation 155 | # install(DIRECTORY include/${PROJECT_NAME}/ 156 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 157 | # FILES_MATCHING PATTERN "*.h" 158 | # PATTERN ".svn" EXCLUDE 159 | # ) 160 | 161 | ## Mark other files for installation (e.g. launch and bag files, etc.) 162 | # install(FILES 163 | # # myfile1 164 | # # myfile2 165 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 166 | # ) 167 | 168 | ############# 169 | ## Testing ## 170 | ############# 171 | 172 | ## Add gtest based cpp test target and link libraries 173 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_omnid_kinematics_plugin.cpp) 174 | # if(TARGET ${PROJECT_NAME}-test) 175 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 176 | # endif() 177 | 178 | ## Add folders to be run by python nosetests 179 | # catkin_add_nosetests(test) 180 | -------------------------------------------------------------------------------- /omnid_planning_adapter_plugin/src/omnid_planning_adapter_plugin.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2012, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /* Author: Rico Ruotong Jia*/ 35 | 36 | /// \file 37 | /// \brief This plugin first calculates each robot's end effector position, given the object platform position. Then, it fixes each end effector's pose, given their joint values. 38 | /// Name initialization. 39 | /// 1. names of non-end-effector planning groups of each robot in SRDF 40 | /// 2. names of the frames: object platform base, world, each robot's "floating world" frame (see URDF of the robot) 41 | 42 | 43 | #include 44 | #include 45 | #include 46 | #include "omnid_core/delta_robot.h" 47 | #include "omnid_planning_adapter_plugin/omnid_planning_adapter_plugin.h" 48 | 49 | using std::vector; 50 | using std::string; 51 | 52 | namespace omnid_planning_adapter_plugin 53 | { 54 | class OmnidPlanningRequestAdapter : public planning_request_adapter::PlanningRequestAdapter 55 | { 56 | public: 57 | std::string getDescription() const override 58 | { 59 | return "Planning adapter for pre-processing and post-processing planning results for the Omnid Delta Arm Project"; 60 | } 61 | 62 | bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, 63 | const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, 64 | std::vector& /*added_path_index*/) const override 65 | { 66 | // Thought: we modify each point along the trajectory, robot by robot. 67 | // Workflow: 68 | // 1. Recognize object platform's roll, pitch, yaw. 69 | // 2. Plan object platform trajectory 70 | // 3. Calculate (Note this is NOT PLANNING)pose of each robot eef based on object platform trajectory, using high level IK 71 | // 4. Call low level IK, get all joint angles 72 | 73 | 74 | bool success = planner(planning_scene, req, res); 75 | 76 | moveit_msgs::RobotTrajectory traj; 77 | res.trajectory_->getRobotTrajectoryMsg(traj); 78 | if(!object_platform_.nameInitialized()) 79 | object_platform_.initializeJointLookup(traj.joint_trajectory.joint_names); 80 | 81 | for(auto& traj_point: traj.joint_trajectory.points) { 82 | //get the pose of the object platform 83 | tf2::Transform platform_pose; 84 | object_platform_.calcPlatformPoseTf(traj_point, platform_pose); 85 | 86 | for (unsigned int i = 0; i < delta_robots_.size(); ++i){ 87 | // high-level planning for the robot 88 | tf2::Transform robot_to_eef; 89 | const auto& robot = delta_robots_.at(i); 90 | object_platform_.getNextEefPose(platform_pose, robot.getRobotToWorldTf(), 91 | robot.getWorldToRobotTf(), robot_to_eef); 92 | 93 | if (!robot.nameInitialized()){ 94 | robot.initializeJointLookup(traj.joint_trajectory.joint_names, planning_scene); 95 | } 96 | 97 | if(!robot.calcLowLevelIk(planning_scene, robot_to_eef, traj_point)){ 98 | ROS_WARN_STREAM("[omnid_planning_adapter_plugin]: Planning failed at robot " << i); 99 | return false; 100 | } 101 | } 102 | 103 | res.trajectory_ -> setRobotTrajectoryMsg (planning_scene -> getCurrentState(), traj); 104 | } 105 | return success; 106 | } 107 | 108 | void initialize(const ros::NodeHandle& /*nh_*/) override 109 | { 110 | nh_ = ros::NodeHandle(); 111 | vector robot_planning_group_names; // each robot (not including the object platform arm) planning group's name in SRDF 112 | vector robot_names; //the first part of joint names in URDF. 113 | string object_platform_name; 114 | string group_reference_frame_name; 115 | string world_frame_name; 116 | string floating_world_frame_name; // the name of the first "floating world" name of each robot. please include '/' if there is any. see each robot's URDF 117 | 118 | //TODO 119 | if (!readRosParamStringVec(robot_planning_group_names, "robot_planning_group_names")) 120 | robot_planning_group_names = {"end_effector_arm_1", "end_effector_arm_2", "end_effector_arm_3"}; 121 | 122 | if(!readRosParamStringVec(robot_names, "robot_names")) 123 | robot_names = {"robot_1", "robot_2", "robot_3"}; 124 | object_platform_name = nh_.param("object_platform_name", string("object_platform")); 125 | group_reference_frame_name = nh_.param("group_reference_frame_name", string("object_platform_base")); 126 | world_frame_name = nh_.param("world_frame_name", string("world")); 127 | floating_world_frame_name = nh_.param("floating_world_frame_name", string("/floating_world_0")); 128 | 129 | // Initialize the robots 130 | object_platform_ = Object_Platform_Adapter(object_platform_name, group_reference_frame_name, 131 | world_frame_name); 132 | delta_robots_.reserve(robot_names.size()); 133 | for (unsigned int i = 0; i < robot_names.size(); ++i){ 134 | //This is for base frame name = robot frames robot_1/floating_world_0. Change this if necessary. 135 | delta_robots_.emplace_back(robot_planning_group_names.at(i), robot_names.at(i), robot_names.at(i) + floating_world_frame_name, world_frame_name); 136 | } 137 | } 138 | 139 | private: 140 | ros::NodeHandle nh_; 141 | 142 | vector delta_robots_; 143 | Object_Platform_Adapter object_platform_; 144 | 145 | bool readRosParamStringVec(vector& vec, string param_name){ 146 | if (nh_.hasParam(param_name)){ 147 | XmlRpc::XmlRpcValue temp; 148 | nh_.getParam(param_name, temp); 149 | for (unsigned int i = 0; i < temp.size(); ++i ){ 150 | vec.push_back(static_cast(temp[i])); 151 | } 152 | return true; 153 | } 154 | else return false; 155 | } 156 | 157 | }; 158 | } 159 | 160 | CLASS_LOADER_REGISTER_CLASS(omnid_planning_adapter_plugin::OmnidPlanningRequestAdapter, planning_request_adapter::PlanningRequestAdapter); 161 | 162 | -------------------------------------------------------------------------------- /omnid/scripts/spring_test/torsion_spring.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import pybullet as p 4 | import numpy as np 5 | 6 | class Spring_Model: 7 | 8 | def __init__(self, urdfRootPath='', joint_val=0.0, joint_name="spring_joint", torque_k=0.1): 9 | """ 10 | In URDF, the spring joint should be attached at the ends of two links, and its frame is aligned with the two links, . 11 | :param urdfRootPath: path to the URDF directory 12 | :param joint_val: initial joint angle (radian) between the child link and the parent link to initialize the spring's potential energy. If set to 0, the spring will start at the pose specified in URDF. 13 | :param joint_name: name of the spring in URDF. By default, it's "spring_joint" 14 | """ 15 | self.urdfRootPath = urdfRootPath 16 | self.reset(torque_k=torque_k, joint_val=joint_val, joint_name=joint_name) 17 | 18 | def reset(self, torque_k=0.1, joint_val=0.0, joint_name="spring_joint"): 19 | self.model_body_unique_id = p.loadURDF("%s/spring/spring.urdf" % self.urdfRootPath, basePosition=[0,0,0.5], useFixedBase=1) #TODO 20 | self.joint_name = joint_name 21 | self.buildJointLinkLookups() 22 | self.enable_joint_sensors() 23 | self.torque_k = torque_k 24 | #TODO to delete 25 | spring_joint_id = self.jointNameToId["spring_joint"] 26 | p.resetJointState(bodyUniqueId = self.model_body_unique_id, jointIndex=spring_joint_id, targetValue=joint_val) 27 | self.resetAllFrictions(friction_coefficient=0) 28 | 29 | def buildJointLinkLookups(self): 30 | """ 31 | builds a dictionary {joint_name: joint_id}. Note that since each link and its parent joint has the 32 | same ID, you can use this to get link_id as well, except that you will have to access base frame by link_id = -1. 33 | """ 34 | nJoints = p.getNumJoints(self.model_body_unique_id) 35 | self.jointNameToId = {} 36 | for i in range(nJoints): 37 | jointInfo = p.getJointInfo(self.model_body_unique_id, i) 38 | self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] 39 | #TODO 40 | self.linkIDs = list(self.jointNameToId.values()) 41 | self.linkIDs.append(-1) #base_link id 42 | print(self.linkIDs) 43 | 44 | def resetAllFrictions(self, friction_coefficient): 45 | for id in self.linkIDs: 46 | p.changeDynamics(bodyUniqueId=self.model_body_unique_id, 47 | linkIndex=id, 48 | jointLowerLimit = -1000, 49 | jointUpperLimit = 1000, 50 | jointLimitForce = 0, 51 | lateralFriction=0.0, 52 | spinningFriction=0.0, 53 | rollingFriction=0.0, 54 | linearDamping = 0.0, 55 | angularDamping = 0.0, 56 | jointDamping = 0.0, 57 | contactStiffness=0.0, 58 | contactDamping=0.0, 59 | maxJointVelocity=10000 60 | ) 61 | # for joint_id in self.jointNameToId.values(): #This is essential, as this disables the default motor imposed by bullet. These default motors prevent overspeeding, and functions as 62 | # #friction drives. 63 | # p.setJointMotorControl2(self.model_body_unique_id, joint_id, 64 | # controlMode=p.VELOCITY_CONTROL, force=0) 65 | def apply_torque_spring(self): 66 | 67 | for joint_id in self.jointNameToId.values(): #This is essential, as this disables the default motor imposed by bullet. These default motors prevent overspeeding, and functions as 68 | #friction drives. 69 | p.setJointMotorControl2(self.model_body_unique_id, joint_id, 70 | controlMode=p.VELOCITY_CONTROL, force=0) 71 | 72 | # spring_joint_down_id = self.jointNameToId["spring_joint_down"] 73 | spring_joint_id = self.jointNameToId[self.joint_name] 74 | base_id = -1 75 | end_effector_id = list(self.jointNameToId.values())[-1] 76 | joint_states_dict = self.get_joint_state(joint_index=spring_joint_id) 77 | joint_info_dict = self.get_joint_info(joint_index=spring_joint_id) 78 | torque = joint_states_dict["jointPosition"] * self.torque_k 79 | 80 | #apply external force 81 | dist_down = 1.05 #moment arm to the downstream arm 82 | dist_up = 1.05 #moment arm to the upstream arm (fixed joint) 83 | force_axis = np.array([0.0, -1.0, 0.0]) 84 | self.apply_external_force(force= torque/dist_down*force_axis, link_index=end_effector_id) 85 | self.apply_external_force(force= 1.0 * torque/dist_up*force_axis, link_index=base_id) 86 | # print("joint: ", joint_states_dict["jointPosition"], " | force: ", torque/dist_down) 87 | 88 | def apply_external_force(self, force, link_index, position=[0,0,0]): 89 | """ 90 | Apply the specified external force on the specified position on the body / link. 91 | Args: 92 | link_index (int): unique link id. If -1, it will be the base. 93 | force (np.array[float[3]]): external force to be applied. 94 | position (np.array[float[3]], None): position on the link where the force is applied. See `flags` for 95 | coordinate systems. If None, it is the center of mass of the body (or the link if specified). 96 | frame (int): Specify the coordinate system of force/position: either `pybullet.WORL D_FRAME` (=2) for 97 | Cartesian world coordinates or `pybullet.LINK_FRAME` (=1) for local link coordinates. 98 | """ 99 | p.applyExternalForce( 100 | objectUniqueId=self.model_body_unique_id, linkIndex=link_index, 101 | forceObj=force, posObj=position, flags=p.LINK_FRAME) 102 | 103 | def non_zero_velocity_control(self): 104 | """ 105 | Test function to be removed - test if mode switching is possible 106 | """ 107 | # for joint_id in self.jointNameToId.values(): #This is essential, as this disables the default motor imposed by bullet. These default motors prevent overspeeding, and functions as 108 | # #friction drives. 109 | for joint_id in self.jointNameToId.values(): #This is essential, as this disables the default motor imposed by bullet. These default motors prevent overspeeding, and functions as 110 | #friction drives. 111 | p.setJointMotorControl2(self.model_body_unique_id, joint_id, 112 | targetPosition= 2.0, 113 | controlMode=p.POSITION_CONTROL, force=100) 114 | 115 | def enable_joint_sensors(self): 116 | for joint_index in self.jointNameToId.values(): 117 | p.enableJointForceTorqueSensor(bodyUniqueId = self.model_body_unique_id, jointIndex = joint_index, enableSensor = True) 118 | 119 | def get_joint_state(self, joint_index=None, joint_name=None): 120 | """ 121 | return a dictionary of joint information. You have to provide one of joint_index or joint_name. If both provided, joint_name will be used to find the joint index 122 | :param link_index: (int) unique link id. if -1, it will be the base 123 | :return: dictionary of joint information: 124 | jointPosition (float) 125 | jointVelocity(float) 126 | jointReactionForces(list of 6 floats) 127 | appliedJointMotorTorque (float): These are the joint reaction forces, if a torque sensor is enabled for this joint it is 128 | [Fx, Fy, Fz, Mx, My, Mz]. Without torque sensor, it is [0,0,0,0,0,0]. 129 | """ 130 | if joint_name is not None: 131 | joint_index = self.jointNameToId[joint_name] 132 | info = p.getJointState(bodyUniqueId = self.model_body_unique_id, jointIndex=joint_index) 133 | return {"jointPosition":info[0], "jointVelocity":info[1], "jointReactionForces":info[2], "appliedJointMotorTorque":info[3]} 134 | 135 | def get_link_state(self, link_index=None): 136 | """ 137 | :param link_index: (int) unique link id. if -1, it will be the base 138 | :return: dictionary of joint information: 139 | link_world_position (float3) 140 | link_world_orientation(float4) - this is quaternion 141 | appliedJointMotorTorque (float): These are the joint reaction forces, if a torque sensor is enabled for this joint it is 142 | [Fx, Fy, Fz, Mx, My, Mz]. Without torque sensor, it is [0,0,0,0,0,0]. 143 | """ 144 | info = p.getLinkState(bodyUniqueId = self.model_body_unique_id, linkIndex=link_index) 145 | return {"link_world_position":info[0], "link_world_orientation":info[1]} 146 | 147 | def get_joint_info(self, joint_index): 148 | info = p.getJointInfo(bodyUniqueId = self.model_body_unique_id, jointIndex=joint_index) 149 | # return {"jointDamping":info[7], "jointFriction":info[8], "jointLowerLimit":info[9], 150 | # "jointUpperLimit":info[10], "jointMaxForce":info[11], "jointMaxVelocity":info[12]} 151 | #TODO 152 | return info 153 | 154 | 155 | 156 | 157 | -------------------------------------------------------------------------------- /omnid_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 | 3 | # For beginners, we downscale velocity and acceleration limits. 4 | # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. 5 | default_velocity_scaling_factor: 0.1 6 | default_acceleration_scaling_factor: 0.1 7 | 8 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 9 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 10 | joint_limits: 11 | object_platform/pitch: 12 | has_velocity_limits: true 13 | max_velocity: 100 14 | has_acceleration_limits: false 15 | max_acceleration: 0 16 | object_platform/roll: 17 | has_velocity_limits: true 18 | max_velocity: 100 19 | has_acceleration_limits: false 20 | max_acceleration: 0 21 | object_platform/x: 22 | has_velocity_limits: true 23 | max_velocity: 100 24 | has_acceleration_limits: false 25 | max_acceleration: 0 26 | object_platform/y: 27 | has_velocity_limits: true 28 | max_velocity: 100 29 | has_acceleration_limits: false 30 | max_acceleration: 0 31 | object_platform/yaw: 32 | has_velocity_limits: true 33 | max_velocity: 100 34 | has_acceleration_limits: false 35 | max_acceleration: 0 36 | object_platform/z: 37 | has_velocity_limits: true 38 | max_velocity: 100 39 | has_acceleration_limits: false 40 | max_acceleration: 0 41 | robot_1/beta_1: 42 | has_velocity_limits: false 43 | max_velocity: 0 44 | has_acceleration_limits: false 45 | max_acceleration: 0 46 | robot_1/beta_2: 47 | has_velocity_limits: false 48 | max_velocity: 0 49 | has_acceleration_limits: false 50 | max_acceleration: 0 51 | robot_1/beta_3: 52 | has_velocity_limits: false 53 | max_velocity: 0 54 | has_acceleration_limits: false 55 | max_acceleration: 0 56 | robot_1/gamma_1: 57 | has_velocity_limits: false 58 | max_velocity: 0 59 | has_acceleration_limits: false 60 | max_acceleration: 0 61 | robot_1/gamma_2: 62 | has_velocity_limits: false 63 | max_velocity: 0 64 | has_acceleration_limits: false 65 | max_acceleration: 0 66 | robot_1/gamma_3: 67 | has_velocity_limits: false 68 | max_velocity: 0 69 | has_acceleration_limits: false 70 | max_acceleration: 0 71 | robot_1/mimic_beta_1: 72 | has_velocity_limits: false 73 | max_velocity: 0 74 | has_acceleration_limits: false 75 | max_acceleration: 0 76 | robot_1/mimic_beta_2: 77 | has_velocity_limits: false 78 | max_velocity: 0 79 | has_acceleration_limits: false 80 | max_acceleration: 0 81 | robot_1/mimic_beta_3: 82 | has_velocity_limits: false 83 | max_velocity: 0 84 | has_acceleration_limits: false 85 | max_acceleration: 0 86 | robot_1/mimic_gamma_1: 87 | has_velocity_limits: false 88 | max_velocity: 0 89 | has_acceleration_limits: false 90 | max_acceleration: 0 91 | robot_1/mimic_gamma_2: 92 | has_velocity_limits: false 93 | max_velocity: 0 94 | has_acceleration_limits: false 95 | max_acceleration: 0 96 | robot_1/mimic_gamma_3: 97 | has_velocity_limits: false 98 | max_velocity: 0 99 | has_acceleration_limits: false 100 | max_acceleration: 0 101 | robot_1/mimic_theta_1: 102 | has_velocity_limits: true 103 | max_velocity: 100 104 | has_acceleration_limits: false 105 | max_acceleration: 0 106 | robot_1/mimic_theta_2: 107 | has_velocity_limits: true 108 | max_velocity: 100 109 | has_acceleration_limits: false 110 | max_acceleration: 0 111 | robot_1/mimic_theta_3: 112 | has_velocity_limits: true 113 | max_velocity: 100 114 | has_acceleration_limits: false 115 | max_acceleration: 0 116 | robot_1/theta_1: 117 | has_velocity_limits: true 118 | max_velocity: 100 119 | has_acceleration_limits: false 120 | max_acceleration: 0 121 | robot_1/theta_2: 122 | has_velocity_limits: true 123 | max_velocity: 100 124 | has_acceleration_limits: false 125 | max_acceleration: 0 126 | robot_1/theta_3: 127 | has_velocity_limits: true 128 | max_velocity: 100 129 | has_acceleration_limits: false 130 | max_acceleration: 0 131 | robot_1/x: 132 | has_velocity_limits: true 133 | max_velocity: 100 134 | has_acceleration_limits: false 135 | max_acceleration: 0 136 | robot_1/y: 137 | has_velocity_limits: true 138 | max_velocity: 100 139 | has_acceleration_limits: false 140 | max_acceleration: 0 141 | robot_1/z: 142 | has_velocity_limits: true 143 | max_velocity: 100 144 | has_acceleration_limits: false 145 | max_acceleration: 0 146 | robot_2/beta_1: 147 | has_velocity_limits: false 148 | max_velocity: 0 149 | has_acceleration_limits: false 150 | max_acceleration: 0 151 | robot_2/beta_2: 152 | has_velocity_limits: false 153 | max_velocity: 0 154 | has_acceleration_limits: false 155 | max_acceleration: 0 156 | robot_2/beta_3: 157 | has_velocity_limits: false 158 | max_velocity: 0 159 | has_acceleration_limits: false 160 | max_acceleration: 0 161 | robot_2/gamma_1: 162 | has_velocity_limits: false 163 | max_velocity: 0 164 | has_acceleration_limits: false 165 | max_acceleration: 0 166 | robot_2/gamma_2: 167 | has_velocity_limits: false 168 | max_velocity: 0 169 | has_acceleration_limits: false 170 | max_acceleration: 0 171 | robot_2/gamma_3: 172 | has_velocity_limits: false 173 | max_velocity: 0 174 | has_acceleration_limits: false 175 | max_acceleration: 0 176 | robot_2/mimic_beta_1: 177 | has_velocity_limits: false 178 | max_velocity: 0 179 | has_acceleration_limits: false 180 | max_acceleration: 0 181 | robot_2/mimic_beta_2: 182 | has_velocity_limits: false 183 | max_velocity: 0 184 | has_acceleration_limits: false 185 | max_acceleration: 0 186 | robot_2/mimic_beta_3: 187 | has_velocity_limits: false 188 | max_velocity: 0 189 | has_acceleration_limits: false 190 | max_acceleration: 0 191 | robot_2/mimic_gamma_1: 192 | has_velocity_limits: false 193 | max_velocity: 0 194 | has_acceleration_limits: false 195 | max_acceleration: 0 196 | robot_2/mimic_gamma_2: 197 | has_velocity_limits: false 198 | max_velocity: 0 199 | has_acceleration_limits: false 200 | max_acceleration: 0 201 | robot_2/mimic_gamma_3: 202 | has_velocity_limits: false 203 | max_velocity: 0 204 | has_acceleration_limits: false 205 | max_acceleration: 0 206 | robot_2/mimic_theta_1: 207 | has_velocity_limits: true 208 | max_velocity: 100 209 | has_acceleration_limits: false 210 | max_acceleration: 0 211 | robot_2/mimic_theta_2: 212 | has_velocity_limits: true 213 | max_velocity: 100 214 | has_acceleration_limits: false 215 | max_acceleration: 0 216 | robot_2/mimic_theta_3: 217 | has_velocity_limits: true 218 | max_velocity: 100 219 | has_acceleration_limits: false 220 | max_acceleration: 0 221 | robot_2/theta_1: 222 | has_velocity_limits: true 223 | max_velocity: 100 224 | has_acceleration_limits: false 225 | max_acceleration: 0 226 | robot_2/theta_2: 227 | has_velocity_limits: true 228 | max_velocity: 100 229 | has_acceleration_limits: false 230 | max_acceleration: 0 231 | robot_2/theta_3: 232 | has_velocity_limits: true 233 | max_velocity: 100 234 | has_acceleration_limits: false 235 | max_acceleration: 0 236 | robot_2/x: 237 | has_velocity_limits: true 238 | max_velocity: 100 239 | has_acceleration_limits: false 240 | max_acceleration: 0 241 | robot_2/y: 242 | has_velocity_limits: true 243 | max_velocity: 100 244 | has_acceleration_limits: false 245 | max_acceleration: 0 246 | robot_2/z: 247 | has_velocity_limits: true 248 | max_velocity: 100 249 | has_acceleration_limits: false 250 | max_acceleration: 0 251 | robot_3/beta_1: 252 | has_velocity_limits: false 253 | max_velocity: 0 254 | has_acceleration_limits: false 255 | max_acceleration: 0 256 | robot_3/beta_2: 257 | has_velocity_limits: false 258 | max_velocity: 0 259 | has_acceleration_limits: false 260 | max_acceleration: 0 261 | robot_3/beta_3: 262 | has_velocity_limits: false 263 | max_velocity: 0 264 | has_acceleration_limits: false 265 | max_acceleration: 0 266 | robot_3/gamma_1: 267 | has_velocity_limits: false 268 | max_velocity: 0 269 | has_acceleration_limits: false 270 | max_acceleration: 0 271 | robot_3/gamma_2: 272 | has_velocity_limits: false 273 | max_velocity: 0 274 | has_acceleration_limits: false 275 | max_acceleration: 0 276 | robot_3/gamma_3: 277 | has_velocity_limits: false 278 | max_velocity: 0 279 | has_acceleration_limits: false 280 | max_acceleration: 0 281 | robot_3/mimic_beta_1: 282 | has_velocity_limits: false 283 | max_velocity: 0 284 | has_acceleration_limits: false 285 | max_acceleration: 0 286 | robot_3/mimic_beta_2: 287 | has_velocity_limits: false 288 | max_velocity: 0 289 | has_acceleration_limits: false 290 | max_acceleration: 0 291 | robot_3/mimic_beta_3: 292 | has_velocity_limits: false 293 | max_velocity: 0 294 | has_acceleration_limits: false 295 | max_acceleration: 0 296 | robot_3/mimic_gamma_1: 297 | has_velocity_limits: false 298 | max_velocity: 0 299 | has_acceleration_limits: false 300 | max_acceleration: 0 301 | robot_3/mimic_gamma_2: 302 | has_velocity_limits: false 303 | max_velocity: 0 304 | has_acceleration_limits: false 305 | max_acceleration: 0 306 | robot_3/mimic_gamma_3: 307 | has_velocity_limits: false 308 | max_velocity: 0 309 | has_acceleration_limits: false 310 | max_acceleration: 0 311 | robot_3/mimic_theta_1: 312 | has_velocity_limits: true 313 | max_velocity: 100 314 | has_acceleration_limits: false 315 | max_acceleration: 0 316 | robot_3/mimic_theta_2: 317 | has_velocity_limits: true 318 | max_velocity: 100 319 | has_acceleration_limits: false 320 | max_acceleration: 0 321 | robot_3/mimic_theta_3: 322 | has_velocity_limits: true 323 | max_velocity: 100 324 | has_acceleration_limits: false 325 | max_acceleration: 0 326 | robot_3/theta_1: 327 | has_velocity_limits: true 328 | max_velocity: 100 329 | has_acceleration_limits: false 330 | max_acceleration: 0 331 | robot_3/theta_2: 332 | has_velocity_limits: true 333 | max_velocity: 100 334 | has_acceleration_limits: false 335 | max_acceleration: 0 336 | robot_3/theta_3: 337 | has_velocity_limits: true 338 | max_velocity: 100 339 | has_acceleration_limits: false 340 | max_acceleration: 0 341 | robot_3/x: 342 | has_velocity_limits: true 343 | max_velocity: 100 344 | has_acceleration_limits: false 345 | max_acceleration: 0 346 | robot_3/y: 347 | has_velocity_limits: true 348 | max_velocity: 100 349 | has_acceleration_limits: false 350 | max_acceleration: 0 351 | robot_3/z: 352 | has_velocity_limits: true 353 | max_velocity: 100 354 | has_acceleration_limits: false 355 | max_acceleration: 0 -------------------------------------------------------------------------------- /omnid/scripts/omnid_simulator: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | ''' 4 | The Omnid simulator that connects bullet to ROS. 5 | Subscribes : /cmd_vel 6 | Workflow Summary: 7 | - read parameters 8 | - calculate centroid of the object and place it there. 9 | - setup pybullet simulator 10 | - initialize delta robots, the disk object 11 | - connect the disk to the robots 12 | - at each step of the simulation, the robot listens to the trajectory waypoints and execute it` 13 | ''' 14 | 15 | import rospy 16 | import pybullet as p 17 | import rospkg 18 | import numpy as np 19 | import pybullet_data 20 | from omnid_model import Omnid_Model 21 | import roslib 22 | from sensor_msgs.msg import JointState 23 | from disk import Disk 24 | 25 | import actionlib 26 | 27 | from std_msgs.msg import Float64 28 | from trajectory_msgs.msg import JointTrajectory 29 | from control_msgs.msg import FollowJointTrajectoryAction 30 | from control_msgs.msg import FollowJointTrajectoryFeedback 31 | from control_msgs.msg import FollowJointTrajectoryResult 32 | 33 | 34 | roslib.load_manifest("omnid") 35 | 36 | class OmnidArmROS: 37 | def __init__(self, robot_name, urdf_path, base_position = [0,0], id = None): 38 | """ 39 | initialize an Omnid Arm with ROS interface 40 | :param urdf_path: string, Relative path (to the root of the package) to the URDF directory 41 | :param base_position: list like [x,y] of the center of the base of the robot 42 | :param id: int, id of the arm. If none, all topics and actions will not be labeled. Else, they will be labeled like "topic_name_id". 43 | """ 44 | self.robot_name = robot_name 45 | id_suffix = '_' + str(id) if id is not None else '' 46 | use_spring = not(rospy.get_param("~test_without_spring", False)) 47 | test_with_end_effector_xyz = rospy.get_param("~test_with_end_effector_xyz") 48 | after_spring_actuated = rospy.get_param("~after_spring_actuated") 49 | urdf_name = rospy.get_param("~urdf_name") 50 | self.omnid_model = Omnid_Model( 51 | urdfRootPath=urdf_path, 52 | use_spring=use_spring, 53 | test_with_end_effector_xyz=test_with_end_effector_xyz, 54 | after_spring_actuated = after_spring_actuated, 55 | urdf_name = urdf_name, 56 | base_position = base_position 57 | ) 58 | test_joint_control = rospy.get_param("~test_joint_control") 59 | if test_joint_control: 60 | self.joint_state_sub = rospy.Subscriber('joint_states_control'+id_suffix, JointState, self.updateJointStates_CB) 61 | 62 | def getJointStates(self): 63 | return self.omnid_model.returnJointStateMsg() 64 | 65 | def executeMotionCommands(self): 66 | self.omnid_model.executeAllMotorPosCommands() 67 | 68 | def updateJointStates(self, name,position): 69 | self.omnid_model.updateJointStates(name, position) 70 | 71 | def updateJointStates_CB(self, msg): 72 | """ 73 | API function for as a ROS Subscriber callback_function updating joint positions. 74 | """ 75 | self.omnid_model.updateJointStates(msg.name, msg.position) 76 | 77 | 78 | class OmnidGroup: 79 | def __init__(self): 80 | urdf_path = ((rospkg.RosPack()).get_path('omnid'))+'/urdf' 81 | base_positions = rospy.get_param("base_positions") 82 | self.robot_names = rospy.get_param("robot_names") 83 | self.object_name = rospy.get_param("object_name") 84 | self.omnid_arms_ros = [] 85 | 86 | #initialize the arms [0, 1 ...] 87 | omnid_num = len(base_positions) 88 | for id in range(omnid_num): 89 | base_position = base_positions[id] 90 | omnid_arm_ros = OmnidArmROS(self.robot_names[id], urdf_path, id=id, base_position=base_position) 91 | self.omnid_arms_ros.append(omnid_arm_ros) 92 | #Initialize a disk 93 | 94 | spawn_position = self.getCentroid(base_positions) 95 | spawn_position[2] = self.omnid_arms_ros[0].omnid_model.getEndEffectorPosition()[2] + (rospy.get_param("h_platform") + rospy.get_param("object_thickness"))/2.0 + rospy.get_param("object_clearance") 96 | self.disk = Disk(urdf_path, spawn_position = spawn_position) 97 | 98 | #connect the disk to the omnids 99 | self.connectObjectToOmnids() 100 | 101 | controller_namespace = rospy.get_param("~controller_namespace") 102 | self.action_server = actionlib.SimpleActionServer(controller_namespace + '/follow_joint_trajectory', 103 | FollowJointTrajectoryAction, 104 | execute_cb=self.processFollowTrajectory, 105 | auto_start=False) 106 | self.action_server.start() 107 | self.joint_state_pub = rospy.Publisher('omnid/joint_states', JointState, queue_size=10) 108 | 109 | def getCentroid(self, points): 110 | """ 111 | return the centroid of massless points 112 | :param points: list of mass less point's [x, y, z] 113 | :return: [x, y, z] 114 | """ 115 | return np.array([sum( [ps[0] for ps in points] ), 116 | sum( [ps[1] for ps in points] ), 117 | sum( [ps[2] for ps in points] )]) \ 118 | /len(points) 119 | 120 | def calTrajStartEndIndices(self, joint_names): 121 | """ 122 | calculate the start and end indices of a robot in a trajectory msg: [start, end) 123 | :param joint_names: joint names list from a trajectory msg. 124 | """ 125 | try: self.start_indices_in_traj 126 | except AttributeError: 127 | robot_num = len(self.robot_names) 128 | self.start_indices_in_traj = -1 * np.ones(robot_num, np.int8) 129 | self.end_indices_in_traj = -1 * np.ones(robot_num, np.int8) 130 | for joint_name_i, joint_name in enumerate(joint_names): 131 | for id, robot_name in enumerate(self.robot_names): 132 | if robot_name in joint_name: 133 | if self.start_indices_in_traj[id] == -1: self.start_indices_in_traj[id] = joint_name_i 134 | self.end_indices_in_traj[id] = joint_name_i + 1 # here we assume in this trajectory_msg, one robot has consecutive fields 135 | 136 | def connectObjectToOmnids(self): 137 | """ 138 | We place the object right on top of the omnid arms. We assume given the object spawn position, the object is large enough to be connected 139 | to the omnid arms. We also assume that the object is spawned right on top of the delta arm platforms. 140 | """ 141 | spawn_position = self.disk.getObjectInfo()["spawn_position"] 142 | maxPoint2PointForce = 500000 143 | for id in range(len(self.omnid_arms_ros)): 144 | platform_pos = np.array(self.omnid_arms_ros[id].omnid_model.getEndEffectorPosition()) 145 | platform_top = np.array([platform_pos[0], platform_pos[1], spawn_position[2]]) #assume that the object is spawned right on top of the delta arm platforms. 146 | child_frame_pos = platform_top - platform_pos 147 | parent_frame_pos = platform_top - spawn_position # Cartesian coordnates on the platform, r_platform = 0.062 148 | 149 | joint_axis = [0,0,0] 150 | 151 | new_joint_id = p.createConstraint(self.disk.model_unique_id, self.disk.getEndEffectorLinkID(), #we assume base_link of the object is the first link of the object 152 | self.omnid_arms_ros[id].omnid_model.model_unique_id, self.omnid_arms_ros[id].omnid_model.getEndEffectorLinkID(), 153 | p.JOINT_POINT2POINT, joint_axis, parent_frame_pos, child_frame_pos) 154 | p.changeConstraint(new_joint_id, maxForce=maxPoint2PointForce) 155 | 156 | def processFollowTrajectory(self, goal): 157 | """ 158 | Call back function for Moveit! trajectory following action request. Note that this is a separate thread 159 | from the main one. 160 | :param goal: the goal trajectory of actuated joints, i.e, phi angles. 161 | """ 162 | # Workflow: 163 | #figure out joint names and their positions 164 | #start from point 1, since the first point is the current starting point 165 | #check for pre-emption 166 | #figure out the duration and joint positions of each trajectory segment 167 | #realize each segment and time it 168 | #check if the action has been preempted 169 | 170 | success = True 171 | traj = goal.trajectory 172 | num_points = len(traj.points) 173 | traj_joint_names = traj.joint_names 174 | self.calTrajStartEndIndices(traj_joint_names) 175 | 176 | for i in range(1, num_points): 177 | if self.action_server.is_preempt_requested(): 178 | rospy.loginfo("%Trajectory Action Preempted on Omnid" ) 179 | self.action_server.set_preempted() 180 | success = False 181 | break 182 | duration = (traj.points[i].time_from_start - traj.points[i-1].time_from_start ).to_sec() 183 | for id, omnid_arm_ros in enumerate(self.omnid_arms_ros): 184 | joint_positions = traj.points[i].positions[self.start_indices_in_traj[id] : self.end_indices_in_traj[id]] 185 | joint_names = traj_joint_names[self.start_indices_in_traj[id] : self.end_indices_in_traj[id]] 186 | joint_names = self.removePrefixList(joint_names, self.robot_names[id]) 187 | omnid_arm_ros.updateJointStates(joint_names, joint_positions) #this is how we update our joint commands 188 | r = rospy.Rate(1.0/duration) 189 | r.sleep() 190 | if success: 191 | msg = 'Trajectory completed' 192 | rospy.loginfo(msg) 193 | res = FollowJointTrajectoryResult() 194 | self.action_server.set_succeeded(result=res, text=msg) 195 | 196 | def publishJointStates(self): 197 | all_joint_states = JointState() 198 | for id, omnid_arm_ros in enumerate(self.omnid_arms_ros): 199 | joint_states = omnid_arm_ros.getJointStates() 200 | joint_states.name = [self.robot_names[id] + '/' + name for name in joint_states.name] 201 | self.appendToJointStateMsg(all_joint_states, joint_states) 202 | 203 | object_joint_states = self.disk.getJointStates() 204 | object_joint_states.name = [self.object_name + '/' + name for name in object_joint_states.name] 205 | self.appendToJointStateMsg(all_joint_states, object_joint_states) 206 | self.joint_state_pub.publish(all_joint_states) 207 | 208 | def executeMotionUpdates(self): 209 | for omnid_arm_ros in self.omnid_arms_ros: 210 | omnid_arm_ros.executeMotionCommands() 211 | 212 | def appendToJointStateMsg(self, to_be_appended_msg, to_append_msg): 213 | to_be_appended_msg.name += to_append_msg.name 214 | to_be_appended_msg.position += to_append_msg.position 215 | to_be_appended_msg.velocity += to_append_msg.velocity 216 | to_be_appended_msg.effort += to_append_msg.effort 217 | 218 | def removePrefixList(self, ls, prefix): 219 | return [str.replace(prefix + "/", "") for str in ls] 220 | 221 | 222 | 223 | if __name__ == "__main__": 224 | try: 225 | rospy.init_node("omnid_node") 226 | c = p.connect(p.GUI) # direct GUI might burn a lot of CPU. 227 | # c = p.connect(p.DIRECT) 228 | p.setAdditionalSearchPath(pybullet_data.getDataPath()) 229 | p.resetSimulation() 230 | p.loadURDF("plane.urdf" ) #loads from the root pybullet library 231 | p.setGravity(0, 0, -10.0) 232 | 233 | p.setRealTimeSimulation(0) 234 | 235 | # global joint_values 236 | timeStep = 0.0005 237 | r = rospy.Rate(1.0/timeStep) 238 | p.setTimeStep(timeStep) 239 | 240 | og = OmnidGroup() 241 | 242 | while p.isConnected() and not rospy.is_shutdown(): 243 | og.publishJointStates() 244 | og.executeMotionUpdates() 245 | p.stepSimulation() 246 | r.sleep() 247 | 248 | except rospy.ROSInterruptException: 249 | pass 250 | 251 | p.disconnect() 252 | -------------------------------------------------------------------------------- /omnid/scripts/omnid_model.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import pybullet as p 4 | import numpy as np 5 | from torsion_spring_omnid import Spring_Model 6 | import rospy 7 | from sensor_msgs.msg import JointState 8 | 9 | class Omnid_Model: 10 | 11 | def __init__(self, urdfRootPath='', use_spring=False, test_with_end_effector_xyz=False, after_spring_actuated=False, urdf_name="delta_robot_pybullet.urdf", base_position = [0,0,0]): 12 | self.urdfRootPath = urdfRootPath 13 | self.use_spring = use_spring 14 | self.reset(test_with_end_effector_xyz, after_spring_actuated, urdf_name, base_position) 15 | 16 | def reset(self, test_with_end_effector_xyz, after_spring_actuated, urdf_name, base_position): 17 | self.buildParamLists(test_with_end_effector_xyz, after_spring_actuated) 18 | self.base_position = [base_position[0],base_position[1],base_position[2]+self.base_offset] 19 | self.model_unique_id = p.loadURDF(self.urdfRootPath + "/" + urdf_name , basePosition=self.base_position, useFixedBase=True) 20 | self.buildLookups() 21 | self.resetLinkFrictions(lateral_friction_coefficient=0) 22 | self.resetJointsAndMotors() 23 | self.buildClosedChains() 24 | if self.use_spring: 25 | self.addSpringJoints() 26 | 27 | 28 | def buildParamLists(self, test_with_end_effector_xyz, after_spring_actuated): 29 | """ 30 | Read in physics params from launch file. Here we assume: there are multiple legs, each leg has exactly the same joint names and link names 31 | the only varying part is its leg id. 32 | Then, we build Link and joint name lookups (dictionary) for various uses: 33 | pre_spring joint names, after_spring_joint_names, spring_parent_link_name, spring_child_link_name, end-effector names 34 | """ 35 | self.base_offset= rospy.get_param("base_offset") #The base was offseted in the URDF of the robot. Change this value if URDF has changed. 36 | self.leg_num = rospy.get_param("leg_num") 37 | self.object_thickness = rospy.get_param("object_thickness") #thickness of the end effector platform 38 | 39 | self.kp = rospy.get_param("~kp") 40 | self.kd = rospy.get_param("~kd") 41 | self.max_motor_force = rospy.get_param("~max_motor_force") 42 | 43 | self.end_effector_radius = rospy.get_param("~end_effector_radius") 44 | self.leg_pos_on_end_effector = rospy.get_param("~leg_pos_on_end_effector") 45 | self.end_effector_name = rospy.get_param("~end_effector_name") 46 | self.end_effector_initial_xyz = rospy.get_param("~end_effector_initial_xyz") 47 | 48 | spring_parent_link_name = rospy.get_param("~spring_parent_link_name") 49 | spring_child_link_name = rospy.get_param("~spring_child_link_name") 50 | upper_leg_name = rospy.get_param("~upper_leg_name") 51 | self.upper_leg_length = rospy.get_param("~upper_leg_length") 52 | 53 | pre_spring_joint_info = rospy.get_param("~pre_spring_joint_info") 54 | after_spring_joint_info = rospy.get_param("~after_spring_joint_info") 55 | self.torque_k = rospy.get_param("~torque_k") 56 | 57 | self.force_axis_c = rospy.get_param("~force_axis_c") 58 | self.moment_arm_c = rospy.get_param("~lower_arm_length")/2.0 59 | self.force_pos_c = rospy.get_param("~force_pos_c") 60 | self.force_axis_p = rospy.get_param("~force_axis_p") 61 | self.moment_arm_p = rospy.get_param("~pre_spring_radius") 62 | self.force_pos_p = rospy.get_param("~force_pos_p") 63 | 64 | 65 | #spring joints and links 66 | self.pre_spring_joint_vals = {} 67 | self.after_spring_joint_vals = {} 68 | self.spring_parent_link_names = {} 69 | self.spring_child_link_names = {} 70 | self.upper_leg_names = {} 71 | for leg_id in range(1, self.leg_num + 1): 72 | pre_spring_joint_name = list(pre_spring_joint_info.keys())[0] 73 | pre_spring_joint_val = list(pre_spring_joint_info.values())[0] 74 | self.pre_spring_joint_vals[pre_spring_joint_name + "_" + str(leg_id)] = pre_spring_joint_val 75 | after_spring_joint_name = list(after_spring_joint_info.keys())[0] 76 | after_spring_joint_val = list(after_spring_joint_info.values())[0] 77 | self.after_spring_joint_vals[after_spring_joint_name + "_" + str(leg_id)] = after_spring_joint_val 78 | self.spring_parent_link_names[leg_id] = spring_parent_link_name + "_" + str(leg_id) 79 | self.spring_child_link_names[leg_id] = spring_child_link_name + "_" + str(leg_id) 80 | self.upper_leg_names[leg_id] = upper_leg_name + "_" + str(leg_id) 81 | 82 | # joints to be initialized 83 | self.init_joint_values = {**self.pre_spring_joint_vals, **self.after_spring_joint_vals, **self.end_effector_initial_xyz} 84 | self.motorDict = {**self.pre_spring_joint_vals} 85 | 86 | #if we are in a test mode, we're going to overwrite some dicts 87 | if test_with_end_effector_xyz: 88 | self.motorDict = {**self.motorDict, **self.end_effector_initial_xyz} 89 | if after_spring_actuated: 90 | self.motorDict = {**self.after_spring_joint_vals} 91 | 92 | def buildLookups(self): 93 | """ 94 | Build following look ups: 95 | 1. jointNameToId, linkNameToID (both dictionary). 96 | Note that since each link and its parent joint has the 97 | same ID, you can use this to get link_id as well, except that you will have to access base frame by link_id = -1. 98 | A very important assumption here is in your URDF, you first have a world link, then have a base_link 99 | """ 100 | # jointNameToId, linkNameToID 101 | nJoints = p.getNumJoints(self.model_unique_id) 102 | self.jointNameToId = {} 103 | self.linkNameToID={} 104 | for i in range(nJoints): 105 | jointInfo = p.getJointInfo(self.model_unique_id, i) 106 | self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] 107 | self.linkNameToID[jointInfo[12].decode('UTF-8')] = jointInfo[0] 108 | 109 | def resetLinkFrictions(self, lateral_friction_coefficient): 110 | """ 111 | Reset all links friction 112 | """ 113 | for id in self.linkNameToID.values(): 114 | p.changeDynamics(bodyUniqueId=self.model_unique_id, 115 | linkIndex=id, 116 | jointLowerLimit = -1000, 117 | jointUpperLimit = 1000, 118 | jointLimitForce = 0, 119 | lateralFriction=lateral_friction_coefficient, 120 | spinningFriction=0.0, 121 | rollingFriction=0.0, 122 | linearDamping = 0.0, 123 | angularDamping = 0.0, 124 | jointDamping = 0.0, 125 | contactStiffness=0.0, 126 | contactDamping=0.0, 127 | maxJointVelocity=10000 128 | ) 129 | 130 | def resetJointsAndMotors(self): 131 | """ 132 | We do two things here: 133 | 1. Look up your URDF and set the desired joint angles here. 134 | 2. Motor Control Set up: 1. Specify which joints need to be controlled by motors. 2. specify motor control parameters 135 | """ 136 | self.maxPoint2PointForce = 5000000 137 | 138 | #disable friction in all joints 139 | for joint_id in self.jointNameToId.values(): 140 | p.setJointMotorControl2(self.model_unique_id, joint_id, 141 | controlMode=p.VELOCITY_CONTROL, force=0) 142 | 143 | #All joint values to be initialized 144 | for joint_name in self.init_joint_values: 145 | p.resetJointState(bodyUniqueId = self.model_unique_id, jointIndex=self.jointNameToId[joint_name], targetValue=self.init_joint_values[joint_name]) 146 | 147 | 148 | def buildClosedChains(self): 149 | """ 150 | Connect links to joints to build closed chain robots, since URDF does not support chain robots. 151 | """ 152 | joint_axis = [0,0,0] 153 | for leg_id in range(1, self.leg_num + 1): 154 | upper_leg_name = self.upper_leg_names[leg_id] 155 | x = self.end_effector_radius * np.cos(self.leg_pos_on_end_effector[upper_leg_name]) 156 | y = self.end_effector_radius * np.sin(self.leg_pos_on_end_effector[upper_leg_name]) 157 | parent_frame_pos = np.array([ x, y, 0.0]) # Cartesian coordnates on the platform, r_platform = 0.062 158 | child_frame_pos = [self.upper_leg_length, 0.0, 0.0] # L_upper = 0.368/2.0 159 | new_joint_id = p.createConstraint(self.model_unique_id, self.linkNameToID[self.end_effector_name], 160 | self.model_unique_id, self.linkNameToID[upper_leg_name], 161 | p.JOINT_POINT2POINT, joint_axis, parent_frame_pos, child_frame_pos) 162 | p.changeConstraint(new_joint_id, maxForce=self.maxPoint2PointForce) 163 | 164 | def addSpringJoints(self): 165 | """ 166 | Embed springs into omnid's leg 167 | """ 168 | self.spring_models = {} 169 | for leg_id in range(1, self.leg_num + 1): 170 | child_link_id = self.linkNameToID[ self.spring_child_link_names[leg_id]] 171 | parent_link_id = self.linkNameToID[ self.spring_parent_link_names[leg_id]] 172 | primary_joint_id = self.jointNameToId[ list(self.pre_spring_joint_vals.keys())[leg_id - 1] ] 173 | secondary_joint_id = self.jointNameToId[ list(self.after_spring_joint_vals.keys())[leg_id - 1] ] 174 | 175 | self.spring_models[leg_id] = Spring_Model(self.model_unique_id, 176 | child_link_id, self.force_axis_c, self.moment_arm_c, self.force_pos_c, 177 | parent_link_id, self.force_axis_p, self.moment_arm_p, self.force_pos_p, 178 | primary_joint_id,secondary_joint_id, 179 | self.torque_k) 180 | 181 | 182 | ########################## Helpers ########################## 183 | def setMotorValueByName(self, motorName, desiredValue): 184 | """ 185 | Joint Position Control using PyBullet's Default joint angle control 186 | :param motorName: string, motor name 187 | :param desiredValue: float, angle value 188 | """ 189 | motorId=self.jointNameToId[motorName] 190 | p.setJointMotorControl2(bodyIndex=self.model_unique_id, 191 | jointIndex=motorId, 192 | controlMode=p.POSITION_CONTROL, 193 | targetPosition=desiredValue, 194 | positionGain=self.kp, 195 | velocityGain=self.kd, 196 | force=self.max_motor_force) 197 | 198 | def executeAllMotorPosCommands(self): 199 | """ 200 | This is a helper function that executes all motor position commands in self.motorDict 201 | """ 202 | if self.use_spring: 203 | for spring_id in range(1, self.leg_num + 1): 204 | self.spring_models[spring_id].apply_spring_torque() 205 | 206 | for name, value in self.motorDict.items(): 207 | self.setMotorValueByName(name, value) 208 | 209 | def returnJointStateMsg(self): 210 | """ 211 | Publish joint states. Note that effort is the motor torque applied during the last stepSimulation. 212 | """ 213 | joint_state_msg = JointState() 214 | for joint_name in self.jointNameToId.keys(): 215 | joint_state = p.getJointState(bodyUniqueId = self.model_unique_id, jointIndex=self.jointNameToId[joint_name]) 216 | joint_state_msg.name.append(joint_name) 217 | joint_state_msg.position.append(joint_state[0]) 218 | joint_state_msg.velocity.append(joint_state[1]) 219 | joint_state_msg.effort.append(joint_state[3]) 220 | return joint_state_msg 221 | 222 | def updateJointStates(self, name_ls, position_ls): 223 | """ 224 | The core function to update joint states. If a joint name is not for an actuated joint, it will be skipped without notice. 225 | :param name_ls: (list-like) names of joints to be updated. 226 | :param position_ls: (list-like) new positions of joints 227 | """ 228 | for i in range(len(name_ls)): 229 | if name_ls[i] in self.motorDict: 230 | self.motorDict[name_ls[i]] = position_ls[i] 231 | 232 | def getEndEffectorLinkID(self): 233 | """ return the end effector link ID """ 234 | return self.linkNameToID[self.end_effector_name] 235 | 236 | def getEndEffectorPosition(self): 237 | """return the end effector position""" 238 | link_states = p.getLinkState(self.model_unique_id, self.getEndEffectorLinkID()) 239 | return link_states[0] 240 | 241 | 242 | -------------------------------------------------------------------------------- /omnid_supplementary_packages/tf2_armadillo/test/test_tf2_armadillo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) Rico Ruotong Jia 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 14 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 18 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 19 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 20 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 21 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 22 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 24 | * POSSIBILITY OF SUCH DAMAGE. 25 | */ 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | typedef arma::Mat mat; 32 | using tf2::fromMsg; 33 | using tf2::toMsg; 34 | using std::endl; 35 | 36 | TEST(Tf2ArmadilloTest, geometryMsgsTest1) 37 | { 38 | geometry_msgs::Transform tf; 39 | tf.translation.x = 1; 40 | tf.translation.y = 2; 41 | tf.translation.z = 3; 42 | tf.rotation.x = 0; 43 | tf.rotation.y = 0; 44 | tf.rotation.z = 0.7071068; 45 | tf.rotation.w = 0.7071068; 46 | mat::fixed<4, 4> ret; 47 | fromMsg(tf, ret); 48 | mat::fixed<4, 4> expected = 49 | {{0.0000000, -1.0000000, 0.0000000, 1}, 50 | {1.0000000, 0.0000000, 0.0000000, 2}, 51 | {0.0000000, 0.0000000, 1.0000000, 3}, 52 | {0, 0, 0, 1}}; 53 | EXPECT_TRUE(arma::approx_equal(expected, ret, "absdiff", 1e-3))<<"ret: "< ret; 66 | fromMsg(tf, ret); 67 | mat::fixed<4, 4>expected = 68 | {{0.3333333, -0.2440169, 0.9106836, 1}, 69 | {0.9106836, 0.3333333, -0.2440169, 2}, 70 | {-0.2440169, 0.9106836, 0.3333333, 3}, 71 | {0,0,0,1}}; 72 | EXPECT_TRUE(arma::approx_equal(expected, ret, "absdiff", 1e-3))<<"ret: "< ret; 85 | fromMsg(tf, ret); 86 | mat::fixed<4, 4>expected = 87 | {{-0.1380712, 0.1607873, 0.9772839, 1}, 88 | {0.9772839, -0.1380712, 0.1607873, 2}, 89 | {0.1607873, 0.9772839, -0.1380712, 3}, 90 | {0,0,0,1}}; 91 | EXPECT_TRUE(arma::approx_equal(expected, ret, "absdiff", 1e-3))<<"ret: "< ret; 104 | fromMsg(tf, ret); 105 | mat::fixed<4, 4>expected = 106 | {{-0.1773630, 0.9597951, 0.2175679, 1}, 107 | {0.2175679, -0.1773630, 0.9597951, 2}, 108 | {0.9597951, 0.2175679, -0.1773630, 3}, 109 | {0,0,0,1}}; 110 | EXPECT_TRUE(arma::approx_equal(expected, ret, "absdiff", 1e-3))<<"ret: "< ret; 119 | fromMsg(tf, ret); 120 | mat::fixed<4, 4> expected = 121 | {{0.0000000, -1.0000000, 0.0000000, 1}, 122 | {1.0000000, 0.0000000, 0.0000000, 2}, 123 | {0.0000000, 0.0000000, 1.0000000, 3}, 124 | {0, 0, 0, 1}}; 125 | EXPECT_TRUE(arma::approx_equal(expected, ret, "absdiff", 1e-3))<<"ret: "< ret; 133 | fromMsg(tf, ret); 134 | mat::fixed<4, 4>expected = 135 | {{0.3333333, -0.2440169, 0.9106836, 1}, 136 | {0.9106836, 0.3333333, -0.2440169, 2}, 137 | {-0.2440169, 0.9106836, 0.3333333, 3}, 138 | {0,0,0,1}}; 139 | EXPECT_TRUE(arma::approx_equal(expected, ret, "absdiff", 1e-3))<<"ret: "< ret; 147 | fromMsg(tf, ret); 148 | mat::fixed<4, 4>expected = 149 | {{-0.1380712, 0.1607873, 0.9772839, 1}, 150 | {0.9772839, -0.1380712, 0.1607873, 2}, 151 | {0.1607873, 0.9772839, -0.1380712, 3}, 152 | {0,0,0,1}}; 153 | EXPECT_TRUE(arma::approx_equal(expected, ret, "absdiff", 1e-3))<<"ret: "< ret; 161 | fromMsg(tf, ret); 162 | mat::fixed<4, 4>expected = 163 | {{-0.1773630, 0.9597951, 0.2175679, 1}, 164 | {0.2175679, -0.1773630, 0.9597951, 2}, 165 | {0.9597951, 0.2175679, -0.1773630, 3}, 166 | {0,0,0,1}}; 167 | EXPECT_TRUE(arma::approx_equal(expected, ret, "absdiff", 1e-3))<<"ret: "< input = 173 | {{0.0000000, -1.0000000, 0.0000000, 1}, 174 | {1.0000000, 0.0000000, 0.0000000, 2}, 175 | {0.0000000, 0.0000000, 1.0000000, 3}, 176 | {0, 0, 0, 1}}; 177 | tf2::Transform tf; 178 | auto output = toMsg(input, tf); 179 | tf.setOrigin({1,2,3}); 180 | tf.setRotation({0, 0, 0.7071068, 0.7071068}); 181 | 182 | EXPECT_NEAR( tf.getRotation().getAngle(), output.getRotation().getAngle(), 1e-3 )<<"expected value: "<< tf.getRotation().getAngle() <<"output: "<< output.getRotation().getAngle(); 183 | auto o_axis = output.getRotation().getAxis(); 184 | auto i_axis = tf.getRotation().getAxis(); 185 | EXPECT_NEAR(o_axis.getX(),i_axis.getX(), 1e-3)<<" actual: "<input = 199 | {{0.3333333, -0.2440169, 0.9106836, 1}, 200 | {0.9106836, 0.3333333, -0.2440169, 2}, 201 | {-0.2440169, 0.9106836, 0.3333333, 3}, 202 | {0,0,0,1}}; 203 | tf2::Transform tf; 204 | auto output = toMsg(input, tf); 205 | tf.setOrigin({1,2,3}); 206 | tf.setRotation({0.4082483, 0.4082483, 0.4082483, 0.7071068}); 207 | 208 | EXPECT_NEAR( tf.getRotation().getAngle(), output.getRotation().getAngle(), 1e-3 )<<"expected value: "<< tf.getRotation().getAngle() <<"output: "<< output.getRotation().getAngle(); 209 | auto o_axis = output.getRotation().getAxis(); 210 | auto i_axis = tf.getRotation().getAxis(); 211 | EXPECT_NEAR(o_axis.getX(),i_axis.getX(), 1e-3)<<" actual: "<input = 225 | {{-0.1380712, 0.1607873, 0.9772839, 1}, 226 | {0.9772839, -0.1380712, 0.1607873, 2}, 227 | {0.1607873, 0.9772839, -0.1380712, 3}, 228 | {0,0,0,1}}; 229 | tf2::Transform tf; 230 | auto output = toMsg(input, tf); 231 | tf.setOrigin({1,2,3}); 232 | tf.setRotation({0.5334021, 0.5334021, 0.5334021, 0.3826834}); 233 | 234 | EXPECT_NEAR( tf.getRotation().getAngle(), output.getRotation().getAngle(), 1e-3 )<<"expected value: "<< tf.getRotation().getAngle() <<"output: "<< output.getRotation().getAngle(); 235 | auto o_axis = output.getRotation().getAxis(); 236 | auto i_axis = tf.getRotation().getAxis(); 237 | EXPECT_NEAR(o_axis.getX(),i_axis.getX(), 1e-3)<<" actual: "<input = 251 | {{-0.1773630, 0.9597951, 0.2175679, 1}, 252 | {0.2175679, -0.1773630, 0.9597951, 2}, 253 | {0.9597951, 0.2175679, -0.1773630, 3}, 254 | {0,0,0,1}}; 255 | tf2::Transform tf; 256 | auto output = toMsg(input, tf); 257 | tf.setOrigin({1,2,3}); 258 | tf.setRotation({0.5425318, 0.5425318, 0.5425318, -0.3420201}); 259 | 260 | EXPECT_NEAR( tf.getRotation().getAngle(), output.getRotation().getAngle(), 1e-3 )<<"expected value: "<< tf.getRotation().getAngle() <<"output: "<< output.getRotation().getAngle(); 261 | auto o_axis = output.getRotation().getAxis(); 262 | auto i_axis = tf.getRotation().getAxis(); 263 | EXPECT_NEAR(o_axis.getX(),i_axis.getX(), 1e-3)<<" actual: "< 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | //TF transforms 18 | #include 19 | #include 20 | 21 | 22 | using std::string; 23 | using std::vector; 24 | 25 | namespace omnid_planning_adapter_plugin{ 26 | void printTfTransform(const tf2::Transform& tf){ 27 | auto origin = tf.getOrigin(); 28 | ROS_INFO_STREAM("x: "<< origin.getX()<<" y: "<< origin.getY()<<" z: "<< origin.getZ()); 29 | auto quat = tf.getRotation(); 30 | auto angle = quat.getAngle(); 31 | ROS_INFO_STREAM("angle: "< World. 39 | const tf2::Transform& getRobotToWorldTf() const 40 | { 41 | return robot_to_world_tf_; 42 | } 43 | 44 | bool nameInitialized() const 45 | { 46 | return name_init_; 47 | } 48 | 49 | protected: 50 | Adapter() = default; 51 | Adapter(const std::string &robot_name, const std::string& body_frame_name, const std::string& world_frame_name) : 52 | robot_name_(robot_name), 53 | body_frame_name_(body_frame_name), 54 | world_frame_name_(world_frame_name), 55 | name_init_(false) 56 | { 57 | getTf2Transform(body_frame_name, world_frame_name, robot_to_world_tf_); 58 | world_to_robot_tf_ = robot_to_world_tf_.inverse(); 59 | } 60 | tf2::Transform robot_to_world_tf_; //T_robot_world 61 | tf2::Transform world_to_robot_tf_; 62 | std::string robot_name_; 63 | std::string body_frame_name_; 64 | std::string world_frame_name_; 65 | mutable bool name_init_; 66 | 67 | /// \brief get the head and tail iterators of a range of elements in a string vector that contains a string. 68 | /// the range is [head, tail). If no range is found, then the end of string is returned. 69 | void getRangeInStringVec(const std::vector &str_vec, const std::string &to_search, 70 | std::vector::const_iterator &head, 71 | std::vector::const_iterator &tail) const{ 72 | // increment head and tail together, until to_search is found. Then, increment tail until the end of the range. 73 | for(head = str_vec.cbegin(), tail = head; head != str_vec.cend(); ++head, ++tail){ 74 | if ((*head).find(to_search) != string::npos){ 75 | for (; tail!=str_vec.cend() && (*tail).find(to_search) != string::npos; ++tail); 76 | break; 77 | } 78 | } 79 | } 80 | 81 | /// \brief return the const_iterator of the first element in a string vector that contains to_search 82 | vector::const_iterator findString(const std::string& to_search, const vector::const_iterator head, const vector::const_iterator tail) const 83 | { 84 | auto itr = head; 85 | for(; itr != tail && (*itr).find(to_search) == string::npos; ++itr ); 86 | return itr; 87 | } 88 | 89 | private: 90 | /// \brief listen to the transform Tsource -> target and store it in result. 91 | /// \param target_frame: frame we express source frame in. 92 | /// \param source_frame: frame we want to express in target_frame. 93 | /// \param result: where the result transform is stored 94 | void getTf2Transform(const std::string& target_frame, const std::string& source_frame, tf2::Transform& result) 95 | { 96 | tf2_ros::Buffer tf_buffer; 97 | tf2_ros::TransformListener tf_listener (tf_buffer); 98 | try{ 99 | auto reference_to_robot_tf_ = tf_buffer.lookupTransform(target_frame, source_frame, ros::Time(0), 100 | ros::Duration(3.0)); 101 | tf2::fromMsg(reference_to_robot_tf_.transform, result); 102 | } 103 | catch(std::exception &e){ 104 | ROS_ERROR_STREAM("omnid_planning_adapter_plugin: cannot initialize TF. " << e.what()); 105 | ros::shutdown(); 106 | }; 107 | } 108 | }; 109 | 110 | class Object_Platform_Adapter : public Adapter{ 111 | public: 112 | Object_Platform_Adapter() = default; 113 | Object_Platform_Adapter(const string& robot_name, const string& body_frame_name, const string& world_frame_name) : 114 | Adapter(robot_name, body_frame_name, world_frame_name) 115 | { 116 | ros::NodeHandle nh; 117 | double object_clearance, object_thickness, h_platform; 118 | nh.getParam("object_clearance", object_clearance); 119 | nh.getParam("object_thickness", object_thickness); 120 | nh.getParam("h_platform", h_platform); 121 | object_z_correction_ = object_clearance + (object_thickness + h_platform)/2.0; 122 | 123 | } 124 | 125 | /// \brief initialize a look up table that maps joint names to their indices in joint_names. 126 | bool initializeJointLookup(const std::vector &joint_names) const { 127 | // first, it finds the positions of head and tail. Then, it will build a map of joint_name to index. 128 | vector::const_iterator head, tail; 129 | getRangeInStringVec(joint_names, robot_name_, head, tail); 130 | name_init_ = true; 131 | vector to_search_keywords{"x", "y", "z", "roll", "pitch", "yaw"}; 132 | for (unsigned int i = 0; i < to_search_keywords.size(); ++i) { 133 | string keyword = to_search_keywords.at(i); 134 | auto itr = findString(keyword, head, tail); 135 | joint_lookup_[keyword] = itr - joint_names.cbegin(); 136 | } 137 | } 138 | 139 | /// \brief calculate the world frame pose of the object platform from a trajectory point and store it. 140 | /// \param traj_point - a point that consists of all joint values 141 | /// \param tf - next pose of the object platform (to be filled in) 142 | void calcPlatformPoseTf(const trajectory_msgs::JointTrajectoryPoint& traj_point, tf2::Transform& tf)const 143 | { 144 | //read x, y, z, roll, pitch, yaw as the body frame pose, then convert it to tf2::Transform 145 | double x = traj_point.positions[joint_lookup_["x"]]; 146 | double y = traj_point.positions[joint_lookup_["y"]]; 147 | double z = traj_point.positions[joint_lookup_["z"]] - object_z_correction_; //this come from the overall clearance between the object and each robot platform 148 | double roll = traj_point.positions[joint_lookup_["roll"]]; 149 | double pitch = traj_point.positions[joint_lookup_["pitch"]]; 150 | double yaw = traj_point.positions[joint_lookup_["yaw"]]; 151 | tf.setOrigin({x, y, z}); 152 | tf2::Quaternion quat; 153 | quat.setEulerZYX(yaw, pitch, roll); 154 | tf.setRotation(quat); 155 | //get world frame pose 156 | tf = world_to_robot_tf_ * tf; 157 | } 158 | 159 | /// \brief calculate the next body-frame eef pose of a robot. The result is stored in robot_to_eef_tf. 160 | void getNextEefPose(const tf2::Transform &world_to_obj_tf, const tf2::Transform & delta_robot_to_world_tf, 161 | const tf2::Transform &world_to_delta_robot_tf, tf2::Transform &robot_to_eef_tf) const 162 | { 163 | robot_to_eef_tf = delta_robot_to_world_tf * world_to_obj_tf * robot_to_world_tf_ * world_to_delta_robot_tf; 164 | } 165 | 166 | private: 167 | mutable std::unordered_map joint_lookup_; 168 | double object_z_correction_; 169 | }; 170 | 171 | class Delta_Robot_Adapter : public Adapter{ 172 | public: 173 | Delta_Robot_Adapter() = default; 174 | Delta_Robot_Adapter(const string& planning_group_name, const string& robot_name, const string& body_frame_name, const string& world_frame_name) : 175 | Adapter(robot_name, body_frame_name, world_frame_name), 176 | planning_group_name_(planning_group_name) 177 | {} 178 | 179 | /// \brief return a constant reference of Transform world -> robot. 180 | const tf2::Transform& getWorldToRobotTf() const 181 | { 182 | return world_to_robot_tf_; 183 | } 184 | 185 | bool initializeJointLookup(std::vector joint_names, const planning_scene::PlanningSceneConstPtr &planning_scene) const 186 | { 187 | // we are going to build a lookup from the IK indices to the the trajectory message (joint_names). 188 | // find the range in the joint_names vector 189 | vector::const_iterator head, tail; 190 | getRangeInStringVec(joint_names, robot_name_, head, tail); 191 | // grab the IK solver sequence from the joint_model_group->getJointModels(), then find the index of the corresponding names. 192 | const robot_model::JointModelGroup* joint_model_group = planning_scene->getRobotModel()->getJointModelGroup( 193 | planning_group_name_); 194 | auto ik_joint_names = joint_model_group->getJointModelNames(); 195 | ik_solution_dimension_ = joint_model_group->getActiveJointModels().size() + joint_model_group->getMimicJointModels().size(); 196 | unsigned int ik_solution_index = 0; 197 | for (std::size_t i=0; i < joint_model_group->getJointModels().size(); ++i) { 198 | const robot_model::JointModel *jm = joint_model_group->getJointModels()[i]; 199 | if (jm->getType() == moveit::core::JointModel::REVOLUTE || 200 | jm->getType() == moveit::core::JointModel::PRISMATIC) 201 | { 202 | //find the corresponding index of the joint name. 203 | auto itr = findString(ik_joint_names[i], head, tail); 204 | if(itr != tail){ 205 | unsigned int traj_msg_index = itr - joint_names.cbegin(); 206 | traj_to_ik_index_lookup_[traj_msg_index] = ik_solution_index; 207 | } 208 | ++ik_solution_index; 209 | } 210 | } 211 | name_init_ = true; 212 | } 213 | 214 | bool 215 | calcLowLevelIk(const planning_scene::PlanningSceneConstPtr &planning_scene, const tf2::Transform &robot_to_eef, 216 | trajectory_msgs::JointTrajectoryPoint &traj_point) const 217 | { 218 | // Workflow: 219 | // 1. convert the robot eef to pose, generate some seed 220 | // 2. get the solver instance and feed the robot with the new pose, seed. 221 | // 3. get success and the new IK poses. If not successful, we are going to return false. TrajPoint is untouched. 222 | // 4. if successful, traverse through all traj point, using the head and tail iterators. 223 | // 5. find each entry in the IK solution, then fill the entry into traj_point. 224 | // 6. return true. 225 | 226 | geometry_msgs::Pose pose = tf2::toMsg(robot_to_eef, pose); 227 | vector seed(ik_solution_dimension_); 228 | vector ik_sol; ik_sol.reserve(ik_solution_dimension_); 229 | moveit_msgs::MoveItErrorCodes error; 230 | kinematics::KinematicsQueryOptions options; 231 | auto solver_ptr = planning_scene -> getRobotModel() -> getJointModelGroup(planning_group_name_) -> getSolverInstance(); 232 | if(solver_ptr){ 233 | bool success = solver_ptr->searchPositionIK(pose, seed, 0.01, ik_sol, error, options); 234 | if(!success) { 235 | return false;} 236 | } 237 | 238 | // Traversing through all joints we're interested in and fill out traj_point 239 | for (auto beg = traj_to_ik_index_lookup_.cbegin(); beg != traj_to_ik_index_lookup_.cend(); ++beg) 240 | { 241 | unsigned int traj_i = beg -> first, ik_i = beg -> second; 242 | traj_point.positions[traj_i] = ik_sol.at(ik_i); 243 | } 244 | return true; 245 | } 246 | 247 | private: 248 | // map for joint name in trajectory_msg -> IK solution index 249 | mutable std::unordered_map traj_to_ik_index_lookup_; 250 | mutable unsigned int ik_solution_dimension_; 251 | std::string planning_group_name_; 252 | }; 253 | 254 | } 255 | 256 | #endif //OMNID_RICO_OMNID_PLANNING_ADAPTER_PLUGIN_H 257 | --------------------------------------------------------------------------------