diff --git a/.gitignore b/.gitignore index abfbf270..ff6b363e 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ -kortex_driver/build \ No newline at end of file +kortex_driver/build +kortex_driver/protos \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 120000 index 00000000..581e61db --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1 @@ +/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/kortex_api/scripts/download_kortex_api.bash b/kortex_api/scripts/download_kortex_api.bash index 07eaf857..f32400b3 100755 --- a/kortex_api/scripts/download_kortex_api.bash +++ b/kortex_api/scripts/download_kortex_api.bash @@ -29,7 +29,7 @@ fi # Download the API from Google Drive echo "Downloading the Kortex API from the Web..." -wget -q -O kortex_api.zip https://artifactory.kinovaapps.com/artifactory/generic-local-public/kortex/API/2.0.0/kortex_api_2.0.0.zip +jfrog rt dl --flat generic-local-stable/API/2.1.0/kortex_api_2.1.0-3.zip ./ RESULT=$? if [ "${RESULT}" -ne 0 ]; then echo "ERROR while fetching the kortex api. code = ${RESULT}" @@ -37,6 +37,7 @@ if [ "${RESULT}" -ne 0 ]; then fi # Unzip it +mv kortex_api_2.1.0-3.zip kortex_api.zip unzip -d kortex_api kortex_api.zip > /dev/null RESULT=$? if [ "${RESULT}" -ne 0 ]; then diff --git a/kortex_control/arms/gen3/config/joint_position_controllers.yaml b/kortex_control/arms/gen3/7dof/config/joint_position_controllers.yaml similarity index 100% rename from kortex_control/arms/gen3/config/joint_position_controllers.yaml rename to kortex_control/arms/gen3/7dof/config/joint_position_controllers.yaml 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 new file mode 100644 index 00000000..05c1c470 --- /dev/null +++ b/kortex_control/arms/gen3_lite/6dof/config/joint_position_controllers.yaml @@ -0,0 +1,75 @@ +# Publish all joint states ----------------------------------- +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + +gen3_lite_joint_trajectory_controller: + type: effort_controllers/JointTrajectoryController + joints: + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 + constraints: + goal_time: 1.0 + stopped_velocity_tolerance: 0.5 + stop_trajectory_duration: 1.0 + 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} + +joint_1_position_controller: + joint: joint_1 + pid: + p: 3000.0 + i: 0.0 + d: 2.0 + type: effort_controllers/JointPositionController + +joint_2_position_controller: + joint: joint_2 + pid: + p: 50000.0 + i: 0.0 + d: 0.0 + type: effort_controllers/JointPositionController + +joint_3_position_controller: + joint: joint_3 + pid: + p: 50000.0 + i: 0.0 + d: 0.0 + type: effort_controllers/JointPositionController + +joint_4_position_controller: + joint: joint_4 + pid: + p: 750.0 + i: 0.0 + d: 0.2 + type: effort_controllers/JointPositionController + +joint_5_position_controller: + joint: joint_5 + pid: + p: 5000.0 + i: 0.0 + d: 1.0 + type: effort_controllers/JointPositionController + +joint_6_position_controller: + joint: joint_6 + pid: + p: 100.0 + i: 0.0 + d: 0.0 + type: effort_controllers/JointPositionController 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 new file mode 100644 index 00000000..7d8c81d0 --- /dev/null +++ b/kortex_control/grippers/gen3_lite_2f/config/gripper_action_controller_parameters.yaml @@ -0,0 +1,11 @@ +gen3_lite_2f_gripper_controller: + type: position_controllers/GripperActionController + joint: 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 diff --git a/kortex_control/readme.md b/kortex_control/readme.md index f90d68cf..65e53502 100644 --- a/kortex_control/readme.md +++ b/kortex_control/readme.md @@ -21,4 +21,4 @@ The `joint_position_controllers.yaml` file for the chosen arm is loaded to the P ## ROS Control support for the real arm -ROS Control support for the Kinova Gen3 Ultra lightweight robot is not currently available and will be part of a future release. +ROS Control support for the Kinova Gen3 and Gen3 lite robots is not currently available and will be part of a future release. 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 new file mode 100644 index 00000000..7cef4ab6 --- /dev/null +++ b/kortex_description/arms/gen3/7dof/config/gazebo_initial_joint_positions.yaml @@ -0,0 +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 diff --git a/kortex_description/arms/gen3/config/joint_limits.yaml b/kortex_description/arms/gen3/7dof/config/joint_limits.yaml similarity index 100% rename from kortex_description/arms/gen3/config/joint_limits.yaml rename to kortex_description/arms/gen3/7dof/config/joint_limits.yaml diff --git a/kortex_description/arms/gen3/meshes/base_link.STL b/kortex_description/arms/gen3/7dof/meshes/base_link.STL similarity index 100% rename from kortex_description/arms/gen3/meshes/base_link.STL rename to kortex_description/arms/gen3/7dof/meshes/base_link.STL diff --git a/kortex_description/arms/gen3/meshes/bracelet_link.STL b/kortex_description/arms/gen3/7dof/meshes/bracelet_link.STL similarity index 100% rename from kortex_description/arms/gen3/meshes/bracelet_link.STL rename to kortex_description/arms/gen3/7dof/meshes/bracelet_link.STL diff --git a/kortex_description/arms/gen3/meshes/end_effector_link.STL b/kortex_description/arms/gen3/7dof/meshes/end_effector_link.STL similarity index 100% rename from kortex_description/arms/gen3/meshes/end_effector_link.STL rename to kortex_description/arms/gen3/7dof/meshes/end_effector_link.STL diff --git a/kortex_description/arms/gen3/meshes/forearm_link.STL b/kortex_description/arms/gen3/7dof/meshes/forearm_link.STL similarity index 100% rename from kortex_description/arms/gen3/meshes/forearm_link.STL rename to kortex_description/arms/gen3/7dof/meshes/forearm_link.STL diff --git a/kortex_description/arms/gen3/meshes/half_arm_1_link.STL b/kortex_description/arms/gen3/7dof/meshes/half_arm_1_link.STL similarity index 100% rename from kortex_description/arms/gen3/meshes/half_arm_1_link.STL rename to kortex_description/arms/gen3/7dof/meshes/half_arm_1_link.STL diff --git a/kortex_description/arms/gen3/meshes/half_arm_2_link.STL b/kortex_description/arms/gen3/7dof/meshes/half_arm_2_link.STL similarity index 100% rename from kortex_description/arms/gen3/meshes/half_arm_2_link.STL rename to kortex_description/arms/gen3/7dof/meshes/half_arm_2_link.STL diff --git a/kortex_description/arms/gen3/meshes/shoulder_link.STL b/kortex_description/arms/gen3/7dof/meshes/shoulder_link.STL similarity index 100% rename from kortex_description/arms/gen3/meshes/shoulder_link.STL rename to kortex_description/arms/gen3/7dof/meshes/shoulder_link.STL diff --git a/kortex_description/arms/gen3/meshes/spherical_wrist_1_link.STL b/kortex_description/arms/gen3/7dof/meshes/spherical_wrist_1_link.STL similarity index 100% rename from kortex_description/arms/gen3/meshes/spherical_wrist_1_link.STL rename to kortex_description/arms/gen3/7dof/meshes/spherical_wrist_1_link.STL diff --git a/kortex_description/arms/gen3/meshes/spherical_wrist_2_link.STL b/kortex_description/arms/gen3/7dof/meshes/spherical_wrist_2_link.STL similarity index 100% rename from kortex_description/arms/gen3/meshes/spherical_wrist_2_link.STL rename to kortex_description/arms/gen3/7dof/meshes/spherical_wrist_2_link.STL diff --git a/kortex_description/arms/gen3/urdf/GEN3_URDF_V12.urdf b/kortex_description/arms/gen3/7dof/urdf/GEN3_URDF_V12.urdf similarity index 100% rename from kortex_description/arms/gen3/urdf/GEN3_URDF_V12.urdf rename to kortex_description/arms/gen3/7dof/urdf/GEN3_URDF_V12.urdf diff --git a/kortex_description/arms/gen3/urdf/gen3_macro.xacro b/kortex_description/arms/gen3/7dof/urdf/gen3_macro.xacro similarity index 89% rename from kortex_description/arms/gen3/urdf/gen3_macro.xacro rename to kortex_description/arms/gen3/7dof/urdf/gen3_macro.xacro index a963c603..5e939203 100644 --- a/kortex_description/arms/gen3/urdf/gen3_macro.xacro +++ b/kortex_description/arms/gen3/7dof/urdf/gen3_macro.xacro @@ -5,7 +5,7 @@ - + @@ -15,7 +15,7 @@ - + @@ -24,7 +24,7 @@ - + @@ -37,7 +37,7 @@ - + @@ -46,7 +46,7 @@ - + @@ -73,7 +73,7 @@ - + @@ -82,7 +82,7 @@ - + @@ -107,7 +107,7 @@ - + @@ -116,7 +116,7 @@ - + @@ -143,7 +143,7 @@ - + @@ -152,7 +152,7 @@ - + @@ -177,7 +177,7 @@ - + @@ -186,7 +186,7 @@ - + @@ -213,7 +213,7 @@ - + @@ -222,7 +222,7 @@ - + @@ -247,7 +247,7 @@ - + @@ -256,7 +256,7 @@ - + diff --git a/kortex_description/arms/gen3/urdf/gen3_transmission_macro.xacro b/kortex_description/arms/gen3/7dof/urdf/gen3_transmission_macro.xacro similarity index 100% rename from kortex_description/arms/gen3/urdf/gen3_transmission_macro.xacro rename to kortex_description/arms/gen3/7dof/urdf/gen3_transmission_macro.xacro diff --git a/kortex_description/arms/gen3_lite/6dof/config/joint_limits.yaml b/kortex_description/arms/gen3_lite/6dof/config/joint_limits.yaml new file mode 100644 index 00000000..8692c090 --- /dev/null +++ b/kortex_description/arms/gen3_lite/6dof/config/joint_limits.yaml @@ -0,0 +1,7 @@ +joint_names: + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 diff --git a/kortex_description/arms/gen3_lite/6dof/meshes/arm_link.STL b/kortex_description/arms/gen3_lite/6dof/meshes/arm_link.STL new file mode 100644 index 00000000..ca9a4bd0 Binary files /dev/null and b/kortex_description/arms/gen3_lite/6dof/meshes/arm_link.STL differ diff --git a/kortex_description/arms/gen3_lite/6dof/meshes/base_link.STL b/kortex_description/arms/gen3_lite/6dof/meshes/base_link.STL new file mode 100644 index 00000000..f861db3d Binary files /dev/null and b/kortex_description/arms/gen3_lite/6dof/meshes/base_link.STL differ diff --git a/kortex_description/arms/gen3_lite/6dof/meshes/forearm_link.STL b/kortex_description/arms/gen3_lite/6dof/meshes/forearm_link.STL new file mode 100644 index 00000000..af4de3d5 Binary files /dev/null and b/kortex_description/arms/gen3_lite/6dof/meshes/forearm_link.STL differ diff --git a/kortex_description/arms/gen3_lite/6dof/meshes/lower_wrist_link.STL b/kortex_description/arms/gen3_lite/6dof/meshes/lower_wrist_link.STL new file mode 100644 index 00000000..4685f96a Binary files /dev/null and b/kortex_description/arms/gen3_lite/6dof/meshes/lower_wrist_link.STL differ diff --git a/kortex_description/arms/gen3_lite/6dof/meshes/shoulder_link.STL b/kortex_description/arms/gen3_lite/6dof/meshes/shoulder_link.STL new file mode 100644 index 00000000..20c2f6d5 Binary files /dev/null and b/kortex_description/arms/gen3_lite/6dof/meshes/shoulder_link.STL differ diff --git a/kortex_description/arms/gen3_lite/6dof/meshes/upper_wrist_link.STL b/kortex_description/arms/gen3_lite/6dof/meshes/upper_wrist_link.STL new file mode 100644 index 00000000..56197361 Binary files /dev/null and b/kortex_description/arms/gen3_lite/6dof/meshes/upper_wrist_link.STL differ diff --git a/kortex_description/arms/gen3_lite/6dof/urdf/GEN3-LITE.urdf b/kortex_description/arms/gen3_lite/6dof/urdf/GEN3-LITE.urdf new file mode 100644 index 00000000..bc02cb9f --- /dev/null +++ b/kortex_description/arms/gen3_lite/6dof/urdf/GEN3-LITE.urdf @@ -0,0 +1,188 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/kortex_description/arms/gen3_lite/6dof/urdf/gen3_lite_macro.xacro b/kortex_description/arms/gen3_lite/6dof/urdf/gen3_lite_macro.xacro new file mode 100644 index 00000000..d7a165bf --- /dev/null +++ b/kortex_description/arms/gen3_lite/6dof/urdf/gen3_lite_macro.xacro @@ -0,0 +1,193 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_description/arms/gen3_lite/6dof/urdf/gen3_lite_transmission_macro.xacro b/kortex_description/arms/gen3_lite/6dof/urdf/gen3_lite_transmission_macro.xacro new file mode 100644 index 00000000..84dc59a3 --- /dev/null +++ b/kortex_description/arms/gen3_lite/6dof/urdf/gen3_lite_transmission_macro.xacro @@ -0,0 +1,68 @@ + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + \ No newline at end of file diff --git a/kortex_description/grippers/gen3_lite_2f/config/joint_limits.yaml b/kortex_description/grippers/gen3_lite_2f/config/joint_limits.yaml new file mode 100644 index 00000000..fb76b03e --- /dev/null +++ b/kortex_description/grippers/gen3_lite_2f/config/joint_limits.yaml @@ -0,0 +1,8 @@ +gripper_joint_names: + - right_finger_bottom_joint + +gripper_joint_limits_min: + - 0.96 + +gripper_joint_limits_max: + - -0.09 \ No newline at end of file diff --git a/kortex_description/grippers/gen3_lite_2f/meshes/gripper_base_link.STL b/kortex_description/grippers/gen3_lite_2f/meshes/gripper_base_link.STL new file mode 100644 index 00000000..87759615 Binary files /dev/null and b/kortex_description/grippers/gen3_lite_2f/meshes/gripper_base_link.STL differ diff --git a/kortex_description/grippers/gen3_lite_2f/meshes/left_finger_dist_link.STL b/kortex_description/grippers/gen3_lite_2f/meshes/left_finger_dist_link.STL new file mode 100644 index 00000000..c4bdc8ff Binary files /dev/null and b/kortex_description/grippers/gen3_lite_2f/meshes/left_finger_dist_link.STL differ diff --git a/kortex_description/grippers/gen3_lite_2f/meshes/left_finger_prox_link.STL b/kortex_description/grippers/gen3_lite_2f/meshes/left_finger_prox_link.STL new file mode 100644 index 00000000..fed7fe59 Binary files /dev/null and b/kortex_description/grippers/gen3_lite_2f/meshes/left_finger_prox_link.STL differ diff --git a/kortex_description/grippers/gen3_lite_2f/meshes/right_finger_dist_link.STL b/kortex_description/grippers/gen3_lite_2f/meshes/right_finger_dist_link.STL new file mode 100644 index 00000000..eb2e844d Binary files /dev/null and b/kortex_description/grippers/gen3_lite_2f/meshes/right_finger_dist_link.STL differ diff --git a/kortex_description/grippers/gen3_lite_2f/meshes/right_finger_prox_link.STL b/kortex_description/grippers/gen3_lite_2f/meshes/right_finger_prox_link.STL new file mode 100644 index 00000000..430f1644 Binary files /dev/null and b/kortex_description/grippers/gen3_lite_2f/meshes/right_finger_prox_link.STL differ diff --git a/kortex_description/grippers/gen3_lite_2f/urdf/GEN3-LITE-GRIPPER_URDF_SIMPLIFIED_V2.urdf b/kortex_description/grippers/gen3_lite_2f/urdf/GEN3-LITE-GRIPPER_URDF_SIMPLIFIED_V2.urdf new file mode 100644 index 00000000..931e71b5 --- /dev/null +++ b/kortex_description/grippers/gen3_lite_2f/urdf/GEN3-LITE-GRIPPER_URDF_SIMPLIFIED_V2.urdf @@ -0,0 +1,125 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_macro.xacro b/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_macro.xacro new file mode 100644 index 00000000..fa26bc53 --- /dev/null +++ b/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_macro.xacro @@ -0,0 +1,157 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 new file mode 100644 index 00000000..57bed74f --- /dev/null +++ b/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_transmission_macro.xacro @@ -0,0 +1,62 @@ + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + hardware_interface/PositionJointInterface + + + + + + right_finger_bottom_joint + right_finger_tip_joint + -0.676 + 0.149 + 5.0 + + + + right_finger_bottom_joint + left_finger_bottom_joint + -1.0 + 0.0 + 5.0 + + + + right_finger_bottom_joint + left_finger_tip_joint + -0.676 + 0.149 + 5.0 + + + + + + my_gen3_lite_gripper + end_effector_link + right_finger_dist_link + left_finger_dist_link + + 100 + 10 + 3 + 10 + 0.001 + false + __default_topic__ + + + + + + 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 b566f703..adda3d77 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,8 @@ gripper_joint_names: - gripper_finger1_joint -gripper_joint_limits: +gripper_joint_limits_min: + - 0.000 + +gripper_joint_limits_max: - 0.804 \ No newline at end of file diff --git a/kortex_description/readme.md b/kortex_description/readme.md index 205f3768..4a52a139 100644 --- a/kortex_description/readme.md +++ b/kortex_description/readme.md @@ -15,11 +15,15 @@ This package contains the URDF (Unified Robot Description Format), STL and confi ## Usage -To load the description of a robot, you simply have to load the **ARM.xacro** or the **ARM_GRIPPER.xacro** file, with **ARM** being your arm's name (gen3), and if you have a gripper, **GRIPPER** being your gripper's name (robotiq_2f_85). +To load the description of a robot, you simply have to load the **ARM.xacro** or the **ARM_GRIPPER.xacro** file, with **ARM** being your arm's name (gen3, gen3_lite), and if you have a gripper, **GRIPPER** being your gripper's name (robotiq_2f_85, gen3_lite_2f). **Arguments**: - **sim** : If this argument is true, the Gazebo-specific files will be loaded. The default value is **false **. -For example, if you want to load for simulation the Gen3 description with a Robotiq 2-F 85 gripper as the gripper, you would put in your launch file : +For example: - \ No newline at end of file +- 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 : + \ No newline at end of file diff --git a/kortex_description/robots/gen3.xacro b/kortex_description/robots/gen3.xacro index e08409c1..d90c02d7 100644 --- a/kortex_description/robots/gen3.xacro +++ b/kortex_description/robots/gen3.xacro @@ -4,10 +4,14 @@ + + + + - + \ No newline at end of file diff --git a/kortex_description/robots/gen3_lite_gen3_lite_2f.xacro b/kortex_description/robots/gen3_lite_gen3_lite_2f.xacro new file mode 100644 index 00000000..54920d8d --- /dev/null +++ b/kortex_description/robots/gen3_lite_gen3_lite_2f.xacro @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/kortex_description/robots/gen3_robotiq_2f_85.xacro b/kortex_description/robots/gen3_robotiq_2f_85.xacro index d4213c31..c18df203 100644 --- a/kortex_description/robots/gen3_robotiq_2f_85.xacro +++ b/kortex_description/robots/gen3_robotiq_2f_85.xacro @@ -4,10 +4,14 @@ + + + + - + \ No newline at end of file diff --git a/kortex_description/robots/kortex_robot.xacro b/kortex_description/robots/kortex_robot.xacro index 85e5dce9..92683f34 100644 --- a/kortex_description/robots/kortex_robot.xacro +++ b/kortex_description/robots/kortex_robot.xacro @@ -2,22 +2,17 @@ - - - - - - - + + - + - + - + @@ -27,7 +22,7 @@ - + @@ -37,17 +32,17 @@ - + - + - + diff --git a/kortex_driver/CMakeLists.txt b/kortex_driver/CMakeLists.txt index 42835421..990cf0ba 100644 --- a/kortex_driver/CMakeLists.txt +++ b/kortex_driver/CMakeLists.txt @@ -1,11 +1,9 @@ - cmake_minimum_required(VERSION 2.8.3) project(kortex_driver) add_compile_options(-std=c++11) add_definitions(-D_OS_UNIX) - find_path (KORTEX_API_DOWNLOADER_PATH # Set this variable download_kortex_api.bash # To the Working dir of this file ${PROJECT_SOURCE_DIR}/../kortex_api/scripts) # Search this file here diff --git a/kortex_driver/include/kortex_driver/generated/base_services.h b/kortex_driver/include/kortex_driver/generated/base_services.h index 62a465f1..bc81955a 100644 --- a/kortex_driver/include/kortex_driver/generated/base_services.h +++ b/kortex_driver/include/kortex_driver/generated/base_services.h @@ -53,8 +53,13 @@ #include "kortex_driver/ReadAllProtectionZones.h" #include "kortex_driver/CreateMapping.h" #include "kortex_driver/ReadMapping.h" +#include "kortex_driver/UpdateMapping.h" +#include "kortex_driver/DeleteMapping.h" #include "kortex_driver/ReadAllMappings.h" #include "kortex_driver/CreateMap.h" +#include "kortex_driver/ReadMap.h" +#include "kortex_driver/UpdateMap.h" +#include "kortex_driver/DeleteMap.h" #include "kortex_driver/ReadAllMaps.h" #include "kortex_driver/ActivateMap.h" #include "kortex_driver/CreateAction.h" @@ -210,8 +215,13 @@ class BaseServices bool ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res); bool CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res); bool ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res); + bool UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res); + bool DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res); bool ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res); bool CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res); + bool ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res); + bool UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res); + bool DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res); bool ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res); bool ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res); bool CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res); @@ -393,8 +403,13 @@ class BaseServices ros::ServiceServer m_serviceReadAllProtectionZones; ros::ServiceServer m_serviceCreateMapping; ros::ServiceServer m_serviceReadMapping; + ros::ServiceServer m_serviceUpdateMapping; + ros::ServiceServer m_serviceDeleteMapping; ros::ServiceServer m_serviceReadAllMappings; ros::ServiceServer m_serviceCreateMap; + ros::ServiceServer m_serviceReadMap; + ros::ServiceServer m_serviceUpdateMap; + ros::ServiceServer m_serviceDeleteMap; ros::ServiceServer m_serviceReadAllMaps; ros::ServiceServer m_serviceActivateMap; ros::ServiceServer m_serviceCreateAction; 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 d6992f54..b7bd6fda 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 @@ -88,9 +88,15 @@ class KortexArmDriver std::string m_gripper_name; std::vector m_gripper_joint_names; - std::vector m_gripper_joint_limits; + std::vector m_gripper_joint_limits_min; + std::vector m_gripper_joint_limits_max; + int m_degrees_of_freedom; + + bool m_is_interconnect_module_present; int m_interconnect_device_id; + + bool m_is_vision_module_present; int m_vision_device_id; // Kortex Api objects diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_math_util.h b/kortex_driver/include/kortex_driver/non-generated/kortex_math_util.h index 3bc6e7ca..9c94d61e 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_math_util.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_math_util.h @@ -23,6 +23,8 @@ class KortexMathUtil static double toDeg(double rad); static double wrapRadiansFromMinusPiToPi(double rad_not_wrapped); static double wrapDegreesFromZeroTo360(double deg_not_wrapped); + static double relative_position_from_absolute(double absolute_position, double min_value, double max_value); + static double absolute_position_from_relative(double relative_position, double min_value, double max_value); }; #endif \ No newline at end of file diff --git a/kortex_driver/include/kortex_driver/non-generated/robotiq_gripper_command_action_server.h b/kortex_driver/include/kortex_driver/non-generated/robotiq_gripper_command_action_server.h index 494b74ff..a44df86e 100644 --- a/kortex_driver/include/kortex_driver/non-generated/robotiq_gripper_command_action_server.h +++ b/kortex_driver/include/kortex_driver/non-generated/robotiq_gripper_command_action_server.h @@ -24,18 +24,18 @@ #include "BaseCyclicClientRpc.h" #include "Errors.pb.h" +#include "kortex_math_util.h" + // Duration timeout for a gripper trajectory (in seconds) #define GRIPPER_TRAJECTORY_TIME_LIMIT 2.0 -// Robotiq Gripper relative position precision -// Augmented to 2/255 instead of 1/255 because sometimes there are false negatives with 1/255 -#define ROBOTIQ_GRIPPER_RELATIVE_ERROR 2.0/255.0 +#define MAX_GRIPPER_RELATIVE_ERROR 0.05 class RobotiqGripperCommandActionServer { public: RobotiqGripperCommandActionServer() = delete; - RobotiqGripperCommandActionServer(const std::string& server_name, const std::string& gripper_joint_name, double gripper_joint_limit, ros::NodeHandle& nh, Kinova::Api::Base::BaseClient* base, Kinova::Api::BaseCyclic::BaseCyclicClient* base_cyclic); + RobotiqGripperCommandActionServer(const std::string& server_name, const std::string& gripper_joint_name, double gripper_joint_limit_min, double gripper_joint_limit_max, ros::NodeHandle& nh, Kinova::Api::Base::BaseClient* base, Kinova::Api::BaseCyclic::BaseCyclicClient* base_cyclic); ~RobotiqGripperCommandActionServer(); private: @@ -60,9 +60,12 @@ class RobotiqGripperCommandActionServer std::mutex m_is_trajectory_running_lock; std::thread m_gripper_position_polling_thread; + KortexMathUtil m_math_util; + // ROS Params std::string m_gripper_joint_name; - double m_gripper_joint_limit; + double m_gripper_joint_limit_min; + double m_gripper_joint_limit_max; // Action Server Callbacks void goal_received_callback(actionlib::ActionServer::GoalHandle new_goal_handle); @@ -76,8 +79,6 @@ class RobotiqGripperCommandActionServer bool is_goal_tolerance_respected(); void stop_all_movement(); void join_polling_thread(); - double relative_position_from_absolute(double absolute_position); - double absolute_position_from_relative(double relative_position); }; #endif \ No newline at end of file diff --git a/kortex_driver/launch/kortex_driver.launch b/kortex_driver/launch/kortex_driver.launch index 53a0cc1a..fcd9faff 100644 --- a/kortex_driver/launch/kortex_driver.launch +++ b/kortex_driver/launch/kortex_driver.launch @@ -1,9 +1,16 @@ - - - + + + + + + + + + + @@ -27,10 +34,10 @@ - - @@ -46,7 +53,8 @@ - + + @@ -56,11 +64,13 @@ - + + + - + + + @@ -68,6 +78,7 @@ [base_feedback/joint_state] + diff --git a/kortex_driver/msg/generated/SubErrorCodes.msg b/kortex_driver/msg/generated/SubErrorCodes.msg index 06c47776..4d8bba03 100644 --- a/kortex_driver/msg/generated/SubErrorCodes.msg +++ b/kortex_driver/msg/generated/SubErrorCodes.msg @@ -173,6 +173,8 @@ uint32 ROUTER_UNVAILABLE = 108 uint32 ADDRESS_NOT_IN_VALID_RANGE = 120 +uint32 ADDRESS_NOT_CONFIGURABLE = 121 + uint32 SESSION_NOT_IN_CONTROL = 130 uint32 METHOD_TIMEOUT = 131 diff --git a/kortex_driver/msg/generated/actuator_config/SafetyIdentifierBankA.msg b/kortex_driver/msg/generated/actuator_config/SafetyIdentifierBankA.msg index f18f02aa..8af8dc2b 100644 --- a/kortex_driver/msg/generated/actuator_config/SafetyIdentifierBankA.msg +++ b/kortex_driver/msg/generated/actuator_config/SafetyIdentifierBankA.msg @@ -52,3 +52,5 @@ uint32 WATCHDOG_TRIGGERED = 8388608 uint32 UNRELIABLE_CAPACITIVE_SENSOR = 16777216 uint32 UNEXPECTED_GEAR_RATIO = 33554432 + +uint32 HALL_MAGNETIC_MISMATCH = 67108864 diff --git a/kortex_driver/msg/generated/base/NetworkType.msg b/kortex_driver/msg/generated/base/NetworkType.msg index ec8fc9c0..3421f3a1 100644 --- a/kortex_driver/msg/generated/base/NetworkType.msg +++ b/kortex_driver/msg/generated/base/NetworkType.msg @@ -4,3 +4,7 @@ uint32 UNSPECIFIED_NETWORK_TYPE = 0 uint32 WIFI = 1 uint32 WIRED_ETHERNET = 2 + +uint32 WIRED_MICROUSB = 3 + +uint32 WIRED_USB = 4 diff --git a/kortex_driver/protos/ActuatorConfig.options b/kortex_driver/protos/ActuatorConfig.options deleted file mode 100644 index bebe2cc0..00000000 --- a/kortex_driver/protos/ActuatorConfig.options +++ /dev/null @@ -1,22 +0,0 @@ -// -// KINOVA (R) KORTEX (TM) -// -// Copyright (c) 2018 Kinova inc. All rights reserved. -// -// This software may be modified and distributed -// under the terms of the BSD 3-Clause license. -// -// Refer to the LICENSE file for details. -// -// - -Kinova.Api.ActuatorConfig.TorqueCalibration.gain max_count:4 fixed_count:true -Kinova.Api.ActuatorConfig.TorqueCalibration.offset max_count:4 fixed_count:true -Kinova.Api.ActuatorConfig.ControlLoopParameters.kAz max_count:5 fixed_count:true -Kinova.Api.ActuatorConfig.ControlLoopParameters.kBz max_count:6 fixed_count:true -Kinova.Api.ActuatorConfig.CustomDataSelection.channel max_count:16 -Kinova.Api.ActuatorConfig.SafetyLimitType long_names:false -Kinova.Api.ActuatorConfig.ControlMode long_names:false -Kinova.Api.ActuatorConfig.CommandMode long_names:false -Kinova.Api.ActuatorConfig.SafetyIdentifierBankA long_names:false -Kinova.Api.ActuatorConfig.ControlLoopSelection long_names:false diff --git a/kortex_driver/protos/ActuatorConfig.proto b/kortex_driver/protos/ActuatorConfig.proto index bd771c70..c805739d 100644 --- a/kortex_driver/protos/ActuatorConfig.proto +++ b/kortex_driver/protos/ActuatorConfig.proto @@ -314,6 +314,7 @@ enum SafetyIdentifierBankA { WATCHDOG_TRIGGERED = 8388608; // 0x800000 - Watchdog triggered UNRELIABLE_CAPACITIVE_SENSOR = 16777216; // 0x1000000 - Capacitive sensor is unreliable UNEXPECTED_GEAR_RATIO = 33554432; // 0x2000000 - Incorrect gear ratio for detected configuration + HALL_MAGNETIC_MISMATCH = 67108864; // 0x4000000 - Position mismatch between hall and magnetic sensors } // Custom data options diff --git a/kortex_driver/protos/Base.options b/kortex_driver/protos/Base.options deleted file mode 100644 index 7d50d5c0..00000000 --- a/kortex_driver/protos/Base.options +++ /dev/null @@ -1,13 +0,0 @@ -// -// KINOVA (R) KORTEX (TM) -// -// Copyright (c) 2019 Kinova inc. All rights reserved. -// -// This software may be modified and distributed -// under the terms of the BSD 3-Clause license. -// -// Refer to the LICENSE file for details. -// -// - -Kinova.Api.Base.TrajectoryErrorReport.trajectory_error_elements max_count:50 diff --git a/kortex_driver/protos/Base.proto b/kortex_driver/protos/Base.proto index 7a6b0100..5c7c83ed 100644 --- a/kortex_driver/protos/Base.proto +++ b/kortex_driver/protos/Base.proto @@ -102,12 +102,27 @@ service Base {//@PROXY_ID=2 @ERROR=Kinova.Api.Error // Retrieves an existing mapping rpc ReadMapping (MappingHandle) returns (Mapping);//@RPC_ID=27 + // Updates an existing mapping + rpc UpdateMapping (Mapping) returns (Kinova.Api.Common.Empty);//@RPC_ID=28 + + // Deletes an existing mapping + rpc DeleteMapping (MappingHandle) returns (Kinova.Api.Common.Empty);//@RPC_ID=29 + // Retrieves a list of all mappings rpc ReadAllMappings (Kinova.Api.Common.Empty) returns (MappingList);//@RPC_ID=30 // Creates a new map rpc CreateMap (Map) returns (MapHandle);//@RPC_ID=36 + // Retrieves an existing map + rpc ReadMap (MapHandle) returns (Map);//@RPC_ID=37 + + // Updates an existing map + rpc UpdateMap (Map) returns (Kinova.Api.Common.Empty);//@RPC_ID=38 + + // Deletes an existing map + rpc DeleteMap (MapHandle) returns (Kinova.Api.Common.Empty);//@RPC_ID=39 + // Retrieves a list of all maps associated to the specified mapping rpc ReadAllMaps (MappingHandle) returns (MapList);//@RPC_ID=40 @@ -649,6 +664,8 @@ enum NetworkType { UNSPECIFIED_NETWORK_TYPE = 0; // Unspecified network type WIFI = 1; // Wi-Fi network WIRED_ETHERNET = 2; // Wired Ethernet network + WIRED_MICROUSB = 3; // Wired Ethernet over USB network (RNDIS) + WIRED_USB = 4; // Wired Ethernet over USB network } // Reference to a network diff --git a/kortex_driver/protos/Common.options b/kortex_driver/protos/Common.options deleted file mode 100644 index 75e5be59..00000000 --- a/kortex_driver/protos/Common.options +++ /dev/null @@ -1,24 +0,0 @@ -// -// KINOVA (R) KORTEX (TM) -// -// Copyright (c) 2018 Kinova inc. All rights reserved. -// -// This software may be modified and distributed -// under the terms of the BSD 3-Clause license. -// -// Refer to the LICENSE file for details. -// -// - -Kinova.Api.Common.DeviceTypes long_names:false -Kinova.Api.Common.SafetyStatusValue long_names:false -Kinova.Api.Common.NotificationType long_names:false -Kinova.Api.Common.Unit long_names:false - - -Kinova.Api.Common.UARTSpeed long_names:false -Kinova.Api.Common.UARTWordLength long_names:false -Kinova.Api.Common.UARTStopBits long_names:false -Kinova.Api.Common.UARTParity long_names:false - - diff --git a/kortex_driver/protos/DeviceConfig.options b/kortex_driver/protos/DeviceConfig.options deleted file mode 100644 index 7a80df2d..00000000 --- a/kortex_driver/protos/DeviceConfig.options +++ /dev/null @@ -1,21 +0,0 @@ -// -// KINOVA (R) KORTEX (TM) -// -// Copyright (c) 2018 Kinova inc. All rights reserved. -// -// This software may be modified and distributed -// under the terms of the BSD 3-Clause license. -// -// Refer to the LICENSE file for details. -// -// - -Kinova.Api.DeviceConfig.ModelNumber.model_number max_size:25 -Kinova.Api.DeviceConfig.PartNumber.part_number max_size:25 -Kinova.Api.DeviceConfig.SerialNumber.serial_number max_size:25 -Kinova.Api.DeviceConfig.PartNumberRevision.part_number_revision max_size:9 -Kinova.Api.DeviceConfig.MACAddress.mac_address max_size:6 fixed_length:true -Kinova.Api.DeviceConfig.RunModes long_names:false -Kinova.Api.DeviceConfig.SafetyLimitType long_names:false - -Kinova.Api.DeviceConfig.Calibration.calibration_parameter max_count:4 fixed_count:false diff --git a/kortex_driver/protos/Errors.options b/kortex_driver/protos/Errors.options deleted file mode 100644 index 8cead0c0..00000000 --- a/kortex_driver/protos/Errors.options +++ /dev/null @@ -1,14 +0,0 @@ -// -// KINOVA (R) KORTEX (TM) -// -// Copyright (c) 2018 Kinova inc. All rights reserved. -// -// This software may be modified and distributed -// under the terms of the BSD 3-Clause license. -// -// Refer to the LICENSE file for details. -// -// - -Kinova.Api.ErrorCodes long_names:false -Kinova.Api.SubErrorCodes long_names:false diff --git a/kortex_driver/protos/Errors.proto b/kortex_driver/protos/Errors.proto index f7fb79eb..7c43acf7 100644 --- a/kortex_driver/protos/Errors.proto +++ b/kortex_driver/protos/Errors.proto @@ -132,7 +132,8 @@ enum SubErrorCodes { ROUTER_UNVAILABLE = 108; // The client router is currently unavailable. This can happen if an API method is called after the router has been deactivated via the method SetActivationStatus. ADDRESS_NOT_IN_VALID_RANGE = 120; // IP Address not valid against netmask - + ADDRESS_NOT_CONFIGURABLE = 121; // IP Address not configurable on specified interface + SESSION_NOT_IN_CONTROL = 130; // Trying to perform command from a non-controlling session in single-level mode METHOD_TIMEOUT = 131; // Timeout occured during method execution diff --git a/kortex_driver/protos/GripperCyclic.options b/kortex_driver/protos/GripperCyclic.options deleted file mode 100644 index e69de29b..00000000 diff --git a/kortex_driver/protos/GripperCyclicMessage.options b/kortex_driver/protos/GripperCyclicMessage.options deleted file mode 100644 index e019b527..00000000 --- a/kortex_driver/protos/GripperCyclicMessage.options +++ /dev/null @@ -1,5 +0,0 @@ -Kinova.Api.GripperCyclic.Command.motor_cmd max_count:4 fixed_count:false -Kinova.Api.GripperCyclic.Feedback.motor max_count:4 fixed_count:false -Kinova.Api.GripperCyclic.CustomData.motor_custom_data max_count:4 fixed_count:false - - diff --git a/kortex_driver/protos/InterconnectConfig.options b/kortex_driver/protos/InterconnectConfig.options deleted file mode 100644 index 91b1c59f..00000000 --- a/kortex_driver/protos/InterconnectConfig.options +++ /dev/null @@ -1,25 +0,0 @@ -// -// KINOVA (R) KORTEX (TM) -// -// Copyright (c) 2018 Kinova inc. All rights reserved. -// -// This software may be modified and distributed -// under the terms of the BSD 3-Clause license. -// -// Refer to the LICENSE file for details. -// -// -Kinova.Api.InterconnectConfig.ServiceVersion long_names:false -Kinova.Api.InterconnectConfig.SafetyIdentifier long_names:false -Kinova.Api.InterconnectConfig.GPIOIdentifier long_names:false -Kinova.Api.InterconnectConfig.GPIOMode long_names:false -Kinova.Api.InterconnectConfig.GPIOPull long_names:false -Kinova.Api.InterconnectConfig.GPIOValue long_names:false -Kinova.Api.InterconnectConfig.EthernetDevice long_names:false -Kinova.Api.InterconnectConfig.EthernetSpeed long_names:false -Kinova.Api.InterconnectConfig.EthernetDuplex long_names:false -Kinova.Api.InterconnectConfig.I2CDevice long_names:false -Kinova.Api.InterconnectConfig.I2CMode long_names:false -Kinova.Api.InterconnectConfig.I2CDeviceAddressing long_names:false -Kinova.Api.InterconnectConfig.I2CRegisterAddressSize long_names:false -Kinova.Api.InterconnectConfig.I2CData.data max_size:128 diff --git a/kortex_driver/protos/Session.proto b/kortex_driver/protos/Session.proto deleted file mode 100644 index f5788b2d..00000000 --- a/kortex_driver/protos/Session.proto +++ /dev/null @@ -1,52 +0,0 @@ -/* - * KINOVA (R) KORTEX (TM) - * - * Copyright (c) 2018 Kinova inc. All rights reserved. - * - * This software may be modified and distributed - * under the terms of the BSD 3-Clause license. - * - * Refer to the LICENSE file for details. - * - */ - -syntax = "proto3"; - -import public "Common.proto"; - -package Kinova.Api.Session; - -// Service to manage user sessions -service Session {//@PROXY_ID=1 @ERROR=Kinova.Api.Error - - // Creates a new session on the robot using given values for user name, session timeout value, and password - rpc CreateSession (CreateSessionInfo) returns (Kinova.Api.Common.Empty);//@RPC_ID=1 - - // Closes an existing open session - rpc CloseSession (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=2 - - // Sends message to robot to keep current session alive - rpc KeepAlive (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=3 - - // Retrieves the list of connections - rpc GetConnections (Kinova.Api.Common.Empty) returns (ConnectionList);//@RPC_ID=4 -} - -// Identifies session service current version -enum ServiceVersion { - RESERVED_0 = 0; // Reserved - CURRENT_VERSION = 1; // Current version -} - -// Parameters needed to create a new session -message CreateSessionInfo{ - string username = 1; // User name - uint32 session_inactivity_timeout = 2; // Inactivity period (in milliseconds) allowed before the session times out and closes on its own - string password = 3; // Password for the user - uint32 connection_inactivity_timeout = 4; // Inactivity period (in milliseconds) allowed before the robot stops any movements initiated from this session -} - -// Array of connections -message ConnectionList { - repeated Kinova.Api.Common.Connection connection = 1; // Connection -} diff --git a/kortex_driver/readme.md b/kortex_driver/readme.md index 4dd5a9bd..ac8ccddb 100644 --- a/kortex_driver/readme.md +++ b/kortex_driver/readme.md @@ -28,7 +28,7 @@ ## Overview -This node allows communication between a ROS node and a Kinova Gen3 Ultra lightweight robot. +This node allows communication between a ROS node and a Kinova Gen3 or Gen3 lite robot. ### License @@ -38,9 +38,7 @@ The source code is released under a [BSD 3-Clause license](../LICENSE). Affiliation: [Kinova inc.](https://www.kinovarobotics.com/)
Maintainer: Kinova inc. support@kinovarobotics.com** -This package has been tested under : -- ROS Kinetic and Ubuntu 16.04 -- ROS Melodic and Ubuntu 18.04 +This package has been tested under ROS Kinetic (Ubuntu 16.04) and ROS Melodic (Ubuntu 18.04). ## Usage @@ -49,7 +47,7 @@ The `kortex_driver` node is the node responsible for the communication between t **Arguments**: - **arm** : Name of your robot arm model. See the `kortex_description/arms` folder to see the available robot models. The default value is **gen3**. -- **gripper** : Name of your robot arm's tool / gripper. See the `kortex_description/grippers` folder to see the available end effector models (or to add your own). The default value is **""** and the only other supported option is **robotiq_2f_85** for now. +- **gripper** : Name of your robot arm's tool / gripper. See the `kortex_description/grippers` folder to see the available end effector models (or to add your own). The default value is **""**. For Gen3, you can also put **robotiq_2f_85**. For Gen3 lite, you need to put **gen3_lite_2f**. - **robot_name** : This is the namespace in which the driver will run. It defaults to **my_$(arg arm)** (so "my_gen3" for arm="gen3"). - **ip_address** : The IP address of the robot you're connecting to. The default value is **192.168.1.10**. - **username** : The username for the robot connection. The default value is **admin**. @@ -149,7 +147,7 @@ The *.srv* files are generated in different sub-folders depending on the sub-mod Many things have been changed in the ros_kortex repository between versions 1.1.7 and 2.0.0 and you will have to modify your code if you don't want it to break. -* The `kortex_actuator_driver`, `kortex_vision_config_driver` and `kortex_device_manager` packages were removed and only the `kortex_driver` package remains. +* The `kortex_actuator_driver`, `kortex_vision_config_driver` and `kortex_device_manager` packages were removed and only the `kortex_driver` package remains (one driver to rule them all). * Since we only have one driver and the ROS message generation does not deal with namespaces, the messages and services that are duplicated are now named differently. For example, the **Feedback** message exists within the `BaseCyclic`, `ActuatorCyclic`, `InterconnectCyclic` and `GripperCyclic` Protocol Buffers .proto files. In ROS, this is now translated as a "PackageName_" prefix before the message name. So, for the **Feedback** message, the **BaseCyclic_Feedback**, **ActuatorCyclic_Feedback**, **InterconnectCyclic_Feedback** and **GripperCyclic_Feedback** ROS messages have been automatically generated. You may encounter build errors (in C++) or runtime errors (in Python) because of this change. You can just go in the `msg/generated` folder and look for the problematic message to find its new name to change the occurrences in your code. * The services are now all **lowercase_with_underscores** instead of **UpperCase**. * The services are now advertised in **/my_robot_name/my_package_name/desired_service** (see the [Services section](#services) to learn about the packages). You can also visualize it if you start the node and type `rosservice list` in a terminal. diff --git a/kortex_driver/src/generated/base_ros_converter.cpp b/kortex_driver/src/generated/base_ros_converter.cpp index 99b3027e..f1c7a47e 100644 --- a/kortex_driver/src/generated/base_ros_converter.cpp +++ b/kortex_driver/src/generated/base_ros_converter.cpp @@ -192,8 +192,8 @@ int ToRosData(Kinova::Api::Base::Action input, kortex_driver::Action &output) output.application_data = input.application_data(); - auto oneof_type = input.action_parameters_case(); - switch(oneof_type) + auto oneof_type_action_parameters = input.action_parameters_case(); + switch(oneof_type_action_parameters) { case Kinova::Api::Base::Action::kSendTwistCommand: @@ -755,8 +755,8 @@ int ToRosData(Kinova::Api::Base::ConfigurationChangeNotification input, kortex_d ToRosData(input.connection(), output.connection); - auto oneof_type = input.configuration_change_case(); - switch(oneof_type) + auto oneof_type_configuration_change = input.configuration_change_case(); + switch(oneof_type_configuration_change) { case Kinova::Api::Base::ConfigurationChangeNotification::kSequenceHandle: @@ -997,8 +997,8 @@ int ToRosData(Kinova::Api::Base::ControllerElementHandle input, kortex_driver::C ToRosData(input.controller_handle(), output.controller_handle); - auto oneof_type = input.identifier_case(); - switch(oneof_type) + auto oneof_type_identifier = input.identifier_case(); + switch(oneof_type_identifier) { case Kinova::Api::Base::ControllerElementHandle::kButton: @@ -1021,8 +1021,8 @@ int ToRosData(Kinova::Api::Base::ControllerNotification input, kortex_driver::Co ToRosData(input.connection(), output.connection); - auto oneof_type = input.state_case(); - switch(oneof_type) + auto oneof_type_state = input.state_case(); + switch(oneof_type_state) { case Kinova::Api::Base::ControllerNotification::kControllerState: @@ -1381,8 +1381,8 @@ int ToRosData(Kinova::Api::Base::MapEvent input, kortex_driver::MapEvent &output output.name = input.name(); - auto oneof_type = input.events_case(); - switch(oneof_type) + auto oneof_type_events = input.events_case(); + switch(oneof_type_events) { case Kinova::Api::Base::MapEvent::kSafetyEvent: @@ -1644,8 +1644,8 @@ int ToRosData(Kinova::Api::Base::CartesianTrajectoryConstraint input, kortex_dri - auto oneof_type = input.type_case(); - switch(oneof_type) + auto oneof_type_type = input.type_case(); + switch(oneof_type_type) { case Kinova::Api::Base::CartesianTrajectoryConstraint::kSpeed: diff --git a/kortex_driver/src/generated/base_services.cpp b/kortex_driver/src/generated/base_services.cpp index f3242cd1..82369aa6 100644 --- a/kortex_driver/src/generated/base_services.cpp +++ b/kortex_driver/src/generated/base_services.cpp @@ -108,8 +108,13 @@ BaseServices::BaseServices(ros::NodeHandle& n, Kinova::Api::Base::BaseClient* ba m_serviceReadAllProtectionZones = m_n.advertiseService("base/read_all_protection_zones", &BaseServices::ReadAllProtectionZones, this); m_serviceCreateMapping = m_n.advertiseService("base/create_mapping", &BaseServices::CreateMapping, this); m_serviceReadMapping = m_n.advertiseService("base/read_mapping", &BaseServices::ReadMapping, this); + m_serviceUpdateMapping = m_n.advertiseService("base/update_mapping", &BaseServices::UpdateMapping, this); + m_serviceDeleteMapping = m_n.advertiseService("base/delete_mapping", &BaseServices::DeleteMapping, this); m_serviceReadAllMappings = m_n.advertiseService("base/read_all_mappings", &BaseServices::ReadAllMappings, this); m_serviceCreateMap = m_n.advertiseService("base/create_map", &BaseServices::CreateMap, this); + m_serviceReadMap = m_n.advertiseService("base/read_map", &BaseServices::ReadMap, this); + m_serviceUpdateMap = m_n.advertiseService("base/update_map", &BaseServices::UpdateMap, this); + m_serviceDeleteMap = m_n.advertiseService("base/delete_map", &BaseServices::DeleteMap, this); m_serviceReadAllMaps = m_n.advertiseService("base/read_all_maps", &BaseServices::ReadAllMaps, this); m_serviceActivateMap = m_n.advertiseService("base/activate_map", &BaseServices::ActivateMap, this); m_serviceCreateAction = m_n.advertiseService("base/create_action", &BaseServices::CreateAction, this); @@ -1081,6 +1086,70 @@ bool BaseServices::ReadMapping(kortex_driver::ReadMapping::Request &req, kortex return true; } +bool BaseServices::UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res) +{ + + Kinova::Api::Base::Mapping input; + ToProtoData(req.input, &input); + kortex_driver::KortexError result_error; + + try + { + m_base->UpdateMapping(input, m_current_device_id, m_api_options); + } + + catch (Kinova::Api::KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + ROS_INFO("%s", ex2.what()); + return false; + } + return true; +} + +bool BaseServices::DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res) +{ + + Kinova::Api::Base::MappingHandle input; + ToProtoData(req.input, &input); + kortex_driver::KortexError result_error; + + try + { + m_base->DeleteMapping(input, m_current_device_id, m_api_options); + } + + catch (Kinova::Api::KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + ROS_INFO("%s", ex2.what()); + return false; + } + return true; +} + bool BaseServices::ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res) { @@ -1149,6 +1218,105 @@ bool BaseServices::CreateMap(kortex_driver::CreateMap::Request &req, kortex_dri return true; } +bool BaseServices::ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res) +{ + + Kinova::Api::Base::MapHandle input; + ToProtoData(req.input, &input); + Kinova::Api::Base::Map output; + + kortex_driver::KortexError result_error; + + try + { + output = m_base->ReadMap(input, m_current_device_id, m_api_options); + } + + catch (Kinova::Api::KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + ROS_INFO("%s", ex2.what()); + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res) +{ + + Kinova::Api::Base::Map input; + ToProtoData(req.input, &input); + kortex_driver::KortexError result_error; + + try + { + m_base->UpdateMap(input, m_current_device_id, m_api_options); + } + + catch (Kinova::Api::KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + ROS_INFO("%s", ex2.what()); + return false; + } + return true; +} + +bool BaseServices::DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res) +{ + + Kinova::Api::Base::MapHandle input; + ToProtoData(req.input, &input); + kortex_driver::KortexError result_error; + + try + { + m_base->DeleteMap(input, m_current_device_id, m_api_options); + } + + catch (Kinova::Api::KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + ROS_INFO("%s", ex2.what()); + return false; + } + return true; +} + bool BaseServices::ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res) { diff --git a/kortex_driver/src/generated/deviceconfig_ros_converter.cpp b/kortex_driver/src/generated/deviceconfig_ros_converter.cpp index 12825da7..49829cc8 100644 --- a/kortex_driver/src/generated/deviceconfig_ros_converter.cpp +++ b/kortex_driver/src/generated/deviceconfig_ros_converter.cpp @@ -222,8 +222,8 @@ int ToRosData(Kinova::Api::DeviceConfig::CalibrationParameter input, kortex_driv output.calibration_parameter_identifier = input.calibration_parameter_identifier(); - auto oneof_type = input.value_case(); - switch(oneof_type) + auto oneof_type_value = input.value_case(); + switch(oneof_type_value) { case Kinova::Api::DeviceConfig::CalibrationParameter::kSignedIntValue: diff --git a/kortex_driver/src/generated/interconnectcyclic_ros_converter.cpp b/kortex_driver/src/generated/interconnectcyclic_ros_converter.cpp index 0dac92e9..46688c53 100644 --- a/kortex_driver/src/generated/interconnectcyclic_ros_converter.cpp +++ b/kortex_driver/src/generated/interconnectcyclic_ros_converter.cpp @@ -32,8 +32,8 @@ int ToRosData(Kinova::Api::InterconnectCyclic::Command input, kortex_driver::Int output.flags = input.flags(); - auto oneof_type = input.tool_command_case(); - switch(oneof_type) + auto oneof_type_tool_command = input.tool_command_case(); + switch(oneof_type_tool_command) { case Kinova::Api::InterconnectCyclic::Command::kGripperCommand: @@ -66,8 +66,8 @@ int ToRosData(Kinova::Api::InterconnectCyclic::Feedback input, kortex_driver::In output.warning_bank_b = input.warning_bank_b(); - auto oneof_type = input.tool_feedback_case(); - switch(oneof_type) + auto oneof_type_tool_feedback = input.tool_feedback_case(); + switch(oneof_type_tool_feedback) { case Kinova::Api::InterconnectCyclic::Feedback::kGripperFeedback: @@ -102,8 +102,8 @@ int ToRosData(Kinova::Api::InterconnectCyclic::CustomData input, kortex_driver:: output.custom_data_15 = input.custom_data_15(); - auto oneof_type = input.tool_customData_case(); - switch(oneof_type) + auto oneof_type_tool_customData = input.tool_customData_case(); + switch(oneof_type_tool_customData) { case Kinova::Api::InterconnectCyclic::CustomData::kGripperCustomData: diff --git a/kortex_driver/src/generated/visionconfig_ros_converter.cpp b/kortex_driver/src/generated/visionconfig_ros_converter.cpp index 0dcf881c..ff83444c 100644 --- a/kortex_driver/src/generated/visionconfig_ros_converter.cpp +++ b/kortex_driver/src/generated/visionconfig_ros_converter.cpp @@ -91,8 +91,8 @@ int ToRosData(Kinova::Api::VisionConfig::SensorFocusAction input, kortex_driver: output.focus_action = input.focus_action(); - auto oneof_type = input.action_parameters_case(); - switch(oneof_type) + auto oneof_type_action_parameters = input.action_parameters_case(); + switch(oneof_type_action_parameters) { case Kinova::Api::VisionConfig::SensorFocusAction::kFocusPoint: diff --git a/kortex_driver/src/non-generated/kortex_arm_driver.cpp b/kortex_driver/src/non-generated/kortex_arm_driver.cpp index dc0fba8b..cea47a0e 100644 --- a/kortex_driver/src/non-generated/kortex_arm_driver.cpp +++ b/kortex_driver/src/non-generated/kortex_arm_driver.cpp @@ -50,9 +50,15 @@ KortexArmDriver::~KortexArmDriver() delete m_control_config_ros_services; delete m_device_config_ros_services; delete m_device_manager_ros_services; - delete m_interconnect_config_ros_services; - delete m_vision_config_ros_services; - + if (m_interconnect_config_ros_services) + { + delete m_interconnect_config_ros_services; + } + if (m_vision_config_ros_services) + { + delete m_vision_config_ros_services; + } + m_tcp_session_manager->CloseSession(); m_udp_session_manager->CloseSession(); m_tcp_router->SetActivationStatus(false); @@ -146,6 +152,13 @@ void KortexArmDriver::parseRosArguments() throw new std::runtime_error(error_string); } + if (!ros::param::get("~dof", m_degrees_of_freedom)) + { + std::string error_string = "Number of degrees of freedom 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("~joint_names", m_arm_joint_names)) { std::string error_string = "Arm joint_names were not specified, shutting down the node..."; @@ -171,9 +184,17 @@ void KortexArmDriver::parseRosArguments() } // Get the gripper_joint_limits size - if (!ros::param::get("~gripper_joint_limits", m_gripper_joint_limits)) + if (!ros::param::get("~gripper_joint_limits_min", m_gripper_joint_limits_min)) { - std::string error_string = "Gripper joint limits were not specified in the launch file, shutting down the node..."; + std::string error_string = "Gripper joint min limits were not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } + + // Get the gripper_joint_limits size + if (!ros::param::get("~gripper_joint_limits_max", m_gripper_joint_limits_max)) + { + std::string error_string = "Gripper joint max limits were not specified in the launch file, shutting down the node..."; ROS_ERROR("%s", error_string.c_str()); throw new std::runtime_error(error_string); } @@ -247,7 +268,7 @@ void KortexArmDriver::verifyProductConfiguration() throw new std::runtime_error(error_string); } } - else if (m_arm_name == "pico") + else if (m_arm_name == "gen3_lite") { if (product_config.model() != Kinova::Api::ProductConfiguration::ModelId::MODEL_ID_L31) { @@ -262,6 +283,13 @@ void KortexArmDriver::verifyProductConfiguration() ROS_ERROR("%s", error_string.c_str()); throw new std::runtime_error(error_string); } + // Compare number of degrees of freedom specified in the launch file with the number of detected actuators + if (product_config.degree_of_freedom() != m_degrees_of_freedom) + { + std::string error_string = "The degrees of freedom specified in the launch file doesn't match the detected number of actuators, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } // Compare gripper type (EndEffectorType) if (!isGripperPresent()) @@ -282,6 +310,15 @@ void KortexArmDriver::verifyProductConfiguration() throw new std::runtime_error(error_string); } } + else if (m_gripper_name == "gen3_lite_2f") + { + if (product_config.end_effector_type() != Kinova::Api::ProductConfiguration::EndEffectorType::END_EFFECTOR_TYPE_L31_GRIPPER_2F) + { + std::string error_string = "The gripper model specified in the launch file doesn't match the detected arm's gripper model, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } + } else { std::string error_string = "The gripper model specified in the launch file is not supported, shutting down the node..."; @@ -292,10 +329,17 @@ void KortexArmDriver::verifyProductConfiguration() // Set the ROS Param for the degrees of freedom m_node_handle.setParam("degrees_of_freedom", int(product_config.degree_of_freedom())); m_node_handle.setParam("is_gripper_present", isGripperPresent()); + m_node_handle.setParam("gripper_joint_names", m_gripper_joint_names); // Find all the devices and print the device ID's 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 auto devices = m_device_manager->ReadAllDevices(); for (auto device : devices.device_handle()) { @@ -313,11 +357,13 @@ void KortexArmDriver::verifyProductConfiguration() else if (device.device_type() == Kinova::Api::Common::DeviceTypes::INTERCONNECT) { m_interconnect_device_id = device.device_identifier(); + m_is_interconnect_module_present = true; ROS_INFO("Interconnect device was found on device identifier %u", m_interconnect_device_id); } else if (device.device_type() == Kinova::Api::Common::DeviceTypes::VISION) { m_vision_device_id = device.device_identifier(); + m_is_vision_module_present = true; ROS_INFO("Vision device was found on device identifier %u", m_vision_device_id); } } @@ -333,8 +379,22 @@ void KortexArmDriver::initRosServices() m_control_config_ros_services = new ControlConfigServices(m_node_handle, m_control_config, 0, m_api_rpc_timeout_ms); m_device_config_ros_services = new DeviceConfigServices(m_node_handle, m_device_config, 0, m_api_rpc_timeout_ms); m_device_manager_ros_services = new DeviceManagerServices(m_node_handle, m_device_manager, 0, m_api_rpc_timeout_ms); - m_interconnect_config_ros_services = new InterconnectConfigServices(m_node_handle, m_interconnect_config, m_interconnect_device_id, m_api_rpc_timeout_ms); - m_vision_config_ros_services = new VisionConfigServices(m_node_handle, m_vision_config, m_vision_device_id, m_api_rpc_timeout_ms); + if (m_is_interconnect_module_present) + { + m_interconnect_config_ros_services = new InterconnectConfigServices(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 VisionConfigServices(m_node_handle, m_vision_config, m_vision_device_id, m_api_rpc_timeout_ms); + } + else + { + m_vision_config_ros_services = nullptr; + } std::this_thread::sleep_for(std::chrono::milliseconds(2000)); ROS_INFO("Kortex Driver's services initialized correctly."); ROS_INFO("-------------------------------------------------"); @@ -346,9 +406,9 @@ void KortexArmDriver::startActionServers() m_action_server_follow_joint_trajectory = new PreComputedJointTrajectoryActionServer(m_arm_name + "_joint_trajectory_controller/follow_joint_trajectory", m_node_handle, m_base, m_base_cyclic); // Start Gripper Action Server if the arm has a gripper m_action_server_gripper_command = nullptr; - if (m_gripper_name == "robotiq_2f_85") + if (isGripperPresent()) { - m_action_server_gripper_command = new RobotiqGripperCommandActionServer(m_gripper_name + "_gripper_controller/gripper_cmd", m_gripper_joint_names[0], m_gripper_joint_limits[0], m_node_handle, m_base, m_base_cyclic); + m_action_server_gripper_command = new RobotiqGripperCommandActionServer(m_gripper_name + "_gripper_controller/gripper_cmd", m_gripper_joint_names[0], m_gripper_joint_limits_min[0], m_gripper_joint_limits_max[0], m_node_handle, m_base, m_base_cyclic); } } @@ -414,14 +474,14 @@ void KortexArmDriver::publishFeedback() joint_state.effort[i] = base_feedback.actuators[i].torque; } - if (m_gripper_name == "robotiq_2f_85") + if (isGripperPresent()) { for (int i = 0; i < base_feedback.interconnect.oneof_tool_feedback.gripper_feedback[0].motor.size(); i++) { int joint_state_index = base_feedback.actuators.size() + i; joint_state.name[joint_state_index] = m_gripper_joint_names[i]; - // Arm feedback is between 0 and 100, and upper limit in URDF is specified in gripper_joint_limits[i] parameter - joint_state.position[joint_state_index] = base_feedback.interconnect.oneof_tool_feedback.gripper_feedback[0].motor[i].position / 100.0 * m_gripper_joint_limits[i]; + // Arm feedback is between 0 and 100, and limits in URDF are specified in gripper_joint_limits_min[i] and gripper_joint_limits_max[i] parameters + joint_state.position[joint_state_index] = m_math_util.absolute_position_from_relative(base_feedback.interconnect.oneof_tool_feedback.gripper_feedback[0].motor[i].position / 100.0, m_gripper_joint_limits_min[i], m_gripper_joint_limits_max[i]); joint_state.velocity[joint_state_index] = base_feedback.interconnect.oneof_tool_feedback.gripper_feedback[0].motor[i].velocity; // Not supported for now joint_state.effort[joint_state_index] = 0.0; diff --git a/kortex_driver/src/non-generated/kortex_math_util.cpp b/kortex_driver/src/non-generated/kortex_math_util.cpp index 469f6e0e..4f140ce0 100644 --- a/kortex_driver/src/non-generated/kortex_math_util.cpp +++ b/kortex_driver/src/non-generated/kortex_math_util.cpp @@ -63,3 +63,15 @@ double KortexMathUtil::wrapDegreesFromZeroTo360(double deg_not_wrapped) } while(!properly_wrapped); return deg_not_wrapped; } + +double KortexMathUtil::relative_position_from_absolute(double absolute_position, double min_value, double max_value) +{ + double range = max_value - min_value; + return (absolute_position - min_value) / range; +} + +double KortexMathUtil::absolute_position_from_relative(double relative_position, double min_value, double max_value) +{ + double range = max_value - min_value; + return relative_position * range + min_value; +} diff --git a/kortex_driver/src/non-generated/robotiq_gripper_command_action_server.cpp b/kortex_driver/src/non-generated/robotiq_gripper_command_action_server.cpp index 442cc6b1..1ff48f0b 100644 --- a/kortex_driver/src/non-generated/robotiq_gripper_command_action_server.cpp +++ b/kortex_driver/src/non-generated/robotiq_gripper_command_action_server.cpp @@ -12,10 +12,11 @@ #include "kortex_driver/non-generated/robotiq_gripper_command_action_server.h" -RobotiqGripperCommandActionServer::RobotiqGripperCommandActionServer(const std::string& server_name, const std::string& gripper_joint_name, double gripper_joint_limit, ros::NodeHandle& nh, Kinova::Api::Base::BaseClient* base, Kinova::Api::BaseCyclic::BaseCyclicClient* base_cyclic): +RobotiqGripperCommandActionServer::RobotiqGripperCommandActionServer(const std::string& server_name, const std::string& gripper_joint_name, double gripper_joint_limit_min, double gripper_joint_limit_max, ros::NodeHandle& nh, Kinova::Api::Base::BaseClient* base, Kinova::Api::BaseCyclic::BaseCyclicClient* base_cyclic): m_server_name(server_name), m_gripper_joint_name(gripper_joint_name), - m_gripper_joint_limit(gripper_joint_limit), + m_gripper_joint_limit_min(gripper_joint_limit_min), + m_gripper_joint_limit_max(gripper_joint_limit_max), m_node_handle(nh), m_server(nh, server_name, boost::bind(&RobotiqGripperCommandActionServer::goal_received_callback, this, _1), boost::bind(&RobotiqGripperCommandActionServer::preempt_received_callback, this, _1), false), m_base(base), @@ -66,7 +67,7 @@ void RobotiqGripperCommandActionServer::goal_received_callback(actionlib::Action proto_gripper_command.set_mode(Kinova::Api::Base::GripperMode::GRIPPER_POSITION); auto gripper = proto_gripper_command.mutable_gripper(); // Position for a finger must be between relative (between 0 and 1), but position is absolute in the Goal coming from ROS - double relative_position = relative_position_from_absolute(ros_gripper_command.position); + double relative_position = m_math_util.relative_position_from_absolute(ros_gripper_command.position, m_gripper_joint_limit_min, m_gripper_joint_limit_max); auto finger = gripper->add_finger(); finger->set_finger_identifier(0); finger->set_value(relative_position); @@ -115,7 +116,7 @@ void RobotiqGripperCommandActionServer::gripper_position_polling_thread() // Get gripper position and find if we're stuck feedback = m_base_cyclic->RefreshFeedback(); actual_gripper_position = feedback.interconnect().gripper_feedback().motor(0).position() / 100.0; - if (fabs(actual_gripper_position - previous_gripper_position) > ROBOTIQ_GRIPPER_RELATIVE_ERROR) + if (fabs(actual_gripper_position - previous_gripper_position) > 0.01) { n_stuck = 0; } @@ -126,7 +127,7 @@ void RobotiqGripperCommandActionServer::gripper_position_polling_thread() previous_gripper_position = actual_gripper_position; // If we're stuck, the trajectory is over - if (n_stuck >= 2) // 100ms in the same position + if (n_stuck >= 4) // 200ms in the same position { m_is_trajectory_running_lock.lock(); m_is_trajectory_running = false; @@ -144,14 +145,15 @@ void RobotiqGripperCommandActionServer::gripper_position_polling_thread() } else { + ROS_ERROR("we're finished"); m_goal.setAborted(); } } // Timeout was reached - // This shouldn't happen, really else { + ROS_ERROR("BANG timeout"); m_goal.setAborted(); } @@ -170,7 +172,7 @@ bool RobotiqGripperCommandActionServer::is_goal_acceptable(actionlib::ActionServ control_msgs::GripperCommandGoalConstPtr goal = goal_handle.getGoal(); // If the position is not in the joint limits range, reject the goal - double relative_position = relative_position_from_absolute(goal_handle.getGoal()->command.position); + double relative_position = m_math_util.relative_position_from_absolute(goal_handle.getGoal()->command.position, m_gripper_joint_limit_min, m_gripper_joint_limit_max); if (relative_position > 1 || relative_position < 0) { return false; @@ -183,9 +185,11 @@ bool RobotiqGripperCommandActionServer::is_goal_tolerance_respected() { Kinova::Api::BaseCyclic::Feedback feedback = m_base_cyclic->RefreshFeedback(); double actual_gripper_position = feedback.interconnect().gripper_feedback().motor(0).position() / 100.0; + double goal_as_relative_position = m_math_util.relative_position_from_absolute(m_goal.getGoal()->command.position, m_gripper_joint_limit_min, m_gripper_joint_limit_max); - return m_goal.getGoal()->command.position < absolute_position_from_relative(actual_gripper_position + ROBOTIQ_GRIPPER_RELATIVE_ERROR) && - m_goal.getGoal()->command.position > absolute_position_from_relative(actual_gripper_position - ROBOTIQ_GRIPPER_RELATIVE_ERROR); + ROS_INFO("%f and %f", actual_gripper_position, goal_as_relative_position); + + return fabs(actual_gripper_position - goal_as_relative_position) < MAX_GRIPPER_RELATIVE_ERROR; } void RobotiqGripperCommandActionServer::join_polling_thread() @@ -211,13 +215,3 @@ void RobotiqGripperCommandActionServer::stop_all_movement() ROS_ERROR("Sending a gripper speed of 0 failed : %s", e.what()); } } - -double RobotiqGripperCommandActionServer::relative_position_from_absolute(double absolute_position) -{ - return absolute_position / m_gripper_joint_limit; -} - -double RobotiqGripperCommandActionServer::absolute_position_from_relative(double relative_position) -{ - return relative_position * m_gripper_joint_limit; -} diff --git a/kortex_driver/srv/generated/base/DeleteMap.srv b/kortex_driver/srv/generated/base/DeleteMap.srv new file mode 100644 index 00000000..1e16cb83 --- /dev/null +++ b/kortex_driver/srv/generated/base/DeleteMap.srv @@ -0,0 +1,3 @@ +MapHandle input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/generated/base/DeleteMapping.srv b/kortex_driver/srv/generated/base/DeleteMapping.srv new file mode 100644 index 00000000..ea8df28a --- /dev/null +++ b/kortex_driver/srv/generated/base/DeleteMapping.srv @@ -0,0 +1,3 @@ +MappingHandle input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/generated/base/ReadMap.srv b/kortex_driver/srv/generated/base/ReadMap.srv new file mode 100644 index 00000000..b250efb5 --- /dev/null +++ b/kortex_driver/srv/generated/base/ReadMap.srv @@ -0,0 +1,3 @@ +MapHandle input +--- +Map output \ No newline at end of file diff --git a/kortex_driver/srv/generated/base/UpdateMap.srv b/kortex_driver/srv/generated/base/UpdateMap.srv new file mode 100644 index 00000000..e58c7af5 --- /dev/null +++ b/kortex_driver/srv/generated/base/UpdateMap.srv @@ -0,0 +1,3 @@ +Map input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/generated/base/UpdateMapping.srv b/kortex_driver/srv/generated/base/UpdateMapping.srv new file mode 100644 index 00000000..98a83927 --- /dev/null +++ b/kortex_driver/srv/generated/base/UpdateMapping.srv @@ -0,0 +1,3 @@ +Mapping input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/templates/ros_converter.cpp.jinja2 b/kortex_driver/templates/ros_converter.cpp.jinja2 index 811082ab..538771dc 100644 --- a/kortex_driver/templates/ros_converter.cpp.jinja2 +++ b/kortex_driver/templates/ros_converter.cpp.jinja2 @@ -52,8 +52,8 @@ int ToRosData({{detailed_message.cpp_namespace}}::{{detailed_message.name}} inpu {%- endfor %} {% for detailed_one_of in detailed_message.one_of_list %} - auto oneof_type = input.{{detailed_one_of.name}}_case(); - switch(oneof_type) + auto oneof_type_{{detailed_one_of.name}} = input.{{detailed_one_of.name}}_case(); + switch(oneof_type_{{detailed_one_of.name}}) { {%- for field in detailed_one_of.fields %} {%- if "_" in field.name -%} {%- set EnumName = field.name.replace("_", " ").title().replace(" ", "") -%} {# In case the field is in the form of "gripper_command" #} diff --git a/kortex_examples/cpp/actuator_config/example_actuator_configuration.cpp b/kortex_examples/cpp/actuator_config/example_actuator_configuration.cpp index 70d7650d..8b140098 100644 --- a/kortex_examples/cpp/actuator_config/example_actuator_configuration.cpp +++ b/kortex_examples/cpp/actuator_config/example_actuator_configuration.cpp @@ -62,6 +62,7 @@ void example_find_actuators_and_set_device_id(ros::NodeHandle& n, const std::str { if((output.device_handle[i].device_type == device_type.BIG_ACTUATOR) || + (output.device_handle[i].device_type == device_type.MEDIUM_ACTUATOR) || (output.device_handle[i].device_type == device_type.SMALL_ACTUATOR)) { // Add the device_id to the vector if we found an actuator diff --git a/kortex_examples/cpp/full_arm/example_cartesian_poses_with_notifications.cpp b/kortex_examples/cpp/full_arm/example_cartesian_poses_with_notifications.cpp index a6af2072..fdfa06ce 100644 --- a/kortex_examples/cpp/full_arm/example_cartesian_poses_with_notifications.cpp +++ b/kortex_examples/cpp/full_arm/example_cartesian_poses_with_notifications.cpp @@ -37,9 +37,10 @@ void notification_callback(const kortex_driver::ActionNotification::ConstPtr& no { switch(notif->action_event) { - //This event is published when an action has ended + //This event is published when an action has ended or was aborted case kortex_driver::ActionEvent::ACTION_END: { + ROS_INFO("Action has ended!"); if(notif->handle.identifier == 1001) { Pose1Done = true; @@ -54,7 +55,29 @@ void notification_callback(const kortex_driver::ActionNotification::ConstPtr& no { Pose3Done = true; } + break; } + case kortex_driver::ActionEvent::ACTION_ABORT: + { + ROS_ERROR("Action was aborted!"); + if(notif->handle.identifier == 1001) + { + Pose1Done = true; + } + + if(notif->handle.identifier == 1002) + { + Pose2Done = true; + } + + if(notif->handle.identifier == 1003) + { + Pose3Done = true; + } + break; + } + default: + break; } } @@ -86,12 +109,12 @@ void example_cartesian_action(ros::NodeHandle n, std::string robot_name) my_constrained_pose.constraint.oneof_type.speed.push_back(my_cartesian_speed); - my_constrained_pose.target_pose.x = 0.365f; - my_constrained_pose.target_pose.y = 0.169f; - my_constrained_pose.target_pose.z = 0.252f; - my_constrained_pose.target_pose.theta_x = -31.3f; - my_constrained_pose.target_pose.theta_y = -176.6f; - my_constrained_pose.target_pose.theta_z = 60.7f; + my_constrained_pose.target_pose.x = 0.374f; + my_constrained_pose.target_pose.y = 0.081f; + my_constrained_pose.target_pose.z = 0.450f; + my_constrained_pose.target_pose.theta_x = -57.6f; + my_constrained_pose.target_pose.theta_y = 91.1f; + my_constrained_pose.target_pose.theta_z = 2.3f; ros::ServiceClient service_client_execute_action = n.serviceClient("/" + robot_name + "/base/execute_action"); kortex_driver::ExecuteAction service_execute_action; @@ -121,7 +144,7 @@ void example_cartesian_action(ros::NodeHandle n, std::string robot_name) service_execute_action.request.input.handle.identifier = 1002; service_execute_action.request.input.name = "pose2"; - my_constrained_pose.target_pose.z = 0.352f; + my_constrained_pose.target_pose.z = 0.3f; service_execute_action.request.input.oneof_action_parameters.reach_pose.clear(); service_execute_action.request.input.oneof_action_parameters.reach_pose.push_back(my_constrained_pose); @@ -147,7 +170,7 @@ void example_cartesian_action(ros::NodeHandle n, std::string robot_name) service_execute_action.request.input.handle.identifier = 1003; service_execute_action.request.input.name = "pose3"; - my_constrained_pose.target_pose.z = 0.452f; + my_constrained_pose.target_pose.z = 0.20f; service_execute_action.request.input.oneof_action_parameters.reach_pose.clear(); service_execute_action.request.input.oneof_action_parameters.reach_pose.push_back(my_constrained_pose); @@ -202,11 +225,22 @@ int main(int argc, char **argv) ros::NodeHandle n; std::string robot_name = "my_gen3"; + // Parameter robot_name + if (!ros::param::get("~robot_name", robot_name)) + { + std::string error_string = "Parameter robot_name was not specified, defaulting to " + robot_name + " as namespace"; + ROS_WARN("%s", error_string.c_str()); + } + else + { + std::string error_string = "Using robot_name " + robot_name + " as namespace"; + ROS_INFO("%s", error_string.c_str()); + } + ros::Subscriber sub = n.subscribe("/" + robot_name + "/action_topic", 1000, notification_callback); example_set_cartesian_reference_frame(n, robot_name); std::this_thread::sleep_for(std::chrono::milliseconds(1000)); example_cartesian_action(n, robot_name); - //ros::spin(); return 0; } \ No newline at end of file diff --git a/kortex_examples/python/actuator_config/example_actuator_configuration.py b/kortex_examples/python/actuator_config/example_actuator_configuration.py index b859dee9..8dc2001f 100755 --- a/kortex_examples/python/actuator_config/example_actuator_configuration.py +++ b/kortex_examples/python/actuator_config/example_actuator_configuration.py @@ -69,7 +69,7 @@ def example_find_actuators_and_set_device_id(self): try: all_devices = self.read_all_devices() for device in all_devices.output.device_handle: - if device.device_type == DeviceTypes.BIG_ACTUATOR or device.device_type == DeviceTypes.SMALL_ACTUATOR: + if device.device_type == DeviceTypes.BIG_ACTUATOR or device.device_type == DeviceTypes.SMALL_ACTUATOR or device.device_type == DeviceTypes.MEDIUM_ACTUATOR: # Add the device_id to the list if we found an actuator actuator_device_ids.append(device.device_identifier) rospy.loginfo("Found an actuator with device id " + str(device.device_identifier)) diff --git a/kortex_examples/python/full_arm/example_cartesian_poses_with_notifications.py b/kortex_examples/python/full_arm/example_cartesian_poses_with_notifications.py index 1116ff62..d8dd52d6 100755 --- a/kortex_examples/python/full_arm/example_cartesian_poses_with_notifications.py +++ b/kortex_examples/python/full_arm/example_cartesian_poses_with_notifications.py @@ -99,6 +99,15 @@ def example_set_cartesian_reference_frame(self): def notification_callback(self, notif): if notif.action_event == ActionEvent.ACTION_END: + rospy.loginfo("Action has ended.") + if notif.handle.identifier == 1001: + self.pose_1_done.set() + if notif.handle.identifier == 1002: + self.pose_2_done.set() + if notif.handle.identifier == 1003: + self.pose_3_done.set() + if notif.action_event == ActionEvent.ACTION_ABORT: + rospy.logerr("Action was aborted!") if notif.handle.identifier == 1001: self.pose_1_done.set() if notif.handle.identifier == 1002: @@ -145,12 +154,12 @@ def main(self): my_constrained_pose = ConstrainedPose() my_constrained_pose.constraint.oneof_type.speed.append(my_cartesian_speed) - my_constrained_pose.target_pose.x = 0.365 - my_constrained_pose.target_pose.y = 0.169 - my_constrained_pose.target_pose.z = 0.252 - my_constrained_pose.target_pose.theta_x = -31.3 - my_constrained_pose.target_pose.theta_y = -176.6 - my_constrained_pose.target_pose.theta_z = 60.7 + my_constrained_pose.target_pose.x = 0.374 + my_constrained_pose.target_pose.y = 0.081 + my_constrained_pose.target_pose.z = 0.450 + my_constrained_pose.target_pose.theta_x = -57.6 + my_constrained_pose.target_pose.theta_y = 91.1 + my_constrained_pose.target_pose.theta_z = 2.3 req = ExecuteActionRequest() req.input.oneof_action_parameters.reach_pose.append(my_constrained_pose) @@ -171,7 +180,7 @@ def main(self): req.input.handle.identifier = 1002 req.input.name = "pose2" - my_constrained_pose.target_pose.z = 0.352 + my_constrained_pose.target_pose.z = 0.3 req.input.oneof_action_parameters.reach_pose[0] = my_constrained_pose @@ -189,7 +198,7 @@ def main(self): req.input.handle.identifier = 1003 req.input.name = "pose3" - my_constrained_pose.target_pose.z = 0.432 + my_constrained_pose.target_pose.z = 0.20 req.input.oneof_action_parameters.reach_pose[0] = my_constrained_pose diff --git a/kortex_examples/python/move_it/example_move_it_trajectories.py b/kortex_examples/python/move_it/example_move_it_trajectories.py index d4110f7c..d7cb6c55 100755 --- a/kortex_examples/python/move_it/example_move_it_trajectories.py +++ b/kortex_examples/python/move_it/example_move_it_trajectories.py @@ -59,6 +59,9 @@ def __init__(self): rospy.init_node('example_move_it_trajectories') self.is_gripper_present = rospy.get_param(rospy.get_namespace() + "is_gripper_present", False) + gripper_joint_names = rospy.get_param(rospy.get_namespace() + "gripper_joint_names", []) + self.gripper_joint_name = gripper_joint_names[0] + self.degrees_of_freedom = rospy.get_param(rospy.get_namespace() + "degrees_of_freedom", 7) # Create the MoveItInterface necessary objects arm_group_name = "arm" @@ -99,13 +102,21 @@ def reach_joint_angles(self, tolerance): self.arm_group.set_goal_joint_tolerance(tolerance) # Set the joint target configuration - joint_positions[0] = pi/2 - joint_positions[1] = 0 - joint_positions[2] = pi/4 - joint_positions[3] = -pi/4 - joint_positions[4] = 0 - joint_positions[5] = pi/2 - joint_positions[6] = 0.2 + if self.degrees_of_freedom == 7: + joint_positions[0] = pi/2 + joint_positions[1] = 0 + joint_positions[2] = pi/4 + joint_positions[3] = -pi/4 + joint_positions[4] = 0 + joint_positions[5] = pi/2 + joint_positions[6] = 0.2 + elif self.degrees_of_freedom == 6: + joint_positions[0] = 0 + joint_positions[1] = 0 + joint_positions[2] = pi/2 + joint_positions[3] = pi/4 + joint_positions[4] = 0 + joint_positions[5] = pi/2 arm_group.set_joint_value_target(joint_positions) # Plan and execute in one command @@ -134,7 +145,6 @@ def reach_cartesian_pose(self, pose, tolerance, constraints): # Set the trajectory constraint if one is specified if constraints is not None: - #import pdb; pdb.set_trace() arm_group.set_path_constraints(constraints) # Get the current Cartesian Position @@ -148,9 +158,10 @@ def reach_gripper_position(self, relative_position): gripper_group = self.gripper_group # We only have to move this joint because all others are mimic! - gripper_joint = self.robot.get_joint("gripper_finger1_joint") + gripper_joint = self.robot.get_joint(self.gripper_joint_name) gripper_max_absolute_pos = gripper_joint.max_bound() - gripper_joint.move(relative_position * gripper_max_absolute_pos, True) + gripper_min_absolute_pos = gripper_joint.min_bound() + gripper_joint.move(relative_position * (gripper_max_absolute_pos - gripper_min_absolute_pos) + gripper_min_absolute_pos, True) def main(): example = ExampleMoveItTrajectories() @@ -170,13 +181,10 @@ def main(): rospy.loginfo("Press any key to start Reach Cartesian Pose sub example") raw_input() actual_pose = example.get_cartesian_pose() - actual_pose.position.x -= 0.1 - actual_pose.position.y += 0.3 - actual_pose.position.z += 0.3 + actual_pose.position.z -= 0.2 example.reach_cartesian_pose(pose=actual_pose, tolerance=0.01, constraints=None) - if example.is_gripper_present: - + if example.degrees_of_freedom == 7: rospy.loginfo("Press any key to start Reach Cartesian Pose With Constraints sub example") raw_input() @@ -192,7 +200,8 @@ def main(): # Send the goal example.reach_cartesian_pose(pose=actual_pose, tolerance=0.01, constraints=constraints) - + + if example.is_gripper_present: rospy.loginfo("Press any key to open the gripper sub example") raw_input() example.reach_gripper_position(0) diff --git a/kortex_examples/readme.md b/kortex_examples/readme.md index d376b698..08fa2943 100644 --- a/kortex_examples/readme.md +++ b/kortex_examples/readme.md @@ -42,9 +42,9 @@ To run the C++ example: `rosrun kortex_examples example_actuator_configuration_c To run the Python example: `rosrun kortex_examples example_actuator_configuration.py` -If you started the `kortex_driver` node in a non-default namespace (not **my_gen3**) or if you want to test the example on another actuator than the first one, you will have to supply node parameters in the command line (the syntax doesn't change if you run the C++ or Python example) : +If you started the `kortex_driver` node in another namespace (not **my_gen3**) or if you want to test the example on another actuator than the first one, you will have to supply node parameters in the command line (the syntax doesn't change if you run the C++ or Python example): -`rosrun kortex_examples example_actuator_configuration_cpp _robot_name:= _device_id:=` +`rosrun kortex_examples example_actuator_configuration_cpp _robot_name:= _device_id:=` ## Full arm examples @@ -52,13 +52,23 @@ If you started the `kortex_driver` node in a non-default namespace (not **my_gen The examples look for advertised services in the **my_gen3** namespace by default. -To run the C++ example: `rosrun kortex_examples example_full_arm_movement_cpp` +- **Simple movement example**: -To run the Python example: `rosrun kortex_examples example_full_arm_movement.py` + - To run the C++ example: `rosrun kortex_examples example_full_arm_movement_cpp` + - To run the Python example: `rosrun kortex_examples example_full_arm_movement.py` + + If you started the `kortex_driver` node in another namespace (not **my_gen3**), you will have to supply the node a parameter in the command line (the syntax doesn't change if you run the C++ or Python example) : + + `rosrun kortex_examples example_full_arm_movement_cpp _robot_name:=` -If you started the `kortex_driver` node in a non-default namespace (not **my_gen3**), you will have to supply the node a parameter in the command line (the syntax doesn't change if you run the C++ or Python example) : +- **Cartesian poses with notifications**: -`rosrun kortex_examples example_full_arm_movement_cpp _robot_name:=` + - To run the C++ example: `rosrun kortex_examples example_cartesian_poses_with_notifications_cpp` + - To run the Python example: `rosrun kortex_examples example_cartesian_poses_with_notifications.py` + + If you started the `kortex_driver` node in another namespace (not **my_gen3**), you will have to supply the node a parameter in the command line (the syntax doesn't change if you run the C++ or Python example) : + + `rosrun kortex_examples example_cartesian_poses_with_notifications_cpp _robot_name:=` ## Vision module configuration examples @@ -70,9 +80,9 @@ To run the C++ example: `rosrun kortex_examples example_vision_configuration_cpp To run the Python example: `rosrun kortex_examples example_vision_configuration.py` -If you started the `kortex_driver` node in a non-default namespace (not **my_gen3**), you will have to supply the node a parameter in the command line (the syntax doesn't change if you run the C++ or Python example) : +If you started the `kortex_driver` node in another namespace (not **my_gen3**), you will have to supply the node a parameter in the command line (the syntax doesn't change if you run the C++ or Python example) : -`rosrun kortex_examples example_full_arm_movement_cpp _robot_name:=` +`rosrun kortex_examples example_vision_configuration_cpp _robot_name:=` ## MoveIt! example diff --git a/kortex_gazebo/launch/spawn_kortex_robot.launch b/kortex_gazebo/launch/spawn_kortex_robot.launch index d59efd18..05e556c2 100644 --- a/kortex_gazebo/launch/spawn_kortex_robot.launch +++ b/kortex_gazebo/launch/spawn_kortex_robot.launch @@ -12,11 +12,14 @@ - - + + + + + - + @@ -36,41 +39,42 @@ - - - + + + -J joint_7 1.57" + if="$(eval not arg('gripper'))"/> + -J joint_7 1.57" + unless="$(eval not arg('gripper'))"/> - + @@ -121,11 +125,13 @@ - + + + - + + + diff --git a/kortex_gazebo/readme.md b/kortex_gazebo/readme.md index 8eeaa1a5..206a55b9 100644 --- a/kortex_gazebo/readme.md +++ b/kortex_gazebo/readme.md @@ -13,7 +13,7 @@ # Kortex Gazebo (**EXPERIMENTAL**) ## Overview -This package contains files to simulate the Kinova Gen3 Ultra lightweight robot in Gazebo. +This package contains files to simulate the Kinova Gen3 and Gen3 lite robots in Gazebo. ### License @@ -23,9 +23,7 @@ The source code is released under a [BSD 3-Clause license](../LICENSE). Affiliation: [Kinova inc.](https://www.kinovarobotics.com/)
Maintainer: Kinova inc. support@kinovarobotics.com** -This package has been tested under: -- ROS Kinetic, Gazebo 7 and Ubuntu 16.04 -- ROS Melodic, Gazebo 9 and Ubuntu 18.04 +This package has been tested under ROS Kinetic, Gazebo 7 and Ubuntu 16.04 and under ROS Melodic, Gazebo 9 and Ubuntu 18.04. ## Why **"EXPERIMENTAL"**? @@ -49,7 +47,7 @@ The launch can be parametrized with arguments : - **y0** : The default Y-axis position of the robot in Gazebo. The default value is **0.0**. - **z0** : The default Z-axis position of the robot in Gazebo. The default value is **0.0**. - **arm** : Name of your robot arm model. See the `kortex_description/arms` folder to see the available robot models. The default value is **gen3**. -- **gripper** : Name of your robot tool / gripper. See the `kortex_description/grippers` folder to see the available tool models (or to add your own). The default value is **""** and the only other supported option is **robotiq_2f_85**. +- **gripper** : Name of your robot arm's tool / gripper. See the `kortex_description/grippers` folder to see the available end effector models (or to add your own). The default value is **""**. For Gen3, you can also put **robotiq_2f_85**. For Gen3 lite, you need to put **gen3_lite_2f**. - **robot_name** : This is the namespace of the arm that is going to be spawned. It defaults to **my_$(arg arm)** (so my_gen3 for arm="gen3"). - **use_trajectory_controller** : If this argument is false, one `JointPositionController` per joint will be launched and the arm will offer a basic ROS Control interface to control every joint individually with topics. If this argument is true, a MoveIt! node will be started for the arm and the arm will offer a `FollowJointTrajectory` interface to control the arm (via a `JointTrajectoryController`). The default value is **true**. - **use_sim_time** : If this value is true, Gazebo will use simulated time instead of system clock. The default value is **true**. @@ -93,13 +91,13 @@ When everything is well loaded, the script unpauses Gazebo's physics and uses Mo ## Plugins -To properly simulate the Robotiq 2F-85 Gripper in Gazebo, we use two Gazebo Plugins. +To properly simulate the grippers in Gazebo, we use two Gazebo Plugins. ### Mimic joint plugin Gazebo doesn't offer support for the URDF **mimic** tag. -By loading one instance of [this plugin](../third_party/roboticsgroup_gazebo_plugins/README.md) per mimic joint, we are able to simulate those joints properly. The plugin parameters are specified with the [transmission elements for the Robotiq gripper](../kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_transmission_macro.xacro). +By loading one instance of [this plugin](../third_party/roboticsgroup_gazebo_plugins/README.md) per mimic joint, we are able to simulate those joints properly. The plugin parameters are specified with the [transmission elements for the Robotiq gripper](../kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_transmission_macro.xacro) and the [transmission elements for the Gen3 lite gripper](../kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_transmission_macro.xacro). ### Gazebo grasp plugin -Gazebo doesn't support grasping very well. By loading [this plugin](../third_party/gazebo-pkgs/README.md), we make sure objects grasped by the gripper will not fall. The plugin parameters are specified in with the [transmission elements for the Robotiq gripper](../kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_transmission_macro.xacro). +Gazebo doesn't support grasping very well. By loading [this plugin](../third_party/gazebo-pkgs/README.md), we make sure objects grasped by the gripper will not fall. The plugin parameters are specified in with the [transmission elements for the Robotiq gripper](../kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_transmission_macro.xacro) and the [transmission elements for the Gen3 lite gripper](../kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_transmission_macro.xacro). 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 new file mode 100644 index 00000000..af9086aa --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/.setup_assistant @@ -0,0 +1,11 @@ +moveit_setup_assistant_config: + URDF: + package: kortex_description + relative_path: robots/gen3_lite_gen3_lite_2f.xacro + xacro_args: "--inorder " + SRDF: + relative_path: config/gen3_lite_gen3_lite_2f.srdf + CONFIG: + author_name: Alexandre Vannobel + author_email: avannobel@kinova.ca + generated_timestamp: 1564078352 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/CMakeLists.txt b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/CMakeLists.txt new file mode 100644 index 00000000..3bef278e --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 2.8.3) +project(gen3_lite_gen3_lite_2f_move_it_config) + +find_package(catkin REQUIRED) + +catkin_package() + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/chomp_planning.yaml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/chomp_planning.yaml new file mode 100644 index 00000000..75258e53 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file 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 new file mode 100644 index 00000000..8eda2826 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/fake_controllers.yaml @@ -0,0 +1,12 @@ +controller_list: + - name: fake_arm_controller + joints: + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 + - name: fake_gripper_controller + joints: + - 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 new file mode 100644 index 00000000..c4315247 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf @@ -0,0 +1,106 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 new file mode 100644 index 00000000..fe12260c --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/joint_limits.yaml @@ -0,0 +1,39 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# 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: + has_velocity_limits: true + max_velocity: 1.58 + has_acceleration_limits: true + max_acceleration: 0.35 + joint_2: + has_velocity_limits: true + max_velocity: 1.58 + has_acceleration_limits: true + max_acceleration: 0.35 + joint_3: + has_velocity_limits: true + max_velocity: 1.58 + has_acceleration_limits: true + max_acceleration: 0.35 + joint_4: + has_velocity_limits: true + max_velocity: 1.58 + has_acceleration_limits: true + max_acceleration: 0.35 + joint_5: + has_velocity_limits: true + max_velocity: 1.58 + has_acceleration_limits: true + max_acceleration: 3.5 + joint_6: + has_velocity_limits: true + max_velocity: 1.58 + has_acceleration_limits: true + max_acceleration: 3.5 + right_finger_bottom_joint: + has_velocity_limits: true + max_velocity: 1000 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/kinematics.yaml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/kinematics.yaml new file mode 100644 index 00000000..a1f17d08 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/kinematics.yaml @@ -0,0 +1,5 @@ +arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 \ No newline at end of file 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 new file mode 100644 index 00000000..a49c255b --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ompl_planning.yaml @@ -0,0 +1,176 @@ +planner_configs: + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 +arm: + default_planner_config: RRTConnect + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(joint_1,joint_2) + longest_valid_segment_fraction: 0.005 +gripper: + default_planner_config: RRTConnect + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo \ No newline at end of file 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 new file mode 100644 index 00000000..a71f1cae --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ros_controllers.yaml @@ -0,0 +1,22 @@ +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: + - name: "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 + + - name: "gen3_lite_2f_gripper_controller" + action_ns: gripper_cmd + default: True + type: GripperCommand + joints: + - 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/sensors_3d.yaml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/sensors_3d.yaml new file mode 100644 index 00000000..d2955dcd --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/sensors_3d.yaml @@ -0,0 +1,3 @@ +# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it +sensors: + - {} \ No newline at end of file diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/chomp_planning_pipeline.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 00000000..d3682c07 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/default_warehouse_db.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/default_warehouse_db.launch new file mode 100644 index 00000000..f2e0e57f --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/demo.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/demo.launch new file mode 100644 index 00000000..0a411e36 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/demo.launch @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/fake_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 00000000..ff37a3ce --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + 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 new file mode 100644 index 00000000..ec664d10 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/gen3_lite_gen3_lite_2f_moveit_controller_manager.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/gen3_lite_gen3_lite_2f_moveit_sensor_manager.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/gen3_lite_gen3_lite_2f_moveit_sensor_manager.launch.xml new file mode 100644 index 00000000..5d02698d --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/gen3_lite_gen3_lite_2f_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/joystick_control.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/joystick_control.launch new file mode 100644 index 00000000..9411f6e6 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + 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 new file mode 100644 index 00000000..474e092a --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch @@ -0,0 +1,81 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/moveit.rviz b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/moveit.rviz new file mode 100644 index 00000000..0877e1e7 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/moveit.rviz @@ -0,0 +1,689 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.74256 + Tree Height: 664 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Time: 5 + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + Name: MotionPlanning + Planned Path: + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: left_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.2 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Scene Robot: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: /base_link + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 2.9965 + Focal Point: + X: 0.113567 + Y: 0.10592 + Z: 2.23518e-07 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.509797 + Target Frame: /base_link + Value: XYOrbit (rviz) + Yaw: 5.65995 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1337 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + Motion Planning: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1828 + X: 459 + Y: -243 diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/moveit_rviz.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/moveit_rviz.launch new file mode 100644 index 00000000..5cc4effa --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/moveit_rviz.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + 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 new file mode 100644 index 00000000..7dc43ada --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + 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 new file mode 100644 index 00000000..f501959f --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + 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 new file mode 100644 index 00000000..b941658f --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ros_controllers.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ros_controllers.launch similarity index 65% rename from kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ros_controllers.launch rename to kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ros_controllers.launch index 00eb8311..a75f7058 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ros_controllers.launch +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ros_controllers.launch @@ -2,16 +2,16 @@ - + + output="screen" ns="gen3_lite_gen3_lite_2f" args=""/> - + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/run_benchmark_ompl.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/run_benchmark_ompl.launch new file mode 100644 index 00000000..0d4f1581 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/run_benchmark_ompl.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/sensor_manager.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/sensor_manager.launch.xml new file mode 100644 index 00000000..c7348ac6 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/setup_assistant.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/setup_assistant.launch new file mode 100644 index 00000000..f20f3eba --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/setup_assistant.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + 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 new file mode 100644 index 00000000..2d825e83 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/trajectory_execution.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/warehouse.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/warehouse.launch new file mode 100644 index 00000000..50e16ef7 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/warehouse_settings.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/warehouse_settings.launch.xml new file mode 100644 index 00000000..e473b083 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/package.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/package.xml new file mode 100644 index 00000000..6ba5a2c3 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/package.xml @@ -0,0 +1,34 @@ + + + gen3_lite_gen3_lite_2f_move_it_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the gen3_lite_gen3_lite_2f with the MoveIt! Motion Planning Framework + + Alexandre Vannobel + Alexandre Vannobel + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + moveit_ros_move_group + moveit_fake_controller_manager + moveit_kinematics + moveit_planners_ompl + moveit_ros_visualization + moveit_setup_assistant + joint_state_publisher + robot_state_publisher + xacro + + + kortex_description + kortex_description + + + 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 new file mode 100644 index 00000000..c693bcb7 --- /dev/null +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/fake_controllers.yaml @@ -0,0 +1,9 @@ +controller_list: + - name: fake_arm_controller + joints: + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - 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 new file mode 100644 index 00000000..fda606b1 --- /dev/null +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/6dof/joint_limits.yaml new file mode 100644 index 00000000..6a93c689 --- /dev/null +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/joint_limits.yaml @@ -0,0 +1,34 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# 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: + has_velocity_limits: true + max_velocity: 0.86 + has_acceleration_limits: true + max_acceleration: 0.3 + joint_2: + has_velocity_limits: true + max_velocity: 0.86 + has_acceleration_limits: true + max_acceleration: 0.3 + joint_3: + has_velocity_limits: true + max_velocity: 0.86 + has_acceleration_limits: true + max_acceleration: 0.3 + joint_4: + has_velocity_limits: true + max_velocity: 0.86 + has_acceleration_limits: true + max_acceleration: 0.3 + joint_5: + has_velocity_limits: true + max_velocity: 0.86 + has_acceleration_limits: true + max_acceleration: 0.3 + joint_6: + has_velocity_limits: true + max_velocity: 0.86 + has_acceleration_limits: true + max_acceleration: 0.3 \ No newline at end of file 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 new file mode 100644 index 00000000..9048503d --- /dev/null +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/ros_controllers.yaml @@ -0,0 +1,15 @@ +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: + - name: "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 diff --git a/kortex_move_it_config/gen3_move_it_config/config/fake_controllers.yaml b/kortex_move_it_config/gen3_move_it_config/config/7dof/fake_controllers.yaml similarity index 100% rename from kortex_move_it_config/gen3_move_it_config/config/fake_controllers.yaml rename to kortex_move_it_config/gen3_move_it_config/config/7dof/fake_controllers.yaml diff --git a/kortex_move_it_config/gen3_move_it_config/config/gen3.srdf b/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf similarity index 100% rename from kortex_move_it_config/gen3_move_it_config/config/gen3.srdf rename to kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf diff --git a/kortex_move_it_config/gen3_move_it_config/config/joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/7dof/joint_limits.yaml similarity index 100% rename from kortex_move_it_config/gen3_move_it_config/config/joint_limits.yaml rename to kortex_move_it_config/gen3_move_it_config/config/7dof/joint_limits.yaml diff --git a/kortex_move_it_config/gen3_move_it_config/config/ros_controllers.yaml b/kortex_move_it_config/gen3_move_it_config/config/7dof/ros_controllers.yaml similarity index 100% rename from kortex_move_it_config/gen3_move_it_config/config/ros_controllers.yaml rename to kortex_move_it_config/gen3_move_it_config/config/7dof/ros_controllers.yaml diff --git a/kortex_move_it_config/gen3_move_it_config/launch/fake_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/fake_moveit_controller_manager.launch.xml index 5d2c996e..d2f4eaf8 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/fake_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_move_it_config/launch/fake_moveit_controller_manager.launch.xml @@ -1,9 +1,11 @@ + + - + 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 0353c23d..e0b4c42f 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,9 +1,11 @@ + + - + 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 fb4955ca..9ab01af0 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 @@ -1,6 +1,11 @@ - + + + + + + @@ -45,6 +50,7 @@ + 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 4da8a8ce..0e8b6ad9 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 @@ -5,15 +5,18 @@ + + + - + - + 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 95b4df70..7f071c1a 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 @@ -1,6 +1,7 @@ + @@ -15,6 +16,8 @@ - + + + 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 new file mode 100644 index 00000000..e3376f77 --- /dev/null +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/fake_controllers.yaml @@ -0,0 +1,11 @@ + - name: fake_arm_controller + joints: + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 + - name: fake_gripper_controller + joints: + - gripper_finger1_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 new file mode 100644 index 00000000..cac6a8a8 --- /dev/null +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf @@ -0,0 +1,167 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/joint_limits.yaml new file mode 100644 index 00000000..3f51e47d --- /dev/null +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/joint_limits.yaml @@ -0,0 +1,64 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# 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: + has_velocity_limits: true + max_velocity: 0.86 + has_acceleration_limits: true + max_acceleration: 0.3 + joint_2: + has_velocity_limits: true + max_velocity: 0.86 + has_acceleration_limits: true + max_acceleration: 0.3 + joint_3: + has_velocity_limits: true + max_velocity: 0.86 + has_acceleration_limits: true + max_acceleration: 0.3 + joint_4: + has_velocity_limits: true + max_velocity: 0.86 + has_acceleration_limits: true + max_acceleration: 0.3 + joint_5: + has_velocity_limits: true + max_velocity: 0.86 + has_acceleration_limits: true + max_acceleration: 0.3 + joint_6: + has_velocity_limits: true + max_velocity: 0.86 + has_acceleration_limits: true + max_acceleration: 0.3 + gripper_finger1_finger_tip_joint: + has_velocity_limits: true + max_velocity: 2 + has_acceleration_limits: false + max_acceleration: 0 + gripper_finger1_inner_knuckle_joint: + has_velocity_limits: true + max_velocity: 2 + has_acceleration_limits: false + max_acceleration: 0 + gripper_finger1_joint: + has_velocity_limits: true + max_velocity: 2 + has_acceleration_limits: false + max_acceleration: 0 + gripper_finger2_finger_tip_joint: + has_velocity_limits: true + max_velocity: 2 + has_acceleration_limits: false + max_acceleration: 0 + gripper_finger2_inner_knuckle_joint: + has_velocity_limits: true + max_velocity: 2 + has_acceleration_limits: false + max_acceleration: 0 + gripper_finger2_joint: + has_velocity_limits: true + max_velocity: 2 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file 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 new file mode 100644 index 00000000..b5a190cd --- /dev/null +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/ros_controllers.yaml @@ -0,0 +1,22 @@ +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: + - name: "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 + + - name: "robotiq_2f_85_gripper_controller" + action_ns: gripper_cmd + default: True + type: GripperCommand + joints: + - gripper_finger1_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/fake_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/fake_controllers.yaml similarity index 100% rename from kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/fake_controllers.yaml rename to kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/fake_controllers.yaml diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/gen3_robotiq_2f_85.srdf b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf similarity index 100% rename from kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/gen3_robotiq_2f_85.srdf rename to kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/joint_limits.yaml similarity index 100% rename from kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/joint_limits.yaml rename to kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/joint_limits.yaml diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/ros_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/ros_controllers.yaml similarity index 100% rename from kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/ros_controllers.yaml rename to kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/ros_controllers.yaml diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/fake_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/fake_moveit_controller_manager.launch.xml index b5236b41..65ad9d3f 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/fake_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/fake_moveit_controller_manager.launch.xml @@ -1,9 +1,12 @@ + + + - + 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 a488b54e..8ba0c8f6 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,9 +1,11 @@ + + - + 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 3d4a83c4..c37858ff 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 @@ -1,6 +1,11 @@ - + + + + + + @@ -45,6 +50,7 @@ + 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 9d3c41fe..44ef9420 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 @@ -5,15 +5,18 @@ + + + - + - + 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 9e035d1d..d8509a8f 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 @@ -1,6 +1,7 @@ + @@ -15,6 +16,8 @@ - + + + diff --git a/kortex_move_it_config/readme.md b/kortex_move_it_config/readme.md index 6f9f7e22..832f49ca 100644 --- a/kortex_move_it_config/readme.md +++ b/kortex_move_it_config/readme.md @@ -18,7 +18,7 @@ This folder contains all the auto-generated MoveIt! configuration ROS packages. ## Naming The packages that don't use a gripper are named `ARM_move_it_config`, where "ARM" is the name of the arm you are using. -See the `kortex_description/arms` folder for a list of supported Kinova Kortex robots. (Currently only the Kinova Gen3 Ultra lightweight robot, but this will expand over time) +See the `kortex_description/arms` folder for a list of supported Kinova Kortex robots. The packages that use a gripper are named `ARM_GRIPPER_move_it_config`, where "ARM" is the name of the arm you are using and "GRIPPER" is the name of the gripper you are using. See the `kortex_description/grippers` folder for a list of supported Kinova Kortex grippers. diff --git a/readme.md b/readme.md index e446e468..156a936e 100644 --- a/readme.md +++ b/readme.md @@ -13,10 +13,9 @@ To access the color and depth streams, you will need to clone and follow the ins ### Setup -[Robot Operating System (ROS)](http://wiki.ros.org) (middleware for robotics) +- [Robot Operating System (ROS)](http://wiki.ros.org) (middleware for robotics) You can find the instructions to install ROS Kinetic (for Ubuntu 16.04) [here](http://wiki.ros.org/kinetic/Installation/Ubuntu). -You can find the instructions to install ROS Melodic (for Ubuntu 18.04) [here](http://wiki.ros.org/melodic/Installation/Ubuntu). [Google Protocol Buffers](https://developers.google.com/protocol-buffers/) is used by Kinova to define the Kortex APIs and to automatically generate ROS messages, services and C++ classes from the Kortex API `.proto` files. The installation of Google Protocol Buffers is required by developers implementing new APIs with the robot. However, since we already provide all the necessary generated files on GitHub, this is not required for most end users of the robot. @@ -39,13 +38,6 @@ These are the instructions to run in a terminal to create the workspace, clone t The following is a description of the packages included in this repository. -### kortex_api -This package contains all the header files and the libraries of the C++ Kortex API. The files are automatically downloaded from the Web and extracted when you `catkin_make` if the `kortex_api/include` and `kortex_api/lib` folders are empty. - -**Note:** Upon a new release of the API, it is important to delete the content of these two folders to make sure the new API gets downloaded and you don't get build errors when you `catkin_make`. - -A more detailed [description](kortex_api/readme.md) can be found in the package subdirectory. - ### kortex_control This package implements the simulation controllers that control the arm in Gazebo. For more details, please consult the [README](kortex_control/readme.md) from the package subdirectory. @@ -55,17 +47,17 @@ This package implements the simulation controllers that control the arm in Gazeb This package contains the URDF (Unified Robot Description Format), STL and configuration files for the Kortex-compatible robots. For more details, please consult the [README](kortex_description/readme.md) from the package subdirectory. ### kortex_driver -This package implements a ROS node that allows communication between a node and a Kinova Gen3 Ultra lightweight robot. For more details, please consult the [README](kortex_driver/readme.md) from the package subdirectory. +This package implements a ROS node that allows communication between a node and a Kinova Gen3 or Gen3 lite robot. For more details, please consult the [README](kortex_driver/readme.md) from the package subdirectory. ### kortex_examples This package holds all the examples needed to understand the basics of `ros_kortex`. Most of the examples are written in both C++ and Python. Only the MoveIt! example is available exclusively in Python for now. A more detailed [description](kortex_examples/readme.md) can be found in the package subdirectory. ### kortex_gazebo -This package contains files to simulate the Kinova Gen3 Ultra lightweight robot in Gazebo. For more details, please consult the [README](kortex_gazebo/readme.md) from the package subdirectory. +This package contains files to simulate the Kinova Gen3 and Gen3 lite robots in Gazebo. For more details, please consult the [README](kortex_gazebo/readme.md) from the package subdirectory. ### kortex_move_it_config -This metapackage contains the auto-generated MoveIt! files to use the Kinova Gen3 arm with the MoveIt! motion planning framework. For more details, please consult the [README](kortex_move_it_config/readme.md) from the package subdirectory. +This metapackage contains the auto-generated MoveIt! files to use the Kinova Gen3 and Gen3 lite arms with the MoveIt! motion planning framework. For more details, please consult the [README](kortex_move_it_config/readme.md) from the package subdirectory. ### third_party This folder contains the third-party packages we use with the ROS Kortex packages. Currently, it consists of two packages used for the simulation of the Robotiq Gripper in Gazebo. We use [gazebo-pkgs](third_party/gazebo-pkgs/README.md) for grasping support in Gazebo and [roboticsgroup_gazebo_plugins](third_party/roboticsgroup_gazebo_plugins/README.md) to mimic joint support in Gazebo. diff --git a/third_party/gazebo-pkgs/gazebo_grasp_plugin/CMakeLists.txt b/third_party/gazebo-pkgs/gazebo_grasp_plugin/CMakeLists.txt index 04185436..a93a2855 100644 --- a/third_party/gazebo-pkgs/gazebo_grasp_plugin/CMakeLists.txt +++ b/third_party/gazebo-pkgs/gazebo_grasp_plugin/CMakeLists.txt @@ -67,6 +67,10 @@ link_directories( ${GAZEBO_LIBRARY_DIRS} ) +# Protobuf 2.6.1 includes were copied in kortex_gazebo/include and are necessary to build Gazebo plugins +include_directories(BEFORE ../../../kortex_gazebo/include/) +link_directories(BEFORE ../../../kortex_gazebo/lib/) + ## Declare a cpp library add_library(gazebo_grasp_fix SHARED src/GazeboGraspFix.cpp src/GazeboGraspGripper.cpp) diff --git a/third_party/gazebo-pkgs/gazebo_version_helpers/CMakeLists.txt b/third_party/gazebo-pkgs/gazebo_version_helpers/CMakeLists.txt index d8dbedc2..09e40695 100644 --- a/third_party/gazebo-pkgs/gazebo_version_helpers/CMakeLists.txt +++ b/third_party/gazebo-pkgs/gazebo_version_helpers/CMakeLists.txt @@ -56,6 +56,10 @@ endif() link_directories(${GAZEBO_LIBRARY_DIRS}) include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} include) +# Protobuf 2.6.1 includes were copied in kortex_gazebo/include and are necessary to build Gazebo plugins +include_directories(BEFORE ../../../kortex_gazebo/include/) +link_directories(BEFORE ../../../kortex_gazebo/lib/) + ## Declare a C++ library add_library(${PROJECT_NAME} SHARED src/GazeboVersionHelpers.cpp diff --git a/third_party/roboticsgroup_gazebo_plugins/CMakeLists.txt b/third_party/roboticsgroup_gazebo_plugins/CMakeLists.txt index b4c6ce1e..61918ff1 100644 --- a/third_party/roboticsgroup_gazebo_plugins/CMakeLists.txt +++ b/third_party/roboticsgroup_gazebo_plugins/CMakeLists.txt @@ -28,6 +28,10 @@ catkin_package( link_directories(${GAZEBO_LIBRARY_DIRS}) include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} include) +# Protobuf 2.6.1 includes were copied in kortex_gazebo/include and are necessary to build Gazebo plugins +include_directories(BEFORE ../../kortex_gazebo/include/) +link_directories(BEFORE ../../kortex_gazebo/lib/) + add_library(roboticsgroup_gazebo_mimic_joint_plugin src/mimic_joint_plugin.cpp) target_link_libraries(roboticsgroup_gazebo_mimic_joint_plugin libprotobuf.so ${catkin_LIBRARIES})