diff --git a/.gitmodules b/.gitmodules
index fa91b01..168c2bb 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -9,3 +9,6 @@
path = src/external_dependencies/picknik_accessories
url = git@github.com:PickNikRobotics/picknik_accessories.git
branch = kinova-main
+[submodule "src/external_dependencies/serial"]
+ path = src/external_dependencies/serial
+ url = git@github.com:PickNikRobotics/serial.git
diff --git a/src/Dockerfile b/src/Dockerfile
new file mode 100644
index 0000000..03c19f0
--- /dev/null
+++ b/src/Dockerfile
@@ -0,0 +1,119 @@
+# Docker image for extending MoveIt Pro with a custom overlay.
+#
+# Example build command (with defaults):
+#
+# docker build -f ./Dockerfile .
+#
+
+# Specify the MoveIt Pro release to build on top of.
+ARG MOVEIT_STUDIO_BASE_IMAGE
+ARG USERNAME=studio-user
+ARG USER_UID=1000
+ARG USER_GID=1000
+
+##################################################
+# Starting from the specified MoveIt Pro release #
+##################################################
+# The image tag is specified in the argument itself.
+# hadolint ignore=DL3006
+FROM ${MOVEIT_STUDIO_BASE_IMAGE} as base
+
+# Create a non-root user
+ARG USERNAME
+ARG USER_UID
+ARG USER_GID
+
+# Copy source code from the workspace's ROS 2 packages to a workspace inside the container
+ARG USER_WS=/home/${USERNAME}/user_ws
+ENV USER_WS=${USER_WS}
+RUN mkdir -p ${USER_WS}/src ${USER_WS}/build ${USER_WS}/install ${USER_WS}/log
+COPY ./src ${USER_WS}/src
+
+# Also mkdir with user permission directories which will be mounted later to avoid docker creating them as root
+WORKDIR $USER_WS
+# hadolint ignore=DL3008
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
+ --mount=type=cache,target=/var/lib/apt,sharing=locked \
+ groupadd --gid $USER_GID ${USERNAME} && \
+ useradd --uid $USER_UID --gid $USER_GID --shell /bin/bash --create-home ${USERNAME} && \
+ apt-get update && \
+ apt-get install -q -y --no-install-recommends sudo && \
+ echo ${USERNAME} ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/${USERNAME} && \
+ chmod 0440 /etc/sudoers.d/${USERNAME} && \
+ cp -r /etc/skel/. /home/${USERNAME} && \
+ mkdir -p \
+ /home/${USERNAME}/.ccache \
+ /home/${USERNAME}/.config \
+ /home/${USERNAME}/.ignition \
+ /home/${USERNAME}/.colcon \
+ /home/${USERNAME}/.ros && \
+ chown -R $USER_UID:$USER_GID /home/${USERNAME} /opt/overlay_ws/
+
+# Install additional Kortex Vision dependencies
+# NOTE: The /opt/overlay_ws folder contains MoveIt Pro binary packages and the source file.
+# hadolint ignore=DL3008, SC1091
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
+ --mount=type=cache,target=/var/lib/apt,sharing=locked \
+ . /opt/overlay_ws/install/setup.sh && \
+ apt-get update && \
+ apt-get install -y --no-install-recommends \
+ gstreamer1.0-tools \
+ gstreamer1.0-libav \
+ libgstreamer1.0-dev \
+ libgstreamer-plugins-base1.0-dev \
+ libgstreamer-plugins-good1.0-dev \
+ gstreamer1.0-plugins-good \
+ gstreamer1.0-plugins-base
+
+# Install additional dependencies
+# You can also add any necessary apt-get install, pip install, etc. commands at this point.
+# NOTE: The /opt/overlay_ws folder contains MoveIt Pro binary packages and the source file.
+# hadolint ignore=SC1091
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
+ --mount=type=cache,target=/var/lib/apt,sharing=locked \
+ . /opt/overlay_ws/install/setup.sh && \
+ apt-get update && \
+ rosdep install -q -y \
+ --from-paths src \
+ --ignore-src
+
+###################################################################
+# Target for the developer build which does not compile any code. #
+###################################################################
+FROM base as user-overlay-dev
+
+ARG USERNAME
+ARG USER_WS=/home/${USERNAME}/user_ws
+ENV USER_WS=${USER_WS}
+
+# Install any additional packages for development work
+# hadolint ignore=DL3008
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
+ --mount=type=cache,target=/var/lib/apt,sharing=locked \
+ apt-get update && \
+ apt-get install -y --no-install-recommends \
+ less \
+ gdb \
+ nano
+
+# Set up the user's .bashrc file and shell.
+CMD ["/usr/bin/bash"]
+
+#########################################
+# Target for compiled, deployable image #
+#########################################
+FROM base as user-overlay
+
+ARG USERNAME
+ARG USER_WS=/home/${USERNAME}/user_ws
+ENV USER_WS=${USER_WS}
+
+# Compile the workspace
+WORKDIR $USER_WS
+# hadolint ignore=SC1091
+RUN --mount=type=cache,target=/home/${USERNAME}/.ccache \
+ . /opt/overlay_ws/install/setup.sh && \
+ colcon build
+
+# Set up the user's .bashrc file and shell.
+CMD ["/usr/bin/bash"]
diff --git a/src/external_dependencies/ros2_kortex b/src/external_dependencies/ros2_kortex
index 915df14..cbfb28e 160000
--- a/src/external_dependencies/ros2_kortex
+++ b/src/external_dependencies/ros2_kortex
@@ -1 +1 @@
-Subproject commit 915df14e83fbc295a985f393afcc9175bdfcd72b
+Subproject commit cbfb28efb8c3007485aa80f86d692cd56e0e9109
diff --git a/src/external_dependencies/serial b/src/external_dependencies/serial
new file mode 160000
index 0000000..d8d1606
--- /dev/null
+++ b/src/external_dependencies/serial
@@ -0,0 +1 @@
+Subproject commit d8d160678aa0b31cdf467c052b954fa287cc6cdf
diff --git a/src/kinova_gen3_base_config/config/control/picknik_kinova_gen3.ros2_control.yaml b/src/kinova_gen3_base_config/config/control/picknik_kinova_gen3.ros2_control.yaml
index 46c9df3..7a4be34 100644
--- a/src/kinova_gen3_base_config/config/control/picknik_kinova_gen3.ros2_control.yaml
+++ b/src/kinova_gen3_base_config/config/control/picknik_kinova_gen3.ros2_control.yaml
@@ -8,8 +8,8 @@ controller_manager:
joint_trajectory_controller:
type: joint_trajectory_controller/JointTrajectoryController
- servo_controller:
- type: joint_trajectory_controller/JointTrajectoryController
+ twist_controller:
+ type: picknik_twist_controller/PicknikTwistController
# The gripper controller just relays position commands
robotiq_gripper_controller:
@@ -18,6 +18,9 @@ controller_manager:
fault_controller:
type: picknik_reset_fault_controller/PicknikResetFaultController
+ servo_controller:
+ type: joint_trajectory_controller/JointTrajectoryController
+
joint_trajectory_controller:
ros__parameters:
joints:
@@ -38,7 +41,7 @@ joint_trajectory_controller:
allow_partial_joints_goal: false
constraints:
stopped_velocity_tolerance: 0.0
- goal_time: 0.0
+ goal_time: 10.0
joint_1:
goal: 0.05
joint_2:
@@ -54,6 +57,23 @@ joint_trajectory_controller:
joint_7:
goal: 0.05
+twist_controller:
+ ros__parameters:
+ joint: tcp
+ interface_names:
+ - twist.linear.x
+ - twist.linear.y
+ - twist.linear.z
+ - twist.angular.x
+ - twist.angular.y
+ - twist.angular.z
+
+robotiq_gripper_controller:
+ ros__parameters:
+ default: true
+ joint: robotiq_85_left_knuckle_joint
+ allow_stalling: true
+
servo_controller:
ros__parameters:
joints:
@@ -69,6 +89,14 @@ servo_controller:
state_interfaces:
- position
- velocity
+ command_joints:
+ - joint_1
+ - joint_2
+ - joint_3
+ - joint_4
+ - joint_5
+ - joint_6
+ - joint_7
state_publish_rate: 100.0
action_monitor_rate: 20.0
allow_partial_joints_goal: false
@@ -89,9 +117,3 @@ servo_controller:
goal: 0.05
joint_7:
goal: 0.05
-
-robotiq_gripper_controller:
- ros__parameters:
- default: true
- joint: robotiq_85_left_knuckle_joint
- allow_stalling: true
diff --git a/src/kinova_gen3_base_config/description/robotiq_2f_85_gripper.urdf b/src/kinova_gen3_base_config/description/robotiq_2f_85_gripper.urdf
new file mode 100644
index 0000000..659cf2c
--- /dev/null
+++ b/src/kinova_gen3_base_config/description/robotiq_2f_85_gripper.urdf
@@ -0,0 +1,465 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 6
+
+ 1.13446
+
+ 640
+ 480
+ R8G8B8
+
+
+ 0.01
+ 300
+
+
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+
+
+
+
+ d415_camera/image_raw:=wrist_mounted_camera/color/image_raw
+ d415_camera/camera_info:=wrist_mounted_camera/color/camera_info
+
+ d415_camera
+ wrist_mounted_camera_depth_optical_frame
+ 0.0
+
+
+
+ 6
+
+ 1.3997540600994522
+
+ B8G8R8
+ 640
+ 480
+
+
+ 0.05
+ 8.0
+
+
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+
+
+
+
+ d415_depth/depth/image_raw:=wrist_mounted_camera/depth/image_rect_raw
+ d415_depth/depth/camera_info:=wrist_mounted_camera/depth/camera_info
+ d415_depth/points:=wrist_mounted_camera/depth/color/points
+ d415_depth/camera_info:=wrist_mounted_camera/unused1
+ d415_depth/image_raw:=wrist_mounted_camera/unused2
+
+ d415_depth
+ wrist_mounted_camera_depth_optical_frame
+ 0.0
+ 0.15
+ 4.0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/kinova_gen3_base_config/objectives/3_waypoint_pick_and_place.xml b/src/kinova_gen3_base_config/objectives/3_waypoint_pick_and_place.xml
index d27c64e..4b2adec 100644
--- a/src/kinova_gen3_base_config/objectives/3_waypoint_pick_and_place.xml
+++ b/src/kinova_gen3_base_config/objectives/3_waypoint_pick_and_place.xml
@@ -5,28 +5,28 @@
-
+
-
+
-
-
+
+
-
+
-
+
-
-
+
+
-
+
diff --git a/src/kinova_gen3_base_config/objectives/move_to_joint_state.xml b/src/kinova_gen3_base_config/objectives/move_to_joint_state.xml
index d4cf653..5a0bc54 100644
--- a/src/kinova_gen3_base_config/objectives/move_to_joint_state.xml
+++ b/src/kinova_gen3_base_config/objectives/move_to_joint_state.xml
@@ -6,7 +6,7 @@
-
+
diff --git a/src/kinova_gen3_base_config/objectives/teleoperate.xml b/src/kinova_gen3_base_config/objectives/teleoperate.xml
index 0af0e03..f9b45da 100644
--- a/src/kinova_gen3_base_config/objectives/teleoperate.xml
+++ b/src/kinova_gen3_base_config/objectives/teleoperate.xml
@@ -2,6 +2,6 @@
-
+
diff --git a/src/kinova_gen3_base_config/package.xml b/src/kinova_gen3_base_config/package.xml
index 3570bc1..fe618eb 100644
--- a/src/kinova_gen3_base_config/package.xml
+++ b/src/kinova_gen3_base_config/package.xml
@@ -1,7 +1,7 @@
kinova_gen3_base_config
- 1.0.0
+ 4.0.0
Base config package for a Kinova Gen3 system
diff --git a/src/moveit_studio_kinova_pstop_manager/package.xml b/src/moveit_studio_kinova_pstop_manager/package.xml
index b82f81d..bffee47 100644
--- a/src/moveit_studio_kinova_pstop_manager/package.xml
+++ b/src/moveit_studio_kinova_pstop_manager/package.xml
@@ -2,7 +2,7 @@
moveit_studio_kinova_pstop_manager
- 2.5.0
+ 4.0.0
Provides a node to monitor the protective stop state of the Kinova, and reset protective stops when necessary.
Lovro Ivanov
Proprietary
diff --git a/src/picknik_kinova_gen3_site_config/objectives/3_waypoint_pick_and_place.xml b/src/picknik_kinova_gen3_site_config/objectives/3_waypoint_pick_and_place.xml
index d27c64e..4b2adec 100644
--- a/src/picknik_kinova_gen3_site_config/objectives/3_waypoint_pick_and_place.xml
+++ b/src/picknik_kinova_gen3_site_config/objectives/3_waypoint_pick_and_place.xml
@@ -5,28 +5,28 @@
-
+
-
+
-
-
+
+
-
+
-
+
-
-
+
+
-
+
diff --git a/src/picknik_kinova_gen3_site_config/package.xml b/src/picknik_kinova_gen3_site_config/package.xml
index ecfad7f..5f6042c 100644
--- a/src/picknik_kinova_gen3_site_config/package.xml
+++ b/src/picknik_kinova_gen3_site_config/package.xml
@@ -1,7 +1,7 @@
picknik_kinova_gen3_site_config
- 1.0.0
+ 4.0.0
Robot config package for a Kinova Gen3 Robot