diff --git a/.github/workflows/ros.yaml b/.github/workflows/ros.yaml
index a3ba093..15b7c59 100644
--- a/.github/workflows/ros.yaml
+++ b/.github/workflows/ros.yaml
@@ -1,13 +1,5 @@
+on: [push]
name: ROS 2 Build and Test
-
-on:
- push:
- branches:
- - main
- pull_request:
- branches:
- - main
-
jobs:
build:
runs-on: ubuntu-latest
diff --git a/README.md b/README.md
index 39428fd..47ad5ae 100644
--- a/README.md
+++ b/README.md
@@ -1,3 +1,4 @@
# Software_2025
+
Check out our [contributing guidlines](https://github.com/UofA-SPEAR/Software_2025/blob/main/documentation/Contributing_README.md)
diff --git a/kipp_arm/src/kipp_arm_description/kipp_arm_description/__init__.py b/kipp_arm/src/kipp_arm_description/kipp_arm_description/__init__.py
deleted file mode 100644
index e69de29..0000000
diff --git a/kipp_arm/src/kipp_arm_description/launch/display.launch b/kipp_arm/src/kipp_arm_description/launch/display.launch
deleted file mode 100755
index b82c37e..0000000
--- a/kipp_arm/src/kipp_arm_description/launch/display.launch
+++ /dev/null
@@ -1,18 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/kipp_arm/src/kipp_arm_description/launch/display.launch.py b/kipp_arm/src/kipp_arm_description/launch/display.launch.py
deleted file mode 100755
index 3daeddf..0000000
--- a/kipp_arm/src/kipp_arm_description/launch/display.launch.py
+++ /dev/null
@@ -1,48 +0,0 @@
-import launch
-from launch.substitutions import Command, LaunchConfiguration
-import launch_ros
-import os
-
-def generate_launch_description():
- pkg_share = launch_ros.substitutions.FindPackageShare(package='kipp_arm_description').find('kipp_arm_description')
- default_model_path = os.path.join(pkg_share, 'urdf/SPEAR_Arm.urdf')
- default_rviz_config_path = os.path.join(pkg_share, 'resource/urdf_config.rviz')
-
- robot_state_publisher_node = launch_ros.actions.Node(
- package='robot_state_publisher',
- executable='robot_state_publisher',
- parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]
- )
- joint_state_publisher_node = launch_ros.actions.Node(
- package='joint_state_publisher',
- executable='joint_state_publisher',
- name='joint_state_publisher',
- arguments=[default_model_path],
- condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui'))
- )
- joint_state_publisher_gui_node = launch_ros.actions.Node(
- package='joint_state_publisher_gui',
- executable='joint_state_publisher_gui',
- name='joint_state_publisher_gui',
- condition=launch.conditions.IfCondition(LaunchConfiguration('gui'))
- )
- rviz_node = launch_ros.actions.Node(
- package='rviz2',
- executable='rviz2',
- name='rviz2',
- output='screen',
- arguments=['-d', LaunchConfiguration('rvizconfig')],
- )
-
- return launch.LaunchDescription([
- launch.actions.DeclareLaunchArgument(name='gui', default_value='True',
- description='Flag to enable joint_state_publisher_gui'),
- launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,
- description='Absolute path to robot urdf file'),
- launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,
- description='Absolute path to rviz config file'),
- #joint_state_publisher_node,
- joint_state_publisher_gui_node,
- robot_state_publisher_node,
- rviz_node
- ])
\ No newline at end of file
diff --git a/kipp_arm/src/kipp_arm_description/meshes/base_drum.stl b/kipp_arm/src/kipp_arm_description/meshes/base_drum.stl
deleted file mode 100644
index e41ac94..0000000
Binary files a/kipp_arm/src/kipp_arm_description/meshes/base_drum.stl and /dev/null differ
diff --git a/kipp_arm/src/kipp_arm_description/meshes/base_link.stl b/kipp_arm/src/kipp_arm_description/meshes/base_link.stl
deleted file mode 100644
index ab1c65c..0000000
Binary files a/kipp_arm/src/kipp_arm_description/meshes/base_link.stl and /dev/null differ
diff --git a/kipp_arm/src/kipp_arm_description/meshes/link1.stl b/kipp_arm/src/kipp_arm_description/meshes/link1.stl
deleted file mode 100644
index 7b4aa65..0000000
Binary files a/kipp_arm/src/kipp_arm_description/meshes/link1.stl and /dev/null differ
diff --git a/kipp_arm/src/kipp_arm_description/meshes/link2.stl b/kipp_arm/src/kipp_arm_description/meshes/link2.stl
deleted file mode 100644
index 9db8390..0000000
Binary files a/kipp_arm/src/kipp_arm_description/meshes/link2.stl and /dev/null differ
diff --git a/kipp_arm/src/kipp_arm_description/meshes/link3.stl b/kipp_arm/src/kipp_arm_description/meshes/link3.stl
deleted file mode 100644
index 6876a70..0000000
Binary files a/kipp_arm/src/kipp_arm_description/meshes/link3.stl and /dev/null differ
diff --git a/kipp_arm/src/kipp_arm_description/meshes/link4.stl b/kipp_arm/src/kipp_arm_description/meshes/link4.stl
deleted file mode 100644
index 6ba5b3a..0000000
Binary files a/kipp_arm/src/kipp_arm_description/meshes/link4.stl and /dev/null differ
diff --git a/kipp_arm/src/kipp_arm_description/meshes/link5.stl b/kipp_arm/src/kipp_arm_description/meshes/link5.stl
deleted file mode 100644
index 40cc228..0000000
Binary files a/kipp_arm/src/kipp_arm_description/meshes/link5.stl and /dev/null differ
diff --git a/kipp_arm/src/kipp_arm_description/package.xml b/kipp_arm/src/kipp_arm_description/package.xml
deleted file mode 100644
index b845413..0000000
--- a/kipp_arm/src/kipp_arm_description/package.xml
+++ /dev/null
@@ -1,18 +0,0 @@
-
-
-
- kipp_arm_description
- 0.0.0
- TODO: Package description
- spearua
- TODO: License declaration
-
- ament_copyright
- ament_flake8
- ament_pep257
- python3-pytest
-
-
- ament_python
-
-
diff --git a/kipp_arm/src/kipp_arm_description/resource/kipp_arm_description b/kipp_arm/src/kipp_arm_description/resource/kipp_arm_description
deleted file mode 100644
index e69de29..0000000
diff --git a/kipp_arm/src/kipp_arm_description/resource/urdf_config.rviz b/kipp_arm/src/kipp_arm_description/resource/urdf_config.rviz
deleted file mode 100644
index 1863d3a..0000000
--- a/kipp_arm/src/kipp_arm_description/resource/urdf_config.rviz
+++ /dev/null
@@ -1,195 +0,0 @@
-Panels:
- - Class: rviz_common/Displays
- Help Height: 78
- Name: Displays
- Property Tree Widget:
- Expanded:
- - /Global Options1
- - /Status1
- - /RobotModel1
- Splitter Ratio: 0.5
- Tree Height: 549
- - Class: rviz_common/Selection
- Name: Selection
- - Class: rviz_common/Tool Properties
- Expanded:
- - /2D Goal Pose1
- - /Publish Point1
- Name: Tool Properties
- Splitter Ratio: 0.5886790156364441
- - Class: rviz_common/Views
- Expanded:
- - /Current View1
- Name: Views
- Splitter Ratio: 0.5
- - Class: rviz_common/Time
- Experimental: false
- Name: Time
- SyncMode: 0
- SyncSource: ""
-Visualization Manager:
- Class: ""
- Displays:
- - Alpha: 0.5
- Cell Size: 1
- Class: rviz_default_plugins/Grid
- Color: 160; 160; 164
- Enabled: true
- Line Style:
- Line Width: 0.029999999329447746
- 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
- - Alpha: 1
- Class: rviz_default_plugins/RobotModel
- Collision Enabled: false
- Description File: ""
- Description Source: Topic
- Description Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /robot_description
- Enabled: true
- Links:
- All Links Enabled: true
- Expand Joint Details: false
- Expand Link Details: false
- Expand Tree: false
- Link Tree Style: Links in Alphabetic Order
- base_drum:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- base_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- link1:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- link2:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- link3:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- link4:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- link5:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- Mass Properties:
- Inertia: false
- Mass: false
- Name: RobotModel
- TF Prefix: ""
- Update Interval: 0
- Value: true
- Visual Enabled: true
- Enabled: true
- Global Options:
- Background Color: 48; 48; 48
- Fixed Frame: base_link
- Frame Rate: 30
- Name: root
- Tools:
- - Class: rviz_default_plugins/Interact
- Hide Inactive Objects: true
- - Class: rviz_default_plugins/MoveCamera
- - Class: rviz_default_plugins/Select
- - Class: rviz_default_plugins/FocusCamera
- - Class: rviz_default_plugins/Measure
- Line color: 128; 128; 0
- - Class: rviz_default_plugins/SetInitialPose
- Covariance x: 0.25
- Covariance y: 0.25
- Covariance yaw: 0.06853891909122467
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /initialpose
- - Class: rviz_default_plugins/SetGoal
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /goal_pose
- - Class: rviz_default_plugins/PublishPoint
- Single click: true
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /clicked_point
- Transformation:
- Current:
- Class: rviz_default_plugins/TF
- Value: true
- Views:
- Current:
- Class: rviz_default_plugins/Orbit
- Distance: 10
- Enable Stereo Rendering:
- Stereo Eye Separation: 0.05999999865889549
- Stereo Focal Distance: 1
- Swap Stereo Eyes: false
- Value: false
- Focal Point:
- X: 0
- Y: 0
- Z: 0
- Focal Shape Fixed Size: true
- Focal Shape Size: 0.05000000074505806
- Invert Z Axis: false
- Name: Current View
- Near Clip Distance: 0.009999999776482582
- Pitch: 0.785398006439209
- Target Frame:
- Value: Orbit (rviz)
- Yaw: 0.785398006439209
- Saved: ~
-Window Geometry:
- Displays:
- collapsed: false
- Height: 846
- Hide Left Dock: false
- Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
- Selection:
- collapsed: false
- Time:
- collapsed: false
- Tool Properties:
- collapsed: false
- Views:
- collapsed: false
- Width: 1200
- X: 579
- Y: 317
diff --git a/kipp_arm/src/kipp_arm_description/setup.cfg b/kipp_arm/src/kipp_arm_description/setup.cfg
deleted file mode 100644
index e36de72..0000000
--- a/kipp_arm/src/kipp_arm_description/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/kipp_arm_description
-[install]
-install_scripts=$base/lib/kipp_arm_description
diff --git a/kipp_arm/src/kipp_arm_description/setup.py b/kipp_arm/src/kipp_arm_description/setup.py
deleted file mode 100644
index afef38b..0000000
--- a/kipp_arm/src/kipp_arm_description/setup.py
+++ /dev/null
@@ -1,33 +0,0 @@
-from setuptools import find_packages, setup
-from glob import glob
-from setup import setup
-import os
-
-
-package_name = 'kipp_arm_description'
-
-setup(
- name=package_name,
- version='0.0.0',
- packages=find_packages(exclude=['test']),
- data_files=[
- ('share/ament_index/resource_index/packages',
- ['resource/' + package_name]),
- ('share/' + package_name, ['package.xml']),
- (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
- (os.path.join('share', package_name, 'meshes'), glob(os.path.join('meshes', '*.stl'))),
- (os.path.join('share', package_name, 'urdf'), glob(os.path.join('urdf', '*.urdf'))),
- (os.path.join('share', package_name, 'resource'), glob(os.path.join('resource', '*.rviz')))
- ],
- install_requires=['setuptools'],
- zip_safe=True,
- maintainer='spearua',
- maintainer_email='spearua@todo.todo',
- description='TODO: Package description',
- license='TODO: License declaration',
- tests_require=['pytest'],
- entry_points={
- 'console_scripts': [
- ],
- },
-)
diff --git a/kipp_arm/src/kipp_arm_description/test/test_copyright.py b/kipp_arm/src/kipp_arm_description/test/test_copyright.py
deleted file mode 100644
index 97a3919..0000000
--- a/kipp_arm/src/kipp_arm_description/test/test_copyright.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_copyright.main import main
-import pytest
-
-
-# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
-@pytest.mark.copyright
-@pytest.mark.linter
-def test_copyright():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found errors'
diff --git a/kipp_arm/src/kipp_arm_description/test/test_flake8.py b/kipp_arm/src/kipp_arm_description/test/test_flake8.py
deleted file mode 100644
index 27ee107..0000000
--- a/kipp_arm/src/kipp_arm_description/test/test_flake8.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# Copyright 2017 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_flake8.main import main_with_errors
-import pytest
-
-
-@pytest.mark.flake8
-@pytest.mark.linter
-def test_flake8():
- rc, errors = main_with_errors(argv=[])
- assert rc == 0, \
- 'Found %d code style errors / warnings:\n' % len(errors) + \
- '\n'.join(errors)
diff --git a/kipp_arm/src/kipp_arm_description/test/test_pep257.py b/kipp_arm/src/kipp_arm_description/test/test_pep257.py
deleted file mode 100644
index b234a38..0000000
--- a/kipp_arm/src/kipp_arm_description/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_pep257.main import main
-import pytest
-
-
-@pytest.mark.linter
-@pytest.mark.pep257
-def test_pep257():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found code style errors / warnings'
diff --git a/kipp_arm/src/kipp_arm_description/urdf/SPEAR_Arm.urdf b/kipp_arm/src/kipp_arm_description/urdf/SPEAR_Arm.urdf
deleted file mode 100644
index 6dd25f2..0000000
--- a/kipp_arm/src/kipp_arm_description/urdf/SPEAR_Arm.urdf
+++ /dev/null
@@ -1,162 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/kipp_arm/src/kipp_moveit/.setup_assistant b/kipp_arm/src/kipp_moveit/.setup_assistant
deleted file mode 100644
index c584c67..0000000
--- a/kipp_arm/src/kipp_moveit/.setup_assistant
+++ /dev/null
@@ -1,23 +0,0 @@
-moveit_setup_assistant_config:
- urdf:
- package: kipp_arm_description
- relative_path: urdf/SPEAR_Arm.urdf
- srdf:
- relative_path: config/kipps_arm.srdf
- package_settings:
- author_name: Ayden Bravender
- author_email: aydenbravender@gmail.com
- generated_timestamp: 1732392254
- control_xacro:
- command:
- - position
- state:
- - position
- modified_urdf:
- xacros:
- - control_xacro
- control_xacro:
- command:
- - position
- state:
- - position
\ No newline at end of file
diff --git a/kipp_arm/src/kipp_moveit/CMakeLists.txt b/kipp_arm/src/kipp_moveit/CMakeLists.txt
deleted file mode 100644
index 61a3682..0000000
--- a/kipp_arm/src/kipp_moveit/CMakeLists.txt
+++ /dev/null
@@ -1,11 +0,0 @@
-cmake_minimum_required(VERSION 3.22)
-project(kipp_moveit)
-
-find_package(ament_cmake REQUIRED)
-
-ament_package()
-
-install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}
- PATTERN "setup_assistant.launch" EXCLUDE)
-install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
-install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME})
diff --git a/kipp_arm/src/kipp_moveit/config/initial_positions.yaml b/kipp_arm/src/kipp_moveit/config/initial_positions.yaml
deleted file mode 100644
index 43e5765..0000000
--- a/kipp_arm/src/kipp_moveit/config/initial_positions.yaml
+++ /dev/null
@@ -1,9 +0,0 @@
-# Default initial positions for kipps_arm's ros2_control fake system
-
-initial_positions:
- base_joint: 0
- eef_joint: 0
- link1_joint: 0
- link2_joint: 0
- link3_joint: 0
- link4_joint: 0
\ No newline at end of file
diff --git a/kipp_arm/src/kipp_moveit/config/joint_limits.yaml b/kipp_arm/src/kipp_moveit/config/joint_limits.yaml
deleted file mode 100644
index b333aa5..0000000
--- a/kipp_arm/src/kipp_moveit/config/joint_limits.yaml
+++ /dev/null
@@ -1,40 +0,0 @@
-# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
-
-# For beginners, we downscale velocity and acceleration limits.
-# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed.
-default_velocity_scaling_factor: 0.1
-default_acceleration_scaling_factor: 0.1
-
-# 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:
- base_joint:
- has_velocity_limits: true
- max_velocity: 0.439
- has_acceleration_limits: false
- max_acceleration: 0
- eef_joint:
- has_velocity_limits: true
- max_velocity: 0.37690000000000001
- has_acceleration_limits: false
- max_acceleration: 0
- link1_joint:
- has_velocity_limits: true
- max_velocity: 0.14660799999999999
- has_acceleration_limits: false
- max_acceleration: 0
- link2_joint:
- has_velocity_limits: true
- max_velocity: 0.36651899999999998
- has_acceleration_limits: false
- max_acceleration: 0
- link3_joint:
- has_velocity_limits: true
- max_velocity: 0.37698999999999999
- has_acceleration_limits: false
- max_acceleration: 0
- link4_joint:
- has_velocity_limits: true
- max_velocity: 0.37690000000000001
- has_acceleration_limits: false
- max_acceleration: 0
\ No newline at end of file
diff --git a/kipp_arm/src/kipp_moveit/config/kinematics.yaml b/kipp_arm/src/kipp_moveit/config/kinematics.yaml
deleted file mode 100644
index 2f6bb96..0000000
--- a/kipp_arm/src/kipp_moveit/config/kinematics.yaml
+++ /dev/null
@@ -1,4 +0,0 @@
-arm:
- kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
- kinematics_solver_search_resolution: 0.0050000000000000001
- kinematics_solver_timeout: 0.0050000000000000001
\ No newline at end of file
diff --git a/kipp_arm/src/kipp_moveit/config/kipps_arm.ros2_control.xacro b/kipp_arm/src/kipp_moveit/config/kipps_arm.ros2_control.xacro
deleted file mode 100644
index 3791da6..0000000
--- a/kipp_arm/src/kipp_moveit/config/kipps_arm.ros2_control.xacro
+++ /dev/null
@@ -1,50 +0,0 @@
-
-
-
-
-
-
-
-
- mock_components/GenericSystem
-
-
-
-
- ${initial_positions['base_joint']}
-
-
-
-
-
- ${initial_positions['link1_joint']}
-
-
-
-
-
- ${initial_positions['link2_joint']}
-
-
-
-
-
- ${initial_positions['link3_joint']}
-
-
-
-
-
- ${initial_positions['link4_joint']}
-
-
-
-
-
- ${initial_positions['eef_joint']}
-
-
-
-
-
-
diff --git a/kipp_arm/src/kipp_moveit/config/kipps_arm.srdf b/kipp_arm/src/kipp_moveit/config/kipps_arm.srdf
deleted file mode 100644
index 415c38d..0000000
--- a/kipp_arm/src/kipp_moveit/config/kipps_arm.srdf
+++ /dev/null
@@ -1,52 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/kipp_arm/src/kipp_moveit/config/kipps_arm.urdf.xacro b/kipp_arm/src/kipp_moveit/config/kipps_arm.urdf.xacro
deleted file mode 100644
index aa08cc8..0000000
--- a/kipp_arm/src/kipp_moveit/config/kipps_arm.urdf.xacro
+++ /dev/null
@@ -1,14 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/kipp_arm/src/kipp_moveit/config/moveit.rviz b/kipp_arm/src/kipp_moveit/config/moveit.rviz
deleted file mode 100644
index 99bb740..0000000
--- a/kipp_arm/src/kipp_moveit/config/moveit.rviz
+++ /dev/null
@@ -1,51 +0,0 @@
-Panels:
- - Class: rviz_common/Displays
- Name: Displays
- Property Tree Widget:
- Expanded:
- - /MotionPlanning1
- - Class: rviz_common/Help
- Name: Help
- - Class: rviz_common/Views
- Name: Views
-Visualization Manager:
- Displays:
- - Class: rviz_default_plugins/Grid
- Name: Grid
- Value: true
- - Class: moveit_rviz_plugin/MotionPlanning
- Name: MotionPlanning
- Planned Path:
- Loop Animation: true
- State Display Time: 0.05 s
- Trajectory Topic: display_planned_path
- Planning Scene Topic: monitored_planning_scene
- Robot Description: robot_description
- Scene Geometry:
- Scene Alpha: 1
- Scene Robot:
- Robot Alpha: 0.5
- Value: true
- Global Options:
- Fixed Frame: base_link
- Tools:
- - Class: rviz_default_plugins/Interact
- - Class: rviz_default_plugins/MoveCamera
- - Class: rviz_default_plugins/Select
- Value: true
- Views:
- Current:
- Class: rviz_default_plugins/Orbit
- Distance: 2.0
- Focal Point:
- X: -0.1
- Y: 0.25
- Z: 0.30
- Name: Current View
- Pitch: 0.5
- Target Frame: base_link
- Yaw: -0.623
-Window Geometry:
- Height: 975
- QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
- Width: 1200
diff --git a/kipp_arm/src/kipp_moveit/config/moveit_controllers.yaml b/kipp_arm/src/kipp_moveit/config/moveit_controllers.yaml
deleted file mode 100644
index 034aed4..0000000
--- a/kipp_arm/src/kipp_moveit/config/moveit_controllers.yaml
+++ /dev/null
@@ -1,21 +0,0 @@
-# MoveIt uses this configuration for controller management
-
-moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
-
-moveit_simple_controller_manager:
- controller_names:
- - kipp_arm_controller
-
- kipp_arm_controller:
- type: FollowJointTrajectory
- action_ns: follow_joint_trajectory
- default: true
- joints:
- - base_joint
- - link1_joint
- - link2_joint
- - link3_joint
- - link4_joint
- - eef_joint
- action_ns: follow_joint_trajectory
- default: true
\ No newline at end of file
diff --git a/kipp_arm/src/kipp_moveit/config/moveit_servo_config.yaml b/kipp_arm/src/kipp_moveit/config/moveit_servo_config.yaml
deleted file mode 100644
index 2a8318d..0000000
--- a/kipp_arm/src/kipp_moveit/config/moveit_servo_config.yaml
+++ /dev/null
@@ -1,66 +0,0 @@
-/arm_servo:
- ros__parameters:
- moveit_servo:
- command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
- scale:
- # Scale parameters are only used if command_in_type=="unitless"
- linear: 0.2 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
- rotational: 8.0 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
- # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
- joint: 0.9
-
- # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling)
- # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
-
- ## Properties of outgoing commands
- publish_period: 0.034 # 1/Nominal publish rate [seconds]
- low_latency_mode: true # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
-
- # What type of topic does your robot driver expect?
- # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
- command_out_type: trajectory_msgs/JointTrajectory
-
- # What to publish? Can save some bandwidth as most robots only require positions or velocities
- publish_joint_positions: true
- publish_joint_velocities: false
- publish_joint_accelerations: false
-
- ## Plugins for smoothing outgoing commands
- smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
-
- # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
- # which other nodes can use as a source for information about the planning environment.
- # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
- # then is_primary_planning_scene_monitor needs to be set to false.
- is_primary_planning_scene_monitor: true
-
- ## MoveIt properties
-
- move_group_name: arm # Often 'manipulator' or 'arm'
- planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world'
-
- ## Other frames
- ee_frame_name: end_effector # The name of the end effector link, used to return the EE pose
- robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector
-
- ## Stopping behaviour
- incoming_command_timeout: 0.5 # Stop servoing if X seconds elapse without a new command
-
- ## Configure handling of singularities and joint limits
- lower_singularity_threshold: 10.0 # Start decelerating when the condition number hits this (close to singularity)
- hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
- joint_limit_margin: 0.4 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
- leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620)
-
- ## Topic names
- cartesian_command_in_topic: /kipps_arm/delta_twist_cmds # Topic for incoming Cartesian twist commands FROM CONTROLLER
- joint_command_in_topic: /kipps_Arm/delta_joint_cmds # Topic for incoming joint angle commands FROM CONTROLLER
- joint_topic: /joint_states
- status_topic: ~/status # Publish status to this topic
- command_out_topic: /Arm_Group_controller/joint_trajectory # Publish outgoing commands here
-
- ## Collision checking for the entire robot body
- check_collisions: true # Check collisions?
- collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
- self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
- scene_collision_proximity_threshold: 0.01 # Start decelerating when a scene collision is this far [m]
\ No newline at end of file
diff --git a/kipp_arm/src/kipp_moveit/config/pilz_cartesian_limits.yaml b/kipp_arm/src/kipp_moveit/config/pilz_cartesian_limits.yaml
deleted file mode 100644
index b2997ca..0000000
--- a/kipp_arm/src/kipp_moveit/config/pilz_cartesian_limits.yaml
+++ /dev/null
@@ -1,6 +0,0 @@
-# Limits for the Pilz planner
-cartesian_limits:
- max_trans_vel: 1.0
- max_trans_acc: 2.25
- max_trans_dec: -5.0
- max_rot_vel: 1.57
diff --git a/kipp_arm/src/kipp_moveit/config/ros2_controllers.yaml b/kipp_arm/src/kipp_moveit/config/ros2_controllers.yaml
deleted file mode 100644
index e9c353f..0000000
--- a/kipp_arm/src/kipp_moveit/config/ros2_controllers.yaml
+++ /dev/null
@@ -1,25 +0,0 @@
-# This config file is used by ros2_control
-controller_manager:
- ros__parameters:
- update_rate: 100 # Hz
-
- kipp_arm_controller:
- type: joint_trajectory_controller/JointTrajectoryController
-
-
- joint_state_broadcaster:
- type: joint_state_broadcaster/JointStateBroadcaster
-
-kipp_arm_controller:
- ros__parameters:
- joints:
- - base_joint
- - link1_joint
- - link2_joint
- - link3_joint
- - link4_joint
- - eef_joint
- command_interfaces:
- - position
- state_interfaces:
- - position
\ No newline at end of file
diff --git a/kipp_arm/src/kipp_moveit/launch/demo.launch.py b/kipp_arm/src/kipp_moveit/launch/demo.launch.py
deleted file mode 100644
index ea3d3df..0000000
--- a/kipp_arm/src/kipp_moveit/launch/demo.launch.py
+++ /dev/null
@@ -1,7 +0,0 @@
-from moveit_configs_utils import MoveItConfigsBuilder
-from moveit_configs_utils.launches import generate_demo_launch
-
-
-def generate_launch_description():
- moveit_config = MoveItConfigsBuilder("kipps_arm", package_name="kipp_moveit").to_moveit_configs()
- return generate_demo_launch(moveit_config)
diff --git a/kipp_arm/src/kipp_moveit/launch/move_group.launch.py b/kipp_arm/src/kipp_moveit/launch/move_group.launch.py
deleted file mode 100644
index e841387..0000000
--- a/kipp_arm/src/kipp_moveit/launch/move_group.launch.py
+++ /dev/null
@@ -1,7 +0,0 @@
-from moveit_configs_utils import MoveItConfigsBuilder
-from moveit_configs_utils.launches import generate_move_group_launch
-
-
-def generate_launch_description():
- moveit_config = MoveItConfigsBuilder("kipps_arm", package_name="kipp_moveit").to_moveit_configs()
- return generate_move_group_launch(moveit_config)
diff --git a/kipp_arm/src/kipp_moveit/launch/moveit_rviz.launch.py b/kipp_arm/src/kipp_moveit/launch/moveit_rviz.launch.py
deleted file mode 100644
index 5146b58..0000000
--- a/kipp_arm/src/kipp_moveit/launch/moveit_rviz.launch.py
+++ /dev/null
@@ -1,7 +0,0 @@
-from moveit_configs_utils import MoveItConfigsBuilder
-from moveit_configs_utils.launches import generate_moveit_rviz_launch
-
-
-def generate_launch_description():
- moveit_config = MoveItConfigsBuilder("kipps_arm", package_name="kipp_moveit").to_moveit_configs()
- return generate_moveit_rviz_launch(moveit_config)
diff --git a/kipp_arm/src/kipp_moveit/launch/rsp.launch.py b/kipp_arm/src/kipp_moveit/launch/rsp.launch.py
deleted file mode 100644
index c6ed66f..0000000
--- a/kipp_arm/src/kipp_moveit/launch/rsp.launch.py
+++ /dev/null
@@ -1,7 +0,0 @@
-from moveit_configs_utils import MoveItConfigsBuilder
-from moveit_configs_utils.launches import generate_rsp_launch
-
-
-def generate_launch_description():
- moveit_config = MoveItConfigsBuilder("kipps_arm", package_name="kipp_moveit").to_moveit_configs()
- return generate_rsp_launch(moveit_config)
diff --git a/kipp_arm/src/kipp_moveit/launch/servo.launch.py b/kipp_arm/src/kipp_moveit/launch/servo.launch.py
deleted file mode 100644
index 6f4dfa9..0000000
--- a/kipp_arm/src/kipp_moveit/launch/servo.launch.py
+++ /dev/null
@@ -1,38 +0,0 @@
-from launch import LaunchDescription
-from ament_index_python.packages import get_package_share_directory
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-from launch.substitutions import PathJoinSubstitution
-from moveit_configs_utils import MoveItConfigsBuilder
-
-
-import os
-
-param_file = os.path.join(get_package_share_directory("kipp_moveit"), "config", "moveit_servo_config.yaml")
-
-def generate_launch_description():
-
- moveit_config = (
- MoveItConfigsBuilder(robot_name = "", package_name ="kipp_moveit")
- .robot_description(file_path="config/kipps_arm.urdf.xacro")
- .to_moveit_configs()
- )
-
- ld = LaunchDescription()
- servo_node = Node(
- package="moveit_servo",
- executable="servo_node_main",
- name='arm_servo',
- parameters= [
- param_file,
- moveit_config.robot_description_kinematics,
- moveit_config.robot_description_semantic,
- moveit_config.robot_description
- ],
- arguments=['--ros-args', '--log-level', 'INFO'],
- output = 'screen',
-
- )
-
- ld.add_action(servo_node)
- return ld
\ No newline at end of file
diff --git a/kipp_arm/src/kipp_moveit/launch/setup_assistant.launch.py b/kipp_arm/src/kipp_moveit/launch/setup_assistant.launch.py
deleted file mode 100644
index 2edd7e9..0000000
--- a/kipp_arm/src/kipp_moveit/launch/setup_assistant.launch.py
+++ /dev/null
@@ -1,7 +0,0 @@
-from moveit_configs_utils import MoveItConfigsBuilder
-from moveit_configs_utils.launches import generate_setup_assistant_launch
-
-
-def generate_launch_description():
- moveit_config = MoveItConfigsBuilder("kipps_arm", package_name="kipp_moveit").to_moveit_configs()
- return generate_setup_assistant_launch(moveit_config)
diff --git a/kipp_arm/src/kipp_moveit/launch/spawn_controllers.launch.py b/kipp_arm/src/kipp_moveit/launch/spawn_controllers.launch.py
deleted file mode 100644
index 6e66d43..0000000
--- a/kipp_arm/src/kipp_moveit/launch/spawn_controllers.launch.py
+++ /dev/null
@@ -1,7 +0,0 @@
-from moveit_configs_utils import MoveItConfigsBuilder
-from moveit_configs_utils.launches import generate_spawn_controllers_launch
-
-
-def generate_launch_description():
- moveit_config = MoveItConfigsBuilder("kipps_arm", package_name="kipp_moveit").to_moveit_configs()
- return generate_spawn_controllers_launch(moveit_config)
diff --git a/kipp_arm/src/kipp_moveit/launch/static_virtual_joint_tfs.launch.py b/kipp_arm/src/kipp_moveit/launch/static_virtual_joint_tfs.launch.py
deleted file mode 100644
index 8872bc0..0000000
--- a/kipp_arm/src/kipp_moveit/launch/static_virtual_joint_tfs.launch.py
+++ /dev/null
@@ -1,7 +0,0 @@
-from moveit_configs_utils import MoveItConfigsBuilder
-from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch
-
-
-def generate_launch_description():
- moveit_config = MoveItConfigsBuilder("kipps_arm", package_name="kipp_moveit").to_moveit_configs()
- return generate_static_virtual_joint_tfs_launch(moveit_config)
diff --git a/kipp_arm/src/kipp_moveit/launch/warehouse_db.launch.py b/kipp_arm/src/kipp_moveit/launch/warehouse_db.launch.py
deleted file mode 100644
index 66570b5..0000000
--- a/kipp_arm/src/kipp_moveit/launch/warehouse_db.launch.py
+++ /dev/null
@@ -1,7 +0,0 @@
-from moveit_configs_utils import MoveItConfigsBuilder
-from moveit_configs_utils.launches import generate_warehouse_db_launch
-
-
-def generate_launch_description():
- moveit_config = MoveItConfigsBuilder("kipps_arm", package_name="kipp_moveit").to_moveit_configs()
- return generate_warehouse_db_launch(moveit_config)
diff --git a/kipp_arm/src/kipp_moveit/package.xml b/kipp_arm/src/kipp_moveit/package.xml
deleted file mode 100644
index fa1db4c..0000000
--- a/kipp_arm/src/kipp_moveit/package.xml
+++ /dev/null
@@ -1,52 +0,0 @@
-
-
-
- kipp_moveit
- 0.3.0
-
- An automatically generated package with all the configuration and launch files for using the kipps_arm with the MoveIt Motion Planning Framework
-
- Ayden Bravender
-
- BSD
-
- http://moveit.ros.org/
- https://github.com/ros-planning/moveit2/issues
- https://github.com/ros-planning/moveit2
-
- Ayden Bravender
-
- ament_cmake
-
- moveit_ros_move_group
- moveit_kinematics
- moveit_planners
- moveit_simple_controller_manager
- joint_state_publisher
- joint_state_publisher_gui
- tf2_ros
- xacro
-
-
-
- controller_manager
- kipp_arm_description
- moveit_configs_utils
- moveit_ros_move_group
- moveit_ros_visualization
- moveit_ros_warehouse
- moveit_setup_assistant
- robot_state_publisher
- rviz2
- rviz_common
- rviz_default_plugins
- tf2_ros
- warehouse_ros_mongo
- xacro
-
-
-
- ament_cmake
-
-
diff --git a/kipp_arm/src/teleop/teleop/moded_joy_arm.py b/kipp_arm/src/teleop/teleop/moded_joy_arm.py
index 7dc3530..a47550f 100644
--- a/kipp_arm/src/teleop/teleop/moded_joy_arm.py
+++ b/kipp_arm/src/teleop/teleop/moded_joy_arm.py
@@ -13,8 +13,7 @@
BASE_FRAME_ID = "base_link"
EEF_Topic = "/SPEAR_Arm/EEF"
-
-# Initalize joystick commands
+# Initialize joystick commands
LEFT_STICK_X = 0
LEFT_STICK_Y = 1
LEFT_TRIGGER = 2
@@ -35,159 +34,151 @@
LEFT_STICK_CLICK = 9
RIGHT_STICK_CLICK = 10
-
Axis_Default = {
-
- "LEFT_TRIGGER" : 1.0,
- "RIGHT_TRIGGER" : 1.0,
+ "LEFT_TRIGGER": 1.0,
+ "RIGHT_TRIGGER": 1.0,
}
class Arm_Control(Node):
def __init__(self):
super().__init__('Arm_Control')
-
- self.frame_to_publish_ = EEF_Frame_ID
-
+ self.frame_to_publish_ = EEF_Frame_ID
+ self.in_joint_control_mode = False
+ self.in_ik_control_mode = False
- self.twist_pub = self.create_publisher(msg_type = TwistStamped, topic = Twist_Topic, qos_profile = QoSProfile(depth=10))
- self.joint_pub = self.create_publisher(msg_type = JointJog, topic = Joint_Topic, qos_profile = QoSProfile(depth=10))
+ self.twist_pub = self.create_publisher(msg_type=TwistStamped, topic=Twist_Topic, qos_profile=QoSProfile(depth=10))
+ self.joint_pub = self.create_publisher(msg_type=JointJog, topic=Joint_Topic, qos_profile=QoSProfile(depth=10))
self.publisher_ = self.create_publisher(Float32, topic=EEF_Topic, qos_profile=QoSProfile(depth=10))
+ self.joy_sub = self.create_subscription(msg_type=Joy, topic=Joy_Topic, qos_profile=rclpy.qos.qos_profile_system_default, callback=self.JoystickMsg)
-
- self.joy_sub = self.create_subscription(msg_type = Joy, topic = Joy_Topic, qos_profile = rclpy.qos.qos_profile_system_default, callback= self.JoystickMsg)
-
-
timer_period = 0.5
+ self.timer = self.create_timer(timer_period_sec=timer_period, callback=self.JoyMain)
- self.timer = self.create_timer(timer_period_sec = timer_period, callback = self.JoyMain)
-
self.joystick_msg = Joy()
-
def JoyMain(self):
self.twist_msg = TwistStamped()
self.joint_msg = JointJog()
ik = self.ConvertJoyToCommand()
-
- if ik:
- self.twist_msg.header.stamp = self.get_clock().now().to_msg()
- self.twist_msg.header.frame_id = "base_link"
- self.twist_pub.publish(self.twist_msg)
+ if ik and not self.in_joint_control_mode:
+ self.publish_twist_command()
+ elif not ik and self.in_joint_control_mode:
+ self.publish_joint_command()
+ def ConvertJoyToCommand(self):
- else:
-
- self.joint_msg.header.stamp = self.get_clock().now().to_msg()
- self.joint_msg.header.frame_id = "base_link"
- self.joint_pub.publish(self.joint_msg)
+ i = 0
+ deadband_threshold = 0.1
+ # Check if joystick_msg has the expected number of axes and buttons
+ num_axes = len(self.joystick_msg.axes)
+ num_buttons = len(self.joystick_msg.buttons)
- def ConvertJoyToCommand(self):
+ # self.get_logger().info(f"Joystick Axes: {num_axes}, Buttons: {num_buttons}")
+ # self.get_logger().info(f"Joystick Axes Values: {self.joystick_msg.axes}")
+ # self.get_logger().info(f"Joystick Buttons Values: {self.joystick_msg.buttons}")
+ if num_axes <= RIGHT_STICK_X or num_buttons <= A:
+ self.get_logger().warn("Joystick message does not have expected number of axes or buttons")
+ return False
- i = 0
- if (abs(self.joystick_msg.axes[RIGHT_STICK_X]) > 0.1 and self.joystick_msg.buttons[A]):
- # Shoulder Roll
- i+=1
+ if abs(self.joystick_msg.axes[RIGHT_STICK_X]) > deadband_threshold and self.joystick_msg.buttons[A]:
+ i += 1
self.joint_msg.joint_names.append("Shoulder Roll")
self.joint_msg.velocities.append(self.joystick_msg.axes[RIGHT_STICK_X])
-
- if (abs(self.joystick_msg.axes[RIGHT_STICK_X]) > 0.1 and self.joystick_msg.buttons[B]):
- i+=1
- # Shoulder pitch
+ if abs(self.joystick_msg.axes[RIGHT_STICK_X]) > deadband_threshold and self.joystick_msg.buttons[B]:
+ i += 1
self.joint_msg.joint_names.append("Shoulder Pitch")
self.joint_msg.velocities.append(self.joystick_msg.axes[RIGHT_STICK_X])
-
- if (abs(self.joystick_msg.axes[RIGHT_STICK_X]) > 0.1 and self.joystick_msg.buttons[X]):
- i+=1
- # Elbow Roll
+ if abs(self.joystick_msg.axes[RIGHT_STICK_X]) > deadband_threshold and self.joystick_msg.buttons[X]:
+ i += 1
self.joint_msg.joint_names.append("Elbow Roll")
self.joint_msg.velocities.append(self.joystick_msg.axes[RIGHT_STICK_X])
-
- if (abs(self.joystick_msg.axes[RIGHT_STICK_X]) > 0.1 and self.joystick_msg.buttons[Y]):
- i+=1
- # Elbow Pitch
+ if abs(self.joystick_msg.axes[RIGHT_STICK_X]) > deadband_threshold and self.joystick_msg.buttons[Y]:
+ i += 1
self.joint_msg.joint_names.append("Elbow Pitch")
self.joint_msg.velocities.append(self.joystick_msg.axes[RIGHT_STICK_X])
-
-
- if (abs(self.joystick_msg.axes[RIGHT_STICK_X]) > 0.1 and self.joystick_msg.buttons[CHANGE_VIEW]):
- i+=1
- # Wrist Roll
+
+ if abs(self.joystick_msg.axes[RIGHT_STICK_X]) > deadband_threshold and self.joystick_msg.buttons[CHANGE_VIEW]:
+ i += 1
self.joint_msg.joint_names.append("Wrist Roll")
self.joint_msg.velocities.append(self.joystick_msg.axes[RIGHT_STICK_X])
-
- if (abs(self.joystick_msg.axes[RIGHT_STICK_X]) > 0.1 and self.joystick_msg.buttons[MENU]):
- i+=1
- # Wrist Pitch
+ if abs(self.joystick_msg.axes[RIGHT_STICK_X]) > deadband_threshold and self.joystick_msg.buttons[MENU]:
+ i += 1
self.joint_msg.joint_names.append("Wrist Pitch")
self.joint_msg.velocities.append(self.joystick_msg.axes[RIGHT_STICK_X])
if self.joystick_msg.buttons[RIGHT_BUMPER]:
- i+=1
- # EEF
+ i += 1
eef_open_msg = Float32()
eef_open_msg.data = self.joystick_msg.axes[RIGHT_BUMPER]
self.publisher_.publish(eef_open_msg)
-
+
if self.joystick_msg.buttons[RIGHT_TRIGGER]:
- i+=1
- # EEF
+ i += 1
eef_close_msg = Float32()
- eef_close_msg.data = -self.joystick_msg.axes[RIGHT_TRIGGER] # You can set the value for EEF Close as -1.0 or any other value as per your requirement
+ eef_close_msg.data = -self.joystick_msg.axes[RIGHT_TRIGGER]
self.publisher_.publish(eef_close_msg)
-
+
if i > 0:
+ self.in_joint_control_mode = True
+ self.in_ik_control_mode = False
return False
-
else:
- # Linear IK
- self.twist_msg.twist.linear.z = (self.joystick_msg.axes[LEFT_STICK_Y])
- self.twist_msg.twist.linear.y = self.joystick_msg.axes[LEFT_STICK_X]
- # When the left bumper is pressed, move back along the x-axis
- lin_x_left_bumper = -0.5 * (self.joystick_msg.buttons[LEFT_BUMPER] - 0)
- # When the right trigger is pressed, move forward along the x-axis
- lin_x_right_trigger = 0.5 * (self.joystick_msg.axes[RIGHT_TRIGGER] - Axis_Default["RIGHT_TRIGGER"])
- # Combine the two values for the final x velocity
- self.twist_msg.twist.linear.x = lin_x_left_bumper + lin_x_right_trigger
-
- # Angular IK
+ self.in_joint_control_mode = False
+ self.in_ik_control_mode = True
+ return True
+
+ def publish_twist_command(self):
+ deadband_threshold = 0.1
+
+ self.twist_msg.header.stamp = self.get_clock().now().to_msg()
+ self.twist_msg.header.frame_id = "base_link"
+
+ lin_x_left_bumper = (self.joystick_msg.buttons[LEFT_BUMPER])
+ lin_x_left_trigger = -0.5 * (self.joystick_msg.axes[LEFT_TRIGGER]+1)
+ lin_x = lin_x_left_bumper + lin_x_left_trigger
+
+ if abs(lin_x) > deadband_threshold or abs(self.joystick_msg.axes[LEFT_STICK_Y]) > deadband_threshold or abs(self.joystick_msg.axes[LEFT_STICK_X]) > deadband_threshold:
+ self.twist_msg.twist.linear.z = -self.joystick_msg.axes[LEFT_STICK_Y]
+ self.twist_msg.twist.linear.y = self.joystick_msg.axes[LEFT_STICK_X]
+ self.twist_msg.twist.linear.x = lin_x
+
self.twist_msg.twist.angular.y = self.joystick_msg.axes[D_PAD_Y]
self.twist_msg.twist.angular.x = self.joystick_msg.axes[D_PAD_Y]
- # Get the Y-axis value from the right stick (assuming positive is up, negative is down)
roll_value = 1.0 * self.joystick_msg.axes[RIGHT_STICK_Y]
- # Update the angular roll based on the right stick's Y-axis value
self.twist_msg.twist.angular.y = roll_value
-
- return True
-
+ self.get_logger().info(f"Publishing twist command: Linear - x: {self.twist_msg.twist.linear.x}, y: {self.twist_msg.twist.linear.y}, z: {self.twist_msg.twist.linear.z}; Angular - x: {self.twist_msg.twist.angular.x}, y: {self.twist_msg.twist.angular.y}, z: {self.twist_msg.twist.angular.z}")
+ self.twist_pub.publish(self.twist_msg)
+ else:
+ self.get_logger().info("No significant joystick movement detected for IK command.")
+
+ def publish_joint_command(self):
+ self.joint_msg.header.stamp = self.get_clock().now().to_msg()
+ self.joint_msg.header.frame_id = "base_link"
+ self.get_logger().info(f"Publishing joint command: {self.joint_msg.joint_names} with velocities: {self.joint_msg.velocities}")
+ self.joint_pub.publish(self.joint_msg)
+
def JoystickMsg(self, msg):
-
self.joystick_msg = msg
-
+
def main(args=None):
rclpy.init(args=args)
-
-
SPEAR_Arm_Node = Arm_Control()
-
rclpy.spin(SPEAR_Arm_Node)
-
SPEAR_Arm_Node.destroy_node()
-
rclpy.shutdown()
-
if __name__ == '__main__':
- main()
\ No newline at end of file
+ main()
diff --git a/kipp_arm/src/teleop_control/launch/teleop_control.launch.py b/kipp_arm/src/teleop_control/launch/teleop_control.launch.py
deleted file mode 100644
index 17eee58..0000000
--- a/kipp_arm/src/teleop_control/launch/teleop_control.launch.py
+++ /dev/null
@@ -1,32 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-
-import os
-from ament_index_python.packages import get_package_share_directory
-from launch.actions import TimerAction
-
-def generate_launch_description():
-
-
- joystick_conv = TimerAction(
- period = 3.0,
- actions = [Node(
- package = 'teleop_control',
- executable = 'kipps_arm_node',
- )]
- )
-
- joy_input = Node(
- package = 'teleop_control',
- executable ='joy_input',
- )
-
-
-
- return LaunchDescription([
-
-
- joy_input,
- joystick_conv
-
- ])
\ No newline at end of file
diff --git a/kipp_arm/src/teleop_control/package.xml b/kipp_arm/src/teleop_control/package.xml
deleted file mode 100644
index 5c9591b..0000000
--- a/kipp_arm/src/teleop_control/package.xml
+++ /dev/null
@@ -1,18 +0,0 @@
-
-
-
- teleop_control
- 0.0.0
- TODO: Package description
- ayden
- TODO: License declaration
-
- ament_copyright
- ament_flake8
- ament_pep257
- python3-pytest
-
-
- ament_python
-
-
diff --git a/kipp_arm/src/teleop_control/resource/teleop_control b/kipp_arm/src/teleop_control/resource/teleop_control
deleted file mode 100644
index e69de29..0000000
diff --git a/kipp_arm/src/teleop_control/setup.cfg b/kipp_arm/src/teleop_control/setup.cfg
deleted file mode 100644
index 8c6986b..0000000
--- a/kipp_arm/src/teleop_control/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/teleop_control
-[install]
-install_scripts=$base/lib/teleop_control
diff --git a/kipp_arm/src/teleop_control/setup.py b/kipp_arm/src/teleop_control/setup.py
deleted file mode 100644
index 3f1291e..0000000
--- a/kipp_arm/src/teleop_control/setup.py
+++ /dev/null
@@ -1,29 +0,0 @@
-from setuptools import find_packages, setup
-
-package_name = 'teleop_control'
-
-setup(
- name=package_name,
- version='0.0.0',
- packages=find_packages(exclude=['test']),
- data_files=[
- ('share/ament_index/resource_index/packages',
- ['resource/' + package_name]),
- ('share/' + package_name, ['package.xml']),
-
- ('share/' + package_name, ['launch/teleop_control.launch.py']),
- ],
- install_requires=['setuptools'],
- zip_safe=True,
- maintainer='ayden',
- maintainer_email='aydenbravender@gmail.com',
- description='TODO: Package description',
- license='TODO: License declaration',
- tests_require=['pytest'],
- entry_points={
- 'console_scripts': [
- 'kipps_arm_node = teleop_control.joy_arm:main',
- 'joy_input = teleop_control.joy_input:main',
- ],
- },
-)
diff --git a/kipp_arm/src/teleop_control/teleop_control/__init__.py b/kipp_arm/src/teleop_control/teleop_control/__init__.py
deleted file mode 100644
index e69de29..0000000
diff --git a/kipp_arm/src/teleop_control/teleop_control/joy_arm.py b/kipp_arm/src/teleop_control/teleop_control/joy_arm.py
deleted file mode 100644
index bc8b9da..0000000
--- a/kipp_arm/src/teleop_control/teleop_control/joy_arm.py
+++ /dev/null
@@ -1,193 +0,0 @@
-import rclpy
-from rclpy.node import Node
-from control_msgs.msg import JointJog
-from geometry_msgs.msg import TwistStamped
-from sensor_msgs.msg import Joy
-from rclpy.qos import QoSProfile
-from std_msgs.msg import Float32
-
-Joy_Topic = "/kipps_arm/Joy_Topic"
-Twist_Topic = "/kipps_arm/delta_twist_cmds"
-Joint_Topic = "/kipps_arm/delta_joint_cmds"
-EEF_Frame_ID = "end_effector"
-BASE_FRAME_ID = "base_link"
-EEF_Topic = "/kipps_arm/EEF"
-
-
-# Initalize joystick commands
-LEFT_STICK_X = 0
-LEFT_STICK_Y = 1
-LEFT_TRIGGER = 2
-RIGHT_STICK_X = 3
-RIGHT_STICK_Y = 4
-RIGHT_TRIGGER = 5
-D_PAD_X = 6
-D_PAD_Y = 7
-A = 0
-B = 1
-X = 2
-Y = 3
-LEFT_BUMPER = 4
-RIGHT_BUMPER = 5
-CHANGE_VIEW = 6
-MENU = 7
-HOME = 8
-LEFT_STICK_CLICK = 9
-RIGHT_STICK_CLICK = 10
-
-
-Axis_Default = {
-
- "LEFT_TRIGGER" : 1.0,
- "RIGHT_TRIGGER" : 1.0,
-}
-
-class Arm_Control(Node):
-
- def __init__(self):
- super().__init__('Arm_Control')
-
- self.frame_to_publish_ = EEF_Frame_ID
-
-
-
- self.twist_pub = self.create_publisher(msg_type = TwistStamped, topic = Twist_Topic, qos_profile = QoSProfile(depth=10))
- self.joint_pub = self.create_publisher(msg_type = JointJog, topic = Joint_Topic, qos_profile = QoSProfile(depth=10))
- self.publisher_ = self.create_publisher(Float32, topic=EEF_Topic, qos_profile=QoSProfile(depth=10))
-
-
-
- self.joy_sub = self.create_subscription(msg_type = Joy, topic = Joy_Topic, qos_profile = rclpy.qos.qos_profile_system_default, callback= self.JoystickMsg)
-
-
- timer_period = 0.5
-
- self.timer = self.create_timer(timer_period_sec = timer_period, callback = self.JoyMain)
-
- self.joystick_msg = Joy()
-
-
- def JoyMain(self):
-
- self.twist_msg = TwistStamped()
- self.joint_msg = JointJog()
-
- ik = self.ConvertJoyToCommand()
-
- if ik:
-
- self.twist_msg.header.stamp = self.get_clock().now().to_msg()
- self.twist_msg.header.frame_id = "base_link"
- self.twist_pub.publish(self.twist_msg)
-
-
- else:
-
- self.joint_msg.header.stamp = self.get_clock().now().to_msg()
- self.joint_msg.header.frame_id = "base_link"
- self.joint_pub.publish(self.joint_msg)
-
-
- def ConvertJoyToCommand(self):
-
-
- i = 0
- if (abs(self.joystick_msg.axes[RIGHT_STICK_X]) > 0.1 and self.joystick_msg.buttons[A]):
- # Shoulder Roll
- i+=1
- self.joint_msg.joint_names.append("base_joint")
- self.joint_msg.velocities.append(self.joystick_msg.axes[RIGHT_STICK_X])
-
-
- if (abs(self.joystick_msg.axes[RIGHT_STICK_X]) > 0.1 and self.joystick_msg.buttons[B]):
- i+=1
- # Shoulder pitch
- self.joint_msg.joint_names.append("link1_joint")
- self.joint_msg.velocities.append(self.joystick_msg.axes[RIGHT_STICK_X])
-
-
- if (abs(self.joystick_msg.axes[RIGHT_STICK_X]) > 0.1 and self.joystick_msg.buttons[X]):
- i+=1
- # Elbow Roll
- self.joint_msg.joint_names.append("link2_joint")
- self.joint_msg.velocities.append(self.joystick_msg.axes[RIGHT_STICK_X])
-
-
- if (abs(self.joystick_msg.axes[RIGHT_STICK_X]) > 0.1 and self.joystick_msg.buttons[Y]):
- i+=1
- # Elbow Pitch
- self.joint_msg.joint_names.append("link3_joint")
- self.joint_msg.velocities.append(self.joystick_msg.axes[RIGHT_STICK_X])
-
-
- if (abs(self.joystick_msg.axes[RIGHT_STICK_X]) > 0.1 and self.joystick_msg.buttons[CHANGE_VIEW]):
- i+=1
- # Wrist Roll
- self.joint_msg.joint_names.append("link4_joint")
- self.joint_msg.velocities.append(self.joystick_msg.axes[RIGHT_STICK_X])
-
-
- if (abs(self.joystick_msg.axes[RIGHT_STICK_X]) > 0.1 and self.joystick_msg.buttons[MENU]):
- i+=1
- # Wrist Pitch
- self.joint_msg.joint_names.append("eef_joint")
- self.joint_msg.velocities.append(self.joystick_msg.axes[RIGHT_STICK_X])
-
- if self.joystick_msg.buttons[RIGHT_BUMPER]:
- i+=1
- # EEF
- eef_open_msg = Float32()
- eef_open_msg.data = self.joystick_msg.axes[RIGHT_BUMPER]
- self.publisher_.publish(eef_open_msg)
-
- if self.joystick_msg.buttons[RIGHT_TRIGGER]:
- i+=1
- # EEF
- eef_close_msg = Float32()
- eef_close_msg.data = -self.joystick_msg.axes[RIGHT_TRIGGER] # You can set the value for EEF Close as -1.0 or any other value as per your requirement
- self.publisher_.publish(eef_close_msg)
-
- if i > 0:
- return False
-
- else:
- # Linear IK
- self.twist_msg.twist.linear.z = (self.joystick_msg.axes[LEFT_STICK_Y])
- self.twist_msg.twist.linear.y = self.joystick_msg.axes[LEFT_STICK_X]
- # When the left bumper is pressed, move back along the x-axis
- lin_x_left_bumper = -0.5 * (self.joystick_msg.buttons[LEFT_BUMPER] - 0)
- # When the right trigger is pressed, move forward along the x-axis
- lin_x_right_trigger = 0.5 * (self.joystick_msg.axes[RIGHT_TRIGGER] - Axis_Default["RIGHT_TRIGGER"])
- # Combine the two values for the final x velocity
- self.twist_msg.twist.linear.x = lin_x_left_bumper + lin_x_right_trigger
-
- # Angular IK
- self.twist_msg.twist.angular.y = self.joystick_msg.axes[D_PAD_Y]
- self.twist_msg.twist.angular.x = self.joystick_msg.axes[D_PAD_Y]
- # Get the Y-axis value from the right stick (assuming positive is up, negative is down)
- roll_value = 1.0 * self.joystick_msg.axes[RIGHT_STICK_Y]
- # Update the angular roll based on the right stick's Y-axis value
- self.twist_msg.twist.angular.y = roll_value
-
- return True
-
-
- def JoystickMsg(self, msg):
-
- self.joystick_msg = msg
-
-def main(args=None):
- rclpy.init(args=args)
-
-
- kipps_arm_node = Arm_Control()
-
- rclpy.spin(kipps_arm_node)
-
- kipps_arm_node.destroy_node()
-
- rclpy.shutdown()
-
-
-if __name__ == '__main__':
- main()
\ No newline at end of file
diff --git a/kipp_arm/src/teleop_control/teleop_control/joy_input.py b/kipp_arm/src/teleop_control/teleop_control/joy_input.py
deleted file mode 100644
index 76bd5f2..0000000
--- a/kipp_arm/src/teleop_control/teleop_control/joy_input.py
+++ /dev/null
@@ -1,65 +0,0 @@
-import rclpy
-from rclpy.node import Node
-from sensor_msgs.msg import Joy
-import pygame
-from rclpy.qos import QoSProfile
-
-class JoyPublisher(Node):
-
- def __init__(self):
- super().__init__('joy_publisher')
-
- # Initialize pygame for joystick input
- pygame.init()
- pygame.joystick.init()
-
- # Initialize publishers
- self.publisher_spear = self.create_publisher(msg_type = Joy, topic = '/kipps_arm/Joy_Topic', qos_profile = QoSProfile(depth=10))
- #self.publisher_rover = self.create_publisher(Joy, '/Rover/Joy_Topic', 10)
- self.threshold = 0.08 #Control threshold for stick
- self.timer = self.create_timer(0.05, self.publish_joystick_input)
-
- def get_joystick_input(self, joystick_id):
- # Get the specific joystick
- joystick = pygame.joystick.Joystick(joystick_id)
- joystick.init()
-
- # Get axes and buttons from the joystick
- axes = [joystick.get_axis(i) for i in range(joystick.get_numaxes())]
- buttons = [joystick.get_button(i) for i in range(joystick.get_numbuttons())]
- hats = joystick.get_hat(0)
- hats_float = tuple(float(x) for x in hats )
- axes.extend(hats_float)
-
- axes = [0.00 if abs(x) < self.threshold else x for x in axes]
-
- # Create Joy message
- joystick_input = Joy()
- joystick_input.header.stamp = self.get_clock().now().to_msg()
- joystick_input.axes = axes
- joystick_input.buttons = buttons
-
- return joystick_input
-
- def publish_joystick_input(self):
- # Get joystick inputs for two controllers
- joystick_input_spear = self.get_joystick_input(0) # Assuming ID 0 for the SPEAR_Arm controller
- #joystick_input_rover = self.get_joystick_input(1) # Assuming ID 1 for the Rover_Arm controller
-
- # Publish joystick inpu
- self.publisher_spear.publish(joystick_input_spear)
- #self.publisher_rover.publish(joystick_input_rover)
-
-def main(args=None):
- rclpy.init(args=args)
-
- joy_publisher = JoyPublisher()
-
- rclpy.spin(joy_publisher)
-
- joy_publisher.destroy_node()
- rclpy.shutdown()
-
-if __name__ == '__main__':
- main()
-
diff --git a/kipp_arm/src/teleop_control/test/test_copyright.py b/kipp_arm/src/teleop_control/test/test_copyright.py
deleted file mode 100644
index 97a3919..0000000
--- a/kipp_arm/src/teleop_control/test/test_copyright.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_copyright.main import main
-import pytest
-
-
-# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
-@pytest.mark.copyright
-@pytest.mark.linter
-def test_copyright():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found errors'
diff --git a/kipp_arm/src/teleop_control/test/test_flake8.py b/kipp_arm/src/teleop_control/test/test_flake8.py
deleted file mode 100644
index 27ee107..0000000
--- a/kipp_arm/src/teleop_control/test/test_flake8.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# Copyright 2017 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_flake8.main import main_with_errors
-import pytest
-
-
-@pytest.mark.flake8
-@pytest.mark.linter
-def test_flake8():
- rc, errors = main_with_errors(argv=[])
- assert rc == 0, \
- 'Found %d code style errors / warnings:\n' % len(errors) + \
- '\n'.join(errors)
diff --git a/kipp_arm/src/teleop_control/test/test_pep257.py b/kipp_arm/src/teleop_control/test/test_pep257.py
deleted file mode 100644
index b234a38..0000000
--- a/kipp_arm/src/teleop_control/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_pep257.main import main
-import pytest
-
-
-@pytest.mark.linter
-@pytest.mark.pep257
-def test_pep257():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found code style errors / warnings'