diff --git a/kortex_control/arms/gen3/6dof/config/joint_position_controllers.yaml b/kortex_control/arms/gen3/6dof/config/joint_position_controllers.yaml index 05e23931..5c2f22bf 100644 --- a/kortex_control/arms/gen3/6dof/config/joint_position_controllers.yaml +++ b/kortex_control/arms/gen3/6dof/config/joint_position_controllers.yaml @@ -1,17 +1,17 @@ # Publish all joint states ----------------------------------- -joint_state_controller: +$(arg prefix)joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 -gen3_joint_trajectory_controller: +$(arg prefix)gen3_joint_trajectory_controller: type: effort_controllers/JointTrajectoryController joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 constraints: goal_time: 1.0 stopped_velocity_tolerance: 0.5 @@ -19,55 +19,55 @@ gen3_joint_trajectory_controller: state_publish_rate: 25 action_monitor_rate: 25 gains: - joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} - joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} - joint_3: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_4: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_5: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_6: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} + $(arg prefix)joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} + $(arg prefix)joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} + $(arg prefix)joint_3: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_4: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_5: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_6: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} -joint_1_position_controller: - joint: joint_1 +$(arg prefix)joint_1_position_controller: + joint: $(arg prefix)joint_1 pid: p: 3000.0 i: 0.0 d: 2.0 type: effort_controllers/JointPositionController -joint_2_position_controller: - joint: joint_2 +$(arg prefix)joint_2_position_controller: + joint: $(arg prefix)joint_2 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_3_position_controller: - joint: joint_3 +$(arg prefix)joint_3_position_controller: + joint: $(arg prefix)joint_3 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_4_position_controller: - joint: joint_4 +$(arg prefix)joint_4_position_controller: + joint: $(arg prefix)joint_4 pid: p: 750.0 i: 0.0 d: 0.2 type: effort_controllers/JointPositionController -joint_5_position_controller: - joint: joint_5 +$(arg prefix)joint_5_position_controller: + joint: $(arg prefix)joint_5 pid: p: 5000.0 i: 0.0 d: 1.0 type: effort_controllers/JointPositionController -joint_6_position_controller: - joint: joint_6 +$(arg prefix)joint_6_position_controller: + joint: $(arg prefix)joint_6 pid: p: 100.0 i: 0.0 diff --git a/kortex_control/arms/gen3/7dof/config/joint_position_controllers.yaml b/kortex_control/arms/gen3/7dof/config/joint_position_controllers.yaml index bb6fa834..621a0b30 100644 --- a/kortex_control/arms/gen3/7dof/config/joint_position_controllers.yaml +++ b/kortex_control/arms/gen3/7dof/config/joint_position_controllers.yaml @@ -1,18 +1,18 @@ # Publish all joint states ----------------------------------- -joint_state_controller: +$(arg prefix)joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 -gen3_joint_trajectory_controller: +$(arg prefix)gen3_joint_trajectory_controller: type: effort_controllers/JointTrajectoryController joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 constraints: goal_time: 1.0 stopped_velocity_tolerance: 0.5 @@ -20,64 +20,64 @@ gen3_joint_trajectory_controller: state_publish_rate: 25 action_monitor_rate: 25 gains: - joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} - joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} - joint_3: {p: 3000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_4: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_5: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_6: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_7: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} + $(arg prefix)joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} + $(arg prefix)joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} + $(arg prefix)joint_3: {p: 3000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_4: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_5: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_6: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_7: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} -joint_1_position_controller: - joint: joint_1 +$(arg prefix)joint_1_position_controller: + joint: $(arg prefix)joint_1 pid: p: 3000.0 i: 0.0 d: 2.0 type: effort_controllers/JointPositionController -joint_2_position_controller: - joint: joint_2 +$(arg prefix)joint_2_position_controller: + joint: $(arg prefix)joint_2 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_3_position_controller: - joint: joint_3 +$(arg prefix)joint_3_position_controller: + joint: $(arg prefix)joint_3 pid: p: 3000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_4_position_controller: - joint: joint_4 +$(arg prefix)joint_4_position_controller: + joint: $(arg prefix)joint_4 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_5_position_controller: - joint: joint_5 +$(arg prefix)joint_5_position_controller: + joint: $(arg prefix)joint_5 pid: p: 750.0 i: 0.0 d: 0.2 type: effort_controllers/JointPositionController -joint_6_position_controller: - joint: joint_6 +$(arg prefix)joint_6_position_controller: + joint: $(arg prefix)joint_6 pid: p: 5000.0 i: 0.0 d: 1.0 type: effort_controllers/JointPositionController -joint_7_position_controller: - joint: joint_7 +$(arg prefix)joint_7_position_controller: + joint: $(arg prefix)joint_7 pid: p: 100.0 i: 0.0 diff --git a/kortex_control/arms/gen3_lite/6dof/config/joint_position_controllers.yaml b/kortex_control/arms/gen3_lite/6dof/config/joint_position_controllers.yaml index 05c1c470..c869f63a 100644 --- a/kortex_control/arms/gen3_lite/6dof/config/joint_position_controllers.yaml +++ b/kortex_control/arms/gen3_lite/6dof/config/joint_position_controllers.yaml @@ -1,17 +1,17 @@ # Publish all joint states ----------------------------------- -joint_state_controller: +$(arg prefix)joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 -gen3_lite_joint_trajectory_controller: +$(arg prefix)gen3_lite_joint_trajectory_controller: type: effort_controllers/JointTrajectoryController joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 constraints: goal_time: 1.0 stopped_velocity_tolerance: 0.5 @@ -19,55 +19,55 @@ gen3_lite_joint_trajectory_controller: state_publish_rate: 25 action_monitor_rate: 25 gains: - joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} - joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} - joint_3: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_4: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_5: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_6: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} + $(arg prefix)joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} + $(arg prefix)joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} + $(arg prefix)joint_3: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_4: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_5: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_6: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} -joint_1_position_controller: - joint: joint_1 +$(arg prefix)joint_1_position_controller: + joint: $(arg prefix)joint_1 pid: p: 3000.0 i: 0.0 d: 2.0 type: effort_controllers/JointPositionController -joint_2_position_controller: - joint: joint_2 +$(arg prefix)joint_2_position_controller: + joint: $(arg prefix)joint_2 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_3_position_controller: - joint: joint_3 +$(arg prefix)joint_3_position_controller: + joint: $(arg prefix)joint_3 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_4_position_controller: - joint: joint_4 +$(arg prefix)joint_4_position_controller: + joint: $(arg prefix)joint_4 pid: p: 750.0 i: 0.0 d: 0.2 type: effort_controllers/JointPositionController -joint_5_position_controller: - joint: joint_5 +$(arg prefix)joint_5_position_controller: + joint: $(arg prefix)joint_5 pid: p: 5000.0 i: 0.0 d: 1.0 type: effort_controllers/JointPositionController -joint_6_position_controller: - joint: joint_6 +$(arg prefix)joint_6_position_controller: + joint: $(arg prefix)joint_6 pid: p: 100.0 i: 0.0 diff --git a/kortex_control/grippers/gen3_lite_2f/config/gripper_action_controller_parameters.yaml b/kortex_control/grippers/gen3_lite_2f/config/gripper_action_controller_parameters.yaml index 7d8c81d0..652cfca3 100644 --- a/kortex_control/grippers/gen3_lite_2f/config/gripper_action_controller_parameters.yaml +++ b/kortex_control/grippers/gen3_lite_2f/config/gripper_action_controller_parameters.yaml @@ -1,11 +1,11 @@ -gen3_lite_2f_gripper_controller: +$(arg prefix)gen3_lite_2f_gripper_controller: type: position_controllers/GripperActionController - joint: right_finger_bottom_joint + joint: $(arg prefix)right_finger_bottom_joint action_monitor_rate: 100 gazebo_ros_control: pid_gains: - right_finger_bottom_joint: {p: 0.1, i: 0.0, d: 0.0} - right_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0} - left_finger_bottom_joint: {p: 10.0, i: 0.0, d: 0.0} - left_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0} \ No newline at end of file + $(arg prefix)right_finger_bottom_joint: {p: 0.1, i: 0.0, d: 0.0} + $(arg prefix)right_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0} + $(arg prefix)left_finger_bottom_joint: {p: 10.0, i: 0.0, d: 0.0} + $(arg prefix)left_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0} \ No newline at end of file diff --git a/kortex_control/grippers/robotiq_2f_140/config/gripper_action_controller_parameters.yaml b/kortex_control/grippers/robotiq_2f_140/config/gripper_action_controller_parameters.yaml index bc61292d..869f515a 100644 --- a/kortex_control/grippers/robotiq_2f_140/config/gripper_action_controller_parameters.yaml +++ b/kortex_control/grippers/robotiq_2f_140/config/gripper_action_controller_parameters.yaml @@ -1,13 +1,13 @@ -robotiq_2f_140_gripper_controller: +$(arg prefix)robotiq_2f_140_gripper_controller: type: position_controllers/GripperActionController - joint: finger_joint + joint: $(arg prefix)finger_joint action_monitor_rate: 100 gazebo_ros_control: pid_gains: - finger_joint: {p: 10.0, i: 0.0, d: 0.01} - right_outer_knuckle_joint: {p: 10.0, i: 0.0, d: 0.00} - right_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} - left_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} - left_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} - right_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} \ No newline at end of file + $(arg prefix)finger_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)right_outer_knuckle_joint: {p: 10.0, i: 0.0, d: 0.00} + $(arg prefix)right_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)left_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)left_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} + $(arg prefix)right_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} \ No newline at end of file diff --git a/kortex_control/grippers/robotiq_2f_85/config/gripper_action_controller_parameters.yaml b/kortex_control/grippers/robotiq_2f_85/config/gripper_action_controller_parameters.yaml index dc06e4f8..4e40277e 100644 --- a/kortex_control/grippers/robotiq_2f_85/config/gripper_action_controller_parameters.yaml +++ b/kortex_control/grippers/robotiq_2f_85/config/gripper_action_controller_parameters.yaml @@ -1,13 +1,13 @@ -robotiq_2f_85_gripper_controller: +$(arg prefix)robotiq_2f_85_gripper_controller: type: position_controllers/GripperActionController - joint: finger_joint + joint: $(arg prefix)finger_joint action_monitor_rate: 100 gazebo_ros_control: pid_gains: - finger_joint: {p: 10.0, i: 0.0, d: 0.01} - right_outer_knuckle_joint: {p: 10.0, i: 0.0, d: 0.00} - right_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} - left_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} - left_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} - right_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} \ No newline at end of file + $(arg prefix)finger_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)right_outer_knuckle_joint: {p: 10.0, i: 0.0, d: 0.00} + $(arg prefix)right_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)left_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)left_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} + $(arg prefix)right_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} \ No newline at end of file diff --git a/kortex_description/arms/gen3/6dof/config/gazebo_initial_joint_positions.yaml b/kortex_description/arms/gen3/6dof/config/gazebo_initial_joint_positions.yaml index 62f19ff8..729f82b9 100644 --- a/kortex_description/arms/gen3/6dof/config/gazebo_initial_joint_positions.yaml +++ b/kortex_description/arms/gen3/6dof/config/gazebo_initial_joint_positions.yaml @@ -1,6 +1,6 @@ -initial_joint_positions: "-J joint_1 1.57 - -J joint_2 -0.35 - -J joint_3 -2.00 - -J joint_4 0 - -J joint_5 -1.00 - -J joint_6 1.57 " \ No newline at end of file +initial_joint_positions: "-J $(arg prefix)joint_1 1.57 + -J $(arg prefix)joint_2 -0.35 + -J $(arg prefix)joint_3 -2.00 + -J $(arg prefix)joint_4 0 + -J $(arg prefix)joint_5 -1.00 + -J $(arg prefix)joint_6 1.57 " \ No newline at end of file diff --git a/kortex_description/arms/gen3/6dof/config/joint_limits.yaml b/kortex_description/arms/gen3/6dof/config/joint_limits.yaml index 8692c090..0f20e6bc 100644 --- a/kortex_description/arms/gen3/6dof/config/joint_limits.yaml +++ b/kortex_description/arms/gen3/6dof/config/joint_limits.yaml @@ -1,7 +1,7 @@ joint_names: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 diff --git a/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro b/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro index db152ab5..34dfa702 100644 --- a/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro +++ b/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro @@ -228,7 +228,7 @@ - + diff --git a/kortex_description/arms/gen3/7dof/config/gazebo_initial_joint_positions.yaml b/kortex_description/arms/gen3/7dof/config/gazebo_initial_joint_positions.yaml index 7cef4ab6..2ab9226f 100644 --- a/kortex_description/arms/gen3/7dof/config/gazebo_initial_joint_positions.yaml +++ b/kortex_description/arms/gen3/7dof/config/gazebo_initial_joint_positions.yaml @@ -1,7 +1,7 @@ -initial_joint_positions: "-J joint_1 1.57 - -J joint_2 -0.35 - -J joint_3 3.14 - -J joint_4 -2.00 - -J joint_5 0 - -J joint_6 -1.00 - -J joint_7 1.57" \ No newline at end of file +initial_joint_positions: "-J $(arg prefix)joint_1 1.57 + -J $(arg prefix)joint_2 -0.35 + -J $(arg prefix)joint_3 3.14 + -J $(arg prefix)joint_4 -2.00 + -J $(arg prefix)joint_5 0 + -J $(arg prefix)joint_6 -1.00 + -J $(arg prefix)joint_7 1.57" \ No newline at end of file diff --git a/kortex_description/arms/gen3/7dof/config/joint_limits.yaml b/kortex_description/arms/gen3/7dof/config/joint_limits.yaml index 37f3ee5b..28d1ff07 100644 --- a/kortex_description/arms/gen3/7dof/config/joint_limits.yaml +++ b/kortex_description/arms/gen3/7dof/config/joint_limits.yaml @@ -1,8 +1,8 @@ joint_names: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 \ No newline at end of file + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 \ No newline at end of file diff --git a/kortex_description/arms/gen3_lite/6dof/config/joint_limits.yaml b/kortex_description/arms/gen3_lite/6dof/config/joint_limits.yaml index 8692c090..0f20e6bc 100644 --- a/kortex_description/arms/gen3_lite/6dof/config/joint_limits.yaml +++ b/kortex_description/arms/gen3_lite/6dof/config/joint_limits.yaml @@ -1,7 +1,7 @@ joint_names: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 diff --git a/kortex_description/grippers/gen3_lite_2f/config/joint_limits.yaml b/kortex_description/grippers/gen3_lite_2f/config/joint_limits.yaml index fb76b03e..b0f21f26 100644 --- a/kortex_description/grippers/gen3_lite_2f/config/joint_limits.yaml +++ b/kortex_description/grippers/gen3_lite_2f/config/joint_limits.yaml @@ -1,5 +1,5 @@ gripper_joint_names: - - right_finger_bottom_joint + - $(arg prefix)right_finger_bottom_joint gripper_joint_limits_min: - 0.96 diff --git a/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_transmission_macro.xacro b/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_transmission_macro.xacro index d6e40348..e04ae667 100644 --- a/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_transmission_macro.xacro +++ b/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_transmission_macro.xacro @@ -3,12 +3,12 @@ - + transmission_interface/SimpleTransmission - + hardware_interface/PositionJointInterface - + 1 hardware_interface/PositionJointInterface @@ -16,24 +16,24 @@ - right_finger_bottom_joint - right_finger_tip_joint + ${prefix}right_finger_bottom_joint + ${prefix}right_finger_tip_joint -0.676 0.149 5.0 - right_finger_bottom_joint - left_finger_bottom_joint + ${prefix}right_finger_bottom_joint + ${prefix}left_finger_bottom_joint -1.0 0.0 5.0 - right_finger_bottom_joint - left_finger_tip_joint + ${prefix}right_finger_bottom_joint + ${prefix}left_finger_tip_joint -0.676 0.149 5.0 @@ -43,9 +43,9 @@ my_gen3_lite_gripper - end_effector_link - right_finger_dist_link - left_finger_dist_link + ${prefix}end_effector_link + ${prefix}right_finger_dist_link + ${prefix}left_finger_dist_link 100 10 diff --git a/kortex_description/grippers/robotiq_2f_140/config/joint_limits.yaml b/kortex_description/grippers/robotiq_2f_140/config/joint_limits.yaml index 3c16e30a..dd1fd9e8 100644 --- a/kortex_description/grippers/robotiq_2f_140/config/joint_limits.yaml +++ b/kortex_description/grippers/robotiq_2f_140/config/joint_limits.yaml @@ -1,5 +1,5 @@ gripper_joint_names: - - finger_joint + - $(arg prefix)finger_joint gripper_joint_limits_min: - 0.000 diff --git a/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_2f_140_transmission_macro.xacro b/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_2f_140_transmission_macro.xacro index 11adb98f..f1a85b8c 100644 --- a/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_2f_140_transmission_macro.xacro +++ b/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_2f_140_transmission_macro.xacro @@ -3,12 +3,12 @@ - + transmission_interface/SimpleTransmission - + hardware_interface/PositionJointInterface - + 1 hardware_interface/PositionJointInterface @@ -16,40 +16,40 @@ - finger_joint - right_outer_knuckle_joint + ${prefix}finger_joint + ${prefix}right_outer_knuckle_joint -1.0 0.0 5.0 - finger_joint - right_inner_knuckle_joint + ${prefix}finger_joint + ${prefix}right_inner_knuckle_joint -1.0 0.0 5.0 - finger_joint - left_inner_knuckle_joint + ${prefix}finger_joint + ${prefix}left_inner_knuckle_joint -1.0 0.0 5.0 - finger_joint - left_inner_finger_joint + ${prefix}finger_joint + ${prefix}left_inner_finger_joint 1.0 0.0 5.0 - finger_joint - right_inner_finger_joint + ${prefix}finger_joint + ${prefix}right_inner_finger_joint 1.0 0.0 5.0 @@ -59,9 +59,9 @@ my_robotiq_gripper - bracelet_link - left_inner_finger - right_inner_finger + ${prefix}bracelet_link + ${prefix}left_inner_finger + ${prefix}right_inner_finger 100 10 diff --git a/kortex_description/grippers/robotiq_2f_85/config/joint_limits.yaml b/kortex_description/grippers/robotiq_2f_85/config/joint_limits.yaml index 3c64028a..d67e31fe 100644 --- a/kortex_description/grippers/robotiq_2f_85/config/joint_limits.yaml +++ b/kortex_description/grippers/robotiq_2f_85/config/joint_limits.yaml @@ -1,5 +1,5 @@ gripper_joint_names: - - finger_joint + - $(arg prefix)finger_joint gripper_joint_limits_min: - 0.000 diff --git a/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_transmission_macro.xacro b/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_transmission_macro.xacro index fe287f37..4ed28bab 100644 --- a/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_transmission_macro.xacro +++ b/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_transmission_macro.xacro @@ -3,12 +3,12 @@ - + transmission_interface/SimpleTransmission - + hardware_interface/PositionJointInterface - + 1 hardware_interface/PositionJointInterface @@ -16,40 +16,40 @@ - finger_joint - right_outer_knuckle_joint + ${prefix}finger_joint + ${prefix}right_outer_knuckle_joint 1.0 0.0 5.0 - finger_joint - right_inner_knuckle_joint + ${prefix}finger_joint + ${prefix}right_inner_knuckle_joint 1.0 0.0 5.0 - finger_joint - left_inner_knuckle_joint + ${prefix}finger_joint + ${prefix}left_inner_knuckle_joint 1.0 0.0 5.0 - finger_joint - left_inner_finger_joint + ${prefix}finger_joint + ${prefix}left_inner_finger_joint -1.0 0.0 5.0 - finger_joint - right_inner_finger_joint + ${prefix}finger_joint + ${prefix}right_inner_finger_joint -1.0 0.0 5.0 @@ -59,9 +59,9 @@ my_robotiq_gripper - bracelet_link - left_inner_finger - right_inner_finger + ${prefix}bracelet_link + ${prefix}left_inner_finger + ${prefix}right_inner_finger 100 10 diff --git a/kortex_description/launch/visualize.launch b/kortex_description/launch/visualize.launch index 30914192..6fe5e26f 100644 --- a/kortex_description/launch/visualize.launch +++ b/kortex_description/launch/visualize.launch @@ -14,10 +14,10 @@ - - diff --git a/kortex_description/readme.md b/kortex_description/readme.md index 2ecf830b..1811c64e 100644 --- a/kortex_description/readme.md +++ b/kortex_description/readme.md @@ -23,10 +23,10 @@ To load the description of a robot, you simply have to load the **ARM.xacro** or For example: - To load the Gen3 description with a Robotiq 2-F 85 gripper for simulation, you would put in your launch file : - + - To load the Gen3 lite description, you would put in your launch file : - + ## Tool frame diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h index 335a8cb0..5249fe33 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h @@ -83,6 +83,9 @@ class KortexArmDriver ros::NodeHandle m_node_handle; + // False if in simulation + bool m_is_real_robot; + // Api options std::string m_ip_address; std::string m_username; @@ -157,7 +160,7 @@ class KortexArmDriver // Private methods bool isGripperPresent(); void setAngularTrajectorySoftLimitsToMax(); - void publishFeedback(); + void publishRobotFeedback(); }; #endif diff --git a/kortex_driver/launch/kortex_driver.launch b/kortex_driver/launch/kortex_driver.launch index 3d6fe61d..79123220 100644 --- a/kortex_driver/launch/kortex_driver.launch +++ b/kortex_driver/launch/kortex_driver.launch @@ -36,19 +36,19 @@ - - - - @@ -56,6 +56,7 @@ + @@ -70,9 +71,9 @@ - + - + @@ -83,11 +84,13 @@ + + @@ -98,7 +101,7 @@ - + @@ -116,9 +119,9 @@ - + - + diff --git a/kortex_driver/launch/kortex_dual_driver.launch b/kortex_driver/launch/kortex_dual_driver.launch index 0939e816..4cee2ba6 100644 --- a/kortex_driver/launch/kortex_dual_driver.launch +++ b/kortex_driver/launch/kortex_dual_driver.launch @@ -60,7 +60,7 @@ - + - + launch-prefix="gdb -ex run args" @@ -109,9 +109,9 @@ - + - + diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp index 5f57e83e..1ead99b9 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp @@ -12,23 +12,36 @@ #include "kortex_driver/non-generated/kortex_arm_driver.h" -KortexArmDriver::KortexArmDriver(ros::NodeHandle nh): m_node_handle(nh), m_node_is_running(true), m_consecutive_base_cyclic_timeouts(0) +KortexArmDriver::KortexArmDriver(ros::NodeHandle nh): m_node_handle(nh), + m_node_is_running(true), + m_consecutive_base_cyclic_timeouts(0), + m_is_interconnect_module_present(false), + m_is_vision_module_present(false) { // Parameter to let the other nodes know this node is up ros::param::set("is_initialized", false); - // Start the node in the different initialization functions parseRosArguments(); - initApi(); - verifyProductConfiguration(); - initSubscribers(); + + // If with a real robot, start the robot-specific stuff + if (m_is_real_robot) + { + initApi(); + verifyProductConfiguration(); + initSubscribers(); + startActionServers(); + } + + // ROS Services are always started initRosServices(); - startActionServers(); // Start the thread to publish the feedback and joint states m_pub_base_feedback = m_node_handle.advertise("base_feedback", 1000); m_pub_joint_state = m_node_handle.advertise("base_feedback/joint_state", 1000); - m_publish_feedback_thread = std::thread(&KortexArmDriver::publishFeedback, this); + if (m_is_real_robot) + { + m_publish_feedback_thread = std::thread(&KortexArmDriver::publishRobotFeedback, this); + } // If we get here and no error was thrown we started the node correctly ROS_INFO("%sThe Kortex driver has been initialized correctly!%s", GREEN_COLOR_CONSOLE, RESET_COLOR_CONSOLE); @@ -44,14 +57,6 @@ KortexArmDriver::~KortexArmDriver() m_publish_feedback_thread.join(); } - delete m_action_server_follow_joint_trajectory; - if (m_action_server_gripper_command) - { - delete m_action_server_gripper_command; - } - - delete m_topic_subscribers; - delete m_actuator_config_ros_services; delete m_base_ros_services; delete m_control_config_ros_services; @@ -65,100 +70,126 @@ KortexArmDriver::~KortexArmDriver() { delete m_vision_config_ros_services; } - - m_tcp_session_manager->CloseSession(); - m_udp_session_manager->CloseSession(); - m_tcp_router->SetActivationStatus(false); - m_udp_router->SetActivationStatus(false); - m_tcp_transport->disconnect(); - m_udp_transport->disconnect(); - - delete m_actuator_config; - delete m_base; - delete m_control_config; - delete m_device_config; - delete m_device_manager; - delete m_interconnect_config; - delete m_vision_config; - delete m_base_cyclic; - - delete m_tcp_session_manager; - delete m_udp_session_manager; - - delete m_tcp_router; - delete m_udp_router; - delete m_tcp_transport; - delete m_udp_transport; -} -void KortexArmDriver::parseRosArguments() -{ - bool use_sim_time = false; - if (ros::param::get("/use_sim_time", use_sim_time)) + if (!m_is_real_robot) { - if (use_sim_time) + + delete m_action_server_follow_joint_trajectory; + if (m_action_server_gripper_command) { - std::string error_string = "ROS parameter /use_sim_time is true : you may experience problems and you should set it to false and restart the driver."; - ROS_WARN("%s", error_string.c_str()); + delete m_action_server_gripper_command; } - } - if (!ros::param::get("~ip_address", m_ip_address)) - { - std::string error_string = "IP address of the robot was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); - } + delete m_topic_subscribers; - if (!ros::param::get("~username", m_username)) - { - std::string error_string = "Username for the robot session was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); + m_tcp_session_manager->CloseSession(); + m_udp_session_manager->CloseSession(); + m_tcp_router->SetActivationStatus(false); + m_udp_router->SetActivationStatus(false); + m_tcp_transport->disconnect(); + m_udp_transport->disconnect(); + + delete m_actuator_config; + delete m_base; + delete m_control_config; + delete m_device_config; + delete m_device_manager; + delete m_interconnect_config; + delete m_vision_config; + delete m_base_cyclic; + + delete m_tcp_session_manager; + delete m_udp_session_manager; + + delete m_tcp_router; + delete m_udp_router; + delete m_tcp_transport; + delete m_udp_transport; } +} - if (!ros::param::get("~password", m_password)) +void KortexArmDriver::parseRosArguments() +{ + bool sim; + if (!ros::param::get("~sim", sim)) { - std::string error_string = "Password for the robot session was not specified in the launch file, shutting down the node..."; + std::string error_string = "Simulation was not specified in the launch file, shutting down the node..."; ROS_ERROR("%s", error_string.c_str()); throw new std::runtime_error(error_string); } + m_is_real_robot = !sim; - if (!ros::param::get("~use_hard_limits", m_use_hard_limits)) + // Some parameters are only necessary with a real robot + if (m_is_real_robot) { - std::string error_string = "Usage of hard limits as soft was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); - } + bool use_sim_time = false; + if (ros::param::get("/use_sim_time", use_sim_time)) + { + if (use_sim_time) + { + std::string error_string = "ROS parameter /use_sim_time is true : you may experience problems and you should set it to false and restart the driver."; + ROS_WARN("%s", error_string.c_str()); + } + } - if (!ros::param::get("~cyclic_data_publish_rate", m_cyclic_data_publish_rate)) - { - std::string error_string = "Publish rate of the cyclic data was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); - } + if (!ros::param::get("~ip_address", m_ip_address)) + { + std::string error_string = "IP address of the robot was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } - if (!ros::param::get("~api_rpc_timeout_ms", m_api_rpc_timeout_ms)) - { - std::string error_string = "API RPC timeout duration was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); - } + if (!ros::param::get("~username", m_username)) + { + std::string error_string = "Username for the robot session was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } - if (!ros::param::get("~api_session_inactivity_timeout_ms", m_api_session_inactivity_timeout_ms)) - { - std::string error_string = "API session inactivity timeout duration was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); + if (!ros::param::get("~password", m_password)) + { + std::string error_string = "Password for the robot session was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } + + if (!ros::param::get("~use_hard_limits", m_use_hard_limits)) + { + std::string error_string = "Usage of hard limits as soft was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } + + if (!ros::param::get("~api_rpc_timeout_ms", m_api_rpc_timeout_ms)) + { + std::string error_string = "API RPC timeout duration was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } + + if (!ros::param::get("~api_session_inactivity_timeout_ms", m_api_session_inactivity_timeout_ms)) + { + std::string error_string = "API session inactivity timeout duration was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } + + if (!ros::param::get("~api_connection_inactivity_timeout_ms", m_api_connection_inactivity_timeout_ms)) + { + std::string error_string = "API connection inactivity timeout duration was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } } - if (!ros::param::get("~api_connection_inactivity_timeout_ms", m_api_connection_inactivity_timeout_ms)) + + if (!ros::param::get("~cyclic_data_publish_rate", m_cyclic_data_publish_rate)) { - std::string error_string = "API connection inactivity timeout duration was not specified in the launch file, shutting down the node..."; + std::string error_string = "Publish rate of the cyclic data was not specified in the launch file, shutting down the node..."; ROS_ERROR("%s", error_string.c_str()); throw new std::runtime_error(error_string); } - + if (!ros::param::get("~arm", m_arm_name)) { std::string error_string = "Arm name was not specified in the launch file, shutting down the node..."; @@ -364,11 +395,7 @@ void KortexArmDriver::verifyProductConfiguration() ROS_INFO("-------------------------------------------------"); ROS_INFO("Scanning all devices in robot... "); - // We suppose we don't have an interconnect or a vision module until we find it - m_is_interconnect_module_present = false; - m_is_vision_module_present = false; - - // Loop through the devices to find the device ID's +// Loop through the devices to find the device ID's auto devices = m_device_manager->ReadAllDevices(); for (auto device : devices.device_handle()) { @@ -415,26 +442,53 @@ void KortexArmDriver::initRosServices() { ROS_INFO("-------------------------------------------------"); ROS_INFO("Initializing Kortex Driver's services..."); - m_actuator_config_ros_services = new ActuatorConfigRobotServices(m_node_handle, m_actuator_config, 0, m_api_rpc_timeout_ms); - m_base_ros_services = new BaseRobotServices(m_node_handle, m_base, 0, m_api_rpc_timeout_ms); - m_control_config_ros_services = new ControlConfigRobotServices(m_node_handle, m_control_config, 0, m_api_rpc_timeout_ms); - m_device_config_ros_services = new DeviceConfigRobotServices(m_node_handle, m_device_config, 0, m_api_rpc_timeout_ms); - m_device_manager_ros_services = new DeviceManagerRobotServices(m_node_handle, m_device_manager, 0, m_api_rpc_timeout_ms); - if (m_is_interconnect_module_present) - { - m_interconnect_config_ros_services = new InterconnectConfigRobotServices(m_node_handle, m_interconnect_config, m_interconnect_device_id, m_api_rpc_timeout_ms); - } - else - { - m_interconnect_config_ros_services = nullptr; - } - if (m_is_vision_module_present) - { - m_vision_config_ros_services = new VisionConfigRobotServices(m_node_handle, m_vision_config, m_vision_device_id, m_api_rpc_timeout_ms); + if (m_is_real_robot) + { + m_actuator_config_ros_services = new ActuatorConfigRobotServices(m_node_handle, m_actuator_config, 0, m_api_rpc_timeout_ms); + m_base_ros_services = new BaseRobotServices(m_node_handle, m_base, 0, m_api_rpc_timeout_ms); + m_control_config_ros_services = new ControlConfigRobotServices(m_node_handle, m_control_config, 0, m_api_rpc_timeout_ms); + m_device_config_ros_services = new DeviceConfigRobotServices(m_node_handle, m_device_config, 0, m_api_rpc_timeout_ms); + m_device_manager_ros_services = new DeviceManagerRobotServices(m_node_handle, m_device_manager, 0, m_api_rpc_timeout_ms); + if (m_is_interconnect_module_present) + { + m_interconnect_config_ros_services = new InterconnectConfigRobotServices(m_node_handle, m_interconnect_config, m_interconnect_device_id, m_api_rpc_timeout_ms); + } + else + { + m_interconnect_config_ros_services = nullptr; + } + if (m_is_vision_module_present) + { + m_vision_config_ros_services = new VisionConfigRobotServices(m_node_handle, m_vision_config, m_vision_device_id, m_api_rpc_timeout_ms); + } + else + { + m_vision_config_ros_services = nullptr; + } } else { - m_vision_config_ros_services = nullptr; + m_actuator_config_ros_services = new ActuatorConfigSimulationServices(m_node_handle); + m_base_ros_services = new BaseSimulationServices(m_node_handle); + m_control_config_ros_services = new ControlConfigSimulationServices(m_node_handle); + m_device_config_ros_services = new DeviceConfigSimulationServices(m_node_handle); + m_device_manager_ros_services = new DeviceManagerSimulationServices(m_node_handle); + if (m_is_interconnect_module_present) + { + m_interconnect_config_ros_services = new InterconnectConfigSimulationServices(m_node_handle); + } + else + { + m_interconnect_config_ros_services = nullptr; + } + if (m_is_vision_module_present) + { + m_vision_config_ros_services = new VisionConfigSimulationServices(m_node_handle); + } + else + { + m_vision_config_ros_services = nullptr; + } } std::this_thread::sleep_for(std::chrono::milliseconds(2000)); ROS_INFO("Kortex Driver's services initialized correctly."); @@ -492,7 +546,7 @@ void KortexArmDriver::setAngularTrajectorySoftLimitsToMax() } } -void KortexArmDriver::publishFeedback() +void KortexArmDriver::publishRobotFeedback() { Kinova::Api::BaseCyclic::Feedback feedback_from_api; diff --git a/kortex_gazebo/launch/spawn_kortex_robot.launch b/kortex_gazebo/launch/spawn_kortex_robot.launch index 646f42c8..836674bc 100644 --- a/kortex_gazebo/launch/spawn_kortex_robot.launch +++ b/kortex_gazebo/launch/spawn_kortex_robot.launch @@ -18,6 +18,8 @@ + + @@ -42,10 +44,10 @@ - - @@ -55,30 +57,30 @@ - - + @@ -87,13 +89,13 @@ - - @@ -103,22 +105,41 @@ - - - + output="screen" args="$(arg prefix)joint_1_position_controller + $(arg prefix)joint_2_position_controller + $(arg prefix)joint_3_position_controller + $(arg prefix)joint_4_position_controller + $(arg prefix)joint_5_position_controller + $(arg prefix)joint_6_position_controller + $(arg prefix)joint_7_position_controller + $(arg prefix)joint_state_controller" /> + + + + + + + + + + + + + + + @@ -130,10 +151,12 @@ + + diff --git a/kortex_gazebo/package.xml b/kortex_gazebo/package.xml index 100ca5da..060b1d94 100644 --- a/kortex_gazebo/package.xml +++ b/kortex_gazebo/package.xml @@ -10,6 +10,7 @@ BSD catkin roslaunch + kortex_driver robot_state_publisher rviz joint_state_publisher diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/.setup_assistant b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/.setup_assistant index af9086aa..8619322a 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/.setup_assistant +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/.setup_assistant @@ -2,7 +2,7 @@ moveit_setup_assistant_config: URDF: package: kortex_description relative_path: robots/gen3_lite_gen3_lite_2f.xacro - xacro_args: "--inorder " + xacro_args: "" SRDF: relative_path: config/gen3_lite_gen3_lite_2f.srdf CONFIG: diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/fake_controllers.yaml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/fake_controllers.yaml index 8eda2826..60730a04 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/fake_controllers.yaml @@ -1,12 +1,12 @@ controller_list: - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - name: fake_gripper_controller + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - name: $(arg prefix)fake_gripper_controller joints: - - right_finger_bottom_joint \ No newline at end of file + - $(arg prefix)right_finger_bottom_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf deleted file mode 100644 index 178ab9bc..00000000 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf +++ /dev/null @@ -1,107 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf.xacro b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf.xacro new file mode 100644 index 00000000..4bbad952 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf.xacro @@ -0,0 +1,108 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/joint_limits.yaml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/joint_limits.yaml index 6bc8c8b7..66e05b12 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/joint_limits.yaml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/joint_limits.yaml @@ -2,32 +2,32 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.99 has_acceleration_limits: true max_acceleration: 0.35 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.99 has_acceleration_limits: true max_acceleration: 0.17 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.99 has_acceleration_limits: true max_acceleration: 0.14 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.99 has_acceleration_limits: true max_acceleration: 0.35 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.99 has_acceleration_limits: true max_acceleration: 3.5 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.55 has_acceleration_limits: true diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ompl_planning.yaml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ompl_planning.yaml index a49c255b..2ee2dbe8 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ompl_planning.yaml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ompl_planning.yaml @@ -146,7 +146,7 @@ arm: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(joint_1,joint_2) + projection_evaluator: joints($(arg prefix)joint_1,$(arg prefix)joint_2) longest_valid_segment_fraction: 0.005 gripper: default_planner_config: RRTConnect diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ros_controllers.yaml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ros_controllers.yaml index a71f1cae..2934c6dc 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ros_controllers.yaml @@ -2,21 +2,21 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_lite_joint_trajectory_controller" + - name: "$(arg prefix)gen3_lite_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 - - name: "gen3_lite_2f_gripper_controller" + - name: "$(arg prefix)gen3_lite_2f_gripper_controller" action_ns: gripper_cmd default: True type: GripperCommand joints: - - right_finger_bottom_joint \ No newline at end of file + - $(arg prefix)right_finger_bottom_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/gen3_lite_gen3_lite_2f_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/gen3_lite_gen3_lite_2f_moveit_controller_manager.launch.xml index ec664d10..afb73408 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/gen3_lite_gen3_lite_2f_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/gen3_lite_gen3_lite_2f_moveit_controller_manager.launch.xml @@ -1,9 +1,11 @@ + + - + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch index 5dfab46d..f26a35d8 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch @@ -5,7 +5,12 @@ - + + + + + + @@ -46,6 +51,7 @@ + @@ -53,6 +59,7 @@ + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ompl_planning_pipeline.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ompl_planning_pipeline.launch.xml index 7dc43ada..b8cd5b6a 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ompl_planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ompl_planning_pipeline.launch.xml @@ -2,6 +2,7 @@ + @@ -21,6 +22,6 @@ - + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch index f501959f..990954d1 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch @@ -5,15 +5,17 @@ + + + - + - - + - + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_pipeline.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_pipeline.launch.xml index b941658f..6f172429 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_pipeline.launch.xml @@ -4,7 +4,10 @@ It is assumed that all planning pipelines are named XXX_planning_pipeline.launch --> + - + + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/trajectory_execution.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/trajectory_execution.launch.xml index 9a8e008c..7a09c04f 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/trajectory_execution.launch.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/trajectory_execution.launch.xml @@ -1,7 +1,7 @@ - + @@ -15,6 +15,8 @@ - + + + diff --git a/kortex_move_it_config/gen3_move_it_config/.setup_assistant b/kortex_move_it_config/gen3_move_it_config/.setup_assistant index f94315d9..94c2fcba 100644 --- a/kortex_move_it_config/gen3_move_it_config/.setup_assistant +++ b/kortex_move_it_config/gen3_move_it_config/.setup_assistant @@ -2,7 +2,7 @@ moveit_setup_assistant_config: URDF: package: kortex_description relative_path: robots/gen3.xacro - xacro_args: --inorder arm:=gen3 + xacro_args: arm:=gen3 SRDF: relative_path: config/gen3.srdf CONFIG: diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/6dof/default_joint_limits.yaml index 6a93c689..7a51e2c1 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/6dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/default_joint_limits.yaml @@ -2,32 +2,32 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/fake_controllers.yaml b/kortex_move_it_config/gen3_move_it_config/config/6dof/fake_controllers.yaml index c693bcb7..84de5e8a 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/6dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/fake_controllers.yaml @@ -1,9 +1,9 @@ controller_list: - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 \ No newline at end of file + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf b/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf deleted file mode 100644 index 586f19ba..00000000 --- a/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf +++ /dev/null @@ -1,62 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf.xacro b/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf.xacro new file mode 100644 index 00000000..1257563e --- /dev/null +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf.xacro @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/6dof/hard_joint_limits.yaml index 6783c6e2..f43b69a2 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/6dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/hard_joint_limits.yaml @@ -2,32 +2,32 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/ros_controllers.yaml b/kortex_move_it_config/gen3_move_it_config/config/6dof/ros_controllers.yaml index 9048503d..221f9604 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/6dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/ros_controllers.yaml @@ -2,14 +2,14 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/7dof/default_joint_limits.yaml index 68e9894d..056e61f7 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/default_joint_limits.yaml @@ -2,37 +2,37 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/fake_controllers.yaml b/kortex_move_it_config/gen3_move_it_config/config/7dof/fake_controllers.yaml index 159da410..9bf3b0da 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/fake_controllers.yaml @@ -1,10 +1,10 @@ controller_list: - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 \ No newline at end of file + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf b/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf deleted file mode 100644 index f7b254af..00000000 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf +++ /dev/null @@ -1,74 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro b/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro new file mode 100644 index 00000000..5930cc1e --- /dev/null +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/7dof/hard_joint_limits.yaml index 642c2528..fdd996b5 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/hard_joint_limits.yaml @@ -2,37 +2,37 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/ros_controllers.yaml b/kortex_move_it_config/gen3_move_it_config/config/7dof/ros_controllers.yaml index 8a0e3f50..cfaa6205 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/ros_controllers.yaml @@ -2,15 +2,15 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 \ No newline at end of file + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_move_it_config/config/ompl_planning.yaml b/kortex_move_it_config/gen3_move_it_config/config/ompl_planning.yaml index 17829f90..1667cab7 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/ompl_planning.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/ompl_planning.yaml @@ -146,5 +146,5 @@ arm: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(joint_1,joint_2) + projection_evaluator: joints($(arg prefix)joint_1,$(arg prefix)joint_2) longest_valid_segment_fraction: 0.005 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_move_it_config/launch/gen3_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/gen3_moveit_controller_manager.launch.xml index e0b4c42f..5b91ac16 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/gen3_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_move_it_config/launch/gen3_moveit_controller_manager.launch.xml @@ -1,11 +1,12 @@ + - + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_move_it_config/launch/move_group.launch index 20422937..56ffd49c 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_move_it_config/launch/move_group.launch @@ -3,10 +3,14 @@ + + + + @@ -48,6 +52,7 @@ + @@ -56,6 +61,7 @@ + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/ompl_planning_pipeline.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/ompl_planning_pipeline.launch.xml index 2ab04a16..bfe3d6c6 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/ompl_planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_move_it_config/launch/ompl_planning_pipeline.launch.xml @@ -2,6 +2,7 @@ + @@ -21,6 +22,6 @@ - + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch index 0ef0c93b..4548bcdc 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch @@ -8,19 +8,22 @@ + + + - + - + - - + + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/planning_pipeline.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/planning_pipeline.launch.xml index 02c62e47..607842b2 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_move_it_config/launch/planning_pipeline.launch.xml @@ -4,7 +4,10 @@ It is assumed that all planning pipelines are named XXX_planning_pipeline.launch --> + - + + + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/trajectory_execution.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/trajectory_execution.launch.xml index 7f071c1a..ebb523b9 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/trajectory_execution.launch.xml +++ b/kortex_move_it_config/gen3_move_it_config/launch/trajectory_execution.launch.xml @@ -2,6 +2,7 @@ + @@ -18,6 +19,7 @@ + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/.setup_assistant b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/.setup_assistant index e8017e1b..05b9cb92 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/.setup_assistant +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/.setup_assistant @@ -2,7 +2,7 @@ moveit_setup_assistant_config: URDF: package: kortex_description relative_path: robots/gen3_robotiq_2f_85.xacro - xacro_args: --inorder arm:=gen3 gripper:=robotiq_2f_85 + xacro_args: arm:=gen3 gripper:=robotiq_2f_85 SRDF: relative_path: config/gen3_robotiq_2f_85.srdf CONFIG: diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/default_joint_limits.yaml index 426d9e04..83fdb6bc 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/default_joint_limits.yaml @@ -2,62 +2,62 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/fake_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/fake_controllers.yaml index 5b130b75..b9ec3932 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/fake_controllers.yaml @@ -1,11 +1,11 @@ - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - name: fake_gripper_controller + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - name: $(arg prefix)fake_gripper_controller joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf deleted file mode 100644 index e2cd635b..00000000 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf +++ /dev/null @@ -1,197 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf.xacro b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf.xacro new file mode 100644 index 00000000..8521c61e --- /dev/null +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf.xacro @@ -0,0 +1,198 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/hard_joint_limits.yaml index eb4a100f..18bcded9 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/hard_joint_limits.yaml @@ -2,62 +2,62 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/ros_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/ros_controllers.yaml index b741c94d..f5d5e610 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/ros_controllers.yaml @@ -2,21 +2,21 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 - - name: "robotiq_2f_140_gripper_controller" + - name: "$(arg prefix)robotiq_2f_140_gripper_controller" action_ns: gripper_cmd default: True type: GripperCommand joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/default_joint_limits.yaml index f943baec..b172331f 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/default_joint_limits.yaml @@ -2,67 +2,67 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/fake_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/fake_controllers.yaml index ca87393a..57fb469b 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/fake_controllers.yaml @@ -1,13 +1,13 @@ controller_list: - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 - - name: fake_gripper_controller + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 + - name: $(arg prefix)fake_gripper_controller joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf deleted file mode 100644 index fd4a5cfd..00000000 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf +++ /dev/null @@ -1,217 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf.xacro b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf.xacro new file mode 100644 index 00000000..cd5bb0e8 --- /dev/null +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf.xacro @@ -0,0 +1,218 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/hard_joint_limits.yaml index 583d6682..7c291587 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/hard_joint_limits.yaml @@ -2,67 +2,67 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/ros_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/ros_controllers.yaml index 53436240..f1b8067b 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/ros_controllers.yaml @@ -2,22 +2,22 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 - - name: "robotiq_2f_140_gripper_controller" + - name: "$(arg prefix)robotiq_2f_140_gripper_controller" action_ns: gripper_cmd default: True type: GripperCommand joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/ompl_planning.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/ompl_planning.yaml index a49c255b..2ee2dbe8 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/ompl_planning.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/ompl_planning.yaml @@ -146,7 +146,7 @@ arm: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(joint_1,joint_2) + projection_evaluator: joints($(arg prefix)joint_1,$(arg prefix)joint_2) longest_valid_segment_fraction: 0.005 gripper: default_planner_config: RRTConnect diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/gen3_robotiq_2f_140_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/gen3_robotiq_2f_140_moveit_controller_manager.launch.xml index 0d27267f..6007253d 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/gen3_robotiq_2f_140_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/gen3_robotiq_2f_140_moveit_controller_manager.launch.xml @@ -1,11 +1,12 @@ + - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/move_group.launch index 2a927a10..765335b7 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/move_group.launch @@ -4,9 +4,13 @@ + + + + @@ -48,6 +52,7 @@ + @@ -56,6 +61,7 @@ + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/ompl_planning_pipeline.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/ompl_planning_pipeline.launch.xml index 56d02bf4..99ec2a50 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/ompl_planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/ompl_planning_pipeline.launch.xml @@ -2,6 +2,7 @@ + @@ -21,6 +22,6 @@ - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch index a9676df5..73d7424e 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch @@ -8,19 +8,22 @@ + + + - + - + - - + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_pipeline.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_pipeline.launch.xml index c2c6d8e2..4e99c9c6 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_pipeline.launch.xml @@ -4,7 +4,10 @@ It is assumed that all planning pipelines are named XXX_planning_pipeline.launch --> + - + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/trajectory_execution.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/trajectory_execution.launch.xml index 92cfbd85..3eb4a38c 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/trajectory_execution.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/trajectory_execution.launch.xml @@ -2,6 +2,7 @@ + @@ -18,6 +19,7 @@ + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/.setup_assistant b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/.setup_assistant index e8017e1b..05b9cb92 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/.setup_assistant +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/.setup_assistant @@ -2,7 +2,7 @@ moveit_setup_assistant_config: URDF: package: kortex_description relative_path: robots/gen3_robotiq_2f_85.xacro - xacro_args: --inorder arm:=gen3 gripper:=robotiq_2f_85 + xacro_args: arm:=gen3 gripper:=robotiq_2f_85 SRDF: relative_path: config/gen3_robotiq_2f_85.srdf CONFIG: diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/default_joint_limits.yaml index 426d9e04..83fdb6bc 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/default_joint_limits.yaml @@ -2,62 +2,62 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/fake_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/fake_controllers.yaml index 5b130b75..b9ec3932 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/fake_controllers.yaml @@ -1,11 +1,11 @@ - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - name: fake_gripper_controller + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - name: $(arg prefix)fake_gripper_controller joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf deleted file mode 100644 index 8ec5af66..00000000 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf +++ /dev/null @@ -1,198 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf.xacro b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf.xacro new file mode 100644 index 00000000..0ff251e0 --- /dev/null +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf.xacro @@ -0,0 +1,199 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/hard_joint_limits.yaml index eb4a100f..18bcded9 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/hard_joint_limits.yaml @@ -2,62 +2,62 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/ros_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/ros_controllers.yaml index 792f1fc1..f13b3e57 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/ros_controllers.yaml @@ -2,21 +2,21 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 - - name: "robotiq_2f_85_gripper_controller" + - name: "$(arg prefix)robotiq_2f_85_gripper_controller" action_ns: gripper_cmd default: True type: GripperCommand joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/default_joint_limits.yaml index f943baec..b172331f 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/default_joint_limits.yaml @@ -2,67 +2,67 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/fake_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/fake_controllers.yaml index ca87393a..57fb469b 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/fake_controllers.yaml @@ -1,13 +1,13 @@ controller_list: - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 - - name: fake_gripper_controller + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 + - name: $(arg prefix)fake_gripper_controller joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf deleted file mode 100644 index a15f8f84..00000000 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf +++ /dev/null @@ -1,217 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf.xacro b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf.xacro new file mode 100644 index 00000000..0463d2de --- /dev/null +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf.xacro @@ -0,0 +1,218 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/hard_joint_limits.yaml index 583d6682..7c291587 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/hard_joint_limits.yaml @@ -2,67 +2,67 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/ros_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/ros_controllers.yaml index 5b62d88e..f401d726 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/ros_controllers.yaml @@ -2,22 +2,22 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 - - name: "robotiq_2f_85_gripper_controller" + - name: "$(arg prefix)robotiq_2f_85_gripper_controller" action_ns: gripper_cmd default: True type: GripperCommand joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/ompl_planning.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/ompl_planning.yaml index a49c255b..2ee2dbe8 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/ompl_planning.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/ompl_planning.yaml @@ -146,7 +146,7 @@ arm: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(joint_1,joint_2) + projection_evaluator: joints($(arg prefix)joint_1,$(arg prefix)joint_2) longest_valid_segment_fraction: 0.005 gripper: default_planner_config: RRTConnect diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/gen3_robotiq_2f_85_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/gen3_robotiq_2f_85_moveit_controller_manager.launch.xml index 8ba0c8f6..1d654026 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/gen3_robotiq_2f_85_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/gen3_robotiq_2f_85_moveit_controller_manager.launch.xml @@ -1,11 +1,12 @@ + - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/move_group.launch index efaa685e..806b0a22 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/move_group.launch @@ -4,9 +4,13 @@ + + + + @@ -48,6 +52,7 @@ + @@ -56,6 +61,7 @@ + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ompl_planning_pipeline.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ompl_planning_pipeline.launch.xml index 6f683627..5ef1a126 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ompl_planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ompl_planning_pipeline.launch.xml @@ -2,6 +2,7 @@ + @@ -21,6 +22,6 @@ - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch index 583764e0..72420090 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch @@ -7,20 +7,23 @@ + + + - + - + - - + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_pipeline.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_pipeline.launch.xml index ab340f3d..438586b2 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_pipeline.launch.xml @@ -4,7 +4,10 @@ It is assumed that all planning pipelines are named XXX_planning_pipeline.launch --> + - + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/trajectory_execution.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/trajectory_execution.launch.xml index d8509a8f..55e216ca 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/trajectory_execution.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/trajectory_execution.launch.xml @@ -2,6 +2,7 @@ + @@ -18,6 +19,7 @@ +