Merge pull request #85 from ohno-atsushi/add_rs030n

Add rs030n

README.md

@@ -54,6 +54,7 @@ Refer to [docs/ConnectingRealRobot.md](docs/ConnectingRealRobot.md)
  * rs013n
  * rs020n
  * rs025n
+ * rs030n
  * rs80n
 
 ## Notes

docs/ConnectingRealRobot-ja.md

@@ -14,6 +14,7 @@
 |rs013n|||✓||✓||
 |rs020n|✓||✓||||
 |rs025n||||✓|||
+|rs030n||||✓|||
 |rs080n||✓||✓|||
 
 ### 1.2 サポートしているASバージョン

docs/ConnectingRealRobot.md

@@ -13,6 +13,7 @@ Controllers with ”✓”  are available.
 |rs013n|||✓||✓||
 |rs020n|✓||✓||||
 |rs025n||||✓|||
+|rs030n||||✓|||
 |rs080n||✓||✓|||
 
 ### 1.2 Supported AS version

docs/README-ja.md

@@ -54,6 +54,7 @@ roslaunch ***_moveit_config moveit_planning_execution.launch
  * rs013n
  * rs020n
  * rs025n
+ * rs030n
  * rs80n
 
 ## ノート

khi_robot_bringup/config/rs030n_controllers.yaml

@@ -0,0 +1,58 @@
+  khi_robot_param:
+    robot: RS030N
+    arm:
+      arm1:
+        - rs030n_arm_controller
+
+  # Publish all joint states -----------------------------------
+  joint_state_controller:
+    type: joint_state_controller/JointStateController
+    publish_rate: 50  
+
+  # Position - Joint Position Trajectory Controller -------------------
+  rs030n_arm_controller:
+    type: "position_controllers/JointTrajectoryController"
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
+    constraints:
+      goal_time: 2.0                   # Defaults to zero
+      stopped_velocity_tolerance: 0.1 # Defaults to 0.01
+      joint1:
+        trajectory: 0 
+        goal: 0.0       
+      joint2:
+        trajectory: 0 
+        goal: 0.0       
+      joint3:
+        trajectory: 0 
+        goal: 0.0       
+      joint4:
+        trajectory: 0 
+        goal: 0.0             
+      joint5:
+        trajectory: 0 
+        goal: 0.0             
+      joint6:
+        trajectory: 0 
+        goal: 0.0           
+
+    state_publish_rate:  50 # Defaults to 50
+    action_monitor_rate: 20 # Defaults to 20
+    #hold_trajectory_duration: 0 # Defaults to 0.5
+
+
+  # Joint Group Position Controller -------------------
+  rs030n_joint_group_controller:
+    type: "position_controllers/JointGroupPositionController"
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6

khi_robot_bringup/launch/rs030n_bringup.launch

@@ -0,0 +1,58 @@
+<?xml version="1.0" encoding="utf-8"?>
+<launch>
+
+  <!-- GDB functionality -->
+  <arg name="debug" default="false" />
+  <arg name="simulation" default="false" />
+  <arg name="viewer" default="false" />
+  <arg name="ip" default="192.168.0.2" />
+  <arg name="period" default="2" />
+  <arg name="robot" default="RS030N" />
+
+  <!-- Load robot description -->
+  <param name="robot_description"
+    command="$(find xacro)/xacro --inorder '$(find khi_rs_description)/urdf/rs030n.urdf.xacro'" />
+
+  <rosparam>
+  </rosparam>
+  <!-- Load hardware interface -->
+  <include file="$(find khi_robot_control)/launch/khi_robot_control.launch" >
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="simulation" value="$(arg simulation)" />
+    <arg name="viewer" value="$(arg viewer)" />
+    <arg name="ip" value="$(arg ip)" />
+    <arg name="period" value="$(arg period)" />
+    <arg name="robot" value="$(arg robot)" />
+  </include>
+
+  <!-- If needed, broadcast static tf for robot root -->
+  <node pkg="tf" type="static_transform_publisher" name="world_link_broadcaster"
+        args="0 0 0 0 0 0 /world /base_link 100" />
+
+  <!-- Given the published joint states, publish tf for the robot links -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+
+
+  <!-- Load joint controller configurations from YAML file to parameter server -->
+  <rosparam file="$(find khi_robot_bringup)/config/rs030n_controllers.yaml"
+            command="load"/>
+  <rosparam file="$(find khi_rs_description)/config/rs030n_joint_limits.yaml"
+            command="load" ns="$(arg robot)"/>
+
+  <!-- Load the default controllers -->
+  <node name="controller_spawner" pkg="controller_manager"
+        type="spawner" respawn="false"
+        output="screen"
+        args="--shutdown-timeout 0.1
+	      joint_state_controller
+	      rs030n_arm_controller
+              " />
+  <!-- position_trajectory_controller -->
+
+  <!-- load joint group controllers -->
+  <node name="joint_group_controller_manager" pkg="controller_manager"
+        type="controller_manager" respawn="false"
+        output="screen"
+        args="load rs030n_joint_group_controller" />
+
+</launch>

khi_robot_test/CMakeLists.txt

@@ -47,5 +47,6 @@ if (CATKIN_ENABLE_TESTING)
   add_rostest(tests/khi_robot_control/test_khi_robot_control_rs007l.xml)
   add_rostest(tests/khi_robot_control/test_khi_robot_control_rs013n.xml)
   add_rostest(tests/khi_robot_control/test_khi_robot_control_rs020n.xml)
+  add_rostest(tests/khi_robot_control/test_khi_robot_control_rs030n.xml)
   add_rostest(tests/khi_robot_control/test_khi_robot_control_rs080n.xml)
 endif()

khi_robot_test/package.xml

@@ -27,6 +27,7 @@
   <test_depend>khi_rs007n_moveit_config</test_depend>
   <test_depend>khi_rs013n_moveit_config</test_depend>
   <test_depend>khi_rs020n_moveit_config</test_depend>
+  <test_depend>khi_rs030n_moveit_config</test_depend>
   <test_depend>khi_rs080n_moveit_config</test_depend>
   <test_depend>moveit_commander</test_depend>
 </package>

khi_robot_test/tests/khi_robot_control/test_khi_robot_control_rs030n.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="utf-8"?>
+<launch>
+
+  <param name="test_group_name" value="test_rs030n"/>
+
+  <group ns="test_rs030n">
+    <!-- Load hardware interface -->
+    <include file="$(find khi_robot_bringup)/launch/rs030n_bringup.launch" >
+      <arg name="simulation" value="true" />
+    </include>
+
+    <!-- Load hardware interface -->
+    <include file="$(find khi_rs030n_moveit_config)/launch/moveit_planning_execution.launch" >
+      <arg name="use_gui" value="false" />
+      <arg name="info" value="false" />
+    </include>
+
+    <test test-name="test_khi_robot_control" pkg="khi_robot_test" type="test_khi_robot_control.py" name="test_khi_robot_control" time-limit="300">
+    </test>
+  </group>
+
+</launch>

khi_rs030n_moveit_config/.setup_assistant

@@ -0,0 +1,11 @@
+moveit_setup_assistant_config:
+  URDF:
+    package: khi_rs_description
+    relative_path: urdf/rs030n.urdf.xacro
+    xacro_args: ""
+  SRDF:
+    relative_path: config/khi_rs030n.srdf
+  CONFIG:
+    author_name: RS030N
+    author_email: kurita_taisuke@khi.co.jp
+    generated_timestamp: 1693526009

khi_rs030n_moveit_config/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.1.3)
+project(khi_rs030n_moveit_config)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(roslaunch REQUIRED)
+  roslaunch_add_file_check(tests/roslaunch_test_moveit_rs030n.xml)
+endif()
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  PATTERN "setup_assistant.launch" EXCLUDE)
+install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

khi_rs030n_moveit_config/config/cartesian_limits.yaml

@@ -0,0 +1,5 @@
+cartesian_limits:
+  max_trans_vel: 1
+  max_trans_acc: 2.25
+  max_trans_dec: -5
+  max_rot_vel: 1.57

khi_rs030n_moveit_config/config/chomp_planning.yaml

@@ -0,0 +1,18 @@
+planning_time_limit: 10.0
+max_iterations: 200
+max_iterations_after_collision_free: 5
+smoothness_cost_weight: 0.1
+obstacle_cost_weight: 1.0
+learning_rate: 0.01
+smoothness_cost_velocity: 0.0
+smoothness_cost_acceleration: 1.0
+smoothness_cost_jerk: 0.0
+ridge_factor: 0.0
+use_pseudo_inverse: false
+pseudo_inverse_ridge_factor: 1e-4
+joint_update_limit: 0.1
+collision_clearance: 0.2
+collision_threshold: 0.07
+use_stochastic_descent: true
+enable_failure_recovery: false
+max_recovery_attempts: 5

khi_rs030n_moveit_config/config/controllers.yaml

@@ -0,0 +1,11 @@
+controller_list:
+  - name: rs030n_arm_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6

khi_rs030n_moveit_config/config/fake_controllers.yaml

@@ -0,0 +1,17 @@
+controller_list:
+  - name: fake_manipulator_controller
+    type: $(arg fake_execution_type)
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
+  - name: fake_tool_controller
+    type: $(arg fake_execution_type)
+    joints:
+      - joint6
+initial:  # Define initial robot poses per group
+  - group: manipulator
+    pose: home

khi_rs030n_moveit_config/config/gazebo_controllers.yaml

@@ -0,0 +1,4 @@
+# Publish joint_states
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50