diff --git a/.ci.rosinstall b/.ci.rosinstall index 8c1d5ae..fe8d477 100644 --- a/.ci.rosinstall +++ b/.ci.rosinstall @@ -1,4 +1,4 @@ - git: uri: https://github.com/rt-net/sciurus17_description.git local-name: sciurus17_description - version: main \ No newline at end of file + version: ros2 \ No newline at end of file diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 3ecf7af..e6ef27d 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -9,6 +9,9 @@ on: - '**.md' workflow_dispatch: +env: + UPSTREAM_WORKSPACE: .ci.rosinstall + jobs: industrial_ci: strategy: diff --git a/sciurus17_control/CMakeLists.txt b/sciurus17_control/CMakeLists.txt index 3f9fab0..d7a5f0c 100644 --- a/sciurus17_control/CMakeLists.txt +++ b/sciurus17_control/CMakeLists.txt @@ -1,53 +1,11 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(sciurus17_control) -add_compile_options(-std=c++11) +find_package(ament_cmake REQUIRED) -find_package(catkin REQUIRED COMPONENTS - controller_manager - dynamixel_sdk - roscpp - rospy - hardware_interface - transmission_interface - sciurus17_msgs - dynamic_reconfigure +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}/ ) -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS roscpp hardware_interface transmission_interface controller_manager dynamixel_sdk -) - -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIR}) - -find_library( dxl_x64_cpp NAME libdxl_x64_cpp.so PATHS src/dynamixel_sdk/build/linux64 ) - -add_executable(sciurus17_control src/hardware.cpp src/joint_control.cpp src/dxlport_control.cpp src/control_setting.cpp) - -target_link_libraries( sciurus17_control - ${catkin_LIBRARIES} -) -set(MESSAGE_DEPENDS sciurus17_msgs_generate_messages) - -install(TARGETS sciurus17_control - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE -) - -install(DIRECTORY config launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -file(GLOB python_scripts scripts/*.py) -catkin_install_python( - PROGRAMS ${python_scripts} - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +ament_package() diff --git a/sciurus17_control/COLCON_IGNORE b/sciurus17_control/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/sciurus17_control/config/sciurus17_control1.yaml b/sciurus17_control/config/sciurus17_control1.yaml deleted file mode 100644 index 94e29b8..0000000 --- a/sciurus17_control/config/sciurus17_control1.yaml +++ /dev/null @@ -1,143 +0,0 @@ -# Publish all joint states ----------------------------------- - joint_state_controller: - type: "joint_state_controller/JointStateController" - publish_rate: 500 - - right_arm_controller: - type: "position_controllers/JointTrajectoryController" - publish_rate: 500 - joints: - - r_arm_joint1 - - r_arm_joint2 - - r_arm_joint3 - - r_arm_joint4 - - r_arm_joint5 - - r_arm_joint6 - - r_arm_joint7 - constraints: - goal_time: 0.0 - stopped_velocity_tolerance: 1.0 - - # for current control - gains: - r_arm_joint1: { p: 5.0, d: 0.1, i: 0.0 } - r_arm_joint2: { p: 5.0, d: 0.1, i: 0.0 } - r_arm_joint3: { p: 5.0, d: 0.1, i: 0.0 } - r_arm_joint4: { p: 5.0, d: 0.1, i: 0.0 } - r_arm_joint5: { p: 1.0, d: 0.1, i: 0.0 } - r_arm_joint6: { p: 1.0, d: 0.1, i: 0.0 } - r_arm_joint7: { p: 1.0, d: 0.1, i: 0.0 } - - - right_hand_controller: - type: "position_controllers/GripperActionController" - publish_rate: 500 - joint: r_hand_joint - action_monitor_rate: 10 - state_publish_rate: 100 - stall_velocity_threshold: 0.001 - goal_tolerance: 0.05 - stall_timeout: 0.1 - - dynamixel_port: - port_name: "/dev/sciurus17spine" - baud_rate: 3000000 - joints: - - r_arm_joint1 - - r_arm_joint2 - - r_arm_joint3 - - r_arm_joint4 - - r_arm_joint5 - - r_arm_joint6 - - r_arm_joint7 - - r_hand_joint - r_arm_joint1: {id: 2, center: 2048, home: 2048, effort_const: 2.79, mode: 3 } - r_arm_joint2: {id: 3, center: 2048, home: 1024, effort_const: 2.79, mode: 3 } - r_arm_joint3: {id: 4, center: 2048, home: 2048, effort_const: 1.69, mode: 3 } - r_arm_joint4: {id: 5, center: 2048, home: 3825, effort_const: 1.79, mode: 3 } - r_arm_joint5: {id: 6, center: 2048, home: 2048, effort_const: 1.79, mode: 3 } - r_arm_joint6: {id: 7, center: 2048, home: 683, effort_const: 1.79, mode: 3 } - r_arm_joint7: {id: 8, center: 2048, home: 2048, effort_const: 1.79, mode: 3 } - r_hand_joint: {id: 9, center: 2048, home: 2048, effort_const: 1.79, mode: 3 } - - joint_limits: - r_arm_joint1: - has_position_limits: true - min_position: -1.5707963268 - max_position: 1.5707963268 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 - r_arm_joint2: - has_position_limits: true - min_position: -1.5707963268 - max_position: 0.0 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 - r_arm_joint3: - has_position_limits: true - min_position: -1.5707963268 - max_position: 1.5707963268 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 - r_arm_joint4: - has_position_limits: true - min_position: 0.0 - max_position: 2.726204 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 - r_arm_joint5: - has_position_limits: true - min_position: -1.5707963268 - max_position: 1.5707963268 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 - r_arm_joint6: - has_position_limits: true - min_position: -2.094395 - max_position: 1.047196 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 - r_arm_joint7: - has_position_limits: true - min_position: -2.967060 - max_position: 2.967060 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 - r_hand_joint: - has_position_limits: true - min_position: 0.0 - max_position: 0.523598776 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 diff --git a/sciurus17_control/config/sciurus17_control2.yaml b/sciurus17_control/config/sciurus17_control2.yaml deleted file mode 100644 index 7f1e532..0000000 --- a/sciurus17_control/config/sciurus17_control2.yaml +++ /dev/null @@ -1,132 +0,0 @@ -# Publish all joint states ----------------------------------- - joint_state_controller: - type: "joint_state_controller/JointStateController" - publish_rate: 500 - - left_arm_controller: - type: "position_controllers/JointTrajectoryController" - publish_rate: 500 - joints: - - l_arm_joint1 - - l_arm_joint2 - - l_arm_joint3 - - l_arm_joint4 - - l_arm_joint5 - - l_arm_joint6 - - l_arm_joint7 - constraints: - goal_time: 0.0 - stopped_velocity_tolerance: 1.0 - - left_hand_controller: - type: "position_controllers/GripperActionController" - publish_rate: 500 - joint: l_hand_joint - action_monitor_rate: 10 - state_publish_rate: 100 - stall_velocity_threshold: 0.001 - goal_tolerance: 0.05 - stall_timeout: 0.1 - - dynamixel_port: - port_name: "/dev/sciurus17spine" - baud_rate: 3000000 - joints: - - l_arm_joint1 - - l_arm_joint2 - - l_arm_joint3 - - l_arm_joint4 - - l_arm_joint5 - - l_arm_joint6 - - l_arm_joint7 - - l_hand_joint - l_arm_joint1: {id: 10, center: 2048, home: 2048, effort_const: 2.79, mode: 3 } - l_arm_joint2: {id: 11, center: 2048, home: 3072, effort_const: 2.79, mode: 3 } - l_arm_joint3: {id: 12, center: 2048, home: 2048, effort_const: 1.69, mode: 3 } - l_arm_joint4: {id: 13, center: 2048, home: 271, effort_const: 1.79, mode: 3 } - l_arm_joint5: {id: 14, center: 2048, home: 2048, effort_const: 1.79, mode: 3 } - l_arm_joint6: {id: 15, center: 2048, home: 3413, effort_const: 1.79, mode: 3 } - l_arm_joint7: {id: 16, center: 2048, home: 2048, effort_const: 1.79, mode: 3 } - l_hand_joint: {id: 17, center: 2048, home: 2048, effort_const: 1.79, mode: 3 } - - joint_limits: - l_arm_joint1: - has_position_limits: true - min_position: -1.5707963268 - max_position: 1.5707963268 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 - l_arm_joint2: - has_position_limits: true - min_position: 0.0 - max_position: 1.5707963268 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 - l_arm_joint3: - has_position_limits: true - min_position: -1.5707963268 - max_position: 1.5707963268 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 - l_arm_joint4: - has_position_limits: true - min_position: -2.726204 - max_position: 0.0 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 - l_arm_joint5: - has_position_limits: true - min_position: -1.5707963268 - max_position: 1.5707963268 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 - l_arm_joint6: - has_position_limits: true - min_position: -1.047196 - max_position: 2.094395 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 - l_arm_joint7: - has_position_limits: true - min_position: -2.967060 - max_position: 2.967060 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 - l_hand_joint: - has_position_limits: true - min_position: -0.523598776 - max_position: 0.0 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 diff --git a/sciurus17_control/config/sciurus17_control3.yaml b/sciurus17_control/config/sciurus17_control3.yaml deleted file mode 100644 index 90f9639..0000000 --- a/sciurus17_control/config/sciurus17_control3.yaml +++ /dev/null @@ -1,70 +0,0 @@ -# Publish all joint states ----------------------------------- - joint_state_controller: - type: "joint_state_controller/JointStateController" - publish_rate: 500 - - neck_controller: - type: "position_controllers/JointTrajectoryController" - publish_rate: 500 - joints: - - neck_yaw_joint - - neck_pitch_joint - constraints: - goal_time: 0.0 - stopped_velocity_tolerance: 1.0 - - waist_yaw_controller: - type: "position_controllers/JointTrajectoryController" - publish_rate: 500 - joints: - - waist_yaw_joint - constraints: - goal_time: 0.0 - stopped_velocity_tolerance: 1.0 - waist_yaw_joint: - trajectory: 0.2 - goal: 0.2 - - dynamixel_port: - port_name: "/dev/sciurus17spine" - baud_rate: 3000000 - joints: - - waist_yaw_joint - - neck_yaw_joint - - neck_pitch_joint - waist_yaw_joint: {id: 18, center: 2048, home: 2048, effort_const: 2.79, mode: 3 } - neck_yaw_joint: {id: 19, center: 2048, home: 2048, effort_const: 1.79, mode: 3 } - neck_pitch_joint: {id: 20, center: 2048, home: 2048, effort_const: 1.79, mode: 3 } - - joint_limits: - waist_yaw_joint: - has_position_limits: true - min_position: -1.919862 - max_position: 1.919862 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 - neck_yaw_joint: - has_position_limits: true - min_position: -2.879793 - max_position: 2.879793 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 - neck_pitch_joint: - has_position_limits: true - min_position: -1.570796 - max_position: 1.570796 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: false - has_jerk_limits: false - has_effort_limits: true - max_effort: 5.0 - diff --git a/sciurus17_control/config/sciurus17_controllers.yaml b/sciurus17_control/config/sciurus17_controllers.yaml new file mode 100644 index 0000000..3197a85 --- /dev/null +++ b/sciurus17_control/config/sciurus17_controllers.yaml @@ -0,0 +1,97 @@ +controller_manager: + ros__parameters: + update_rate: 500 # Hz + + right_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + right_hand_controller: + type: position_controllers/GripperActionController + left_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + left_hand_controller: + type: position_controllers/GripperActionController + neck_controller: + type: joint_trajectory_controller/JointTrajectoryController + waist_yaw_controller: + type: joint_trajectory_controller/JointTrajectoryController + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +right_arm_controller: + ros__parameters: + joints: + - r_arm_joint1 + - r_arm_joint2 + - r_arm_joint3 + - r_arm_joint4 + - r_arm_joint5 + - r_arm_joint6 + - r_arm_joint7 + + command_interfaces: + - position + + state_interfaces: + - position + +right_hand_controller: + ros__parameters: + joint: r_hand_joint + goal_tolerance: 0.1 + + command_interfaces: + - position + + state_interfaces: + - position + +left_arm_controller: + ros__parameters: + joints: + - l_arm_joint1 + - l_arm_joint2 + - l_arm_joint3 + - l_arm_joint4 + - l_arm_joint5 + - l_arm_joint6 + - l_arm_joint7 + + command_interfaces: + - position + + state_interfaces: + - position + +left_hand_controller: + ros__parameters: + joint: l_hand_joint + goal_tolerance: 0.1 + + command_interfaces: + - position + + state_interfaces: + - position + +neck_controller: + ros__parameters: + joints: + - neck_yaw_joint + - neck_pitch_joint + + command_interfaces: + - position + + state_interfaces: + - position + +waist_yaw_controller: + ros__parameters: + joints: + - waist_yaw_joint + + command_interfaces: + - position + + state_interfaces: + - position diff --git a/sciurus17_control/config/sciurus17_fake_control1.yaml b/sciurus17_control/config/sciurus17_fake_control1.yaml deleted file mode 100644 index 4ccfb0e..0000000 --- a/sciurus17_control/config/sciurus17_fake_control1.yaml +++ /dev/null @@ -1,51 +0,0 @@ -# Publish all joint states ----------------------------------- -joint_state_controller: - type: "joint_state_controller/JointStateController" - publish_rate: 100 - -right_arm_controller: - type: "position_controllers/JointTrajectoryController" - publish_rate: 100 - joints: - - r_arm_joint1 - - r_arm_joint2 - - r_arm_joint3 - - r_arm_joint4 - - r_arm_joint5 - - r_arm_joint6 - - r_arm_joint7 - constraints: - goal_time: 0.0 - stopped_velocity_tolerance: 1.0 - - # for effort control - gains: - r_arm_joint1: { p: 5.0, d: 0.1, i: 0.0 } - r_arm_joint2: { p: 5.0, d: 0.1, i: 0.0 } - r_arm_joint3: { p: 5.0, d: 0.1, i: 0.0 } - r_arm_joint4: { p: 5.0, d: 0.1, i: 0.0 } - r_arm_joint5: { p: 1.0, d: 0.1, i: 0.0 } - r_arm_joint6: { p: 1.0, d: 0.1, i: 0.0 } - r_arm_joint7: { p: 1.0, d: 0.1, i: 0.0 } - -right_hand_controller: - type: "position_controllers/GripperActionController" - action_ns: gripper_cmd - publish_rate: 100 - joint: r_hand_joint - action_monitor_rate: 50 - state_publish_rate: 100 - stalled_velocity_threshold: 0.001 - goal_tolerance: 0.05 - stall_timeout: 0.1 - -gazebo_ros_control: - pid_gains: - r_arm_joint1: { p: 100.0, d: 0.0, i: 0.0 } - r_arm_joint2: { p: 100.0, d: 0.0, i: 0.0 } - r_arm_joint3: { p: 100.0, d: 0.0, i: 0.0 } - r_arm_joint4: { p: 100.0, d: 0.0, i: 0.0 } - r_arm_joint5: { p: 100.0, d: 0.0, i: 0.0 } - r_arm_joint6: { p: 100.0, d: 0.0, i: 0.0 } - r_arm_joint7: { p: 100.0, d: 0.0, i: 0.0 } - r_hand_joint: { p: 100.0, d: 0.0, i: 0.0 } diff --git a/sciurus17_control/config/sciurus17_fake_control2.yaml b/sciurus17_control/config/sciurus17_fake_control2.yaml deleted file mode 100644 index 6eb51b5..0000000 --- a/sciurus17_control/config/sciurus17_fake_control2.yaml +++ /dev/null @@ -1,46 +0,0 @@ -# Publish all joint states ----------------------------------- -joint_state_controller: - type: "joint_state_controller/JointStateController" - publish_rate: 100 - -left_arm_controller: - type: "position_controllers/JointTrajectoryController" - publish_rate: 100 - joints: - - l_arm_joint1 - - l_arm_joint2 - - l_arm_joint3 - - l_arm_joint4 - - l_arm_joint5 - - l_arm_joint6 - - l_arm_joint7 - constraints: - goal_time: 0.0 - stopped_velocity_tolerance: 1.0 - -left_wrist_controller: - type: "effort_controllers/JointEffortController" - joint: l_arm_joint7 - pid: {p: 1.0, d: 0.0, i: 0.0} - -left_hand_controller: - type: "position_controllers/GripperActionController" - action_ns: gripper_cmd - publish_rate: 100 - joint: l_hand_joint - action_monitor_rate: 50 - state_publish_rate: 100 - stalled_velocity_threshold: 0.001 - goal_tolerance: 0.05 - stall_timeout: 0.1 - -gazebo_ros_control: - pid_gains: - l_arm_joint1: { p: 100.0, d: 0.0, i: 0.0 } - l_arm_joint2: { p: 100.0, d: 0.0, i: 0.0 } - l_arm_joint3: { p: 100.0, d: 0.0, i: 0.0 } - l_arm_joint4: { p: 100.0, d: 0.0, i: 0.0 } - l_arm_joint5: { p: 100.0, d: 0.0, i: 0.0 } - l_arm_joint6: { p: 100.0, d: 0.0, i: 0.0 } - l_arm_joint7: { p: 100.0, d: 0.0, i: 0.0 } - l_hand_joint: { p: 100.0, d: 0.0, i: 0.0 } diff --git a/sciurus17_control/config/sciurus17_fake_control3.yaml b/sciurus17_control/config/sciurus17_fake_control3.yaml deleted file mode 100644 index 7afed9f..0000000 --- a/sciurus17_control/config/sciurus17_fake_control3.yaml +++ /dev/null @@ -1,36 +0,0 @@ -# Publish all joint states ----------------------------------- -joint_state_controller: - type: "joint_state_controller/JointStateController" - publish_rate: 100 - -neck_controller: - type: "position_controllers/JointTrajectoryController" - publish_rate: 100 - joints: - - neck_yaw_joint - - neck_pitch_joint - constraints: - goal_time: 0.0 - stopped_velocity_tolerance: 1.0 - -waist_yaw_controller: - type: "position_controllers/JointTrajectoryController" - publish_rate: 100 - joints: - - waist_yaw_joint - constraints: - goal_time: 0.0 - stopped_velocity_tolerance: 1.0 - waist_yaw_joint: - trajectory: 0.05 # Not enforced if unspecified - goal: 0.02 # Not enforced if unspecified - state_publish_rate: 25 # Override default - action_monitor_rate: 30 # Override default - stop_trajectory_duration: 0 # Override default - -gazebo_ros_control: - pid_gains: - neck_yaw_joint: { p: 100.0, d: 0.0, i: 0.0 } - neck_pitch_joint: { p: 100.0, d: 0.0, i: 0.0 } - waist_yaw_joint: { p: 100.0, d: 0.0, i: 0.0 } - diff --git a/sciurus17_control/include/sciurus17_control/control_setting.h b/sciurus17_control/include/sciurus17_control/control_setting.h deleted file mode 100644 index 0e4cbeb..0000000 --- a/sciurus17_control/include/sciurus17_control/control_setting.h +++ /dev/null @@ -1,77 +0,0 @@ -#ifndef DXL_CONTROL_CONTROL_SETTING_H -#define DXL_CONTROL_CONTROL_SETTING_H - -#include -#include -#include -#include -#include -#include - -/* Operating Mode value */ -#define OPE_CURRENT_MODE (0) -#define OPE_VELOCITY_MODE (1) -#define OPE_POSITION_MODE (3) -#define OPE_EXT_POS_MODE (4) -#define OPE_CURR_POS_MODE (5) -#define OPE_PWM_MODE (16) - -/* SETTING KEY STRINGS */ -#define KEY_DXL_PORT ("dynamixel_port") -#define KEY_PORTNAME ("/port_name") -#define KEY_BAUDRATE ("/baud_rate") -#define KEY_JOINTS ("/joints") -#define KEY_JPARAM_ID ("/id") -#define KEY_JPARAM_CENTER ("/center") -#define KEY_JPARAM_HOME ("/home") -#define KEY_JPARAM_EFFCNST ("/effort_const") -#define KEY_JPARAM_OPEMODE ("/mode") - -#define DEFAULT_CENTER (2048) -#define DEFAULT_EFF_CNST (1.0) -#define DEFAULT_OPE_MODE (OPE_POSITION_MODE) - - -typedef struct SERVO_PARAM { - SERVO_PARAM(){ - name = ""; - id = 0; - center = home = DEFAULT_CENTER; - eff_cnst = DEFAULT_EFF_CNST; - mode = DEFAULT_OPE_MODE; - } - std::string name; - uint8_t id; - uint16_t center; - uint16_t home; - double eff_cnst; - uint8_t mode; -} ST_SERVO_PARAM; - -class CONTROL_SETTING -{ -public: - CONTROL_SETTING( ros::NodeHandle handle ); - ~CONTROL_SETTING(); - bool load( void ); - std::string getPortName( void ){ return port_name; } - std::string loadPortName( void ); - std::vector getServoParam( void ){ return joint_list; } - uint32_t getjointNum( void ){ return joint_num; } - uint32_t getBaudrate( void ){ return port_baud_rate; } - -private: - ros::NodeHandle node_handle; - uint8_t port_num; - std::string port_name; - uint32_t port_baud_rate; - uint32_t joint_num; - std::vector joint_list; - - bool isPortDefine( void ); - uint32_t loadBaudRate( void ); - bool loadJointList( void ); - bool loadJointParam( void ); -}; - -#endif/* DXL_CONTROL_CONTROL_SETTING_H */ diff --git a/sciurus17_control/include/sciurus17_control/device_mutex.h b/sciurus17_control/include/sciurus17_control/device_mutex.h deleted file mode 100644 index 82c9d4d..0000000 --- a/sciurus17_control/include/sciurus17_control/device_mutex.h +++ /dev/null @@ -1,111 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -typedef struct { - int count; - pid_t pid; -} ST_MUTEX_MEM; - - -class DEVICE_MUTEX -{ -public: - DEVICE_MUTEX( const char* shm_key, int timeout_sec=1 ) - { - if( (shm_id = shmget( ftok( shm_key, 'R' ), 80, IPC_CREAT|IPC_EXCL|0666)) == -1){ - if( errno == EEXIST ){ - perror("exist"); - shm_id = shmget( ftok( shm_key, 'R' ), 80, 0666); - perror("exis"); - }else{ - perror("shmget"); - exit(-1); - } - } - mutex_mem = (ST_MUTEX_MEM *)shmat( shm_id, NULL, 0 ); - // printf("debug %d %08x %d\r\n",shm_id,(void*)mutex_mem, mutex_mem->pid ); - if( mutex_mem == (void *)-1 ){ - perror("shmat"); - } - - int fa; - for( fa=strlen(shm_key)-1 ; fa > 0 ; --fa ){ - if( shm_key[fa] == '/' ){ - break; - } - } - - sem = sem_open(&shm_key[fa], O_CREAT, (S_IRUSR|S_IWUSR|S_IRGRP|S_IROTH), 1); - lock_flg = false; - if( sem == SEM_FAILED ){ - perror("semopen"); - } - - timespec timeout; - clock_gettime( CLOCK_REALTIME, &timeout); - timeout.tv_sec += 1; - if( (sem_timedwait( sem, &timeout ) != 0) && (errno == ETIMEDOUT) ){ - if( !is_alive(mutex_mem->pid) ){ - int semval; - sem_getvalue( sem, &semval ); - if( semval <= 0 ){ - sem_post( sem ); - } - mutex_mem->pid = getpid(); - mutex_mem->count = 0; - } - }else{ - mutex_mem->pid = getpid(); - unlock(); - } - - ++mutex_mem->count; - } - virtual ~DEVICE_MUTEX() - { - --mutex_mem->count; - if( lock_flg ){ - unlock(); - } - sem_close( sem ); - shmdt( mutex_mem ); - shmctl(shm_id, IPC_RMID, 0); - } - void lock() - { - sem_wait( sem ); - mutex_mem->pid = getpid(); - lock_flg = true; - } - void unlock() - { - sem_post( sem ); - mutex_mem->pid = 0; - lock_flg = false; - } -private: - bool is_alive( pid_t check_pid ) - { - char filepath[256]; - struct stat get_stat; - snprintf( filepath, sizeof(filepath), "/proc/%d", (int)check_pid ); - return (stat(filepath,&get_stat)==0)?true:false; - } - int shm_id; - int timeout; - ST_MUTEX_MEM* mutex_mem; - sem_t* sem; - bool lock_flg; -}; - - - diff --git a/sciurus17_control/include/sciurus17_control/dxlport_control.h b/sciurus17_control/include/sciurus17_control/dxlport_control.h deleted file mode 100644 index 0f4a9ec..0000000 --- a/sciurus17_control/include/sciurus17_control/dxlport_control.h +++ /dev/null @@ -1,107 +0,0 @@ -#ifndef DXLPORT_CONTROL_H -#define DXLPORT_CONTROL_H - -#include -#include -#include -#include -#include -#include -#include -#include - -#include // Uses Dynamixel SDK library -#include -#include - -#include "device_mutex.h" - -// Motion data -typedef struct HOME_MOTION_DATA { - uint16_t home; - double home_rad; - uint16_t start; - double start_rad; - uint16_t step; - double step_rad; -} ST_HOME_MOTION_DATA; - - -// Dynamixel protocol serial port class -class DXLPORT_CONTROL : public hardware_interface::RobotHW -{ -public: - DXLPORT_CONTROL( ros::NodeHandle handle, CONTROL_SETTING &setting ); - ~DXLPORT_CONTROL(); - ros::Time getTime() const { return ros::Time::now(); } - ros::Duration getDuration( ros::Time t ) const { return (ros::Time::now() - t); } - bool read( ros::Time, ros::Duration ); - bool readId( ros::Time, ros::Duration ); - bool readPos( ros::Time, ros::Duration ); - bool readCurrent( ros::Time, ros::Duration ); - bool readTemp( ros::Time, ros::Duration ); - bool readVel( ros::Time, ros::Duration ); - void write( ros::Time, ros::Duration ); - void set_torque_all( bool torque ); - bool set_torque( uint8_t dxl_id, bool torque ); - void set_watchdog( uint8_t dxl_id, uint8_t value ); - void set_watchdog_all( uint8_t value ); - void startup_motion( void ); - void set_gain_all( uint16_t gain ); - void set_gain( uint8_t dxl_id, uint16_t gain ); - void set_goal_current_all( uint16_t current ); - void set_goal_current( uint8_t dxl_id, uint16_t current ); - bool get_init_stat( void ){ return init_stat; } - uint8_t get_joint_num( void ){ return joint_num; } - std::string self_check( void ); - void init_joint_params( ST_JOINT_PARAM ¶m, int table_id, int value ); - - void set_param_delay_time( uint8_t dxl_id, int val ); - void set_param_drive_mode( uint8_t dxl_id, int val ); - void set_param_ope_mode( uint8_t dxl_id, int val ); - void set_param_home_offset( uint8_t dxl_id, int val ); - void set_param_moving_threshold( uint8_t dxl_id, int val ); - void set_param_temp_limit( uint8_t dxl_id, int val ); - void set_param_vol_limit( uint8_t dxl_id, int max, int min ); - void set_param_current_limit( uint8_t dxl_id, int val ); - void set_param_vel_gain( uint8_t dxl_id, int p, int i ); - void set_param_pos_gain_all( int p, int i, int d ); - void set_param_pos_gain( uint8_t dxl_id, int p, int i, int d ); - - bool is_change_positions( void ); - void lock_port( void ){ dev_mtx->lock(); } - void unlock_port( void ){ dev_mtx->unlock(); } - std::string::size_type get_error( std::string& errorlog ); - bool setup_indirect( uint8_t dxl_id ); - - uint32_t tempCount; - std::vector joints; - -private: - uint8_t joint_num; - bool port_stat; - dynamixel::PacketHandler *packetHandler; - dynamixel::PortHandler *portHandler; - hardware_interface::JointStateInterface joint_stat_if; - hardware_interface::PositionJointInterface joint_pos_if; - hardware_interface::EffortJointInterface joint_eff_if; - joint_limits_interface::PositionJointSoftLimitsInterface joint_limits_if; - dynamixel::GroupBulkRead *readTempGroup; - dynamixel::GroupBulkRead *readIndirectGroup; - dynamixel::GroupBulkWrite *writeGoalGroup; - dynamixel::GroupBulkWrite *writePosGoalGroup; - - bool init_stat; - uint32_t rx_err; - uint32_t tx_err; - ros::Time tempTime; - - DEVICE_MUTEX* dev_mtx; - std::queue error_queue; - - bool check_servo_param( uint8_t dxl_id, uint32_t test_addr, uint8_t equal, uint8_t& read_val ); - bool check_servo_param( uint8_t dxl_id, uint32_t test_addr, uint16_t equal, uint16_t& read_val ); - bool check_servo_param( uint8_t dxl_id, uint32_t test_addr, uint32_t equal, uint32_t& read_val ); -}; - -#endif /* DXLPORT_CONTROL_H */ diff --git a/sciurus17_control/include/sciurus17_control/joint_control.h b/sciurus17_control/include/sciurus17_control/joint_control.h deleted file mode 100644 index 9043b43..0000000 --- a/sciurus17_control/include/sciurus17_control/joint_control.h +++ /dev/null @@ -1,298 +0,0 @@ -#ifndef DXL_JOINT_CONTROL_H -#define DXL_JOINT_CONTROL_H - -#include -#include -#include -#include -#include -#include -#include - -// Protocol version -#define PROTOCOL_VERSION (2.0) // See which protocol version is used in the Dynamixel - -// DYNAMIXEL REGISTER TABLE (Dynamixel X) -typedef enum { - enDXL_ROM, - enDXL_RAM -} EN_DXL_MEMTYPE; -typedef struct { - std::string name; /* データ名称 */ - uint32_t address; /* アドレス */ - uint32_t length; /* データ長 */ - uint32_t init_value; /* 初期値 */ - EN_DXL_MEMTYPE type; /* メモリ種別 */ - bool selfcheck; /* セルフチェック対象 */ -} ST_DYNAMIXEL_REG_TABLE; - -#define REG_LENGTH_BYTE (1) -#define REG_LENGTH_WORD (2) -#define REG_LENGTH_DWORD (4) - -typedef enum { - enTableId_ReturnDelay = 0, - enTableId_DriveMode, - enTableId_OpeMode, - enTableId_HomingOffset, - enTableId_MovingThreshold, - enTableId_TempLimit, - enTableId_MaxVolLimit, - enTableId_MinVolLimit, - enTableId_CurrentLimit, - enTableId_Shutdown, - enTableId_TorqueEnable, - enTableId_VelocityIGain, - enTableId_VelocityPGain, - enTableId_PositionDGain, - enTableId_PositionIGain, - enTableId_PositionPGain, - enTableId_BusWatchdog, - enTableId_GoalCurrent, - enTableId_GoalVelocity, - enTableId_GoalPosition, - enTableId_PresentCurrent, - enTableId_PresentVelocity, - enTableId_PresentPosition, - enTableId_PresentTemp, -} EN_TABLE_ID; - -static const ST_DYNAMIXEL_REG_TABLE RegTable[] ={ - /* NAME ADDR LEN INIT TYPE */ - { "RETURN_DELAY_TIME", 9, REG_LENGTH_BYTE, 250, enDXL_ROM, false }, - { "DRIVE_MODE", 10, REG_LENGTH_BYTE, 0, enDXL_ROM, false }, - { "OPERATION_MODE", 11, REG_LENGTH_BYTE, 3, enDXL_ROM, false }, - { "HOMING_OFFSET", 20, REG_LENGTH_DWORD, 0, enDXL_ROM, false }, - { "MOVING_THRESHOLD", 24, REG_LENGTH_DWORD, 10, enDXL_ROM, false }, - { "TEMPRATURE_LIMIT", 31, REG_LENGTH_BYTE, 80, enDXL_ROM, false }, - { "MAX_VOL_LIMIT", 32, REG_LENGTH_WORD, 160, enDXL_ROM, false }, - { "MIN_VOL_LIMIT", 34, REG_LENGTH_WORD, 95, enDXL_ROM, false }, - { "CURRENT_LIMIT", 38, REG_LENGTH_WORD, 1193, enDXL_ROM, false }, - { "SHUTDOWN", 63, REG_LENGTH_BYTE, 52, enDXL_ROM, false }, - { "TORQUE_ENABLE", 64, REG_LENGTH_BYTE, 0, enDXL_RAM, false }, - { "VELOCITY_I_GAIN", 76, REG_LENGTH_WORD, 1920, enDXL_RAM, true }, - { "VELOCITY_P_GAIN", 78, REG_LENGTH_WORD, 100, enDXL_RAM, true }, - { "POSITION_D_GAIN", 80, REG_LENGTH_WORD, 0, enDXL_RAM, true }, - { "POSITION_I_GAIN", 82, REG_LENGTH_WORD, 0, enDXL_RAM, true }, - { "POSITION_P_GAIN", 84, REG_LENGTH_WORD, 800, enDXL_RAM, false }, - { "BUS_WATCHDOG", 98, REG_LENGTH_BYTE, 0, enDXL_RAM, false }, - { "GOAL_CURRENT", 102, REG_LENGTH_WORD, 0, enDXL_RAM, false }, - { "GOAL_VELOCITY", 104, REG_LENGTH_DWORD, 0, enDXL_RAM, false }, - { "GOAL_POSITION", 116, REG_LENGTH_DWORD, 0, enDXL_RAM, false }, - { "PRESENT_CURRENT", 126, REG_LENGTH_WORD, 0, enDXL_RAM, false }, - { "PRESENT_VELOCITY", 128, REG_LENGTH_DWORD, 0, enDXL_RAM, false }, - { "PRESENT_POSITION", 132, REG_LENGTH_DWORD, 0, enDXL_RAM, false }, - { "PRESENT_TEMPRATURE", 146, REG_LENGTH_BYTE, 0, enDXL_RAM, false }, -}; - -typedef struct ST_JOINT_PARAM -{ - uint8_t dxl_id; - uint8_t return_delay_time; - uint8_t drive_mode; - uint8_t operation_mode; - uint16_t moving_threshold; - int32_t homing_offset; - uint8_t temprature_limit; - uint8_t max_vol_limit; - uint8_t min_vol_limit; - uint16_t current_limit; - uint8_t torque_enable; - uint16_t velocity_i_gain; - uint16_t velocity_p_gain; - uint16_t position_d_gain; - uint16_t position_i_gain; - uint16_t position_p_gain; -} ST_JOINT_PARAM; - -// Parameter -#define BAUDRATE (3000000) // 通信速度 -#define ADDR_RETURN_DELAY (RegTable[enTableId_ReturnDelay].address) -#define LEN_PRETURN_DELAY (RegTable[enTableId_ReturnDelay].length) -#define ADDR_DRIVE_MODE (RegTable[enTableId_DriveMode].address) -#define LEN_DRIVE_MODE (RegTable[enTableId_DriveMode].length) -#define ADDR_OPE_MODE (RegTable[enTableId_OpeMode].address) -#define LEN_OPE_MODE (RegTable[enTableId_OpeMode].length) -#define ADDR_HOMING_OFFSET (RegTable[enTableId_HomingOffset].address) -#define LEN_HOMING_OFFSET (RegTable[enTableId_HomingOffset].length) -#define ADDR_MOVING_THRESHOLD (RegTable[enTableId_MovingThreshold].address) -#define LEN_MOVING_THRESHOLD (RegTable[enTableId_MovingThreshold].length) -#define ADDR_TEMPRATURE_LIMIT (RegTable[enTableId_TempLimit].address) -#define LEN_TEMPRATURE_LIMIT (RegTable[enTableId_TempLimit].length) -#define ADDR_MAX_VOL_LIMIT (RegTable[enTableId_MaxVolLimit].address) -#define LEN_MAX_VOL_LIMIT (RegTable[enTableId_MaxVolLimit].length) -#define ADDR_MIN_VOL_LIMIT (RegTable[enTableId_MinVolLimit].address) -#define LEN_MIN_VOL_LIMIT (RegTable[enTableId_MinVolLimit].length) -#define ADDR_CURRENT_LIMIT (RegTable[enTableId_CurrentLimit].address) -#define LEN_CURRENT_LIMIT (RegTable[enTableId_CurrentLimit].length) -#define ADDR_TORQUE_ENABLE (RegTable[enTableId_TorqueEnable].address) // Control table address is different in Dynamixel model -#define ADDR_VELOCITY_IGAIN (RegTable[enTableId_VelocityIGain].address) -#define LEN_VELOCITY_IGAIN (RegTable[enTableId_VelocityIGain].length) -#define ADDR_VELOCITY_PGAIN (RegTable[enTableId_VelocityPGain].address) -#define LEN_VELOCITY_PGAIN (RegTable[enTableId_VelocityPGain].length) -#define ADDR_POSITION_DGAIN (RegTable[enTableId_PositionDGain].address) -#define LEN_POSITION_DGAIN (RegTable[enTableId_PositionDGain].length) -#define ADDR_POSITION_IGAIN (RegTable[enTableId_PositionIGain].address) -#define LEN_POSITION_IGAIN (RegTable[enTableId_PositionIGain].length) -#define ADDR_POSITION_PGAIN (RegTable[enTableId_PositionPGain].address) -#define LEN_POSITION_PGAIN (RegTable[enTableId_PositionPGain].length) -#define ADDR_BUS_WATCHDOG (RegTable[enTableId_BusWatchdog].address) -#define LEN_BUS_WATCHDOG (RegTable[enTableId_BusWatchdog].length) -#define ADDR_GOAL_CURRENT (RegTable[enTableId_GoalCurrent].address) // ゴールカレントアドレス -#define LEN_GOAL_CURRENT (RegTable[enTableId_GoalCurrent].length) // ゴールカレント -#define ADDR_GOAL_POSITION (RegTable[enTableId_GoalPosition].address) // ゴールポジションアドレス -#define LEN_GOAL_POSITION (RegTable[enTableId_GoalPosition].length) // ゴールポジション -#define ADDR_PRESENT_CURRENT (RegTable[enTableId_PresentCurrent].address) -#define LEN_PRESENT_CURRENT (RegTable[enTableId_PresentCurrent].length) -#define ADDR_PRESENT_VEL (RegTable[enTableId_PresentVelocity].address) -#define LEN_PRESENT_VEL (RegTable[enTableId_PresentVelocity].length) -#define ADDR_PRESENT_POSITION (RegTable[enTableId_PresentPosition].address) -#define LEN_PRESENT_POSITION (RegTable[enTableId_PresentPosition].length) -#define ADDR_PRESENT_TEMP (RegTable[enTableId_PresentTemp].address) -#define LEN_PRESENT_TEMP (RegTable[enTableId_PresentTemp].length) - -#define ADDR_INDIRECT_TOP (168) -#define DATA_INDIRECT_TOP (224) -#define LEN_INDIRECT_GROUP (12) -#define ADDR_INDIRECT_DXLID (DATA_INDIRECT_TOP) -#define LEN_INDIRECT_DXLID (1) -#define ADDR_INDIRECT_CURRENT (DATA_INDIRECT_TOP+1) -#define ADDR_INDIRECT_VELOCITY (DATA_INDIRECT_TOP+3) -#define ADDR_INDIRECT_POSITION (DATA_INDIRECT_TOP+7) -#define ADDR_INDIRECT_TEMP (DATA_INDIRECT_TOP+11) - -#define TORQUE_ENABLE (1) // Value for enabling the torque -#define TORQUE_DISABLE (0) // Value for disabling the torque - -#define POSITION_STEP (4096.0) -#define DXL_MIN_LIMIT (0.0) -#define DXL_MAX_LIMIT (4095.0) -#define DXL_CURRENT_UNIT (2.69) // mA -#define DXL_EFFORT_CONST (1.79) -#define DXL_TEMP_READ_DURATION (5) -#define DXL_PGAIN_MAX (16383) -#define DXL_DEFAULT_PGAIN (800) -#define DXL_FREE_PGAIN (0) -#define DXL_FREE_IGAIN (0) -#define DXL_FREE_DGAIN (0) - -#define DXL_OFFSET_DEFAULT (2048) - -#define DXL_TORQUE_ON_TIME (5*1000) // msec -#define DXL_TORQUR_ON_STEP (20) // Hz -#define DXL_TORQUE_ON_STEP_MAX (DXL_TORQUE_ON_TIME / (1000 / DXL_TORQUR_ON_STEP)) - -#define DXL_CURRENT2EFFORT(c,coef) (DXL_CURRENT_UNIT * (c) * (coef) * 0.001)// (mA*CONST*0.001)=Nm -#define EFFORT2DXL_CURRENT(e,coef) ((e) / (coef) / 0.001 / DXL_CURRENT_UNIT)// (Nm/CONST/0.001)=mA -#define DEFAULT_MAX_EFFORT (5.0) -#define EFFORT_LIMITING_CNT (10) -#define DXL_VELOCITY2RAD_S(v) ((v) * 0.229 * 0.1047) - -#define DXL_WATCHDOG_RESET_VALUE (0) -#define MSEC2DXL_WATCHDOG(msec) ((uint8_t)(msec) / 20) - -#define OPERATING_MODE_CURRENT (0) -#define OPERATING_MODE_VELOCITY (1) -#define OPERATING_MODE_POSITION (3) -#define OPERATING_MODE_EXT_POS (4) -#define OPERATING_MODE_CURR_POS (5) -#define OPERATING_MODE_PWM (16) - - -// Joint control class -class JOINT_CONTROL -{ -public: - JOINT_CONTROL(void); - JOINT_CONTROL( std::string init_name, uint8_t init_dxlid, uint16_t init_center, uint16_t init_home, double init_eff_const, uint8_t init_mode ); - JOINT_CONTROL( const JOINT_CONTROL &src ); - ~JOINT_CONTROL(void){ /* Nothing todo... */ } - void init_parameter( std::string init_name, uint8_t init_dxlid, uint16_t init_center, uint16_t init_home, double init_eff_const, uint8_t init_mode ); - void set_joint_name( std::string set_name ) { name = set_name; } - void set_dxl_id( uint8_t set_id ){ id = set_id; } - void set_position( double set_rad ){ pos = set_rad; } - void set_velocity( double set_vel ){ vel = set_vel; } - void set_effort( double set_eff ){ eff = set_eff; } - void set_command( double set_cmd ){ cmd = set_cmd; } - void set_torque( bool set_trq ){ torque = set_trq; } - void set_center( uint16_t set_center ){ center = set_center; } - void set_home( uint16_t set_home ){ home = set_home; } - void set_current( double set_curr ){ curr = set_curr; } - void set_temprature( double set_temp ){ temp = set_temp; } - void set_connect( bool set_connect ){ connect = set_connect; } - void set_dxl_pos( uint32_t set_dxl_pos ){ dxl_pos = set_dxl_pos; } - void set_dxl_curr( uint16_t set_dxl_curr ){ dxl_curr = set_dxl_curr; } - void set_dxl_temp( uint8_t set_dxl_temp ){ dxl_temp = set_dxl_temp; } - void set_limits( joint_limits_interface::JointLimits &set_limits ){ limits = set_limits; } - void set_eff_limiting( bool set_limiting ){ eff_limiting = set_limiting; } - void inc_eff_over( void ){ ++eff_over_cnt; } - void clear_eff_over( void ){ eff_over_cnt = 0; } - void set_joint_param( ST_JOINT_PARAM set_param){ param = set_param; } - - std::string get_joint_name( void ) { return name; } - uint8_t get_dxl_id( void ){ return id; } - double get_position( void ){ return pos; } - double* get_position_addr( void ){ return &pos; } - double get_velocity( void ){ return vel; } - double* get_velocity_addr( void ){ return &vel; } - double get_effort( void ){ return eff; } - double get_max_effort( void ){ return (limits.has_effort_limits?limits.max_effort:DEFAULT_MAX_EFFORT); } - double* get_effort_addr( void ){ return &eff; } - double get_command( void ){ return cmd; } - double* get_command_addr( void ){ return &cmd; } - bool get_torque( void ){ return torque; } - uint16_t get_center( void ){ return center; } - uint16_t get_home( void ){ return home; } - double get_current( void ){ return curr; } - double get_temprature( void ){ return temp; } - bool is_connect( void ){ return connect; } - uint32_t get_dxl_pos( void ){ return dxl_pos; } - uint16_t get_dxl_curr( void ){ return dxl_curr; } - uint8_t get_dxl_temp( void ){ return dxl_temp; } - uint8_t get_dxl_goal( void ){ return (uint32_t)(dxl_goal[0]&0x000000FF) - | (uint32_t)((dxl_goal[1]<<8)&0x0000FF00) - | (uint32_t)((dxl_goal[2]<<16)&0x00FF0000) - | (uint32_t)((dxl_goal[3]<<24)&0xFF0000FF); } - uint8_t* get_dxl_goal_addr( void ){ return dxl_goal; } - bool is_effort_limiting( void ){ return eff_limiting; } - uint8_t get_eff_over_cnt( void ){ return eff_over_cnt; } - double get_eff_const( void ){ return eff_const; } - uint8_t get_ope_mode( void ){ return ope_mode; } - ST_JOINT_PARAM get_joint_param( void ){ return param; } - void updt_d_command( double val ){ d_cmd = (val - prev_cmd); prev_cmd = val; } - double get_d_command( void ){ return d_cmd; } - -private: - std::string name; // ROS joint name - uint8_t id; // Dynamixel ServoID - double pos; // Present position - double vel; // Present verocity - double eff; // Present effort - double cmd; // ROS HwInterface value - double d_cmd; // cmd value delta - double prev_cmd; // Previous cmd value - double curr; // Present current[mA] - double temp; // Present temprature - bool torque; // Servo torque - uint16_t center; // Servo center position offset( dynamixel position value, default:2048 ) - uint16_t home; // Servo home position( dynamixel position value ) - bool connect; // Servo connect status - double eff_const; // Servo effort constant - bool eff_limiting;// Effort limiting status - uint8_t eff_over_cnt;// Effort limiting status - uint8_t ope_mode; // Operating mode - - double goal_pos; // Goal position[rad] - double goal_vel; // Goal velocity - double goal_eff; // Goal effort - - uint32_t dxl_pos; // Dynamixel present position - uint16_t dxl_curr; // Dynamixel current - uint8_t dxl_temp; // Dynamixel temprature - uint8_t dxl_goal[4];// Dynamixel goal position - - joint_limits_interface::JointLimits limits;// Joint limit parameters - - ST_JOINT_PARAM param; -}; -#endif /*DXL_JOINT_CONTROL_H */ \ No newline at end of file diff --git a/sciurus17_control/launch/controller1.launch b/sciurus17_control/launch/controller1.launch deleted file mode 100644 index 22597f6..0000000 --- a/sciurus17_control/launch/controller1.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - diff --git a/sciurus17_control/launch/controller2.launch b/sciurus17_control/launch/controller2.launch deleted file mode 100644 index 8732833..0000000 --- a/sciurus17_control/launch/controller2.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - diff --git a/sciurus17_control/launch/controller3.launch b/sciurus17_control/launch/controller3.launch deleted file mode 100644 index adcd773..0000000 --- a/sciurus17_control/launch/controller3.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - diff --git a/sciurus17_control/launch/sciurus17_control.launch b/sciurus17_control/launch/sciurus17_control.launch deleted file mode 100644 index fc9e671..0000000 --- a/sciurus17_control/launch/sciurus17_control.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/sciurus17_control/launch/sciurus17_fake_control.launch b/sciurus17_control/launch/sciurus17_fake_control.launch deleted file mode 100644 index cc02970..0000000 --- a/sciurus17_control/launch/sciurus17_fake_control.launch +++ /dev/null @@ -1,69 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sciurus17_control/package.xml b/sciurus17_control/package.xml index f06538c..c32829e 100644 --- a/sciurus17_control/package.xml +++ b/sciurus17_control/package.xml @@ -1,37 +1,28 @@ - + + sciurus17_control 2.0.0 The Sciurus17 control package - RT Corporation + Apache License 2.0 + Hiroyuki Nomura + Atsushi Kuwagata - Apache License 2.0 + ament_cmake - catkin - controller_manager - hardware_interface - transmission_interface - dynamixel_sdk - roscpp - rospy - sciurus17_msgs - joint_limits_interface - controller_manager - effort_controllers - hardware_interface - transmission_interface - dynamixel_sdk - roscpp - rospy - joint_limits_interface - joint_state_controller - joint_trajectory_controller - gripper_action_controller - sciurus17_msgs + controller_manager + gripper_controllers + hardware_interface + pluginlib + rclcpp + ros2_controllers - + ament_lint_auto + ament_lint_common + + ament_cmake diff --git a/sciurus17_control/scripts/preset_reconfigure.py b/sciurus17_control/scripts/preset_reconfigure.py deleted file mode 100755 index 9186219..0000000 --- a/sciurus17_control/scripts/preset_reconfigure.py +++ /dev/null @@ -1,180 +0,0 @@ -#!/usr/bin/env python -# coding: utf-8 - -import roslib -import rospy -import dynamic_reconfigure.client -from std_msgs.msg import UInt8 - -class PRESET_RECONFIGURE: - def __init__( self ): - rospy.loginfo("Wait reconfig server...") - ### Dynamic Reconfigure 接続先一覧 ### - self.joint_list = [ - {"control":"/sciurus17/controller1/joints","joint":"r_arm_joint1"}, \ - {"control":"/sciurus17/controller1/joints","joint":"r_arm_joint2"}, \ - {"control":"/sciurus17/controller1/joints","joint":"r_arm_joint3"}, \ - {"control":"/sciurus17/controller1/joints","joint":"r_arm_joint4"}, \ - {"control":"/sciurus17/controller1/joints","joint":"r_arm_joint5"}, \ - {"control":"/sciurus17/controller1/joints","joint":"r_arm_joint6"}, \ - {"control":"/sciurus17/controller1/joints","joint":"r_arm_joint7"}, \ - {"control":"/sciurus17/controller2/joints","joint":"l_arm_joint1"}, \ - {"control":"/sciurus17/controller2/joints","joint":"l_arm_joint2"}, \ - {"control":"/sciurus17/controller2/joints","joint":"l_arm_joint3"}, \ - {"control":"/sciurus17/controller2/joints","joint":"l_arm_joint4"}, \ - {"control":"/sciurus17/controller2/joints","joint":"l_arm_joint5"}, \ - {"control":"/sciurus17/controller2/joints","joint":"l_arm_joint6"}, \ - {"control":"/sciurus17/controller2/joints","joint":"l_arm_joint7"}, \ - {"control":"/sciurus17/controller3/joints","joint":"neck_pitch_joint"},\ - {"control":"/sciurus17/controller3/joints","joint":"neck_yaw_joint"}, \ - {"control":"/sciurus17/controller3/joints","joint":"waist_yaw_joint"}, \ - ] - ### プリセット定義 - 初期値 ### - self.preset_init = [] - for i in range(17): - self.preset_init.append( { "p_gain": 800, "i_gain": 0, "d_gain": 0 } ) - ### プリセット定義 - 1.Free ### - self.preset_1 = [ { "name":"r_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"neck_pitch_joint","p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"neck_yaw_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"waist_yaw_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - ] - ### プリセット定義 - 2 ### - self.preset_2 = [ { "name":"r_arm_joint1", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\ - { "name":"r_arm_joint2", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\ - { "name":"r_arm_joint3", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\ - { "name":"r_arm_joint4", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\ - { "name":"r_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint1", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\ - { "name":"l_arm_joint2", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\ - { "name":"l_arm_joint3", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\ - { "name":"l_arm_joint4", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\ - { "name":"l_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"neck_pitch_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"neck_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"waist_yaw_joint", "p_gain": 1500, "i_gain": 0, "d_gain": 0 },\ - ] - ### プリセット定義 - 3 ### - self.preset_3 = [ { "name":"r_arm_joint1", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\ - { "name":"r_arm_joint2", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\ - { "name":"r_arm_joint3", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint4", "p_gain": 800, "i_gain": 100, "d_gain": 0 },\ - { "name":"r_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint1", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\ - { "name":"l_arm_joint2", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint3", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\ - { "name":"l_arm_joint4", "p_gain": 800, "i_gain": 100, "d_gain": 0 },\ - { "name":"l_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"neck_pitch_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"neck_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"waist_yaw_joint", "p_gain": 1500, "i_gain": 0, "d_gain": 0 },\ - ] - ### プリセット定義 - 右手脱力 ### - self.preset_free_right_arm = [ - { "name":"r_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint1", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint2", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint3", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint4", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"neck_pitch_joint","p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"neck_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"waist_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - ] - ### プリセット定義 - 左手脱力 ### - self.preset_free_left_arm = [ - { "name":"r_arm_joint1", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint2", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint3", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint4", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"neck_pitch_joint","p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"neck_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"waist_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - ] - ### プリセット定義 - 両手脱力 ### - self.preset_free_two_arms = [ - { "name":"r_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"r_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"l_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\ - { "name":"neck_pitch_joint","p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"neck_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - { "name":"waist_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\ - ] - ### プリセットデータリスト ### - self.preset_list = [] - self.preset_list.append( self.preset_init ) - self.preset_list.append( self.preset_1 ) - self.preset_list.append( self.preset_2 ) - self.preset_list.append( self.preset_3 ) - self.preset_list.append( self.preset_free_right_arm) - self.preset_list.append( self.preset_free_left_arm) - self.preset_list.append( self.preset_free_two_arms) - - self.reconfigure = [] - for joint in self.joint_list: - self.reconfigure.append( {"client":dynamic_reconfigure.client.Client( joint["control"]+"/"+joint["joint"],timeout=10 ), \ - "joint:":joint["joint"]} ) - rospy.loginfo("Wait sub...") - self.subscribe = rospy.Subscriber("preset_gain_no", UInt8, self.preset_no_callback) - rospy.loginfo("Init finished.") - - def preset_no_callback(self, no): - joint_no = 0 - for conf in self.reconfigure: - conf["client"].update_configuration( {"position_p_gain":self.preset_list[no.data][joint_no]["p_gain"],"position_i_gain":self.preset_list[no.data][joint_no]["i_gain"],"position_d_gain":self.preset_list[no.data][joint_no]["d_gain"]} ) - joint_no = joint_no + 1 - -if __name__ == '__main__': - rospy.init_node('preset_reconfigure') - pr = PRESET_RECONFIGURE() - rospy.spin() diff --git a/sciurus17_control/src/control_setting.cpp b/sciurus17_control/src/control_setting.cpp deleted file mode 100644 index 171c655..0000000 --- a/sciurus17_control/src/control_setting.cpp +++ /dev/null @@ -1,168 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -CONTROL_SETTING::CONTROL_SETTING( ros::NodeHandle handle ) - : node_handle(handle) -{ - joint_num = 0; - port_name = ""; - joint_list.clear(); -} - -CONTROL_SETTING::~CONTROL_SETTING() -{ - /* Nothing todo... */ -} - -bool CONTROL_SETTING::load( void ) -{ - bool result; - - /* Loading tty port name */ - std::string device_name; - uint32_t baudrate; - - /* Loading tty port name */ - result = true; - device_name = loadPortName(); - if( device_name.length() > 0 ){ - port_name = device_name; - }else{ - return false; - } - /* Loading tty baudrate */ - baudrate = loadBaudRate(); - if( baudrate > 0 ){ - port_baud_rate = baudrate; - }else{ - result = false; - } - /* Loading joints list */ - if( loadJointList() ){ - loadJointParam(); - }else{ - result = false; - } - - return result; -} - -std::string CONTROL_SETTING::loadPortName( void ) -{ - std::string key_port_name = KEY_DXL_PORT; - std::string result; - - key_port_name += KEY_PORTNAME; - if( !node_handle.getParam( key_port_name, result ) ){ - ROS_ERROR("Undefined key %s.", key_port_name.c_str()); - result = ""; - } - - return result; -} - -uint32_t CONTROL_SETTING::loadBaudRate( void ) -{ - std::string key_baud_rate = KEY_DXL_PORT; - key_baud_rate += KEY_BAUDRATE; - int result; - - if( !node_handle.getParam( key_baud_rate, result ) ){ - ROS_ERROR("Undefined key %s.", key_baud_rate.c_str()); - result = 0; - } - - return (uint32_t)result; -} - -bool CONTROL_SETTING::loadJointList( void ) -{ - std::string key_joint_list = KEY_DXL_PORT; - bool result = false; - XmlRpc::XmlRpcValue load_joints; - - key_joint_list += KEY_JOINTS; - if( !node_handle.getParam( key_joint_list, load_joints ) ){ - ROS_ERROR("Undefined key %s.", key_joint_list.c_str()); - }else{ - if( load_joints.getType() != XmlRpc::XmlRpcValue::TypeArray ){ - ROS_ERROR("XmlRpc get type error! line%d", __LINE__); - }else{ - for( int32_t i = 0; i < load_joints.size(); ++i ){ - if( load_joints[i].getType() != XmlRpc::XmlRpcValue::TypeString ){ - ROS_ERROR("XmlRpc get type[%d] error! line%d", i, __LINE__); - }else{ - XmlRpc::XmlRpcValue &joint_name_work = load_joints[i]; - ST_SERVO_PARAM work; - work.name = (std::string)joint_name_work; - joint_list.push_back( work ); - } - } - joint_num = joint_list.size(); - result = true; - } - } - return result; -} - -bool CONTROL_SETTING::loadJointParam( void ) -{ - std::string key_joint_param = std::string(KEY_DXL_PORT) + "/"; - XmlRpc::XmlRpcValue load_joint_param; - int load_id = 0; - int load_center = DEFAULT_CENTER; - int load_home = DEFAULT_CENTER; - double load_eff_cnst = DEFAULT_EFF_CNST; - int load_mode = DEFAULT_OPE_MODE; - bool load_result; - bool result = true; - - for( int jj=0 ; jj -#include -#include -#include -#include -#include - -/* MACRO */ -#define DXLPOS2RAD(pos) (( ((pos)*(360.0/POSITION_STEP) )/360) *2*M_PI) -#define RAD2DXLPOS(rad) (( ((rad)/2.0/M_PI)*360.0 ) * (POSITION_STEP / 360.0)) -#define INIT_EFFCNST_UNIT(c) ((c)*0.001) - -//#define MASTER_WAIST_SLAVE_NECK - -DXLPORT_CONTROL::DXLPORT_CONTROL( ros::NodeHandle handle, CONTROL_SETTING &setting ) -{ - int jj; - joint_limits_interface::JointLimits limits; - joint_limits_interface::SoftJointLimits soft_limits; - int position_mode_joint_num = 0; - int current_mode_joint_num = 0; - int current_position_mode_joint_num = 0; - - init_stat = false; - tx_err = rx_err = 0; - tempCount = 0; - tempTime = getTime(); - - portHandler = NULL; - writeGoalGroup = NULL; - writePosGoalGroup= NULL; - readTempGroup = NULL; - readIndirectGroup= NULL; - - joint_num = setting.getjointNum(); - std::vector list = setting.getServoParam(); - for( jj=0 ; jjaddParam( dxl_id, ADDR_GOAL_CURRENT, LEN_GOAL_CURRENT, joints[jj].get_dxl_goal_addr() ) ){// [TODO] - error_queue.push("Bulk current write setting failed."); - return; - } - }else if( joints[jj].get_ope_mode() == OPERATING_MODE_CURR_POS ){ - //CURRENT base POSITION MODE - if( !writePosGoalGroup->addParam( dxl_id, ADDR_GOAL_POSITION, LEN_GOAL_POSITION, joints[jj].get_dxl_goal_addr() ) ){// [TODO] - error_queue.push("Bulk pos write setting failed."); - return; - } - }else{ - //POSITION MODE - if( !writeGoalGroup->addParam( dxl_id, ADDR_GOAL_POSITION, LEN_GOAL_POSITION, joints[jj].get_dxl_goal_addr() ) ){// [TODO] - error_queue.push("Bulk pos write setting failed."); - return; - } - } - if( !readTempGroup->addParam( dxl_id, ADDR_PRESENT_TEMP, LEN_PRESENT_TEMP ) ){ - error_queue.push("Bulk temp read setting failed."); - return; - } - if( !readIndirectGroup->addParam( dxl_id, DATA_INDIRECT_TOP, LEN_INDIRECT_GROUP ) ){ - error_queue.push("Bulk group read setting failed."); - return; - } - } - - // Open port - lock_port(); - if( !portHandler->openPort() ){ - error_queue.push("Port open failed."); - port_stat = false; - }else{ - // Set port baudrate - if( !portHandler->setBaudRate( setting.getBaudrate() ) ){ - error_queue.push("Setup baudrate failed."); - port_stat = false; - }else{ - port_stat = true; // 有効と仮定してread - for( jj=0 ; jj 0 ){ - registerInterface( &joint_pos_if ); - } - if( current_mode_joint_num > 0 ){ - registerInterface( &joint_eff_if ); - } - - init_stat = true; -} - -DXLPORT_CONTROL::~DXLPORT_CONTROL() -{ - portHandler->closePort(); - delete( portHandler ); - delete( dev_mtx ); - if(readTempGroup!=NULL) delete( readTempGroup ); - if(writeGoalGroup!=NULL) delete( writeGoalGroup ); - if(writePosGoalGroup!=NULL) delete( writePosGoalGroup ); - if(readIndirectGroup!=NULL) delete( readIndirectGroup ); -} - -bool DXLPORT_CONTROL::read( ros::Time time, ros::Duration period ) -{ - bool result = false; - - if( !port_stat ){ - return true; - } - - int dxl_comm_result = readIndirectGroup->txRxPacket(); - if (dxl_comm_result != COMM_SUCCESS){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++rx_err; - }else{ - if( readId(time, period) && readCurrent( time, period ) && readVel( time, period ) && readPos( time, period ) && readTemp( time, period ) ){ - result = true; - } - } - - return result; -} - -bool DXLPORT_CONTROL::readId( ros::Time time, ros::Duration period ) -{ - bool result = false; - - for( int jj=0 ; jjisAvailable( dxl_id, ADDR_INDIRECT_DXLID, LEN_INDIRECT_DXLID ); - if( !dxl_getdata_result ){ - ++rx_err; - ROS_INFO("readId error [%d]",dxl_id); - break; - }else{ - get_id = readIndirectGroup->getData( dxl_id, ADDR_INDIRECT_DXLID, LEN_INDIRECT_DXLID ); - if( get_id == dxl_id ){ - result = true; - }else{ - break; - } - } - } - return result; -} - -bool DXLPORT_CONTROL::readPos( ros::Time time, ros::Duration period ) -{ - bool result = false; - - for( int jj=0 ; jjisAvailable( dxl_id, ADDR_INDIRECT_POSITION, LEN_PRESENT_POSITION ); - if( !dxl_getdata_result ){ - ++rx_err; - break; - }else{ - present_pos = readIndirectGroup->getData( dxl_id, ADDR_INDIRECT_POSITION, LEN_PRESENT_POSITION ); - joints[jj].set_dxl_pos( present_pos ); - present_pos = (present_pos - joints[jj].get_center()); - joints[jj].set_position( DXLPOS2RAD( present_pos ) ); - result = true; - } - } - return result; -} - -bool DXLPORT_CONTROL::readCurrent( ros::Time time, ros::Duration period ) -{ - bool result = false; - - for( int jj=0 ; jjisAvailable( dxl_id, ADDR_INDIRECT_CURRENT, LEN_PRESENT_CURRENT ); - if( !dxl_getdata_result ){ - ++rx_err; - break; - }else{ - present_current = readIndirectGroup->getData( dxl_id, ADDR_INDIRECT_CURRENT, LEN_PRESENT_CURRENT ); - joints[jj].set_dxl_curr( present_current ); - joints[jj].set_current( (DXL_CURRENT_UNIT * present_current) ); - joints[jj].set_effort( DXL_CURRENT2EFFORT( present_current, joints[jj].get_eff_const() ) ); - result = true; - } - } - return result; -} - -bool DXLPORT_CONTROL::readTemp( ros::Time time, ros::Duration period ) -{ - bool result = false; - - for( int jj=0 ; jjisAvailable( dxl_id, ADDR_INDIRECT_TEMP, LEN_PRESENT_TEMP ); - if( !dxl_getdata_result ){ - ++rx_err; - break; - }else{ - uint8_t present_temp = readIndirectGroup->getData( dxl_id, ADDR_INDIRECT_TEMP, LEN_PRESENT_TEMP ); - joints[jj].set_dxl_temp( present_temp ); - joints[jj].set_temprature( present_temp ); - result = true; - } - } - ++tempCount; - return result; -} - -bool DXLPORT_CONTROL::readVel( ros::Time time, ros::Duration period ) -{ - bool result = false; - - for( int jj=0 ; jjisAvailable( dxl_id, ADDR_INDIRECT_VELOCITY, LEN_PRESENT_VEL ); - if( !dxl_getdata_result ){ - ++rx_err; - break; - }else{ - int32_t present_velocity = readIndirectGroup->getData( dxl_id, ADDR_INDIRECT_VELOCITY, LEN_PRESENT_VEL ); - joints[jj].set_velocity( DXL_VELOCITY2RAD_S(present_velocity) ); - result = true; - } - } - return result; -} - -void DXLPORT_CONTROL::write( ros::Time time, ros::Duration period ) -{ - double get_cmd; - float* tau; - - if( !port_stat){ - for( int jj=0 ; jj>8); - - writeGoalGroup->changeParam( joints[jj].get_dxl_id(), ADDR_GOAL_CURRENT, LEN_GOAL_CURRENT, goal_data ); - }else if(joints[jj].get_ope_mode() == OPERATING_MODE_CURR_POS ){ - // Current update - uint16_t dxl_cur = (uint32_t)round( tau[jj] ); - uint8_t* goal_data = joints[jj].get_dxl_goal_addr(); - goal_data[0] = (uint8_t)(dxl_cur&0x000000FF); - goal_data[1] = (uint8_t)((dxl_cur&0x0000FF00)>>8); - writeGoalGroup->changeParam( joints[jj].get_dxl_id(), ADDR_GOAL_CURRENT, LEN_GOAL_CURRENT, goal_data ); - }else{ - // Position control - double work_pos = RAD2DXLPOS( get_cmd ); - joints[jj].updt_d_command( get_cmd ); - work_pos += joints[jj].get_center(); // ROS(-180 <=> +180) => DXL(0 <=> 4095) - if( work_pos < DXL_MIN_LIMIT ){ - work_pos = DXL_MIN_LIMIT; - } - if( work_pos > DXL_MAX_LIMIT ){ - work_pos = DXL_MAX_LIMIT; - } - - uint32_t dxl_pos = (uint32_t)round( work_pos ); - uint8_t* goal_data = joints[jj].get_dxl_goal_addr(); - - goal_data[0] = (uint8_t)(dxl_pos&0x000000FF); - goal_data[1] = (uint8_t)((dxl_pos&0x0000FF00)>>8); - goal_data[2] = (uint8_t)((dxl_pos&0x00FF0000)>>16); - goal_data[3] = (uint8_t)((dxl_pos&0xFF000000)>>24); - - writeGoalGroup->changeParam( joints[jj].get_dxl_id(), ADDR_GOAL_POSITION, LEN_GOAL_POSITION, goal_data ); - } - } - int dxl_comm_result = writeGoalGroup->txPacket(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - } -} - -bool DXLPORT_CONTROL::is_change_positions( void ) -{ - bool result = false; - - for( int jj=0 ; jj 0.0 ){ - result = true; - break; - } - } - return result; -} - -void DXLPORT_CONTROL::set_gain_all( uint16_t gain ) -{ - if( !port_stat ){ - return; - } - for( int jj=0 ; jjwrite2ByteTxRx( portHandler, dxl_id, ADDR_POSITION_PGAIN, gain, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } -} - -void DXLPORT_CONTROL::set_goal_current_all( uint16_t current ) -{ - if( !port_stat ){ - return; - } - for( int jj=0 ; jjwrite2ByteTxRx( portHandler, dxl_id, ADDR_GOAL_CURRENT, current, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } -} - -bool DXLPORT_CONTROL::set_torque( uint8_t dxl_id, bool torque ) -{ - uint32_t set_param = torque ? TORQUE_ENABLE:TORQUE_DISABLE; - bool result = false; - - if( !port_stat ){ - return true; - } - - uint8_t dxl_error = 0; // Dynamixel error - lock_port(); - int dxl_comm_result = packetHandler->write1ByteTxRx( portHandler, dxl_id, ADDR_TORQUE_ENABLE, set_param, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - }else{ - result = true; - } - return result; -} -void DXLPORT_CONTROL::set_torque_all( bool torque ) -{ - for( uint8_t jj=0 ; jjwrite1ByteTxRx( portHandler, dxl_id, ADDR_BUS_WATCHDOG, value, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } -} - -void DXLPORT_CONTROL::set_watchdog_all( uint8_t value ) -{ - for( uint8_t jj=0 ; jj home_motion_data; - - /* 開始位置取り込みと差分計算 */ - - lock_port(); - read( t, d ); - unlock_port(); - - for( int jj=0 ; jj motion_work.start) ? ((motion_work.home_rad - motion_work.start_rad)/step_max) - : -((motion_work.start_rad - motion_work.home_rad)/step_max); - if( joints[jj].get_ope_mode() == OPERATING_MODE_CURRENT ){ - joints[jj].set_command( 0.0 ); - }else{ - joints[jj].set_command( joints[jj].get_position() ); - } - home_motion_data.push_back( motion_work ); - } - lock_port(); - write( t, d ); - unlock_port(); - - set_torque_all( true ); // 全関節トルクON - set_gain_all( DXL_DEFAULT_PGAIN ); - - /* ホームポジションへ移動する */ - for( int step=0 ; stepread1ByteTxRx(portHandler, dxl_id, test_addr, &read_data, &dxl_error); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++rx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++rx_err; - } - if( read_data == equal ){ - result = true; - } - read_val = read_data; - return result; -} -bool DXLPORT_CONTROL::check_servo_param( uint8_t dxl_id, uint32_t test_addr, uint16_t equal, uint16_t& read_val ) -{ - uint8_t dxl_error = 0; // Dynamixel error - uint16_t read_data; - bool result = false; - - lock_port(); - int dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, dxl_id, test_addr, &read_data, &dxl_error); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++rx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++rx_err; - } - if( read_data == equal ){ - result = true; - } - read_val = read_data; - return result; -} -bool DXLPORT_CONTROL::check_servo_param( uint8_t dxl_id, uint32_t test_addr, uint32_t equal, uint32_t& read_val ) -{ - uint8_t dxl_error = 0; // Dynamixel error - uint32_t read_data; - bool result = false; - - lock_port(); - int dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, dxl_id, test_addr, &read_data, &dxl_error); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++rx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++rx_err; - } - if( read_data == equal ){ - result = true; - } - read_val = read_data; - return result; -} - -void DXLPORT_CONTROL::init_joint_params( ST_JOINT_PARAM ¶m, int table_id, int value ) -{ - switch( table_id ){ - case enTableId_ReturnDelay: - param.return_delay_time = (uint8_t)value; - break; - case enTableId_DriveMode: - param.drive_mode = (uint8_t)value; - break; - case enTableId_OpeMode: - param.operation_mode = (uint8_t)value; - break; - case enTableId_HomingOffset: - param.homing_offset = (int32_t)value; - break; - case enTableId_MovingThreshold: - param.moving_threshold = (uint16_t)value; - break; - case enTableId_TempLimit: - param.temprature_limit = (uint8_t)value; - break; - case enTableId_MaxVolLimit: - param.max_vol_limit = (uint8_t)value; - break; - case enTableId_MinVolLimit: - param.min_vol_limit = (uint8_t)value; - break; - case enTableId_CurrentLimit: - param.current_limit = (uint16_t)value; - break; - case enTableId_TorqueEnable: - param.torque_enable = (uint8_t)value; - break; - case enTableId_VelocityIGain: - param.velocity_i_gain = (uint16_t)value; - break; - case enTableId_VelocityPGain: - param.velocity_p_gain = (uint16_t)value; - break; - case enTableId_PositionDGain: - param.position_d_gain = (uint16_t)value; - break; - case enTableId_PositionIGain: - param.position_i_gain = (uint16_t)value; - break; - case enTableId_PositionPGain: - param.position_p_gain = (uint16_t)value; - break; - case enTableId_GoalCurrent: - case enTableId_GoalVelocity: - case enTableId_GoalPosition: - case enTableId_PresentCurrent: - case enTableId_PresentVelocity: - case enTableId_PresentPosition: - case enTableId_PresentTemp: - case enTableId_Shutdown: - default: - break; - } -} - -std::string DXLPORT_CONTROL::self_check( void ) -{ - std::string res_str = "[DYNAMIXEL PARAMETER SELF CHECK]\n"; - - if( !port_stat ){ - res_str = "SKIP SELF CHECK..."; - return res_str; - } - for( int ii=0 ; ii<(sizeof(RegTable)/sizeof(ST_DYNAMIXEL_REG_TABLE)) ; ++ii ){ - bool check_result = true; - bool read_result; - uint8_t chk_8data, read_8data; - uint16_t chk_16data, read_16data; - uint32_t chk_32data, read_32data; - if( RegTable[ii].selfcheck ){ - res_str += RegTable[ii].name + " test...\n"; - } - switch( RegTable[ii].length ){ - case REG_LENGTH_BYTE: - chk_8data = (uint8_t)RegTable[ii].init_value; - if( RegTable[ii].name == "OPERATION_MODE" ){ - chk_8data = joints[ii].get_ope_mode(); - } - for( int jj=0 ; jjwrite1ByteTxRx( portHandler, dxl_id, ADDR_RETURN_DELAY, set_param, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } - } - new_param.return_delay_time = set_param; - joints[jj].set_joint_param( new_param ); - break; - } - } -} -void DXLPORT_CONTROL::set_param_drive_mode( uint8_t dxl_id, int val ) -{ - uint8_t set_param = (uint8_t)val; - - if( !port_stat ){ - return; - } - for( int jj=0 ; jjwrite1ByteTxRx( portHandler, dxl_id, ADDR_DRIVE_MODE, set_param, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } - } - new_param.drive_mode = set_param; - joints[jj].set_joint_param( new_param ); - break; - } - } -} -void DXLPORT_CONTROL::set_param_ope_mode( uint8_t dxl_id, int val ) -{ - uint8_t set_param = (uint8_t)val; - - if( !port_stat ){ - return; - } - for( int jj=0 ; jjwrite1ByteTxRx( portHandler, dxl_id, ADDR_OPE_MODE, set_param, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } - } - new_param.operation_mode = set_param; - joints[jj].set_joint_param( new_param ); - break; - } - } -} -void DXLPORT_CONTROL::set_param_home_offset( uint8_t dxl_id, int val ) -{ - uint32_t set_param = (uint32_t)val; - - if( !port_stat ){ - return; - } - for( int jj=0 ; jjwrite4ByteTxRx( portHandler, dxl_id, ADDR_HOMING_OFFSET, set_param, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } - } - new_param.homing_offset = (int32_t)set_param; - joints[jj].set_joint_param( new_param ); - break; - } - } -} -void DXLPORT_CONTROL::set_param_moving_threshold( uint8_t dxl_id, int val ) -{ - uint32_t set_param = (uint32_t)val; - - if( !port_stat ){ - return; - } - for( int jj=0 ; jjwrite4ByteTxRx( portHandler, dxl_id, ADDR_MOVING_THRESHOLD, set_param, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } - } - new_param.moving_threshold = set_param; - joints[jj].set_joint_param( new_param ); - break; - } - } -} -void DXLPORT_CONTROL::set_param_temp_limit( uint8_t dxl_id, int val ) -{ - uint8_t set_param = (uint8_t)val; - - if( !port_stat ){ - return; - } - for( int jj=0 ; jjwrite1ByteTxRx( portHandler, dxl_id, ADDR_TEMPRATURE_LIMIT, set_param, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } - } - new_param.temprature_limit = set_param; - joints[jj].set_joint_param( new_param ); - break; - } - } -} -void DXLPORT_CONTROL::set_param_vol_limit( uint8_t dxl_id, int max, int min ) -{ - uint16_t set_max_param = (uint32_t)max; - uint16_t set_min_param = (uint32_t)min; - - if( !port_stat ){ - return; - } - for( int jj=0 ; jjwrite2ByteTxRx( portHandler, dxl_id, ADDR_MAX_VOL_LIMIT, set_max_param, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } - } - if( new_param.min_vol_limit != set_min_param ){ - uint8_t dxl_error = 0; // Dynamixel error - lock_port(); - int dxl_comm_result = packetHandler->write2ByteTxRx( portHandler, dxl_id, ADDR_MIN_VOL_LIMIT, set_min_param, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } - } - new_param.max_vol_limit = set_max_param; - new_param.min_vol_limit = set_min_param; - joints[jj].set_joint_param( new_param ); - break; - } - } -} -void DXLPORT_CONTROL::set_param_current_limit( uint8_t dxl_id, int val ) -{ - uint16_t set_param = (uint16_t)val; - - if( !port_stat ){ - return; - } - for( int jj=0 ; jjwrite2ByteTxRx( portHandler, dxl_id, ADDR_CURRENT_LIMIT, set_param, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } - } - new_param.current_limit = set_param; - joints[jj].set_joint_param( new_param ); - break; - } - } -} -void DXLPORT_CONTROL::set_param_vel_gain( uint8_t dxl_id, int p, int i ) -{ - uint16_t set_p_param = (uint16_t)p; - uint16_t set_i_param = (uint16_t)i; - - if( !port_stat ){ - return; - } - for( int jj=0 ; jjwrite2ByteTxRx( portHandler, dxl_id, ADDR_VELOCITY_PGAIN, set_p_param, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } - } - if( (new_param.velocity_i_gain != set_i_param) ){ - uint8_t dxl_error = 0; // Dynamixel error - lock_port(); - int dxl_comm_result = packetHandler->write2ByteTxRx( portHandler, dxl_id, ADDR_VELOCITY_IGAIN, set_i_param, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } - } - new_param.velocity_p_gain = set_p_param; - new_param.velocity_i_gain = set_i_param; - joints[jj].set_joint_param( new_param ); - break; - } - } -} - -void DXLPORT_CONTROL::set_param_pos_gain_all( int p, int i, int d ) -{ - if( !port_stat ){ - return; - } - for( int jj=0 ; jjwrite2ByteTxRx( portHandler, dxl_id, ADDR_POSITION_PGAIN, set_p_param, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } - } - if( new_param.position_i_gain != set_i_param ){ - uint8_t dxl_error = 0; // Dynamixel error - lock_port(); - int dxl_comm_result = packetHandler->write2ByteTxRx( portHandler, dxl_id, ADDR_POSITION_IGAIN, set_i_param, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } - } - if( new_param.position_d_gain != set_d_param ){ - uint8_t dxl_error = 0; // Dynamixel error - lock_port(); - int dxl_comm_result = packetHandler->write2ByteTxRx( portHandler, dxl_id, ADDR_POSITION_DGAIN, set_d_param, &dxl_error ); - unlock_port(); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } - } - new_param.position_p_gain = set_p_param; - new_param.position_i_gain = set_i_param; - new_param.position_d_gain = set_d_param; - joints[jj].set_joint_param( new_param ); - break; - } - } -} - -std::string::size_type DXLPORT_CONTROL::get_error( std::string& errorlog ) -{ - std::string::size_type result = error_queue.size(); - if( result > 0 ){ - errorlog = error_queue.front(); - error_queue.pop(); - } - return result; -} - -bool DXLPORT_CONTROL::setup_indirect( uint8_t dxl_id ) -{ - uint8_t dxl_error = 0; // Dynamixel error - uint16_t setup_data[] = { 224, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 146 }; - - // Write DXL ID data - int dxl_comm_result = packetHandler->write1ByteTxRx( portHandler, dxl_id, ADDR_INDIRECT_DXLID, dxl_id, &dxl_error ); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } - - for( int idx=0 ; idxwrite2ByteTxRx( portHandler, dxl_id, set_addr, setup_data[idx], &dxl_error ); - if( dxl_comm_result != COMM_SUCCESS ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getTxRxResult( dxl_comm_result ) ); - ++tx_err; - }else if( dxl_error != 0 ){ - error_queue.push( (std::string(__func__) + " ") + packetHandler->getRxPacketError( dxl_error ) ); - ++tx_err; - } - } - - return true; -} diff --git a/sciurus17_control/src/hardware.cpp b/sciurus17_control/src/hardware.cpp deleted file mode 100644 index 433721a..0000000 --- a/sciurus17_control/src/hardware.cpp +++ /dev/null @@ -1,317 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* ROS rate setting */ -#define REACTIVE_RATE_FUNCTION //指示姿勢に変化がない場合に制御周期を落としてCPU負荷を下げる機能 -#ifdef REACTIVE_RATE_FUNCTION -#define CONTROL_WAIT_HZ (50) -#define CONTROL_ACTIVE_HZ (200) -#else -#define CONTROL_HZ (200) -#endif - -static std_msgs::String lasterror_out; -static std::vector current_pub; -static std::vector dxl_position_pub; -static std::vector temp_pub; -static std::vector gain_sub; -typedef dynamic_reconfigure::Server RECONFIG_TYPE; -static std::vector> reconfig_srv; -static DXLPORT_CONTROL* driver_addr; - -typedef struct SET_GAIN_QUEUE -{ - SET_GAIN_QUEUE() - { - dxl_id = 0; - gain = 0; - } - SET_GAIN_QUEUE( const SET_GAIN_QUEUE &src ) - { - dxl_id = src.dxl_id; - gain = src.gain; - } - uint8_t dxl_id; - uint16_t gain; -} ST_SET_GAIN_QUEUE; -static std::queue set_gain_request; - -static std::queue set_joint_param_request; - -std::vector split(const std::string& input, char delimiter) -{ - std::istringstream stream(input); - - std::string field; - std::vector result; - while (std::getline(stream, field, delimiter)) { - result.push_back(field); - } - return result; -} - -void gainCallback(const ros::MessageEvent& event) -{ - const ros::M_string& header = event.getConnectionHeader(); - std::string topic = header.at("topic"); - const std_msgs::UInt16ConstPtr& msg = event.getMessage(); - uint16_t set_gain_data = msg->data; - if( set_gain_data >= DXL_PGAIN_MAX ){ - set_gain_data = DXL_PGAIN_MAX; - } - - std::vector topic_list = split(topic,'/'); - std::string joint_name = topic_list[ topic_list.size()-2 ];// Parent topic name - - for( std::vector::iterator it=driver_addr->joints.begin() ; it!=driver_addr->joints.end() ; ++it ){ - if( it->get_joint_name() == joint_name ){ - ST_SET_GAIN_QUEUE set_req_data; - set_req_data.dxl_id = it->get_dxl_id(); - set_req_data.gain = set_gain_data; - set_gain_request.push( set_req_data ); - break; - } - } -} -void reconfigureCallback(sciurus17_msgs::ServoParameterConfig &config, uint32_t level, uint8_t id ) -{ - ST_JOINT_PARAM set_req_data; - set_req_data.dxl_id = id; - set_req_data.return_delay_time = config.return_delay_time; - set_req_data.drive_mode = config.drive_mode; - set_req_data.operation_mode = config.operation_mode; - set_req_data.moving_threshold = config.moving_threshold; - set_req_data.homing_offset = config.homing_offset; - set_req_data.temprature_limit = config.temprature_limit; - set_req_data.max_vol_limit = config.max_vol_limit; - set_req_data.min_vol_limit = config.min_vol_limit; - set_req_data.current_limit = config.current_limit; - set_req_data.torque_enable = config.torque_enable?1:0; - set_req_data.velocity_i_gain = config.velocity_i_gain; - set_req_data.velocity_p_gain = config.velocity_p_gain; - set_req_data.position_d_gain = config.position_d_gain; - set_req_data.position_i_gain = config.position_i_gain; - set_req_data.position_p_gain = config.position_p_gain; - if( level != 0 ){ - for( std::vector::iterator it=driver_addr->joints.begin() ; it!=driver_addr->joints.end() ; ++it ){ - if( it->get_dxl_id() == id ){ - ST_JOINT_PARAM load_data = it->get_joint_param(); - config.return_delay_time = load_data.return_delay_time; - config.drive_mode = load_data.drive_mode; - config.operation_mode = load_data.operation_mode; - config.moving_threshold = load_data.moving_threshold; - config.homing_offset = load_data.homing_offset; - config.temprature_limit = load_data.temprature_limit; - config.max_vol_limit = load_data.max_vol_limit; - config.min_vol_limit = load_data.min_vol_limit; - config.current_limit = load_data.current_limit; - config.torque_enable = true; - config.velocity_i_gain = load_data.velocity_i_gain; - config.velocity_p_gain = load_data.velocity_p_gain; - config.position_d_gain = load_data.position_d_gain; - config.position_i_gain = load_data.position_i_gain; - config.position_p_gain = DXL_DEFAULT_PGAIN; - } - } - }else{ - set_joint_param_request.push( set_req_data ); - } -} - -void init_topics( DXLPORT_CONTROL *driver, ros::NodeHandle nh ) -{ - std::string current_name; - std::string dxl_pos_name; - std::string temp_name; - std::string gain_name; - std::string joint_name; - - for( std::vector::iterator it=driver->joints.begin() ; it!=driver->joints.end() ; ++it ){ - joint_name = it->get_joint_name(); - current_name = ( joint_name + "/current"); - current_pub.push_back( nh.advertise(current_name, 10) ); - dxl_pos_name = ( joint_name + "/dxl_position"); - dxl_position_pub.push_back( nh.advertise(dxl_pos_name, 10) ); - temp_name = ( joint_name + "/temp"); - temp_pub.push_back( nh.advertise(temp_name, 10) ); - gain_name = ( joint_name + "/gain"); - gain_sub.push_back( nh.subscribe(gain_name, 100, gainCallback) ); - } -} - -void init_reconfigure( DXLPORT_CONTROL *driver ) -{ - for( std::vector::iterator it=driver->joints.begin() ; it!=driver->joints.end() ; ++it ){ - RECONFIG_TYPE* server = new RECONFIG_TYPE( "~/"+it->get_joint_name() ); - dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(reconfigureCallback, _1, _2, it->get_dxl_id()); - server->setCallback(f); - reconfig_srv.push_back( std::unique_ptr(server) ); - } -} - -void publish_topic_data( DXLPORT_CONTROL *driver, bool temp_flg ) -{ - int joint_index; - std::vector::iterator current_it; - std::vector::iterator dxl_pos_it; - std::vector::iterator temp_it; - - std_msgs::Float64 current_out; - std_msgs::Int16 dxl_pos_out; - std_msgs::Float64 temp_out; - - current_it = current_pub.begin(); - dxl_pos_it = dxl_position_pub.begin(); - temp_it = temp_pub.begin(); - - for( std::vector::iterator it=driver->joints.begin() ; it!=driver->joints.end() ; ++it ){ - current_out.data = it->get_current(); - current_it->publish( current_out ); - ++current_it; - dxl_pos_out.data = it->get_dxl_pos(); - dxl_pos_it->publish( dxl_pos_out ); - ++dxl_pos_it; - if( temp_flg ){ - temp_out.data = it->get_temprature(); - temp_it->publish( temp_out ); - ++temp_it; - } - } -} - -void write_joint_param( DXLPORT_CONTROL& driver, ST_JOINT_PARAM set_param ) -{ - uint8_t dxl_id = set_param.dxl_id; - - driver.set_param_delay_time( dxl_id, set_param.return_delay_time ); - driver.set_param_drive_mode( dxl_id, set_param.drive_mode ); - driver.set_param_ope_mode( dxl_id, set_param.operation_mode ); - driver.set_param_home_offset( dxl_id, set_param.homing_offset ); - driver.set_param_moving_threshold( dxl_id, set_param.moving_threshold ); - driver.set_param_temp_limit( dxl_id, set_param.temprature_limit ); - driver.set_param_vol_limit( dxl_id, set_param.max_vol_limit, set_param.min_vol_limit ); - driver.set_param_current_limit( dxl_id, set_param.current_limit ); - driver.set_param_vel_gain( dxl_id, set_param.velocity_p_gain, set_param.velocity_i_gain ); - driver.set_param_pos_gain( dxl_id, set_param.position_p_gain, set_param.position_i_gain, set_param.position_d_gain ); - driver.set_torque( dxl_id, (set_param.torque_enable?true:false) ); -} - -void SigintHandler( int sig ) -{ - ros::shutdown(); -} - -int main( int argc, char* argv[] ) -{ - ros::init( argc, argv, ros::this_node::getName(), ros::init_options::NoSigintHandler ); - ros::NodeHandle nh; - ros::NodeHandle nhPrivate("~"); - signal(SIGINT, SigintHandler); - - CONTROL_SETTING setting( nhPrivate ); - if( !setting.load() ){ - return -1; - } - - DXLPORT_CONTROL sciurus17( nhPrivate, setting ); - if( !sciurus17.get_init_stat() ){ - std::string errorlog; - if( sciurus17.get_error( errorlog ) > 0 ){ - ROS_INFO("Initialize error (%s)",errorlog.c_str()); - } - return -1; - } - controller_manager::ControllerManager cm( &sciurus17, nh ); - driver_addr = &sciurus17; - - ros::Publisher lasterror_pub = nhPrivate.advertise("lasterror", 10); - init_topics( &sciurus17, nhPrivate ); - -#ifdef REACTIVE_RATE_FUNCTION - ros::Rate rate_wait( CONTROL_WAIT_HZ ); - ros::Rate rate_active( CONTROL_ACTIVE_HZ ); -#else - ros::Rate rate( CONTROL_HZ ); -#endif - ros::AsyncSpinner spinner(4); - spinner.start(); - - ros::Time t = sciurus17.getTime(); - ros::Duration d = sciurus17.getDuration(t); - uint32_t prev_tempCount = 0; - bool read_temp_flg = false; - - ROS_INFO( "%s", sciurus17.self_check().c_str() ); - init_reconfigure( &sciurus17 ); - -// sciurus17.set_watchdog_all( DXL_WATCHDOG_RESET_VALUE );//Currentモードで使用する - sciurus17.startup_motion(); -// sciurus17.set_watchdog_all( MSEC2DXL_WATCHDOG(1000) ); //1秒の間,無通信で停止 - - while( ros::ok() ){ - d = sciurus17.getDuration(t); - t = sciurus17.getTime(); - - if( sciurus17.tempCount != prev_tempCount ){ - read_temp_flg = true; - prev_tempCount = sciurus17.tempCount; - }else{ - read_temp_flg = false; - } - publish_topic_data( &sciurus17, read_temp_flg ); - - sciurus17.lock_port(); - if( sciurus17.read( t, d ) ){ - cm.update( t, d ); - sciurus17.write( t, d ); - } - sciurus17.unlock_port(); - - while( set_gain_request.size() > 0 ){ - ST_SET_GAIN_QUEUE gain_data = set_gain_request.front(); - sciurus17.set_gain( gain_data.dxl_id, gain_data.gain );//手間がかかるだけなのでparamとは連動しない - set_gain_request.pop(); - } - // Dynamixelとの通信タイムアウトを防ぐため、 - // write_joint_param()は1制御ループで1回のみ実行する - if( set_joint_param_request.size() > 0 ){ - write_joint_param( sciurus17, set_joint_param_request.front() ); - set_joint_param_request.pop(); - } - - std::string errorlog; - while( sciurus17.get_error( errorlog ) > 0 ){ // error log check - lasterror_out.data = errorlog; - lasterror_pub.publish( lasterror_out ); - } - -#ifdef REACTIVE_RATE_FUNCTION - if( sciurus17.is_change_positions() ){ - rate_active.sleep(); - }else{ - rate_wait.sleep(); - } -#else - rate.sleep(); -#endif - } - - sciurus17.set_param_pos_gain_all( DXL_FREE_PGAIN, DXL_FREE_IGAIN, DXL_FREE_DGAIN ); - sciurus17.set_goal_current_all( 0 ); - spinner.stop(); - - return 0; -} diff --git a/sciurus17_control/src/joint_control.cpp b/sciurus17_control/src/joint_control.cpp deleted file mode 100644 index 99efba7..0000000 --- a/sciurus17_control/src/joint_control.cpp +++ /dev/null @@ -1,78 +0,0 @@ -#include -#include -#include -#include - -void JOINT_CONTROL::init_parameter( std::string init_name, uint8_t init_dxlid, uint16_t init_center, uint16_t init_home, double init_eff_const, uint8_t init_mode ) -{ - name = init_name; - id = init_dxlid; - pos = 0.0; - vel = 0.0; - eff = 0.0; - curr = 0.0; - torque = false; - center = init_center; - home = init_home; - connect = false; - - goal_pos = 0.0; - goal_vel = 0.0; - goal_eff = 0.0; - - dxl_pos = 0; - dxl_curr = 0; - dxl_temp = 0; - for( int ii=0 ; ii Value: true - - Acceleration_Scaling_Factor: 1 + - Acceleration_Scaling_Factor: 0.1 Class: moveit_rviz_plugin/MotionPlanning Enabled: true Move Group Namespace: "" @@ -43,12 +43,10 @@ Visualization Manager: MoveIt_Allow_External_Program: false MoveIt_Allow_Replanning: false MoveIt_Allow_Sensor_Positioning: false - MoveIt_Goal_Tolerance: 0 MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 5 - MoveIt_Use_Constraint_Aware_IK: true - MoveIt_Warehouse_Host: 127.0.0.1 - MoveIt_Warehouse_Port: 33829 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false MoveIt_Workspace: Center: X: 0 @@ -78,12 +76,20 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - camera_link: + chest_camera_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - chest_camera_link: + dummy_mimic_fix_l: + Alpha: 1 + Show Axes: false + Show Trail: false + dummy_mimic_fix_r: + Alpha: 1 + Show Axes: false + Show Trail: false + head_camera_link: Alpha: 1 Show Axes: false Show Trail: false @@ -198,6 +204,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false Loop Animation: true Robot Alpha: 0.5 Robot Color: 150; 50; 150 @@ -206,19 +216,20 @@ Visualization Manager: Show Trail: false State Display Time: 0.05 s Trail Step Size: 1 - Trajectory Topic: move_group/display_planned_path + Trajectory Topic: display_planned_path + Use Sim Time: false Planning Metrics: Payload: 1 Show Joint Torques: false Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false - TextHeight: 0.0799999982 + TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 - Interactive Marker Size: 0 + Interactive Marker Size: 0.15000000596046448 Joint Violation Color: 255; 0; 255 Planning Group: two_arm_group Query Goal State: true @@ -226,12 +237,12 @@ Visualization Manager: Show Workspace: false Start State Alpha: 1 Start State Color: 0; 255; 0 - Planning Scene Topic: /move_group/monitored_planning_scene + Planning Scene Topic: monitored_planning_scene Robot Description: robot_description Scene Geometry: Scene Alpha: 1 Scene Color: 50; 230; 50 - Scene Display Time: 0.200000003 + Scene Display Time: 0.009999999776482582 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels @@ -253,12 +264,20 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - camera_link: + chest_camera_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - chest_camera_link: + dummy_mimic_fix_l: + Alpha: 1 + Show Axes: false + Show Trail: false + dummy_mimic_fix_r: + Alpha: 1 + Show Axes: false + Show Trail: false + head_camera_link: Alpha: 1 Show Axes: false Show Trail: false @@ -373,106 +392,68 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false Robot Alpha: 0.5 Show Robot Collision: false Show Robot Visual: true Value: true - Velocity_Scaling_Factor: 1 - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 1 - Size (m): 0.00999999978 - Style: Points - Topic: /sciurus17/camera/depth_registered/points - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /sciurus17/chest_camera_node/image - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true + Velocity_Scaling_Factor: 0.1 Enabled: true Global Options: Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: base_link + Fixed Frame: world Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/XYOrbit - Distance: 2.99650002 + Class: rviz_default_plugins/Orbit + Distance: 1.5 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.113567002 - Y: 0.105920002 - Z: 2.23518001e-07 + X: 0 + Y: 0 + Z: 0.30000001192092896 Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.360203385 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5 Target Frame: world - Value: XYOrbit (rviz) - Yaw: 3.24994731 + Value: Orbit (rviz_default_plugins) + Yaw: 0 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1056 + Height: 975 Help: collapsed: false Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false + Hide Right Dock: true MotionPlanning: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002b9000003dafc020000000bfb000000100044006900730070006c00610079007301000000280000011f000000d700fffffffb0000000a0049006d006100670065010000014d000001210000001600fffffffb0000000800480065006c00700000000342000000bb0000007300fffffffb0000000a0056006900650077007300000003b0000000b0000000ad00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000002740000018e0000018300fffffffb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004a00fffffffb0000000c00430061006d0065007200610000000321000000c50000000000000000fb0000000c00430061006d006500720061000000031b000000cb000000000000000000000480000003da00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000002000000000000024200000375fc0200000004fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000172000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b5000001fd0000017d00fffffffb0000000800480065006c0070000000029a0000006e0000006e00ffffff00000001000001c500000375fc0200000001fb0000000a00560069006500770073000000003d00000375000000a400ffffff000002680000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: - collapsed: false - Width: 1855 - X: 65 - Y: 24 + collapsed: true + Width: 1200 + X: 170 + Y: 27 diff --git a/sciurus17_moveit_config/config/sciurus17_controllers.yaml b/sciurus17_moveit_config/config/moveit_controllers.yaml similarity index 62% rename from sciurus17_moveit_config/config/sciurus17_controllers.yaml rename to sciurus17_moveit_config/config/moveit_controllers.yaml index 53674b2..d3a5111 100644 --- a/sciurus17_moveit_config/config/sciurus17_controllers.yaml +++ b/sciurus17_moveit_config/config/moveit_controllers.yaml @@ -1,18 +1,17 @@ -controller_manager_ns: / -controller_list: - - name: /sciurus17/controller2/left_arm_controller - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - default: true - joints: - - l_arm_joint1 - - l_arm_joint2 - - l_arm_joint3 - - l_arm_joint4 - - l_arm_joint5 - - l_arm_joint6 - - l_arm_joint7 - - name: /sciurus17/controller1/right_arm_controller +# MoveIt uses this configuration for controller management + +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - right_arm_controller + - right_hand_controller + - left_arm_controller + - left_hand_controller + - neck_controller + - waist_yaw_controller + + right_arm_controller: action_ns: follow_joint_trajectory type: FollowJointTrajectory default: true @@ -24,28 +23,45 @@ controller_list: - r_arm_joint5 - r_arm_joint6 - r_arm_joint7 - - name: /sciurus17/controller2/left_hand_controller + + right_hand_controller: action_ns: gripper_cmd type: GripperCommand default: true joints: - - l_hand_joint - - name: /sciurus17/controller1/right_hand_controller + - r_hand_joint + + left_arm_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - l_arm_joint1 + - l_arm_joint2 + - l_arm_joint3 + - l_arm_joint4 + - l_arm_joint5 + - l_arm_joint6 + - l_arm_joint7 + + left_hand_controller: action_ns: gripper_cmd type: GripperCommand default: true joints: - - r_hand_joint - - name: /sciurus17/controller3/waist_yaw_controller + - l_hand_joint + + neck_controller: action_ns: follow_joint_trajectory type: FollowJointTrajectory default: true joints: - - waist_yaw_joint - - name: /sciurus17/controller3/neck_controller + - neck_yaw_joint + - neck_pitch_joint + + waist_yaw_controller: action_ns: follow_joint_trajectory type: FollowJointTrajectory default: true joints: - - neck_yaw_joint - - neck_pitch_joint \ No newline at end of file + - waist_yaw_joint \ No newline at end of file diff --git a/sciurus17_moveit_config/config/ompl_planning.yaml b/sciurus17_moveit_config/config/ompl_planning.yaml deleted file mode 100644 index 28c5905..0000000 --- a/sciurus17_moveit_config/config/ompl_planning.yaml +++ /dev/null @@ -1,311 +0,0 @@ -planner_configs: - SBLkConfigDefault: - type: geometric::SBL - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - ESTkConfigDefault: - type: geometric::EST - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - LBKPIECEkConfigDefault: - type: geometric::LBKPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - BKPIECEkConfigDefault: - type: geometric::BKPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 - failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - KPIECEkConfigDefault: - type: geometric::KPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] - failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - RRTkConfigDefault: - type: geometric::RRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - RRTConnectkConfigDefault: - type: geometric::RRTConnect - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - RRTstarkConfigDefault: - type: geometric::RRTstar - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 - TRRTkConfigDefault: - type: geometric::TRRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - max_states_failed: 10 # when to start increasing temp. default: 10 - temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 - min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 - init_temperature: 10e-6 # initial temperature. default: 10e-6 - frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() - frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 - k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() - PRMkConfigDefault: - type: geometric::PRM - max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 - PRMstarkConfigDefault: - type: geometric::PRMstar - FMTkConfigDefault: - type: geometric::FMT - num_samples: 1000 # number of states that the planner should sample. default: 1000 - radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 - nearest_k: 1 # use Knearest strategy. default: 1 - cache_cc: 1 # use collision checking cache. default: 1 - heuristics: 0 # activate cost to go heuristics. default: 0 - extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 - BFMTkConfigDefault: - type: geometric::BFMT - num_samples: 1000 # number of states that the planner should sample. default: 1000 - radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 - nearest_k: 1 # use the Knearest strategy. default: 1 - balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 - optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 - heuristics: 1 # activates cost to go heuristics. default: 1 - cache_cc: 1 # use the collision checking cache. default: 1 - extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 - PDSTkConfigDefault: - type: geometric::PDST - STRIDEkConfigDefault: - type: geometric::STRIDE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 - degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 - max_degree: 18 # max degree of a node in the GNAT. default: 12 - min_degree: 12 # min degree of a node in the GNAT. default: 12 - max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 - estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 - min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 - BiTRRTkConfigDefault: - type: geometric::BiTRRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 - init_temperature: 100 # initial temperature. default: 100 - frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() - frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 - cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf - LBTRRTkConfigDefault: - type: geometric::LBTRRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - epsilon: 0.4 # optimality approximation factor. default: 0.4 - BiESTkConfigDefault: - type: geometric::BiEST - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - ProjESTkConfigDefault: - type: geometric::ProjEST - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - LazyPRMkConfigDefault: - type: geometric::LazyPRM - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - LazyPRMstarkConfigDefault: - type: geometric::LazyPRMstar - SPARSkConfigDefault: - type: geometric::SPARS - stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 - sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 - dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 - max_failures: 1000 # maximum consecutive failure limit. default: 1000 - SPARStwokConfigDefault: - type: geometric::SPARStwo - stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 - sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 - dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 - max_failures: 5000 # maximum consecutive failure limit. default: 5000 -r_arm_group: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault - - FMTkConfigDefault - - BFMTkConfigDefault - - PDSTkConfigDefault - - STRIDEkConfigDefault - - BiTRRTkConfigDefault - - LBTRRTkConfigDefault - - BiESTkConfigDefault - - ProjESTkConfigDefault - - LazyPRMkConfigDefault - - LazyPRMstarkConfigDefault - - SPARSkConfigDefault - - SPARStwokConfigDefault - projection_evaluator: joints(r_arm_joint1,r_arm_joint2) - longest_valid_segment_fraction: 0.005 -l_arm_group: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault - - FMTkConfigDefault - - BFMTkConfigDefault - - PDSTkConfigDefault - - STRIDEkConfigDefault - - BiTRRTkConfigDefault - - LBTRRTkConfigDefault - - BiESTkConfigDefault - - ProjESTkConfigDefault - - LazyPRMkConfigDefault - - LazyPRMstarkConfigDefault - - SPARSkConfigDefault - - SPARStwokConfigDefault - projection_evaluator: joints(l_arm_joint1,l_arm_joint2) - longest_valid_segment_fraction: 0.005 -l_hand_group: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault - - FMTkConfigDefault - - BFMTkConfigDefault - - PDSTkConfigDefault - - STRIDEkConfigDefault - - BiTRRTkConfigDefault - - LBTRRTkConfigDefault - - BiESTkConfigDefault - - ProjESTkConfigDefault - - LazyPRMkConfigDefault - - LazyPRMstarkConfigDefault - - SPARSkConfigDefault - - SPARStwokConfigDefault - projection_evaluator: joints(l_hand_joint,l_hand_mimic_joint) - longest_valid_segment_fraction: 0.005 -r_hand_group: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault - - FMTkConfigDefault - - BFMTkConfigDefault - - PDSTkConfigDefault - - STRIDEkConfigDefault - - BiTRRTkConfigDefault - - LBTRRTkConfigDefault - - BiESTkConfigDefault - - ProjESTkConfigDefault - - LazyPRMkConfigDefault - - LazyPRMstarkConfigDefault - - SPARSkConfigDefault - - SPARStwokConfigDefault - projection_evaluator: joints(r_hand_joint,r_hand_mimic_joint) - longest_valid_segment_fraction: 0.005 -r_arm_waist_group: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault - - FMTkConfigDefault - - BFMTkConfigDefault - - PDSTkConfigDefault - - STRIDEkConfigDefault - - BiTRRTkConfigDefault - - LBTRRTkConfigDefault - - BiESTkConfigDefault - - ProjESTkConfigDefault - - LazyPRMkConfigDefault - - LazyPRMstarkConfigDefault - - SPARSkConfigDefault - - SPARStwokConfigDefault - projection_evaluator: joints(waist_yaw_joint,r_arm_joint1) - longest_valid_segment_fraction: 0.005 -l_arm_waist_group: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault - - FMTkConfigDefault - - BFMTkConfigDefault - - PDSTkConfigDefault - - STRIDEkConfigDefault - - BiTRRTkConfigDefault - - LBTRRTkConfigDefault - - BiESTkConfigDefault - - ProjESTkConfigDefault - - LazyPRMkConfigDefault - - LazyPRMstarkConfigDefault - - SPARSkConfigDefault - - SPARStwokConfigDefault - projection_evaluator: joints(waist_yaw_joint,l_arm_joint1) - longest_valid_segment_fraction: 0.005 -two_arm_group: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault - - FMTkConfigDefault - - BFMTkConfigDefault - - PDSTkConfigDefault - - STRIDEkConfigDefault - - BiTRRTkConfigDefault - - LBTRRTkConfigDefault - - BiESTkConfigDefault - - ProjESTkConfigDefault - - LazyPRMkConfigDefault - - LazyPRMstarkConfigDefault - - SPARSkConfigDefault - - SPARStwokConfigDefault - projection_evaluator: joints(l_arm_joint1,l_arm_joint2) - longest_valid_segment_fraction: 0.005 diff --git a/sciurus17_moveit_config/config/pilz_cartesian_limits.yaml b/sciurus17_moveit_config/config/pilz_cartesian_limits.yaml new file mode 100644 index 0000000..b2997ca --- /dev/null +++ b/sciurus17_moveit_config/config/pilz_cartesian_limits.yaml @@ -0,0 +1,6 @@ +# Limits for the Pilz planner +cartesian_limits: + max_trans_vel: 1.0 + max_trans_acc: 2.25 + max_trans_dec: -5.0 + max_rot_vel: 1.57 diff --git a/sciurus17_moveit_config/config/ros_controllers.yaml b/sciurus17_moveit_config/config/ros_controllers.yaml deleted file mode 100644 index 43e4e23..0000000 --- a/sciurus17_moveit_config/config/ros_controllers.yaml +++ /dev/null @@ -1,39 +0,0 @@ -sciurus17: -# MoveIt-specific simulation settings - moveit_sim_hw_interface: - joint_model_group: controllers_initial_group_ - joint_model_group_pose: controllers_initial_pose_ -# Settings for ros_control control loop - generic_hw_control_loop: - loop_hz: 300 - cycle_time_error_threshold: 0.01 -# Settings for ros_control hardware interface - hardware_interface: - joints: - - waist_yaw_joint - - l_arm_joint1 - - l_arm_joint2 - - l_arm_joint3 - - l_arm_joint4 - - l_arm_joint5 - - l_arm_joint6 - - l_arm_joint7 - - l_hand_joint - - neck_yaw_joint - - neck_pitch_joint - - r_arm_joint1 - - r_arm_joint2 - - r_arm_joint3 - - r_arm_joint4 - - r_arm_joint5 - - r_arm_joint6 - - r_arm_joint7 - - r_hand_joint - sim_control_mode: 1 # 0: position, 1: velocity -# Publish all joint states -# Creates the /joint_states topic necessary in ROS - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 - controller_list: - [] \ No newline at end of file diff --git a/sciurus17_moveit_config/config/sciurus17.srdf b/sciurus17_moveit_config/config/sciurus17.srdf index fa5baf5..e865716 100644 --- a/sciurus17_moveit_config/config/sciurus17.srdf +++ b/sciurus17_moveit_config/config/sciurus17.srdf @@ -1,4 +1,4 @@ - + - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + - - - - + + + + + + + + + + + - - - - - - - - - + + + + + + + + - - - - - - - - - + + + + + + + + + + + + - - + + - - - - - - - + + + + + + + - - - - - - - + + + + + + + - - - - - - - - + + + + + + + + - - - - - - - - + + + + + + + + - - - - - - - - - - - - - - + + + + + + + + + + + + + + - - + + + - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sciurus17_moveit_config/config/sensors_3d.yaml b/sciurus17_moveit_config/config/sensors_3d.yaml deleted file mode 100644 index d2955dc..0000000 --- a/sciurus17_moveit_config/config/sensors_3d.yaml +++ /dev/null @@ -1,3 +0,0 @@ -# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it -sensors: - - {} \ No newline at end of file diff --git a/sciurus17_moveit_config/launch/chomp_planning_pipeline.launch.xml b/sciurus17_moveit_config/launch/chomp_planning_pipeline.launch.xml deleted file mode 100644 index d966952..0000000 --- a/sciurus17_moveit_config/launch/chomp_planning_pipeline.launch.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/default_warehouse_db.launch b/sciurus17_moveit_config/launch/default_warehouse_db.launch deleted file mode 100644 index d176605..0000000 --- a/sciurus17_moveit_config/launch/default_warehouse_db.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/demo.launch b/sciurus17_moveit_config/launch/demo.launch deleted file mode 100644 index fee23da..0000000 --- a/sciurus17_moveit_config/launch/demo.launch +++ /dev/null @@ -1,63 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - [/sciurus17/controller1/joint_states,/sciurus17/controller2/joint_states,/sciurus17/controller3/joint_states] - - - [/sciurus17/controller1/joint_states,/sciurus17/controller2/joint_states,/sciurus17/controller3/joint_states] - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/sciurus17_moveit_config/launch/fake_moveit_controller_manager.launch.xml deleted file mode 100644 index 9d82b63..0000000 --- a/sciurus17_moveit_config/launch/fake_moveit_controller_manager.launch.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/joystick_control.launch b/sciurus17_moveit_config/launch/joystick_control.launch deleted file mode 100644 index f741735..0000000 --- a/sciurus17_moveit_config/launch/joystick_control.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/move_group.launch b/sciurus17_moveit_config/launch/move_group.launch deleted file mode 100644 index 5a6e90e..0000000 --- a/sciurus17_moveit_config/launch/move_group.launch +++ /dev/null @@ -1,82 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/moveit_rviz.launch b/sciurus17_moveit_config/launch/moveit_rviz.launch deleted file mode 100644 index 99bebd7..0000000 --- a/sciurus17_moveit_config/launch/moveit_rviz.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/ompl_planning_pipeline.launch.xml b/sciurus17_moveit_config/launch/ompl_planning_pipeline.launch.xml deleted file mode 100644 index 27cbcbd..0000000 --- a/sciurus17_moveit_config/launch/ompl_planning_pipeline.launch.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/planning_context.launch b/sciurus17_moveit_config/launch/planning_context.launch deleted file mode 100644 index 4fc6caf..0000000 --- a/sciurus17_moveit_config/launch/planning_context.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/planning_pipeline.launch.xml b/sciurus17_moveit_config/launch/planning_pipeline.launch.xml deleted file mode 100644 index 331a6c0..0000000 --- a/sciurus17_moveit_config/launch/planning_pipeline.launch.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/ros_controllers.launch b/sciurus17_moveit_config/launch/ros_controllers.launch deleted file mode 100644 index 0117f7c..0000000 --- a/sciurus17_moveit_config/launch/ros_controllers.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/run_benchmark_ompl.launch b/sciurus17_moveit_config/launch/run_benchmark_ompl.launch deleted file mode 100644 index 4b01293..0000000 --- a/sciurus17_moveit_config/launch/run_benchmark_ompl.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/run_move_group.launch.py b/sciurus17_moveit_config/launch/run_move_group.launch.py new file mode 100644 index 0000000..5612775 --- /dev/null +++ b/sciurus17_moveit_config/launch/run_move_group.launch.py @@ -0,0 +1,58 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_move_group_launch +from moveit_configs_utils.launches import generate_moveit_rviz_launch +from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch +from moveit_configs_utils.launches import generate_rsp_launch +from sciurus17_description.robot_description_loader import RobotDescriptionLoader +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = LaunchDescription() + + description_loader = RobotDescriptionLoader() + + ld.add_action( + DeclareLaunchArgument( + 'loaded_description', + default_value=description_loader.load(), + description='Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() in sciurus17_description.' + ) + ) + + ld.add_action( + DeclareLaunchArgument( + 'rviz_config', + default_value=get_package_share_directory( + 'sciurus17_moveit_config') + '/config/moveit.rviz', + description='Set the path to rviz configuration file.' + ) + ) + + moveit_config = ( + MoveItConfigsBuilder("sciurus17") + .planning_pipelines("ompl", ["ompl"]) + .to_moveit_configs() + ) + + moveit_config.robot_description = { + 'robot_description': LaunchConfiguration('loaded_description') + } + + # Move group + ld.add_entity(generate_move_group_launch(moveit_config)) + + # RViz + ld.add_entity(generate_moveit_rviz_launch(moveit_config)) + + # Static TF + ld.add_entity(generate_static_virtual_joint_tfs_launch(moveit_config)) + + # Publish TF + ld.add_entity(generate_rsp_launch(moveit_config)) + + return ld diff --git a/sciurus17_moveit_config/launch/sciurus17_moveit_controller_manager.launch.xml b/sciurus17_moveit_config/launch/sciurus17_moveit_controller_manager.launch.xml deleted file mode 100644 index 781d502..0000000 --- a/sciurus17_moveit_config/launch/sciurus17_moveit_controller_manager.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/sciurus17_moveit_sensor_manager.launch.xml b/sciurus17_moveit_config/launch/sciurus17_moveit_sensor_manager.launch.xml deleted file mode 100644 index 5d02698..0000000 --- a/sciurus17_moveit_config/launch/sciurus17_moveit_sensor_manager.launch.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/sciurus17_moveit_config/launch/sciurus17_planning.launch b/sciurus17_moveit_config/launch/sciurus17_planning.launch deleted file mode 100644 index b174609..0000000 --- a/sciurus17_moveit_config/launch/sciurus17_planning.launch +++ /dev/null @@ -1,59 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - [/sciurus17/controller1/joint_states,/sciurus17/controller2/joint_states,/sciurus17/controller3/joint_states] - - - [/sciurus17/controller1/joint_states,/sciurus17/controller2/joint_states,/sciurus17/controller3/joint_states] - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/sensor_manager.launch.xml b/sciurus17_moveit_config/launch/sensor_manager.launch.xml deleted file mode 100644 index 4bbffa6..0000000 --- a/sciurus17_moveit_config/launch/sensor_manager.launch.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/setup_assistant.launch b/sciurus17_moveit_config/launch/setup_assistant.launch deleted file mode 100644 index c1f6a93..0000000 --- a/sciurus17_moveit_config/launch/setup_assistant.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/setup_assistant.launch.py b/sciurus17_moveit_config/launch/setup_assistant.launch.py new file mode 100644 index 0000000..ccf56d0 --- /dev/null +++ b/sciurus17_moveit_config/launch/setup_assistant.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_setup_assistant_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("sciurus17", package_name="sciurus17_moveit_config").to_moveit_configs() + return generate_setup_assistant_launch(moveit_config) diff --git a/sciurus17_moveit_config/launch/trajectory_execution.launch.xml b/sciurus17_moveit_config/launch/trajectory_execution.launch.xml deleted file mode 100644 index e110dd8..0000000 --- a/sciurus17_moveit_config/launch/trajectory_execution.launch.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/warehouse.launch b/sciurus17_moveit_config/launch/warehouse.launch deleted file mode 100644 index f2c54c0..0000000 --- a/sciurus17_moveit_config/launch/warehouse.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/sciurus17_moveit_config/launch/warehouse_settings.launch.xml b/sciurus17_moveit_config/launch/warehouse_settings.launch.xml deleted file mode 100644 index 967e0ff..0000000 --- a/sciurus17_moveit_config/launch/warehouse_settings.launch.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/sciurus17_moveit_config/package.xml b/sciurus17_moveit_config/package.xml index 6ac8617..a774a3d 100644 --- a/sciurus17_moveit_config/package.xml +++ b/sciurus17_moveit_config/package.xml @@ -1,30 +1,49 @@ - - + + + sciurus17_moveit_config 2.0.0 - An automatically generated package with all the configuration and launch files for using the sciurus17 with the MoveIt! Motion Planning Framework + An automatically generated package with all the configuration and launch files for using the sciurus17 with the MoveIt Motion Planning Framework RT Corporation Hiroyuki Nomura + Atsushi Kuwagata + Apache License 2.0 - catkin + http://moveit.ros.org/ + https://github.com/ros-planning/moveit2/issues + https://github.com/ros-planning/moveit2 - moveit_ros_move_group - moveit_simple_controller_manager - moveit_fake_controller_manager - moveit_kinematics - moveit_planners_ompl - moveit_ros_visualization - moveit_setup_assistant - joint_state_publisher - joint_state_publisher_gui - robot_state_publisher - xacro + ament_cmake - sciurus17_description - sciurus17_description + moveit_ros_move_group + moveit_kinematics + moveit_planners + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + tf2_ros + xacro + + + + moveit_configs_utils + moveit_ros_move_group + moveit_ros_visualization + moveit_setup_assistant + robot_state_publisher + rviz2 + rviz_common + rviz_default_plugins + sciurus17_description + tf2_ros + xacro + + ament_cmake +