diff --git a/robots/spider/CMakeLists.txt b/robots/spidar/CMakeLists.txt similarity index 92% rename from robots/spider/CMakeLists.txt rename to robots/spidar/CMakeLists.txt index b21fc2f4a..df86773a5 100644 --- a/robots/spider/CMakeLists.txt +++ b/robots/spidar/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(spider) +project(spidar) ## Compile as C++11, supported in ROS Kinetic and newer add_compile_options(-std=c++14) @@ -115,7 +115,7 @@ find_package(OsqpEigen REQUIRED) ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include - LIBRARIES spider_robot_model spider_controllib + LIBRARIES spidar_robot_model spidar_controllib CATKIN_DEPENDS aerial_robot_base aerial_robot_model aerial_robot_msgs aerial_robot_simulation dragon roscpp sensor_msgs spinal std_msgs # DEPENDS system_lib ) @@ -135,7 +135,7 @@ include_directories( ## Declare a C++ library # add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/spider.cpp +# src/${PROJECT_NAME}/spidar.cpp # ) ## Add cmake target dependencies of the library @@ -146,7 +146,7 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/spider_node.cpp) +# add_executable(${PROJECT_NAME}_node src/spidar_node.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -205,20 +205,20 @@ include_directories( # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} # ) -add_library(spider_robot_model +add_library(spidar_robot_model src/model/hydrus_like_robot_model.cpp src/model/full_vectoring_robot_model.cpp src/model/ground_robot_model.cpp) -target_link_libraries(spider_robot_model ${catkin_LIBRARIES} ${NLOPT_LIBRARIES} ${OsqpEigen_LIBRARIES}) +target_link_libraries(spidar_robot_model ${catkin_LIBRARIES} ${NLOPT_LIBRARIES} ${OsqpEigen_LIBRARIES}) -add_library(spider_controllib src/control/walk_control.cpp) -target_link_libraries (spider_controllib ${catkin_LIBRARIES} spider_robot_model ${OsqpEigen_LIBRARIES}) +add_library(spidar_controllib src/control/walk_control.cpp) +target_link_libraries (spidar_controllib ${catkin_LIBRARIES} spidar_robot_model ${OsqpEigen_LIBRARIES}) -add_library(spider_navigation +add_library(spidar_navigation src/navigation/walk_navigation.cpp src/navigation/flight_navigation.cpp) -target_link_libraries(spider_navigation ${catkin_LIBRARIES} ${OsqpEigen_LIBRARIES}) # no need to add spider_controllib +target_link_libraries(spidar_navigation ${catkin_LIBRARIES} ${OsqpEigen_LIBRARIES}) # no need to add spidar_controllib ############# @@ -226,7 +226,7 @@ target_link_libraries(spider_navigation ${catkin_LIBRARIES} ${OsqpEigen_LIBRARIE ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_spider.cpp) +# catkin_add_gtest(${PROJECT_NAME}-test test/test_spidar.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() diff --git a/robots/spidar/bin/rosbag_play_spider.sh b/robots/spidar/bin/rosbag_play_spider.sh new file mode 100755 index 000000000..28fd9504c --- /dev/null +++ b/robots/spidar/bin/rosbag_play_spider.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +rosbag play $1 --pause --clock /spidar/joints_ctrl:=/spidar/joints_ctrl_orig /spidar/joints_torque_ctrl:=/spidar/joints_torque_ctrl_orig /spidar/debug/pose/pid:=/spidar/debug/pose/pid_orig /spidar/four_axes/command:=/spidar/four_axes/command_orig /spidar/debug/target_vectoring_force:=/spidar/debug/target_vectoring_force_orig /spidar/servo/target_states:=/spidar/servo/target_states_orig ${@:2} diff --git a/robots/spider/config/octo/Battery.yaml b/robots/spidar/config/octo/Battery.yaml similarity index 100% rename from robots/spider/config/octo/Battery.yaml rename to robots/spidar/config/octo/Battery.yaml diff --git a/robots/spider/config/octo/MotorInfo.yaml b/robots/spidar/config/octo/MotorInfo.yaml similarity index 100% rename from robots/spider/config/octo/MotorInfo.yaml rename to robots/spidar/config/octo/MotorInfo.yaml diff --git a/robots/spider/config/octo/RvizInit.yaml b/robots/spidar/config/octo/RvizInit.yaml similarity index 100% rename from robots/spider/config/octo/RvizInit.yaml rename to robots/spidar/config/octo/RvizInit.yaml diff --git a/robots/spider/config/octo/Servo.yaml b/robots/spidar/config/octo/Servo.yaml similarity index 100% rename from robots/spider/config/octo/Servo.yaml rename to robots/spidar/config/octo/Servo.yaml diff --git a/robots/spider/config/octo/Simulation.yaml b/robots/spidar/config/octo/Simulation.yaml similarity index 100% rename from robots/spider/config/octo/Simulation.yaml rename to robots/spidar/config/octo/Simulation.yaml diff --git a/robots/spider/config/octo/StateEstimation.yaml b/robots/spidar/config/octo/StateEstimation.yaml similarity index 100% rename from robots/spider/config/octo/StateEstimation.yaml rename to robots/spidar/config/octo/StateEstimation.yaml diff --git a/robots/spider/config/octo/control/FullVectoringControlConfig.yaml b/robots/spidar/config/octo/control/FullVectoringControlConfig.yaml similarity index 100% rename from robots/spider/config/octo/control/FullVectoringControlConfig.yaml rename to robots/spidar/config/octo/control/FullVectoringControlConfig.yaml diff --git a/robots/spider/config/octo/control/LQIGimbalControlConfig.yaml b/robots/spidar/config/octo/control/LQIGimbalControlConfig.yaml similarity index 100% rename from robots/spider/config/octo/control/LQIGimbalControlConfig.yaml rename to robots/spidar/config/octo/control/LQIGimbalControlConfig.yaml diff --git a/robots/spider/config/octo/control/WalkControlConfig.yaml b/robots/spidar/config/octo/control/WalkControlConfig.yaml similarity index 96% rename from robots/spider/config/octo/control/WalkControlConfig.yaml rename to robots/spidar/config/octo/control/WalkControlConfig.yaml index 48f284da4..fae5a371e 100644 --- a/robots/spider/config/octo/control/WalkControlConfig.yaml +++ b/robots/spidar/config/octo/control/WalkControlConfig.yaml @@ -1,4 +1,4 @@ -aerial_robot_control_name: aerial_robot_control/spider_walk +aerial_robot_control_name: aerial_robot_control/spidar_walk controller: diff --git a/robots/spider/config/octo/model/FullVectoringRobotModel.yaml b/robots/spidar/config/octo/model/FullVectoringRobotModel.yaml similarity index 87% rename from robots/spider/config/octo/model/FullVectoringRobotModel.yaml rename to robots/spidar/config/octo/model/FullVectoringRobotModel.yaml index d0d808ef9..11af87858 100644 --- a/robots/spider/config/octo/model/FullVectoringRobotModel.yaml +++ b/robots/spidar/config/octo/model/FullVectoringRobotModel.yaml @@ -1,6 +1,6 @@ ### full vectoring robot model -robot_model_plugin_name: spider/full_vectoring_robot_model +robot_model_plugin_name: spidar/full_vectoring_robot_model # gimbal lock debug_verbose: false diff --git a/robots/spider/config/octo/model/GroundRobotModel.yaml b/robots/spidar/config/octo/model/GroundRobotModel.yaml similarity index 90% rename from robots/spider/config/octo/model/GroundRobotModel.yaml rename to robots/spidar/config/octo/model/GroundRobotModel.yaml index 045861300..decbb6f6a 100644 --- a/robots/spider/config/octo/model/GroundRobotModel.yaml +++ b/robots/spidar/config/octo/model/GroundRobotModel.yaml @@ -1,6 +1,6 @@ ### full vectoring robot model -robot_model_plugin_name: spider/ground_robot_model +robot_model_plugin_name: spidar/ground_robot_model # gimbal lock debug_verbose: false diff --git a/robots/spider/config/octo/model/HydrusLikeRobotModel.yaml b/robots/spidar/config/octo/model/HydrusLikeRobotModel.yaml similarity index 65% rename from robots/spider/config/octo/model/HydrusLikeRobotModel.yaml rename to robots/spidar/config/octo/model/HydrusLikeRobotModel.yaml index 48ece61d2..db77a040c 100644 --- a/robots/spider/config/octo/model/HydrusLikeRobotModel.yaml +++ b/robots/spidar/config/octo/model/HydrusLikeRobotModel.yaml @@ -1,6 +1,6 @@ # robot model plugin -robot_model_plugin_name: spider/hydrus_like_robot_model +robot_model_plugin_name: spidar/hydrus_like_robot_model fc_rp_min_thre: 2.2 rp_position_margin_thre: 0.1 # deprecated diff --git a/robots/spider/config/octo/navigate/FlightNavigateConfig.yaml b/robots/spidar/config/octo/navigate/FlightNavigateConfig.yaml similarity index 91% rename from robots/spider/config/octo/navigate/FlightNavigateConfig.yaml rename to robots/spidar/config/octo/navigate/FlightNavigateConfig.yaml index de00547ed..748a8ff70 100644 --- a/robots/spider/config/octo/navigate/FlightNavigateConfig.yaml +++ b/robots/spidar/config/octo/navigate/FlightNavigateConfig.yaml @@ -1,6 +1,6 @@ # Teleop Basic Param -flight_navigation_plugin_name: aerial_robot_navigation/spider/flight_navigation +flight_navigation_plugin_name: aerial_robot_navigation/spidar/flight_navigation navigation: xy_control_mode: 0 diff --git a/robots/spider/config/octo/navigate/WalkNavigateConfig.yaml b/robots/spidar/config/octo/navigate/WalkNavigateConfig.yaml similarity index 96% rename from robots/spider/config/octo/navigate/WalkNavigateConfig.yaml rename to robots/spidar/config/octo/navigate/WalkNavigateConfig.yaml index a431b87d7..69acec01b 100644 --- a/robots/spider/config/octo/navigate/WalkNavigateConfig.yaml +++ b/robots/spidar/config/octo/navigate/WalkNavigateConfig.yaml @@ -1,6 +1,6 @@ # Teleop Basic Param -flight_navigation_plugin_name: aerial_robot_navigation/spider/walk_navigation +flight_navigation_plugin_name: aerial_robot_navigation/spidar/walk_navigation navigation: xy_control_mode: 0 diff --git a/robots/spider/config/rviz_config b/robots/spidar/config/rviz_config similarity index 81% rename from robots/spider/config/rviz_config rename to robots/spidar/config/rviz_config index 832639363..640eafa19 100644 --- a/robots/spider/config/rviz_config +++ b/robots/spidar/config/rviz_config @@ -448,7 +448,7 @@ Visualization Manager: Show Trail: false Name: RobotModel Robot Description: robot_description - TF Prefix: spider + TF Prefix: spidar Update Interval: 0 Value: true Visual Enabled: true @@ -457,167 +457,167 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true - spider/bat1: + spidar/bat1: Value: true - spider/bat2: + spidar/bat2: Value: true - spider/bat3: + spidar/bat3: Value: true - spider/bat4: + spidar/bat4: Value: true - spider/bat5: + spidar/bat5: Value: true - spider/bat6: + spidar/bat6: Value: true - spider/bat7: + spidar/bat7: Value: true - spider/bat8: + spidar/bat8: Value: true - spider/center_link: + spidar/center_link: Value: true - spider/cog: + spidar/cog: Value: true - spider/edf1_left: + spidar/edf1_left: Value: true - spider/edf1_right: + spidar/edf1_right: Value: true - spider/edf2_left: + spidar/edf2_left: Value: true - spider/edf2_right: + spidar/edf2_right: Value: true - spider/edf3_left: + spidar/edf3_left: Value: true - spider/edf3_right: + spidar/edf3_right: Value: true - spider/edf4_left: + spidar/edf4_left: Value: true - spider/edf4_right: + spidar/edf4_right: Value: true - spider/edf5_left: + spidar/edf5_left: Value: true - spider/edf5_right: + spidar/edf5_right: Value: true - spider/edf6_left: + spidar/edf6_left: Value: true - spider/edf6_right: + spidar/edf6_right: Value: true - spider/edf7_left: + spidar/edf7_left: Value: true - spider/edf7_right: + spidar/edf7_right: Value: true - spider/edf8_left: + spidar/edf8_left: Value: true - spider/edf8_right: + spidar/edf8_right: Value: true - spider/end_leg2: + spidar/end_leg2: Value: true - spider/end_leg4: + spidar/end_leg4: Value: true - spider/end_leg6: + spidar/end_leg6: Value: true - spider/end_leg8: + spidar/end_leg8: Value: true - spider/fc: + spidar/fc: Value: true - spider/gimbal1_pitch_module: + spidar/gimbal1_pitch_module: Value: true - spider/gimbal1_roll_module: + spidar/gimbal1_roll_module: Value: true - spider/gimbal2_pitch_module: + spidar/gimbal2_pitch_module: Value: true - spider/gimbal2_roll_module: + spidar/gimbal2_roll_module: Value: true - spider/gimbal3_pitch_module: + spidar/gimbal3_pitch_module: Value: true - spider/gimbal3_roll_module: + spidar/gimbal3_roll_module: Value: true - spider/gimbal4_pitch_module: + spidar/gimbal4_pitch_module: Value: true - spider/gimbal4_roll_module: + spidar/gimbal4_roll_module: Value: true - spider/gimbal5_pitch_module: + spidar/gimbal5_pitch_module: Value: true - spider/gimbal5_roll_module: + spidar/gimbal5_roll_module: Value: true - spider/gimbal6_pitch_module: + spidar/gimbal6_pitch_module: Value: true - spider/gimbal6_roll_module: + spidar/gimbal6_roll_module: Value: true - spider/gimbal7_pitch_module: + spidar/gimbal7_pitch_module: Value: true - spider/gimbal7_roll_module: + spidar/gimbal7_roll_module: Value: true - spider/gimbal8_pitch_module: + spidar/gimbal8_pitch_module: Value: true - spider/gimbal8_roll_module: + spidar/gimbal8_roll_module: Value: true - spider/joint_junction_leg1: + spidar/joint_junction_leg1: Value: true - spider/joint_junction_leg2: + spidar/joint_junction_leg2: Value: true - spider/joint_junction_leg3: + spidar/joint_junction_leg3: Value: true - spider/joint_junction_leg4: + spidar/joint_junction_leg4: Value: true - spider/joint_junction_leg5: + spidar/joint_junction_leg5: Value: true - spider/joint_junction_leg6: + spidar/joint_junction_leg6: Value: true - spider/joint_junction_leg7: + spidar/joint_junction_leg7: Value: true - spider/joint_junction_leg8: + spidar/joint_junction_leg8: Value: true - spider/joint_junction_link1: + spidar/joint_junction_link1: Value: true - spider/joint_junction_link2: + spidar/joint_junction_link2: Value: true - spider/joint_junction_link3: + spidar/joint_junction_link3: Value: true - spider/joint_junction_link4: + spidar/joint_junction_link4: Value: true - spider/joint_junction_link5: + spidar/joint_junction_link5: Value: true - spider/joint_junction_link6: + spidar/joint_junction_link6: Value: true - spider/joint_junction_link7: + spidar/joint_junction_link7: Value: true - spider/joint_junction_link8: + spidar/joint_junction_link8: Value: true - spider/link1: + spidar/link1: Value: true - spider/link2: + spidar/link2: Value: true - spider/link3: + spidar/link3: Value: true - spider/link4: + spidar/link4: Value: true - spider/link5: + spidar/link5: Value: true - spider/link6: + spidar/link6: Value: true - spider/link7: + spidar/link7: Value: true - spider/link8: + spidar/link8: Value: true - spider/pc: + spidar/pc: Value: true - spider/root: + spidar/root: Value: true - spider/thrust1: + spidar/thrust1: Value: true - spider/thrust2: + spidar/thrust2: Value: true - spider/thrust3: + spidar/thrust3: Value: true - spider/thrust4: + spidar/thrust4: Value: true - spider/thrust5: + spidar/thrust5: Value: true - spider/thrust6: + spidar/thrust6: Value: true - spider/thrust7: + spidar/thrust7: Value: true - spider/thrust8: + spidar/thrust8: Value: true world: Value: true @@ -628,133 +628,133 @@ Visualization Manager: Show Names: true Tree: world: - spider/root: - spider/center_link: - spider/fc: + spidar/root: + spidar/center_link: + spidar/fc: {} - spider/joint_junction_link1: - spider/joint_junction_leg1: + spidar/joint_junction_link1: + spidar/joint_junction_leg1: {} - spider/link1: - spider/bat1: + spidar/link1: + spidar/bat1: {} - spider/gimbal1_roll_module: - spider/gimbal1_pitch_module: - spider/edf1_left: + spidar/gimbal1_roll_module: + spidar/gimbal1_pitch_module: + spidar/edf1_left: {} - spider/edf1_right: + spidar/edf1_right: {} - spider/thrust1: + spidar/thrust1: {} - spider/joint_junction_link2: - spider/joint_junction_leg2: + spidar/joint_junction_link2: + spidar/joint_junction_leg2: {} - spider/link2: - spider/bat2: + spidar/link2: + spidar/bat2: {} - spider/end_leg2: + spidar/end_leg2: {} - spider/gimbal2_roll_module: - spider/gimbal2_pitch_module: - spider/edf2_left: + spidar/gimbal2_roll_module: + spidar/gimbal2_pitch_module: + spidar/edf2_left: {} - spider/edf2_right: + spidar/edf2_right: {} - spider/thrust2: + spidar/thrust2: {} - spider/joint_junction_link3: - spider/joint_junction_leg3: + spidar/joint_junction_link3: + spidar/joint_junction_leg3: {} - spider/link3: - spider/bat3: + spidar/link3: + spidar/bat3: {} - spider/gimbal3_roll_module: - spider/gimbal3_pitch_module: - spider/edf3_left: + spidar/gimbal3_roll_module: + spidar/gimbal3_pitch_module: + spidar/edf3_left: {} - spider/edf3_right: + spidar/edf3_right: {} - spider/thrust3: + spidar/thrust3: {} - spider/joint_junction_link4: - spider/joint_junction_leg4: + spidar/joint_junction_link4: + spidar/joint_junction_leg4: {} - spider/link4: - spider/bat4: + spidar/link4: + spidar/bat4: {} - spider/end_leg4: + spidar/end_leg4: {} - spider/gimbal4_roll_module: - spider/gimbal4_pitch_module: - spider/edf4_left: + spidar/gimbal4_roll_module: + spidar/gimbal4_pitch_module: + spidar/edf4_left: {} - spider/edf4_right: + spidar/edf4_right: {} - spider/thrust4: + spidar/thrust4: {} - spider/joint_junction_link5: - spider/joint_junction_leg5: + spidar/joint_junction_link5: + spidar/joint_junction_leg5: {} - spider/link5: - spider/bat5: + spidar/link5: + spidar/bat5: {} - spider/gimbal5_roll_module: - spider/gimbal5_pitch_module: - spider/edf5_left: + spidar/gimbal5_roll_module: + spidar/gimbal5_pitch_module: + spidar/edf5_left: {} - spider/edf5_right: + spidar/edf5_right: {} - spider/thrust5: + spidar/thrust5: {} - spider/joint_junction_link6: - spider/joint_junction_leg6: + spidar/joint_junction_link6: + spidar/joint_junction_leg6: {} - spider/link6: - spider/bat6: + spidar/link6: + spidar/bat6: {} - spider/end_leg6: + spidar/end_leg6: {} - spider/gimbal6_roll_module: - spider/gimbal6_pitch_module: - spider/edf6_left: + spidar/gimbal6_roll_module: + spidar/gimbal6_pitch_module: + spidar/edf6_left: {} - spider/edf6_right: + spidar/edf6_right: {} - spider/thrust6: + spidar/thrust6: {} - spider/joint_junction_link7: - spider/joint_junction_leg7: + spidar/joint_junction_link7: + spidar/joint_junction_leg7: {} - spider/link7: - spider/bat7: + spidar/link7: + spidar/bat7: {} - spider/gimbal7_roll_module: - spider/gimbal7_pitch_module: - spider/edf7_left: + spidar/gimbal7_roll_module: + spidar/gimbal7_pitch_module: + spidar/edf7_left: {} - spider/edf7_right: + spidar/edf7_right: {} - spider/thrust7: + spidar/thrust7: {} - spider/joint_junction_link8: - spider/joint_junction_leg8: + spidar/joint_junction_link8: + spidar/joint_junction_leg8: {} - spider/link8: - spider/bat8: + spidar/link8: + spidar/bat8: {} - spider/end_leg8: + spidar/end_leg8: {} - spider/gimbal8_roll_module: - spider/gimbal8_pitch_module: - spider/edf8_left: + spidar/gimbal8_roll_module: + spidar/gimbal8_pitch_module: + spidar/edf8_left: {} - spider/edf8_right: + spidar/edf8_right: {} - spider/thrust8: + spidar/thrust8: {} - spider/pc: + spidar/pc: {} - spider/cog: + spidar/cog: {} Update Interval: 0 Value: true diff --git a/robots/spider/include/spider/control/walk_control.h b/robots/spidar/include/spidar/control/walk_control.h similarity index 97% rename from robots/spider/include/spider/control/walk_control.h rename to robots/spidar/include/spidar/control/walk_control.h index e6069217c..c6ebeb9f7 100644 --- a/robots/spider/include/spider/control/walk_control.h +++ b/robots/spidar/include/spidar/control/walk_control.h @@ -36,8 +36,8 @@ #pragma once #include -#include -#include +#include +#include #include #include #include @@ -83,8 +83,8 @@ namespace aerial_robot_control ros::Subscriber joint_no_load_sub_; ros::ServiceServer joint_yaw_torque_srv_, joint_pitch_torque_srv_; - boost::shared_ptr<::Spider::GroundRobotModel> spider_robot_model_; - boost::shared_ptr spider_walk_navigator_; + boost::shared_ptr<::Spider::GroundRobotModel> spidar_robot_model_; + boost::shared_ptr spidar_walk_navigator_; std::vector walk_pid_controllers_; std::vector > walk_pid_reconf_servers_; diff --git a/robots/spider/include/spider/model/full_vectoring_robot_model.h b/robots/spidar/include/spidar/model/full_vectoring_robot_model.h similarity index 100% rename from robots/spider/include/spider/model/full_vectoring_robot_model.h rename to robots/spidar/include/spidar/model/full_vectoring_robot_model.h diff --git a/robots/spider/include/spider/model/ground_robot_model.h b/robots/spidar/include/spidar/model/ground_robot_model.h similarity index 100% rename from robots/spider/include/spider/model/ground_robot_model.h rename to robots/spidar/include/spidar/model/ground_robot_model.h diff --git a/robots/spider/include/spider/model/hydrus_like_robot_model.h b/robots/spidar/include/spidar/model/hydrus_like_robot_model.h similarity index 100% rename from robots/spider/include/spider/model/hydrus_like_robot_model.h rename to robots/spidar/include/spidar/model/hydrus_like_robot_model.h diff --git a/robots/spider/include/spider/navigation/flight_navigation.h b/robots/spidar/include/spidar/navigation/flight_navigation.h similarity index 100% rename from robots/spider/include/spider/navigation/flight_navigation.h rename to robots/spidar/include/spidar/navigation/flight_navigation.h diff --git a/robots/spider/include/spider/navigation/walk_navigation.h b/robots/spidar/include/spidar/navigation/walk_navigation.h similarity index 98% rename from robots/spider/include/spider/navigation/walk_navigation.h rename to robots/spidar/include/spidar/navigation/walk_navigation.h index dd51fa41b..094478ca8 100644 --- a/robots/spider/include/spider/navigation/walk_navigation.h +++ b/robots/spidar/include/spidar/navigation/walk_navigation.h @@ -36,7 +36,7 @@ #pragma once #include -#include +#include #include #include #include @@ -157,7 +157,7 @@ namespace aerial_robot_navigation ros::Publisher simulate_flight_config_pub_; ros::Subscriber simulate_flight_config_sub_; - boost::shared_ptr<::Spider::GroundRobotModel> spider_robot_model_; + boost::shared_ptr<::Spider::GroundRobotModel> spidar_robot_model_; boost::shared_ptr robot_model_for_nav_; aerial_robot_control::Spider::WalkController* walk_controller_; diff --git a/robots/spider/launch/bringup.launch b/robots/spidar/launch/bringup.launch similarity index 94% rename from robots/spider/launch/bringup.launch rename to robots/spidar/launch/bringup.launch index cadfc5fd2..63744e50c 100644 --- a/robots/spider/launch/bringup.launch +++ b/robots/spidar/launch/bringup.launch @@ -21,8 +21,8 @@ - - + + ########### Parameters ########### @@ -78,17 +78,17 @@ - + - + ########### Sensors ########### - + @@ -122,7 +122,7 @@ - + init_pos: [0, 0, 0.4] diff --git a/robots/spider/launch/includes/v1_default/sensors.launch.xml b/robots/spidar/launch/includes/v1_default/sensors.launch.xml similarity index 96% rename from robots/spider/launch/includes/v1_default/sensors.launch.xml rename to robots/spidar/launch/includes/v1_default/sensors.launch.xml index 3b59dec72..4da92b56b 100644 --- a/robots/spider/launch/includes/v1_default/sensors.launch.xml +++ b/robots/spidar/launch/includes/v1_default/sensors.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/robots/spider/launch/includes/v1_leg/sensors.launch.xml b/robots/spidar/launch/includes/v1_leg/sensors.launch.xml similarity index 96% rename from robots/spider/launch/includes/v1_leg/sensors.launch.xml rename to robots/spidar/launch/includes/v1_leg/sensors.launch.xml index 3b59dec72..4da92b56b 100644 --- a/robots/spider/launch/includes/v1_leg/sensors.launch.xml +++ b/robots/spidar/launch/includes/v1_leg/sensors.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/robots/spider/package.xml b/robots/spidar/package.xml similarity index 96% rename from robots/spider/package.xml rename to robots/spidar/package.xml index 6d331a865..d8b26c438 100644 --- a/robots/spider/package.xml +++ b/robots/spidar/package.xml @@ -1,8 +1,8 @@ - spider + spidar 0.0.0 - The spider package + The spidar package @@ -19,7 +19,7 @@ - + diff --git a/robots/spidar/plugins/flight_control_plugins.xml b/robots/spidar/plugins/flight_control_plugins.xml new file mode 100644 index 000000000..cda9db0d1 --- /dev/null +++ b/robots/spidar/plugins/flight_control_plugins.xml @@ -0,0 +1,7 @@ + + + + spidar control plugin: walk control + + + diff --git a/robots/spidar/plugins/flight_navigation_plugin.xml b/robots/spidar/plugins/flight_navigation_plugin.xml new file mode 100644 index 000000000..b683e6f35 --- /dev/null +++ b/robots/spidar/plugins/flight_navigation_plugin.xml @@ -0,0 +1,9 @@ + + + spidar walk navigation plugin + + + + spidar flight navigation plugin + + diff --git a/robots/spidar/plugins/robot_model_plugins.xml b/robots/spidar/plugins/robot_model_plugins.xml new file mode 100644 index 000000000..72aa90bc1 --- /dev/null +++ b/robots/spidar/plugins/robot_model_plugins.xml @@ -0,0 +1,15 @@ + + + + plugin for hydrus-like spidar robot model + + + + plugin for full vectoring spidar robot model + + + + plugin for fround spidar robot model + + + diff --git a/robots/spidar/robots/octo/spidar_v1_base.urdf.xacro b/robots/spidar/robots/octo/spidar_v1_base.urdf.xacro new file mode 100644 index 000000000..5ffa7621a --- /dev/null +++ b/robots/spidar/robots/octo/spidar_v1_base.urdf.xacro @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + diff --git a/robots/spider/robots/octo/v1_default/robot.gazebo.xacro b/robots/spidar/robots/octo/v1_default/robot.gazebo.xacro similarity index 61% rename from robots/spider/robots/octo/v1_default/robot.gazebo.xacro rename to robots/spidar/robots/octo/v1_default/robot.gazebo.xacro index 0605bc4f9..0ae4cebad 100644 --- a/robots/spider/robots/octo/v1_default/robot.gazebo.xacro +++ b/robots/spidar/robots/octo/v1_default/robot.gazebo.xacro @@ -1,9 +1,9 @@ - + xmlns:xacro="http://www.ros.org/wiki/xacro" name="spidar_v1" > + - + diff --git a/robots/spider/robots/octo/v1_default/robot.urdf.xacro b/robots/spidar/robots/octo/v1_default/robot.urdf.xacro similarity index 81% rename from robots/spider/robots/octo/v1_default/robot.urdf.xacro rename to robots/spidar/robots/octo/v1_default/robot.urdf.xacro index 15193202c..fbb8b20f4 100644 --- a/robots/spider/robots/octo/v1_default/robot.urdf.xacro +++ b/robots/spidar/robots/octo/v1_default/robot.urdf.xacro @@ -1,14 +1,14 @@ + xmlns:xacro="http://www.ros.org/wiki/xacro" name="spidar" > - + - + #### flight controller #### + model_url = "package://spidar/urdf/v1/mesh/board/spinal.dae"> diff --git a/robots/spider/robots/octo/v1_leg/robot.gazebo.xacro b/robots/spidar/robots/octo/v1_leg/robot.gazebo.xacro similarity index 83% rename from robots/spider/robots/octo/v1_leg/robot.gazebo.xacro rename to robots/spidar/robots/octo/v1_leg/robot.gazebo.xacro index b3038679e..92befd89e 100644 --- a/robots/spider/robots/octo/v1_leg/robot.gazebo.xacro +++ b/robots/spidar/robots/octo/v1_leg/robot.gazebo.xacro @@ -1,9 +1,9 @@ - + xmlns:xacro="http://www.ros.org/wiki/xacro" name="spidar_v1" > + - + diff --git a/robots/spider/robots/octo/v1_leg/robot.urdf.xacro b/robots/spidar/robots/octo/v1_leg/robot.urdf.xacro similarity index 84% rename from robots/spider/robots/octo/v1_leg/robot.urdf.xacro rename to robots/spidar/robots/octo/v1_leg/robot.urdf.xacro index 2c4d62d3f..c854d7249 100644 --- a/robots/spider/robots/octo/v1_leg/robot.urdf.xacro +++ b/robots/spidar/robots/octo/v1_leg/robot.urdf.xacro @@ -1,15 +1,15 @@ + xmlns:xacro="http://www.ros.org/wiki/xacro" name="spidar" > - + - - + + #### flight controller #### + model_url = "package://spidar/urdf/v1/mesh/board/spinal.dae"> @@ -50,7 +50,7 @@ - + diff --git a/robots/spider/script/gazebo_quick_init_pose.py b/robots/spidar/script/gazebo_quick_init_pose.py similarity index 98% rename from robots/spider/script/gazebo_quick_init_pose.py rename to robots/spidar/script/gazebo_quick_init_pose.py index 69a843947..0da65d3b9 100755 --- a/robots/spider/script/gazebo_quick_init_pose.py +++ b/robots/spidar/script/gazebo_quick_init_pose.py @@ -65,7 +65,7 @@ def servoCb(self, msg): if __name__ == "__main__": - rospy.init_node("spider_init_pose") + rospy.init_node("spidar_init_pose") pose_controller = InitPose() diff --git a/robots/spider/script/servo_cofig.py b/robots/spidar/script/servo_cofig.py similarity index 98% rename from robots/spider/script/servo_cofig.py rename to robots/spidar/script/servo_cofig.py index 696eb1d82..c2e5529c3 100755 --- a/robots/spider/script/servo_cofig.py +++ b/robots/spidar/script/servo_cofig.py @@ -7,7 +7,7 @@ from argparse import ArgumentParser -rospy.init_node("spider_neuron_configure") +rospy.init_node("spidar_neuron_configure") parser = ArgumentParser() parser.add_argument('-n', nargs='+', dest="neuron_id", help='board id(s) to configure') diff --git a/robots/spider/script/spider_octo_transformation.py b/robots/spidar/script/spider_octo_transformation.py similarity index 92% rename from robots/spider/script/spider_octo_transformation.py rename to robots/spidar/script/spider_octo_transformation.py index 26b992373..0aa947722 100755 --- a/robots/spider/script/spider_octo_transformation.py +++ b/robots/spidar/script/spider_octo_transformation.py @@ -7,10 +7,10 @@ class SpiderOctoTransformation: def __init__(self): - rospy.init_node('spider_octo_transformation') + rospy.init_node('spidar_octo_transformation') self.joint_state = JointState() - self.pub = rospy.Publisher('/spider/joints_ctrl', JointState, queue_size = 1) - self.sub = rospy.Subscriber('/spider/joint_states', JointState, self.jointStateCallback) + self.pub = rospy.Publisher('/spidar/joints_ctrl', JointState, queue_size = 1) + self.sub = rospy.Subscriber('/spidar/joint_states', JointState, self.jointStateCallback) rospy.sleep(0.5) diff --git a/robots/spider/src/control/walk_control.cpp b/robots/spidar/src/control/walk_control.cpp similarity index 94% rename from robots/spider/src/control/walk_control.cpp rename to robots/spidar/src/control/walk_control.cpp index a69b68c73..1851b451f 100644 --- a/robots/spider/src/control/walk_control.cpp +++ b/robots/spidar/src/control/walk_control.cpp @@ -33,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include using namespace aerial_robot_model; using namespace aerial_robot_control::Spider; @@ -63,9 +63,9 @@ void WalkController::initialize(ros::NodeHandle nh, ros::NodeHandle nhp, PoseLinearController::initialize(nh, nhp, robot_model, estimator, navigator, ctrl_loop_rate); rosParamInit(); - spider_robot_model_ = boost::dynamic_pointer_cast<::Spider::GroundRobotModel>(robot_model); - spider_walk_navigator_ = boost::dynamic_pointer_cast(navigator); - spider_walk_navigator_->setController(this); + spidar_robot_model_ = boost::dynamic_pointer_cast<::Spider::GroundRobotModel>(robot_model); + spidar_walk_navigator_ = boost::dynamic_pointer_cast(navigator); + spidar_walk_navigator_->setController(this); /* initialize the gimbal target angles */ target_base_thrust_.resize(motor_num_); @@ -88,8 +88,8 @@ void WalkController::initialize(ros::NodeHandle nh, ros::NodeHandle nhp, joint_pitch_torque_srv_ = nh_.advertiseService("joint_pitch/torque_enable", boost::bind(&WalkController::servoTorqueCtrlCallback, this, _1, _2, "pitch")); joint_force_compliance_sub_ = nh_.subscribe("joint_force_compliance", 1, &WalkController::jointSoftComplianceCallback, this); - target_joint_angles_.name = spider_robot_model_->getLinkJointNames(); - int joint_num = spider_robot_model_->getLinkJointNames().size(); + target_joint_angles_.name = spidar_robot_model_->getLinkJointNames(); + int joint_num = spidar_robot_model_->getLinkJointNames().size(); target_joint_angles_.position.assign(joint_num, 0); prior_raise_leg_target_joint_angles_ = target_joint_angles_; @@ -208,7 +208,7 @@ bool WalkController::update() ControlBase::update(); // skip before the model initialization - if (spider_robot_model_->getMass() == 0) { + if (spidar_robot_model_->getMass() == 0) { return false; } @@ -249,9 +249,9 @@ void WalkController::thrustControl() tf::Vector3 baselink_vel = estimator_->getVel(Frame::BASELINK, estimate_mode_); tf::Vector3 baselink_rpy = estimator_->getEuler(Frame::BASELINK, estimate_mode_); - tf::Vector3 baselink_target_pos = spider_walk_navigator_->getTargetBaselinkPos(); - tf::Vector3 baselink_target_vel = spider_walk_navigator_->getTargetBaselinkVel(); - tf::Vector3 baselink_target_rpy = spider_walk_navigator_->getTargetBaselinkRpy(); + tf::Vector3 baselink_target_pos = spidar_walk_navigator_->getTargetBaselinkPos(); + tf::Vector3 baselink_target_vel = spidar_walk_navigator_->getTargetBaselinkVel(); + tf::Vector3 baselink_target_rpy = spidar_walk_navigator_->getTargetBaselinkRpy(); // 1. feed-forwared control: compensate the static balance target_vectoring_f_ = static_thrust_force_; @@ -298,13 +298,13 @@ void WalkController::thrustControl() // constant modification in raise different case // a) raise phase - if (spider_walk_navigator_->getRaiseLegFlag() && !raise_transition_) { + if (spidar_walk_navigator_->getRaiseLegFlag() && !raise_transition_) { - int leg_id = spider_walk_navigator_->getFreeleg(); + int leg_id = spidar_walk_navigator_->getFreeleg(); int j = 4 * leg_id + 1; double angle = getCurrentJointAngles().at(j); - double target_angle = spider_walk_navigator_->getTargetJointState().position.at(j); - bool raise_converge = spider_walk_navigator_->isRaiseLegConverge(); + double target_angle = spidar_walk_navigator_->getTargetJointState().position.at(j); + bool raise_converge = spidar_walk_navigator_->isRaiseLegConverge(); double t = ros::Time::now().toSec(); if (t - prev_t_ > check_interval_) { @@ -326,9 +326,9 @@ void WalkController::thrustControl() } // b) lower phase - if (spider_walk_navigator_->getLowerLegFlag()) { + if (spidar_walk_navigator_->getLowerLegFlag()) { - int leg_id = spider_walk_navigator_->getFreeleg(); + int leg_id = spidar_walk_navigator_->getFreeleg(); int j = 4 * leg_id + 1; double angle = getCurrentJointAngles().at(j); double target_angle = target_joint_angles_.position.at(j); @@ -354,12 +354,12 @@ void WalkController::thrustControl() } // c) burst thrust force for better raising - if (spider_walk_navigator_->getRaiseLegFlag() && - !spider_walk_navigator_->isRaiseLegConverge()) { - int leg_id = spider_walk_navigator_->getFreeleg(); + if (spidar_walk_navigator_->getRaiseLegFlag() && + !spidar_walk_navigator_->isRaiseLegConverge()) { + int leg_id = spidar_walk_navigator_->getFreeleg(); int j = 4 * leg_id + 1; double angle = getCurrentJointAngles().at(j); - double target_angle = spider_walk_navigator_->getTargetJointState().position.at(j); + double target_angle = spidar_walk_navigator_->getTargetJointState().position.at(j); double err = angle - target_angle - raise_leg_burst_bias_; err = std::max(err, 0.0); // err should be non-nengative @@ -440,8 +440,8 @@ void WalkController::thrustControl() // allocation // use all vecotoring angles // consider cog and baselink are all level - const auto rotors_origin = spider_robot_model_->getRotorsOriginFromCog(); - const auto links_rot = spider_robot_model_->getLinksRotationFromCog(); + const auto rotors_origin = spidar_robot_model_->getRotorsOriginFromCog(); + const auto links_rot = spidar_robot_model_->getLinksRotationFromCog(); // Note: only consider the inside links Eigen::MatrixXd q_mat = Eigen::MatrixXd::Zero(6, 3 * motor_num_ / 2); @@ -463,9 +463,9 @@ void WalkController::thrustControl() // 3. feed-back control: link-wise rotation control Eigen::VectorXd fb_vectoring_l = Eigen::VectorXd::Zero(3 * motor_num_); - auto target_link_rots = spider_walk_navigator_->getTargetLinkRots(); - const auto& seg_tf_map = spider_robot_model_->getSegmentsTf(); - KDL::Frame fr_baselink = seg_tf_map.at(spider_robot_model_->getBaselinkName()); + auto target_link_rots = spidar_walk_navigator_->getTargetLinkRots(); + const auto& seg_tf_map = spidar_robot_model_->getSegmentsTf(); + KDL::Frame fr_baselink = seg_tf_map.at(spidar_robot_model_->getBaselinkName()); tf::Transform tf_w_baselink(estimator_->getOrientation(Frame::BASELINK, estimate_mode_), baselink_pos); KDL::Frame fw_baselink; tf::transformTFToKDL(tf_w_baselink, fw_baselink); @@ -487,7 +487,7 @@ void WalkController::thrustControl() // negative means the pivot point is the leg end point int direct = -1; - if (i / 2 == spider_walk_navigator_->getFreeleg()) { + if (i / 2 == spidar_walk_navigator_->getFreeleg()) { // no contorl for free leg which may induce unstability continue; } @@ -568,7 +568,7 @@ void WalkController::jointControl() } // basically, use position control for all joints - auto navi_target_joint_angles = spider_walk_navigator_->getTargetJointState().position; + auto navi_target_joint_angles = spidar_walk_navigator_->getTargetJointState().position; target_joint_angles_.position = navi_target_joint_angles; if (prev_navi_target_joint_angles_.size() == 0) { prev_navi_target_joint_angles_ = navi_target_joint_angles; @@ -601,9 +601,9 @@ void WalkController::jointControl() // modification for target joint angle - bool raise_flag = spider_walk_navigator_->getRaiseLegFlag(); - bool raise_converge = spider_walk_navigator_->isRaiseLegConverge(); - int free_leg_id = spider_walk_navigator_->getFreeleg(); + bool raise_flag = spidar_walk_navigator_->getRaiseLegFlag(); + bool raise_converge = spidar_walk_navigator_->isRaiseLegConverge(); + int free_leg_id = spidar_walk_navigator_->getFreeleg(); for(int i = 0; i < joint_num; i++) { double tor = static_joint_torque_(i); @@ -741,11 +741,11 @@ void WalkController::jointControl() } // heuristic rule for joints in free leg - if (leg_id == spider_walk_navigator_->getFreeleg()) { + if (leg_id == spidar_walk_navigator_->getFreeleg()) { prev_navi_target_joint_angles_.at(i) = navi_target_joint_angles.at(i); - bool lower_flag = spider_walk_navigator_->getLowerLegFlag(); - bool raise_flag = spider_walk_navigator_->getRaiseLegFlag(); + bool lower_flag = spidar_walk_navigator_->getLowerLegFlag(); + bool raise_flag = spidar_walk_navigator_->getRaiseLegFlag(); std::string status = raise_flag?std::string("raise"):std::string("lower"); if (j % 2 == 0) { @@ -783,9 +783,9 @@ void WalkController::jointControl() } // heuristic rule for joints in rest of the legs when there is a free leg - // if ((leg_id + leg_num / 2) % leg_num == spider_walk_navigator_->getFreeleg()) { - if (spider_walk_navigator_->getFreeleg() != -1 && - leg_id != spider_walk_navigator_->getFreeleg()) { + // if ((leg_id + leg_num / 2) % leg_num == spidar_walk_navigator_->getFreeleg()) { + if (spidar_walk_navigator_->getFreeleg() != -1 && + leg_id != spidar_walk_navigator_->getFreeleg()) { if (j % 2 == 0) { // inner joint (e.g., joint5_pitch) @@ -905,11 +905,11 @@ void WalkController::jointControl() void WalkController::calcStaticBalance() { using orig = aerial_robot_model::RobotModel; - const KDL::JntArray gimbal_processed_joint = spider_robot_model_->getGimbalProcessedJoint(); - const std::vector links_rotation_from_cog = spider_robot_model_->getLinksRotationFromCog(); - const int joint_num = spider_robot_model_->getJointNum(); - const int link_joint_num = spider_robot_model_->getLinkJointIndices().size(); - const int rotor_num = spider_robot_model_->getRotorNum(); + const KDL::JntArray gimbal_processed_joint = spidar_robot_model_->getGimbalProcessedJoint(); + const std::vector links_rotation_from_cog = spidar_robot_model_->getLinksRotationFromCog(); + const int joint_num = spidar_robot_model_->getJointNum(); + const int link_joint_num = spidar_robot_model_->getLinkJointIndices().size(); + const int rotor_num = spidar_robot_model_->getRotorNum(); const int fr_ndof = 3 * rotor_num; Eigen::MatrixXd A1_fr_all = Eigen::MatrixXd::Zero(joint_num, fr_ndof); @@ -918,7 +918,7 @@ void WalkController::calcStaticBalance() for (int i = 0; i < rotor_num; i++) { std::string seg_name = std::string("thrust") + std::to_string(i + 1); Eigen::MatrixXd jac - = spider_robot_model_->orig::getJacobian(gimbal_processed_joint, seg_name).topRows(3); + = spidar_robot_model_->orig::getJacobian(gimbal_processed_joint, seg_name).topRows(3); Eigen::MatrixXd r = aerial_robot_model::kdlToEigen(links_rotation_from_cog.at(i)); // describe force w.r.t. local (link) frame A1_fr_all.middleCols(3 * i, 3) = -jac.rightCols(joint_num).transpose() * r; @@ -926,7 +926,7 @@ void WalkController::calcStaticBalance() } const int leg_num = rotor_num / 2; - int free_leg_id = spider_walk_navigator_->getFreeleg(); + int free_leg_id = spidar_walk_navigator_->getFreeleg(); if (free_leg_id >= leg_num) { ROS_WARN_ONCE("[Spider][Walk][Static] the untouch leg ID exceeds the total leg number, force reset to -1"); @@ -962,7 +962,7 @@ void WalkController::calcStaticBalance() Eigen::VectorXd b1_all = Eigen::VectorXd::Zero(joint_num); Eigen::VectorXd b2 = Eigen::VectorXd::Zero(6); - for(const auto& inertia : spider_robot_model_->getInertiaMap()) { + for(const auto& inertia : spidar_robot_model_->getInertiaMap()) { // describe jacobian w.r.t the world frame, thus need baselink rotation Eigen::MatrixXd jac @@ -970,7 +970,7 @@ void WalkController::calcStaticBalance() inertia.first, inertia.second.getCOG())).topRows(3); - Eigen::VectorXd g = inertia.second.getMass() * (-spider_robot_model_->getGravity3d()); + Eigen::VectorXd g = inertia.second.getMass() * (-spidar_robot_model_->getGravity3d()); b1_all -= jac.rightCols(joint_num).transpose() * g; b2 += jac.leftCols(6).transpose() * g; } @@ -982,7 +982,7 @@ void WalkController::calcStaticBalance() cnt = 0; for(int i = 0; i < joint_num; i++) { - if(spider_robot_model_->getJointNames().at(i) == spider_robot_model_->getLinkJointNames().at(cnt)) + if(spidar_robot_model_->getJointNames().at(i) == spidar_robot_model_->getLinkJointNames().at(cnt)) { A1_fe.row(cnt) = A1_fe_all.row(i); A1_fr.row(cnt) = A1_fr_all.row(i); @@ -1166,7 +1166,7 @@ void WalkController::startRaiseTransition() raise_static_thrust_force_ = static_thrust_force_; // record the thrust force for raise prev_t_ = ros::Time::now().toSec(); - int free_leg_id = spider_walk_navigator_->getFreeleg(); + int free_leg_id = spidar_walk_navigator_->getFreeleg(); int i = free_leg_id * 4 + 1; prior_raise_leg_target_joint_angles_ = target_joint_angles_; @@ -1178,7 +1178,7 @@ void WalkController::startLowerLeg() prev_t_ = ros::Time::now().toSec(); auto current_angles = getCurrentJointAngles(); - int leg_id = spider_walk_navigator_->getFreeleg(); + int leg_id = spidar_walk_navigator_->getFreeleg(); if (leg_id < 0) { ROS_ERROR_STREAM("[Spider][Walk][Thrust Control] no valid leg to lower, leg id is " << leg_id); } @@ -1206,7 +1206,7 @@ bool WalkController::servoTorqueCtrlCallback(std_srvs::SetBool::Request &req, st if (name == std::string("yaw")) offset = 0; if (name == std::string("pitch")) offset = 1; - int rotor_num = spider_robot_model_->getRotorNum(); + int rotor_num = spidar_robot_model_->getRotorNum(); spinal::ServoTorqueCmd servo_enable_msg; for (int i = 0; i < rotor_num; i++) { servo_enable_msg.index.push_back(4 * i + offset); @@ -1260,7 +1260,7 @@ void WalkController::cfgPidCallback(aerial_robot_control::PidControlConfig &conf // utils void WalkController::setJointIndexMap() { - const auto current_joint_state = spider_robot_model_->getGimbalProcessedJoint(); + const auto current_joint_state = spidar_robot_model_->getGimbalProcessedJoint(); const auto& search_v = current_joint_state.name; joint_index_map_.resize(0); // resize @@ -1282,7 +1282,7 @@ void WalkController::setJointIndexMap() std::vector WalkController::getCurrentJointAngles() { - const auto current_joint_state = spider_robot_model_->getGimbalProcessedJoint(); + const auto current_joint_state = spidar_robot_model_->getGimbalProcessedJoint(); std::vector angles(0); for(const auto id: joint_index_map_) { angles.push_back(current_joint_state.position.at(id)); diff --git a/robots/spider/src/model/full_vectoring_robot_model.cpp b/robots/spidar/src/model/full_vectoring_robot_model.cpp similarity index 98% rename from robots/spider/src/model/full_vectoring_robot_model.cpp rename to robots/spidar/src/model/full_vectoring_robot_model.cpp index 8da61bc7f..6ae914d55 100644 --- a/robots/spider/src/model/full_vectoring_robot_model.cpp +++ b/robots/spidar/src/model/full_vectoring_robot_model.cpp @@ -33,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include using namespace Spider; diff --git a/robots/spider/src/model/ground_robot_model.cpp b/robots/spidar/src/model/ground_robot_model.cpp similarity index 99% rename from robots/spider/src/model/ground_robot_model.cpp rename to robots/spidar/src/model/ground_robot_model.cpp index 1ee9e3faa..ddcd3127b 100644 --- a/robots/spider/src/model/ground_robot_model.cpp +++ b/robots/spidar/src/model/ground_robot_model.cpp @@ -33,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include using namespace Spider; diff --git a/robots/spider/src/model/hydrus_like_robot_model.cpp b/robots/spidar/src/model/hydrus_like_robot_model.cpp similarity index 94% rename from robots/spider/src/model/hydrus_like_robot_model.cpp rename to robots/spidar/src/model/hydrus_like_robot_model.cpp index a6d3b0fea..356cf81ca 100644 --- a/robots/spider/src/model/hydrus_like_robot_model.cpp +++ b/robots/spidar/src/model/hydrus_like_robot_model.cpp @@ -1,4 +1,4 @@ -#include +#include using namespace Spider; diff --git a/robots/spider/src/navigation/flight_navigation.cpp b/robots/spidar/src/navigation/flight_navigation.cpp similarity index 98% rename from robots/spider/src/navigation/flight_navigation.cpp rename to robots/spidar/src/navigation/flight_navigation.cpp index 5d0b89b16..534ef8364 100644 --- a/robots/spider/src/navigation/flight_navigation.cpp +++ b/robots/spidar/src/navigation/flight_navigation.cpp @@ -1,4 +1,4 @@ -#include +#include using namespace aerial_robot_navigation; using namespace aerial_robot_navigation::Spider; diff --git a/robots/spider/src/navigation/walk_navigation.cpp b/robots/spidar/src/navigation/walk_navigation.cpp similarity index 97% rename from robots/spider/src/navigation/walk_navigation.cpp rename to robots/spidar/src/navigation/walk_navigation.cpp index 6dc4f12ed..6628478a0 100644 --- a/robots/spider/src/navigation/walk_navigation.cpp +++ b/robots/spidar/src/navigation/walk_navigation.cpp @@ -1,5 +1,5 @@ -#include -#include +#include +#include using namespace aerial_robot_model; using namespace aerial_robot_navigation::Spider; @@ -31,7 +31,7 @@ void WalkNavigator::initialize(ros::NodeHandle nh, ros::NodeHandle nhp, /* initialize the flight control */ BaseNavigator::initialize(nh, nhp, robot_model, estimator); - spider_robot_model_ = boost::dynamic_pointer_cast<::Spider::GroundRobotModel>(robot_model); + spidar_robot_model_ = boost::dynamic_pointer_cast<::Spider::GroundRobotModel>(robot_model); robot_model_for_nav_ = boost::make_shared(); @@ -63,7 +63,7 @@ void WalkNavigator::initialize(ros::NodeHandle nh, ros::NodeHandle nhp, } sensor_msgs::JointState joint_msg = simulated_joint_state_; joint_msg.header.stamp = ros::Time::now(); - for(int i = 0; i < spider_robot_model_->getRotorNum(); i++) { + for(int i = 0; i < spidar_robot_model_->getRotorNum(); i++) { joint_msg.name.push_back(std::string("gimbal") + std::to_string(i+1) + std::string("_roll")); joint_msg.name.push_back(std::string("gimbal") + std::to_string(i+1) + std::string("_pitch")); joint_msg.position.push_back(0); @@ -269,7 +269,7 @@ void WalkNavigator::walkPattern() // instant update to the target joint angle if (walk_simulation_) { - spider_robot_model_->updateRobotModel(target_joint_state_); + spidar_robot_model_->updateRobotModel(target_joint_state_); for(int i = 0; i < simulated_joint_state_.position.size(); i++) { std::string name = simulated_joint_state_.name.at(i); @@ -334,7 +334,7 @@ void WalkNavigator::update() BaseNavigator::update(); // skip before the model initialization - if (spider_robot_model_->getMass() == 0) { + if (spidar_robot_model_->getMass() == 0) { return; } @@ -351,7 +351,7 @@ void WalkNavigator::update() if (target_joint_state_.name.size() == 0) { // initialize the target joint - target_joint_state_.name = spider_robot_model_->getLinkJointNames(); + target_joint_state_.name = spidar_robot_model_->getLinkJointNames(); target_joint_state_.position.resize(target_joint_state_.name.size()); // set (initialize) joint index map @@ -365,19 +365,19 @@ void WalkNavigator::update() // target link rotation if (target_link_rots_.size() == 0) { - target_link_rots_.resize(spider_robot_model_->getRotorNum()); + target_link_rots_.resize(spidar_robot_model_->getRotorNum()); } // get current leg position w.r.t. world frame - const auto& seg_tf_map = spider_robot_model_->getSegmentsTf(); + const auto& seg_tf_map = spidar_robot_model_->getSegmentsTf(); if(seg_tf_map.size() == 0) return; tf::Transform tf_w_baselink(estimator_->getOrientation(Frame::BASELINK, estimate_mode_), baselink_pos); KDL::Frame fw_baselink; tf::transformTFToKDL(tf_w_baselink, fw_baselink); std::vector curr_leg_ends; - const int leg_num = spider_robot_model_->getRotorNum() / 2; - KDL::Frame fr_baselink = seg_tf_map.at(spider_robot_model_->getBaselinkName()); + const int leg_num = spidar_robot_model_->getRotorNum() / 2; + KDL::Frame fr_baselink = seg_tf_map.at(spidar_robot_model_->getBaselinkName()); for (int i = 0; i < leg_num; i++) { std::string frame_name = std::string("link") + std::to_string(2 * (i + 1)) + std::string("_foot"); KDL::Frame fr_end = seg_tf_map.at(frame_name); // w.r.t. root link @@ -576,7 +576,7 @@ void WalkNavigator::update() ROS_DEBUG_STREAM_THROTTLE(1.0, "[Spider][Walk][Navigator] analytical joint angles from leg end and baselink: " << ss.str()); // get the target link orientation - auto joint_state = spider_robot_model_->getGimbalProcessedJoint(); + auto joint_state = spidar_robot_model_->getGimbalProcessedJoint(); for(int i = 0; i < joint_index_map_.size(); i++) { auto id = joint_index_map_.at(i); joint_state.position.at(id) = target_angles.at(i); @@ -584,7 +584,7 @@ void WalkNavigator::update() robot_model_for_nav_->updateRobotModel(joint_state); const auto& target_seg_tf_map = robot_model_for_nav_->getSegmentsTf(); - for(int i = 0; i < spider_robot_model_->getRotorNum(); i++) { + for(int i = 0; i < spidar_robot_model_->getRotorNum(); i++) { std::string link_name = std::string("link") + std::to_string(i+1); KDL::Frame fr_target_link = target_seg_tf_map.at(link_name); KDL::Frame fb_target_link = fr_baselink.Inverse() * fr_target_link; @@ -656,7 +656,7 @@ void WalkNavigator::simulate() } sensor_msgs::JointState joint_msg = simulated_joint_state_; joint_msg.header.stamp = ros::Time::now(); - for(int i = 0; i < spider_robot_model_->getRotorNum(); i++) { + for(int i = 0; i < spidar_robot_model_->getRotorNum(); i++) { joint_msg.name.push_back(std::string("gimbal") + std::to_string(i+1) + std::string("_roll")); joint_msg.name.push_back(std::string("gimbal") + std::to_string(i+1) + std::string("_pitch")); joint_msg.position.push_back(0); @@ -684,7 +684,7 @@ void WalkNavigator::failSafeAction() // pitch joint of opposite of raise leg if (raise_leg_flag_) { - int leg_num = spider_robot_model_->getRotorNum() / 2; + int leg_num = spidar_robot_model_->getRotorNum() / 2; int leg_id = (free_leg_id_ + leg_num / 2) % leg_num; int j = 4 * leg_id + 1; double target_angle = target_joint_state_.position.at(j); @@ -714,7 +714,7 @@ void WalkNavigator::resetTargetLegEnds(std::vector frames) void WalkNavigator::setJointIndexMap() { - const auto joint_state = spider_robot_model_->getGimbalProcessedJoint(); + const auto joint_state = spidar_robot_model_->getGimbalProcessedJoint(); const auto& search_v = joint_state.name; joint_index_map_.resize(0); // resize @@ -736,7 +736,7 @@ void WalkNavigator::setJointIndexMap() std::vector WalkNavigator::getCurrentJointAngles() { - const auto joint_state = spider_robot_model_->getGimbalProcessedJoint(); + const auto joint_state = spidar_robot_model_->getGimbalProcessedJoint(); std::vector angles(0); for(const auto id: joint_index_map_) { angles.push_back(joint_state.position.at(id)); @@ -774,7 +774,7 @@ void WalkNavigator::raiseLeg(int leg_id) raise_converge_ = false; lower_leg_flag_ = false; - spider_robot_model_->setFreeleg(free_leg_id_); + spidar_robot_model_->setFreeleg(free_leg_id_); walk_controller_->startRaiseTransition(); } @@ -791,7 +791,7 @@ void WalkNavigator::contactLeg() lower_leg_flag_ = false; raise_leg_flag_ = false; raise_converge_ = false; - spider_robot_model_->resetFreeleg(); + spidar_robot_model_->resetFreeleg(); walk_controller_->startContactTransition(free_leg_id_); free_leg_id_ = -1; } diff --git a/robots/spider/test/spider_control.test b/robots/spidar/test/spider_control.test similarity index 90% rename from robots/spider/test/spider_control.test rename to robots/spidar/test/spider_control.test index 3103a963f..c19bb4183 100644 --- a/robots/spider/test/spider_control.test +++ b/robots/spidar/test/spider_control.test @@ -1,10 +1,10 @@ - + - + @@ -30,7 +30,7 @@ init_joint_names: ['joint1_pitch', 'joint1_yaw', 'joint2_pitch', 'joint2_yaw', 'joint3_pitch', 'joint3_yaw', 'joint4_pitch', 'joint4_yaw', 'joint5_pitch', 'joint5_yaw', 'joint6_pitch', 'joint6_yaw', 'joint7_pitch', 'joint7_yaw', 'joint8_pitch', 'joint8_yaw'] init_joint_angles: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] tasks: - - command: "rosrun spider spider_octo_transformation.py -t 3000 -q 0.8 -0.8 0 0.8" + - command: "rosrun spidar spidar_octo_transformation.py -t 3000 -q 0.8 -0.8 0 0.8" threshold: [0.1, 0.08, 0.08] angle_threshold: 0.2 timeout: 20 diff --git a/robots/spider/urdf/v1/common.xacro b/robots/spidar/urdf/v1/common.xacro similarity index 96% rename from robots/spider/urdf/v1/common.xacro rename to robots/spidar/urdf/v1/common.xacro index fc2f3f89c..e42fa8536 100644 --- a/robots/spider/urdf/v1/common.xacro +++ b/robots/spidar/urdf/v1/common.xacro @@ -1,6 +1,6 @@ + xmlns:xacro="http://www.ros.org/wiki/xacro" name="spidar_common" > @@ -24,7 +24,7 @@ - + diff --git a/robots/spider/urdf/v1/link.urdf.xacro b/robots/spidar/urdf/v1/link.urdf.xacro similarity index 94% rename from robots/spider/urdf/v1/link.urdf.xacro rename to robots/spidar/urdf/v1/link.urdf.xacro index 8b1776910..973077175 100644 --- a/robots/spider/urdf/v1/link.urdf.xacro +++ b/robots/spidar/urdf/v1/link.urdf.xacro @@ -1,9 +1,9 @@ - + xmlns:xacro="http://www.ros.org/wiki/xacro" name="spidar_link"> + - + @@ -57,7 +57,7 @@ - + @@ -73,7 +73,7 @@ #### CENTER LINK #### - + #### dummy root link for KDL #### @@ -114,10 +114,10 @@ - - - - + + + + #### joints #### @@ -164,7 +164,7 @@ #### LINK #### - + #### link #### @@ -277,7 +277,7 @@ #### joint junction #### - + #### joints #### @@ -324,7 +324,7 @@ #### battery #### + model_url="package://spidar/urdf/v1/mesh/battery/ZIPPY-3000-25C-6S.dae"> @@ -345,7 +345,7 @@ + model_url="package://spidar/urdf/v1/mesh/leg/end_leg.dae"> diff --git a/robots/spider/urdf/v1/mesh/battery/ZIPPY-3000-25C-6S.dae b/robots/spidar/urdf/v1/mesh/battery/ZIPPY-3000-25C-6S.dae similarity index 100% rename from robots/spider/urdf/v1/mesh/battery/ZIPPY-3000-25C-6S.dae rename to robots/spidar/urdf/v1/mesh/battery/ZIPPY-3000-25C-6S.dae diff --git a/robots/spider/urdf/v1/mesh/board/spinal.dae b/robots/spidar/urdf/v1/mesh/board/spinal.dae similarity index 100% rename from robots/spider/urdf/v1/mesh/board/spinal.dae rename to robots/spidar/urdf/v1/mesh/board/spinal.dae diff --git a/robots/spider/urdf/v1/mesh/leg/end_leg.dae b/robots/spidar/urdf/v1/mesh/leg/end_leg.dae similarity index 100% rename from robots/spider/urdf/v1/mesh/leg/end_leg.dae rename to robots/spidar/urdf/v1/mesh/leg/end_leg.dae diff --git a/robots/spider/urdf/v1/mesh/leg/leg.dae b/robots/spidar/urdf/v1/mesh/leg/leg.dae similarity index 100% rename from robots/spider/urdf/v1/mesh/leg/leg.dae rename to robots/spidar/urdf/v1/mesh/leg/leg.dae diff --git a/robots/spider/urdf/v1/mesh/leg/leg_cap.dae b/robots/spidar/urdf/v1/mesh/leg/leg_cap.dae similarity index 100% rename from robots/spider/urdf/v1/mesh/leg/leg_cap.dae rename to robots/spidar/urdf/v1/mesh/leg/leg_cap.dae diff --git a/robots/spider/urdf/v1/mesh/leg/long_leg.dae b/robots/spidar/urdf/v1/mesh/leg/long_leg.dae similarity index 100% rename from robots/spider/urdf/v1/mesh/leg/long_leg.dae rename to robots/spidar/urdf/v1/mesh/leg/long_leg.dae diff --git a/robots/spider/urdf/v1/mesh/link/center_link.dae b/robots/spidar/urdf/v1/mesh/link/center_link.dae similarity index 100% rename from robots/spider/urdf/v1/mesh/link/center_link.dae rename to robots/spidar/urdf/v1/mesh/link/center_link.dae diff --git a/robots/spider/urdf/v1/mesh/link/end_link.dae b/robots/spidar/urdf/v1/mesh/link/end_link.dae similarity index 100% rename from robots/spider/urdf/v1/mesh/link/end_link.dae rename to robots/spidar/urdf/v1/mesh/link/end_link.dae diff --git a/robots/spider/urdf/v1/mesh/link/general_link.dae b/robots/spidar/urdf/v1/mesh/link/general_link.dae similarity index 100% rename from robots/spider/urdf/v1/mesh/link/general_link.dae rename to robots/spidar/urdf/v1/mesh/link/general_link.dae diff --git a/robots/spider/urdf/v1/mesh/link/joint_junction_link.dae b/robots/spidar/urdf/v1/mesh/link/joint_junction_link.dae similarity index 100% rename from robots/spider/urdf/v1/mesh/link/joint_junction_link.dae rename to robots/spidar/urdf/v1/mesh/link/joint_junction_link.dae diff --git a/robots/spider/urdf/v1/mesh/link/joint_junction_link_large_pitch_pulley.dae b/robots/spidar/urdf/v1/mesh/link/joint_junction_link_large_pitch_pulley.dae similarity index 100% rename from robots/spider/urdf/v1/mesh/link/joint_junction_link_large_pitch_pulley.dae rename to robots/spidar/urdf/v1/mesh/link/joint_junction_link_large_pitch_pulley.dae diff --git a/robots/spider/urdf/v1/mesh/link/pitch_link.dae b/robots/spidar/urdf/v1/mesh/link/pitch_link.dae similarity index 100% rename from robots/spider/urdf/v1/mesh/link/pitch_link.dae rename to robots/spidar/urdf/v1/mesh/link/pitch_link.dae diff --git a/robots/spider/urdf/v1/mesh/link/roll_link.dae b/robots/spidar/urdf/v1/mesh/link/roll_link.dae similarity index 100% rename from robots/spider/urdf/v1/mesh/link/roll_link.dae rename to robots/spidar/urdf/v1/mesh/link/roll_link.dae diff --git a/robots/spider/bin/rosbag_play_spider.sh b/robots/spider/bin/rosbag_play_spider.sh deleted file mode 100755 index dd41c7f43..000000000 --- a/robots/spider/bin/rosbag_play_spider.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -rosbag play $1 --pause --clock /spider/joints_ctrl:=/spider/joints_ctrl_orig /spider/joints_torque_ctrl:=/spider/joints_torque_ctrl_orig /spider/debug/pose/pid:=/spider/debug/pose/pid_orig /spider/four_axes/command:=/spider/four_axes/command_orig /spider/debug/target_vectoring_force:=/spider/debug/target_vectoring_force_orig /spider/servo/target_states:=/spider/servo/target_states_orig ${@:2} diff --git a/robots/spider/plugins/flight_control_plugins.xml b/robots/spider/plugins/flight_control_plugins.xml deleted file mode 100644 index c35a70a82..000000000 --- a/robots/spider/plugins/flight_control_plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - spider control plugin: walk control - - - diff --git a/robots/spider/plugins/flight_navigation_plugin.xml b/robots/spider/plugins/flight_navigation_plugin.xml deleted file mode 100644 index ff2cc17a5..000000000 --- a/robots/spider/plugins/flight_navigation_plugin.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - spider walk navigation plugin - - - - spider flight navigation plugin - - diff --git a/robots/spider/plugins/robot_model_plugins.xml b/robots/spider/plugins/robot_model_plugins.xml deleted file mode 100644 index 664c84767..000000000 --- a/robots/spider/plugins/robot_model_plugins.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - - plugin for hydrus-like spider robot model - - - - plugin for full vectoring spider robot model - - - - plugin for fround spider robot model - - - diff --git a/robots/spider/robots/octo/spider_v1_base.urdf.xacro b/robots/spider/robots/octo/spider_v1_base.urdf.xacro deleted file mode 100644 index 96710d9da..000000000 --- a/robots/spider/robots/octo/spider_v1_base.urdf.xacro +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - -