├── examples ├── __init__.py └── scripts │ ├── joint_sender_action_client.py │ ├── pose_sender_action_client.py │ ├── joint_sender_action_client_async.py │ ├── pose_sender_action_client_async.py │ ├── trajectory_sender_action_client.py │ ├── servo_twist_sender.py │ ├── trajectory_sender_action_client_async.py │ └── servo_watchdog.py ├── .gitignore ├── utils ├── CS_C_easy.csv ├── CS_S_easy.csv ├── tmux_configs │ ├── kinova_api2.yml │ ├── kinova_gui2.yml │ ├── ur_api2.yml │ ├── ur_gui2.yml │ └── copy_tmuxinator_config.sh ├── move_group_sim.txt ├── move_group_demo.txt ├── demo_sem_rdsc.txt ├── demo_rdsc.txt └── CS_S.csv ├── config ├── piper │ ├── piper_kinematics.yaml │ ├── piper_sim.yaml │ └── piper_servo_sim.yaml ├── abb │ ├── abb_kinematics.yaml │ ├── abb_sim.yaml │ └── abb_servo_sim.yaml ├── default_kinematics.yaml ├── ur │ ├── ur_kinematics.yaml │ ├── ur_sim.yaml │ └── ur_servo_sim.yaml ├── kinova │ ├── kinova_kinematics.yaml │ ├── kinova_sim.yaml │ └── kinova_servo_sim.yaml ├── franka │ ├── franka_kinematics.yaml │ ├── franka_sim.yaml │ └── franka_servo_sim.yaml └── big_cell │ └── big_cell_servo_sim.yaml ├── include └── arm_api2 │ ├── grippers │ ├── gripper.hpp │ └── robotiq_gripper.hpp │ ├── utils.hpp │ ├── arm_joy.hpp │ ├── moveit2_simple_iface.hpp │ └── moveit2_iface.hpp ├── .github └── ISSUE_TEMPLATE │ ├── feature_request.md │ └── bug_report.md ├── CHANGELOG.md ├── package.xml ├── LICENSE.md ├── src ├── arm_joy_node.cpp ├── moveit2_iface_node.cpp ├── moveit2_simple_iface_node.cpp ├── arm_joy.cpp └── utils.cpp ├── CMakeLists.txt ├── CONTRIBUTE.md ├── arm_api2 └── scripts │ └── trajectory_sender.py └── launch ├── moveit2_iface.launch.py └── moveit2_simple_iface.launch.py /examples/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .trunk 2 | .vscode 3 | build 4 | */__pycache__/* 5 | -------------------------------------------------------------------------------- /utils/CS_C_easy.csv: -------------------------------------------------------------------------------- 1 | x,y,z 2 | 0.0,0.0,0.0 3 | -0.1,0.0,0.0 4 | -0.1,0.2,0.0 5 | 0.0,0.2,0.0 6 | 0.0,0.0,0.0 7 | -------------------------------------------------------------------------------- /utils/CS_S_easy.csv: -------------------------------------------------------------------------------- 1 | x,y,z 2 | 0.0,0.0,0.0 3 | 0.1,0.0,0.0 4 | 0.1,0.1,0.0 5 | 0.0,0.1,0.0 6 | 0.0,0.2,0.0 7 | 0.1,0.2,0.0 8 | 0.0,0.0,0.0 9 | -------------------------------------------------------------------------------- /config/piper/piper_kinematics.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.0050000000000000001 4 | kinematics_solver_timeout: 0.0050000000000000001 5 | -------------------------------------------------------------------------------- /config/abb/abb_kinematics.yaml: -------------------------------------------------------------------------------- 1 | manipulator: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.005 4 | kinematics_solver_timeout: 0.005 5 | kinematics_solver_attempts: 3 6 | -------------------------------------------------------------------------------- /config/default_kinematics.yaml: -------------------------------------------------------------------------------- 1 | : 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.0050000000000000001 4 | kinematics_solver_timeout: 0.0050000000000000001 5 | -------------------------------------------------------------------------------- /config/ur/ur_kinematics.yaml: -------------------------------------------------------------------------------- 1 | ur_manipulator: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.0050000000000000001 4 | kinematics_solver_timeout: 0.0050000000000000001 5 | -------------------------------------------------------------------------------- /config/kinova/kinova_kinematics.yaml: -------------------------------------------------------------------------------- 1 | manipulator: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.0050000000000000001 4 | kinematics_solver_timeout: 0.0050000000000000001 5 | -------------------------------------------------------------------------------- /config/franka/franka_kinematics.yaml: -------------------------------------------------------------------------------- 1 | manipulator: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.005 4 | kinematics_solver_timeout: 0.005 5 | kinematics_solver_attempts: 3 6 | -------------------------------------------------------------------------------- /utils/tmux_configs/kinova_api2.yml: -------------------------------------------------------------------------------- 1 | # /root/.config/tmuxinator/kinova_api2.yml 2 | 3 | name: kinova_api2 4 | root: ~/ 5 | 6 | windows: 7 | - editor: 8 | layout: tiled 9 | panes: 10 | - kinova_sim 11 | - ros2 launch kinova_gen3_7dof_robotiq_2f_85_moveit_config sim.launch.py 12 | - sleep 10; ros2 launch arm_api2 moveit2_iface.launch.py 13 | - echo "YOU CAN START GUI NOW!" 14 | -------------------------------------------------------------------------------- /utils/tmux_configs/kinova_gui2.yml: -------------------------------------------------------------------------------- 1 | # /root/.config/tmuxinator/kinova_gui2.yml 2 | 3 | name: kinova_gui2 4 | root: ~/ 5 | 6 | windows: 7 | - editor: 8 | layout: tiled 9 | panes: 10 | - kinova_sim 11 | - ros2 launch kinova_gen3_7dof_robotiq_2f_85_moveit_config sim.launch.py 12 | - sleep 10; ros2 launch arm_api2 moveit2_iface.launch.py 13 | - sleep 15; ros2 launch ros2_dash_gui bridge.launch.py 14 | - echo "YOU CAN START GUI NOW!" 15 | -------------------------------------------------------------------------------- /utils/tmux_configs/ur_api2.yml: -------------------------------------------------------------------------------- 1 | # /root/.config/tmuxinator/ur_api2.yml 2 | 3 | name: ur_api2 4 | root: ~/ 5 | 6 | # TODO: Add UR type as param to the tmuxinator also 7 | windows: 8 | - editor: 9 | layout: tiled 10 | panes: 11 | - ros2 launch ur_simulation_gz ur_sim_control.launch.py ur_type:="ur10" use_sim_time:=true 12 | - sleep 5; ros2 launch ur_moveit_config uir_moveit.launch.py ur_type:="ur10" 13 | - sleep 10; ros2 launch arm_api2 moveit2_iface.launch.py robot_name:="ur" 14 | - echo "YOU CAN START GUI NOW!" 15 | -------------------------------------------------------------------------------- /include/arm_api2/grippers/gripper.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GRIPPER_H 2 | #define GRIPPER_H 3 | 4 | #include 5 | #include 6 | 7 | class Gripper { 8 | public: 9 | // Constructor 10 | Gripper(){}; 11 | virtual ~Gripper(){}; 12 | 13 | // Method to open the gripper 14 | virtual void open() = 0; 15 | 16 | // Method to close the gripper 17 | virtual void close() = 0; 18 | 19 | private: 20 | bool isOpen; 21 | // TODO: Add ROS2 action client or topic publisher which is going to be used 22 | }; 23 | 24 | #endif // GRIPPER_H -------------------------------------------------------------------------------- /utils/tmux_configs/ur_gui2.yml: -------------------------------------------------------------------------------- 1 | # /root/.config/tmuxinator/ur_gui2.yml 2 | 3 | name: ur_gui2 4 | root: ~/ 5 | 6 | # TODO: Add UR type as param to the tmuxinator also 7 | windows: 8 | - editor: 9 | layout: tiled 10 | panes: 11 | - ros2 launch ur_simulation_gz ur_sim_control.launch.py ur_type:="ur10" use_sim_time:=true 12 | - sleep 5; ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:="ur10" 13 | - sleep 10; ros2 launch arm_api2 moveit2_iface.launch.py 14 | - sleep 15; ros2 launch ros2_dash_gui bridge.launch.py 15 | - echo "YOU CAN START GUI NOW!" 16 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Create a feature request to help us improve 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | ## Motivation 11 | 12 | Please describe what would be the main purpose of introducing such feature, and how do you envision it. 13 | What is the motivation behind introducing such feature. 14 | 15 | ## Use-case 16 | 17 | Describe practical use-case or example where new feature would be used. 18 | 19 | ## Additional 20 | 21 | Additional data that could be useful. 22 | 23 | ### Implementation 24 | 25 | Practical implementation details that could help facilitate technical discussion. 26 | 27 | ### Time frame 28 | 29 | Estimated time frame in which you would need it. 30 | -------------------------------------------------------------------------------- /utils/tmux_configs/copy_tmuxinator_config.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Define the source file and the destination directory 4 | SOURCE_FILE="$1" 5 | DEST_DIR="$HOME/.config/tmuxinator" 6 | 7 | # Check if the source file is provided and exists 8 | if [ -z "$SOURCE_FILE" ]; then 9 | echo "Usage: $0 " 10 | exit 1 11 | fi 12 | 13 | if [ ! -f "$SOURCE_FILE" ]; then 14 | echo "Error: Source file '$SOURCE_FILE' does not exist." 15 | exit 1 16 | fi 17 | 18 | # Check if the destination directory exists, create if not 19 | if [ ! -d "$DEST_DIR" ]; then 20 | echo "Destination directory '$DEST_DIR' does not exist. Creating it now." 21 | mkdir -p "$DEST_DIR" 22 | fi 23 | 24 | # Copy the file to the destination directory 25 | cp "$SOURCE_FILE" "$DEST_DIR" 26 | 27 | # Verify the copy operation 28 | if [ $? -eq 0 ]; then 29 | echo "File '$SOURCE_FILE' has been successfully copied to '$DEST_DIR'." 30 | else 31 | echo "Error: Failed to copy file." 32 | exit 1 33 | fi 34 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a bug report to help us improve 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | ## Environment 11 | * OS Version: 12 | * OS Installation: 13 | * Docker or manual installation?: 14 | 15 | 16 | ## Description 17 | * Expected behavior: 18 | * Actual behavior: 19 | 20 | ## Steps to reproduce 21 | 22 | 23 | 1. Go to '...' 24 | 2. Click on '....' 25 | 3. Scroll down to '....' 26 | 4. See error 27 | 28 | ## Output 29 |
30 | 31 | ``` 32 | # Paste the entire output here. 33 | ``` 34 | 35 |
36 | 37 | 38 | ## Screenshots 39 | If applicable, add screenshots to help explain your problem. 40 | 41 | 42 | ## Additional context 43 | Add any other context about the problem here. 44 | -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # Changes added by KIT (Edgar Welte) 2 | 3 | ## Sep 2024 4 | 5 | - add frame_id to published current pose 6 | - add service to set max_vel and max_acc scaling factor 7 | - add action servers instead of subscribers to receive commands (pose, joint_state, or pose path) 8 | - add example script to send pose to action server 9 | - add LIN, EST and PRM Planner for pose action 10 | 11 | ## Oct 2024 12 | 13 | - add keyboard control node to control the robot via keyboard in servo mode 14 | 15 | ## Nov 2024 16 | - integrate xbox joy ctl 17 | 18 | 19 | ## Jan 2025 20 | 21 | - refactor code for planning 22 | - add gripper functionality incl. action client for robotiq_2f 23 | - add service for setting end effector link in moveit2 interface 24 | - add servo watchdog node to monitor twist/jog commands and publish zero velocity if inactive 25 | - add cumotion as planner 26 | 27 | ## Feb 2025 28 | - set proper planner priority: 1. cumotion, if failed then PRM, EST, LIN (each 3 times) 29 | - add planonly mode 30 | 31 | # Future potential features 32 | 33 | - realtime pose follower (teleoperation: servo via absolute position) 34 | - constraints for planners (e.g. orientation constraint for cup transport) 35 | - dynamic scene object update 36 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | arm_api2 5 | 0.0.0 6 | MoveIt2 interface for robot manipulators 7 | root 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | ament_cmake_python 12 | rosidl_default_generators 13 | 14 | moveit_ros_planning_interface 15 | moveit_ros 16 | rclcpp 17 | rclcpp_action 18 | rclcpp_components 19 | rclpy 20 | std_msgs 21 | moveit_visual_tools 22 | arm_api2_msgs 23 | moveit_servo 24 | geometry_msgs 25 | sensor_msgs 26 | control_msgs 27 | tf2 28 | std_srvs 29 | python3 30 | 31 | ament_lint_auto 32 | ament_lint_common 33 | 34 | ros2launch 35 | 36 | 37 | ament_cmake 38 | 39 | 40 | rosidl_interface_packages 41 | 42 | 43 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) Crobotic Solutions d.o.o. 4 | 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | * Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | * Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | * Neither the name of the copyright holder nor the names of its 18 | contributors may be used to endorse or promote products derived from 19 | this software without specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | -------------------------------------------------------------------------------- /config/franka/franka_sim.yaml: -------------------------------------------------------------------------------- 1 | robot: 2 | arm_name: arm 3 | ee_link_name: panda_hand_tcp 4 | robot_desc: robot_description 5 | move_group_ns: 6 | planning_scene: planning_scene 7 | planning_frame: panda_link0 8 | num_cart_pts: 20 9 | joint_states: joint_states 10 | max_vel_scaling_factor: 0.1 11 | max_acc_scaling_factor: 0.1 12 | with_planner: true 13 | topic: 14 | sub: 15 | cmd_pose: 16 | name: arm/cmd/pose 17 | queue: 1 18 | cmd_traj: 19 | name: arm/cmd/traj 20 | queue: 1 21 | cmd_tool_orientation: 22 | name: tool/cmd/orient 23 | queue: 1 24 | cmd_delta_pose: 25 | name: arm/cmd/delta_pose 26 | queue: 1 27 | joint_states: 28 | name: joint_states 29 | queue: 1 30 | pub: 31 | display_trajectory: 32 | name: arm/display_path 33 | queue: 1 34 | current_pose: 35 | name: arm/state/current_pose 36 | queue: 1 37 | current_robot_state: 38 | name: arm/state/ctl_state 39 | queue: 1 40 | srv: 41 | disable_collision: 42 | name: /move_group/disable_collision 43 | add_collision: 44 | name: /move_group/add_collision 45 | change_robot_state: 46 | name: arm/change_state 47 | set_vel_acc: 48 | name: arm/set_vel_acc 49 | set_planner: 50 | name: arm/set_planner 51 | set_eelink: 52 | name: arm/set_eelink 53 | set_planonly: 54 | name: arm/set_planonly 55 | open_gripper: 56 | name: arm/open_gripper 57 | close_gripper: 58 | name: arm/close_gripper 59 | action: 60 | move_to_pose: 61 | name: arm/move_to_pose 62 | move_to_joint: 63 | name: arm/move_to_joint 64 | move_to_pose_path: 65 | name: arm/move_to_pose_path 66 | gripper_control: 67 | name: arm/gripper_control -------------------------------------------------------------------------------- /config/piper/piper_sim.yaml: -------------------------------------------------------------------------------- 1 | robot: 2 | arm_name: piper_arm 3 | ee_link_name: piper_tcp 4 | robot_desc: robot_description 5 | move_group_ns: 6 | planning_scene: planning_scene 7 | planning_frame: base_link 8 | num_cart_pts: 20 9 | joint_states: joint_states 10 | max_vel_scaling_factor: 0.1 11 | max_acc_scaling_factor: 0.1 12 | with_planner: true 13 | topic: 14 | sub: 15 | cmd_pose: 16 | name: arm/cmd/pose 17 | queue: 1 18 | cmd_traj: 19 | name: arm/cmd/traj 20 | queue: 1 21 | cmd_tool_orientation: 22 | name: tool/cmd/orient 23 | queue: 1 24 | cmd_delta_pose: 25 | name: arm/cmd/delta_pose 26 | queue: 1 27 | joint_states: 28 | name: joint_states 29 | queue: 1 30 | pub: 31 | display_trajectory: 32 | name: arm/display_path 33 | queue: 1 34 | current_pose: 35 | name: arm/state/current_pose 36 | queue: 1 37 | current_robot_state: 38 | name: arm/state/ctl_state 39 | queue: 1 40 | srv: 41 | disable_collision: 42 | name: /move_group/disable_collision 43 | add_collision: 44 | name: /move_group/add_collision 45 | change_robot_state: 46 | name: arm/change_state 47 | set_vel_acc: 48 | name: arm/set_vel_acc 49 | set_planner: 50 | name: arm/set_planner 51 | set_eelink: 52 | name: arm/set_eelink 53 | set_planonly: 54 | name: arm/set_planonly 55 | open_gripper: 56 | name: arm/open_gripper 57 | close_gripper: 58 | name: arm/close_gripper 59 | action: 60 | move_to_pose: 61 | name: arm/move_to_pose 62 | move_to_joint: 63 | name: arm/move_to_joint 64 | move_to_pose_path: 65 | name: arm/move_to_pose_path 66 | gripper_control: 67 | name: arm/gripper_control -------------------------------------------------------------------------------- /config/abb/abb_sim.yaml: -------------------------------------------------------------------------------- 1 | robot: 2 | arm_name: manipulator 3 | ee_link_name: gripper_base 4 | robot_desc: robot_description 5 | move_group_ns: 6 | planning_scene: planning_scene 7 | planning_frame: base_link 8 | num_cart_pts: 20 9 | joint_states: joint_states 10 | max_vel_scaling_factor: 0.5 11 | max_acc_scaling_factor: 0.5 12 | with_planner: true 13 | topic: 14 | sub: 15 | cmd_pose: 16 | name: arm/cmd/pose 17 | queue: 1 18 | cmd_traj: 19 | name: arm/cmd/traj 20 | queue: 1 21 | cmd_tool_orientation: 22 | name: tool/cmd/orient 23 | queue: 1 24 | cmd_delta_pose: 25 | name: arm/cmd/delta_pose 26 | queue: 1 27 | joint_states: 28 | name: joint_states 29 | queue: 1 30 | pub: 31 | display_trajectory: 32 | name: arm/display_path 33 | queue: 1 34 | current_pose: 35 | name: arm/state/current_pose 36 | queue: 1 37 | current_robot_state: 38 | name: arm/state/ctl_state 39 | queue: 1 40 | srv: 41 | disable_collision: 42 | name: /move_group/disable_collision 43 | add_collision: 44 | name: /move_group/add_collision 45 | change_robot_state: 46 | name: arm/change_state 47 | set_vel_acc: 48 | name: arm/set_vel_acc 49 | set_planner: 50 | name: arm/set_planner 51 | set_eelink: 52 | name: arm/set_eelink 53 | set_planonly: 54 | name: arm/set_planonly 55 | open_gripper: 56 | name: arm/open_gripper 57 | close_gripper: 58 | name: arm/close_gripper 59 | action: 60 | move_to_pose: 61 | name: arm/move_to_pose 62 | move_to_joint: 63 | name: arm/move_to_joint 64 | move_to_pose_path: 65 | name: arm/move_to_pose_path 66 | gripper_control: 67 | name: arm/gripper_control 68 | -------------------------------------------------------------------------------- /config/kinova/kinova_sim.yaml: -------------------------------------------------------------------------------- 1 | robot: 2 | arm_name: manipulator 3 | ee_link_name: end_effector_link 4 | robot_desc: robot_description 5 | move_group_ns: 6 | planning_scene: planning_scene 7 | planning_frame: world 8 | num_cart_pts: 20 9 | joint_states: joint_states 10 | max_vel_scaling_factor: 0.1 11 | max_acc_scaling_factor: 0.1 12 | with_planner: true 13 | topic: 14 | sub: 15 | cmd_pose: 16 | name: arm/cmd/pose 17 | queue: 1 18 | cmd_traj: 19 | name: arm/cmd/traj 20 | queue: 1 21 | cmd_tool_orientation: 22 | name: tool/cmd/orient 23 | queue: 1 24 | cmd_delta_pose: 25 | name: arm/cmd/delta_pose 26 | queue: 1 27 | joint_states: 28 | name: joint_states 29 | queue: 1 30 | pub: 31 | display_trajectory: 32 | name: arm/display_path 33 | queue: 1 34 | current_pose: 35 | name: arm/state/current_pose 36 | queue: 1 37 | current_robot_state: 38 | name: arm/state/ctl_state 39 | queue: 1 40 | srv: 41 | disable_collision: 42 | name: /move_group/disable_collision 43 | add_collision: 44 | name: /move_group/add_collision 45 | change_robot_state: 46 | name: arm/change_state 47 | set_vel_acc: 48 | name: arm/set_vel_acc 49 | set_planner: 50 | name: arm/set_planner 51 | set_eelink: 52 | name: arm/set_eelink 53 | set_planonly: 54 | name: arm/set_planonly 55 | open_gripper: 56 | name: arm/open_gripper 57 | close_gripper: 58 | name: arm/close_gripper 59 | action: 60 | move_to_pose: 61 | name: arm/move_to_pose 62 | move_to_joint: 63 | name: arm/move_to_joint 64 | move_to_pose_path: 65 | name: arm/move_to_pose_path 66 | gripper_control: 67 | name: arm/gripper_control -------------------------------------------------------------------------------- /examples/scripts/joint_sender_action_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import threading 4 | import rclpy 5 | from arm_api2_msgs.action import MoveJoint 6 | from sensor_msgs.msg import JointState 7 | from rclpy.action import ActionClient 8 | from rclpy.node import Node 9 | 10 | 11 | class JointSenderActionClient(Node): 12 | 13 | def __init__(self): 14 | super().__init__("joint_sender_action_client") 15 | self._action_client = ActionClient(self, MoveJoint, "arm/move_to_joint") 16 | 17 | # new thread to handle the actual task without blocking the main thread 18 | self._execution_thread = threading.Thread(target=self._execute) 19 | self._execution_thread.start() 20 | 21 | def _send_goal(self, goal: JointState): 22 | goal_msg = MoveJoint.Goal() 23 | goal_msg.joint_state = goal 24 | 25 | self.get_logger().info("Waiting for action server...") 26 | 27 | self._action_client.wait_for_server() 28 | 29 | self.get_logger().info("Sending goal request...") 30 | 31 | result = self._action_client.send_goal(goal_msg) 32 | 33 | self.get_logger().info("Result: {0}".format(result)) 34 | rclpy.shutdown() 35 | 36 | def _execute(self): 37 | 38 | names =["shoulder_pan_joint","shoulder_lift_joint","elbow_joint","wrist_1_joint","wrist_2_joint","wrist_3_joint"] 39 | positions = [-4.0,-2.067,1.849,-1.428,-1.395,2.045] 40 | goal = JointState() 41 | goal.name = names 42 | goal.position = positions 43 | 44 | self._send_goal(goal) 45 | 46 | 47 | def main(args=None): 48 | rclpy.init(args=args) 49 | 50 | action_client = JointSenderActionClient() 51 | 52 | rclpy.spin(action_client) 53 | 54 | 55 | if __name__ == "__main__": 56 | main() 57 | -------------------------------------------------------------------------------- /utils/move_group_sim.txt: -------------------------------------------------------------------------------- 1 | /move_group 2 | Subscribers: 3 | /clock: rosgraph_msgs/msg/Clock 4 | /parameter_events: rcl_interfaces/msg/ParameterEvent 5 | /trajectory_execution_event: std_msgs/msg/String 6 | Publishers: 7 | /display_contacts: visualization_msgs/msg/MarkerArray 8 | /display_planned_path: moveit_msgs/msg/DisplayTrajectory 9 | /motion_plan_request: moveit_msgs/msg/MotionPlanRequest 10 | /parameter_events: rcl_interfaces/msg/ParameterEvent 11 | /rosout: rcl_interfaces/msg/Log 12 | Service Servers: 13 | /apply_planning_scene: moveit_msgs/srv/ApplyPlanningScene 14 | /check_state_validity: moveit_msgs/srv/GetStateValidity 15 | /clear_octomap: std_srvs/srv/Empty 16 | /compute_cartesian_path: moveit_msgs/srv/GetCartesianPath 17 | /compute_fk: moveit_msgs/srv/GetPositionFK 18 | /compute_ik: moveit_msgs/srv/GetPositionIK 19 | /get_planner_params: moveit_msgs/srv/GetPlannerParams 20 | /load_map: moveit_msgs/srv/LoadMap 21 | /move_group/describe_parameters: rcl_interfaces/srv/DescribeParameters 22 | /move_group/get_parameter_types: rcl_interfaces/srv/GetParameterTypes 23 | /move_group/get_parameters: rcl_interfaces/srv/GetParameters 24 | /move_group/list_parameters: rcl_interfaces/srv/ListParameters 25 | /move_group/set_parameters: rcl_interfaces/srv/SetParameters 26 | /move_group/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically 27 | /plan_kinematic_path: moveit_msgs/srv/GetMotionPlan 28 | /query_planner_interface: moveit_msgs/srv/QueryPlannerInterfaces 29 | /save_map: moveit_msgs/srv/SaveMap 30 | /set_planner_params: moveit_msgs/srv/SetPlannerParams 31 | Service Clients: 32 | 33 | Action Servers: 34 | /execute_trajectory: moveit_msgs/action/ExecuteTrajectory 35 | /move_action: moveit_msgs/action/MoveGroup 36 | Action Clients: 37 | 38 | -------------------------------------------------------------------------------- /config/ur/ur_sim.yaml: -------------------------------------------------------------------------------- 1 | robot: 2 | arm_name: ur_manipulator 3 | ee_link_name: tool0 4 | robot_desc: robot_description 5 | move_group_ns: 6 | planning_scene: planning_scene 7 | planning_frame: world #or base 8 | num_cart_pts: 20 9 | joint_states: joint_states 10 | max_vel_scaling_factor: 0.1 11 | max_acc_scaling_factor: 0.1 12 | with_planner: true 13 | topic: 14 | sub: 15 | cmd_pose: 16 | name: arm/cmd/pose 17 | queue: 1 18 | cmd_traj: 19 | name: arm/cmd/traj 20 | queue: 1 21 | cmd_tool_orientation: 22 | name: tool/cmd/orient 23 | queue: 1 24 | cmd_delta_pose: 25 | name: arm/cmd/delta_pose 26 | queue: 1 27 | cmd_pose: 28 | name: arm/cmd/pose 29 | queue: 1 30 | cmd_traj: 31 | name: arm/cmd/traj 32 | queue: 1 33 | joint_states: 34 | name: joint_states 35 | queue: 1 36 | pub: 37 | display_trajectory: 38 | name: arm/display_path 39 | queue: 1 40 | current_pose: 41 | name: arm/state/current_pose 42 | queue: 1 43 | current_robot_state: 44 | name: arm/state/ctl_state 45 | queue: 1 46 | srv: 47 | disable_collision: 48 | name: /move_group/disable_collision 49 | add_collision: 50 | name: /move_group/add_collision 51 | change_robot_state: 52 | name: arm/change_state 53 | set_vel_acc: 54 | name: arm/set_vel_acc 55 | set_planner: 56 | name: arm/set_planner 57 | set_eelink: 58 | name: arm/set_eelink 59 | set_planonly: 60 | name: arm/set_planonly 61 | open_gripper: 62 | name: arm/open_gripper 63 | close_gripper: 64 | name: arm/close_gripper 65 | action: 66 | move_to_pose: 67 | name: arm/move_to_pose 68 | move_to_joint: 69 | name: arm/move_to_joint 70 | move_to_pose_path: 71 | name: arm/move_to_pose_path 72 | gripper_control: 73 | name: arm/gripper_control -------------------------------------------------------------------------------- /examples/scripts/pose_sender_action_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import threading 4 | import rclpy 5 | from arm_api2_msgs.action import MoveCartesian 6 | from geometry_msgs.msg import PoseStamped 7 | from rclpy.action import ActionClient 8 | from rclpy.node import Node 9 | 10 | 11 | class PoseSenderActionClient(Node): 12 | 13 | def __init__(self): 14 | super().__init__("pose_sender_action_client") 15 | self._action_client = ActionClient(self, MoveCartesian, "arm/move_to_pose") 16 | 17 | # new thread to handle the actual task without blocking the main thread 18 | self._execution_thread = threading.Thread(target=self._execute) 19 | self._execution_thread.start() 20 | 21 | def _send_goal(self, goal): 22 | goal_msg = MoveCartesian.Goal() 23 | goal_msg.goal = goal 24 | 25 | self.get_logger().info("Waiting for action server...") 26 | 27 | self._action_client.wait_for_server() 28 | 29 | self.get_logger().info("Sending goal request...") 30 | 31 | result = self._action_client.send_goal(goal_msg) 32 | 33 | self.get_logger().info("Result: {0}".format(result)) 34 | rclpy.shutdown() 35 | 36 | def _execute(self): 37 | 38 | goal = PoseStamped() 39 | goal.header.frame_id = "world" 40 | goal.pose.position.x = 0.1 41 | goal.pose.position.y = -0.7 42 | goal.pose.position.z = 1.4 43 | goal.pose.orientation.x = 1.0 44 | goal.pose.orientation.y = 0.0 45 | goal.pose.orientation.z = 0.0 46 | goal.pose.orientation.w = 0.0 47 | 48 | self._send_goal(goal) 49 | 50 | 51 | 52 | 53 | def main(args=None): 54 | rclpy.init(args=args) 55 | 56 | action_client = PoseSenderActionClient() 57 | 58 | rclpy.spin(action_client) 59 | 60 | 61 | if __name__ == "__main__": 62 | main() 63 | -------------------------------------------------------------------------------- /utils/move_group_demo.txt: -------------------------------------------------------------------------------- 1 | /move_group 2 | Subscribers: 3 | /parameter_events: rcl_interfaces/msg/ParameterEvent 4 | /trajectory_execution_event: std_msgs/msg/String 5 | Publishers: 6 | /display_contacts: visualization_msgs/msg/MarkerArray 7 | /display_planned_path: moveit_msgs/msg/DisplayTrajectory 8 | /motion_plan_request: moveit_msgs/msg/MotionPlanRequest 9 | /parameter_events: rcl_interfaces/msg/ParameterEvent 10 | /robot_description: std_msgs/msg/String 11 | /robot_description_semantic: std_msgs/msg/String 12 | /rosout: rcl_interfaces/msg/Log 13 | Service Servers: 14 | /apply_planning_scene: moveit_msgs/srv/ApplyPlanningScene 15 | /check_state_validity: moveit_msgs/srv/GetStateValidity 16 | /clear_octomap: std_srvs/srv/Empty 17 | /compute_cartesian_path: moveit_msgs/srv/GetCartesianPath 18 | /compute_fk: moveit_msgs/srv/GetPositionFK 19 | /compute_ik: moveit_msgs/srv/GetPositionIK 20 | /get_planner_params: moveit_msgs/srv/GetPlannerParams 21 | /load_map: moveit_msgs/srv/LoadMap 22 | /move_group/describe_parameters: rcl_interfaces/srv/DescribeParameters 23 | /move_group/get_parameter_types: rcl_interfaces/srv/GetParameterTypes 24 | /move_group/get_parameters: rcl_interfaces/srv/GetParameters 25 | /move_group/list_parameters: rcl_interfaces/srv/ListParameters 26 | /move_group/set_parameters: rcl_interfaces/srv/SetParameters 27 | /move_group/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically 28 | /plan_kinematic_path: moveit_msgs/srv/GetMotionPlan 29 | /query_planner_interface: moveit_msgs/srv/QueryPlannerInterfaces 30 | /save_map: moveit_msgs/srv/SaveMap 31 | /set_planner_params: moveit_msgs/srv/SetPlannerParams 32 | Service Clients: 33 | 34 | Action Servers: 35 | /execute_trajectory: moveit_msgs/action/ExecuteTrajectory 36 | /move_action: moveit_msgs/action/MoveGroup 37 | Action Clients: 38 | 39 | -------------------------------------------------------------------------------- /examples/scripts/joint_sender_action_client_async.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rclpy 4 | from arm_api2_msgs.action import MoveJoint 5 | from sensor_msgs.msg import JointState 6 | from rclpy.action import ActionClient 7 | from rclpy.node import Node 8 | 9 | 10 | class JointSenderActionClientAsync(Node): 11 | 12 | def __init__(self): 13 | super().__init__("joint_sender_action_client_async") 14 | self._action_client = ActionClient(self, MoveJoint, "arm/move_to_joint") 15 | 16 | def send_goal(self, goal: JointState): 17 | goal_msg = MoveJoint.Goal() 18 | goal_msg.joint_state = goal 19 | 20 | self.get_logger().info("Waiting for action server...") 21 | 22 | self._action_client.wait_for_server() 23 | 24 | self.get_logger().info("Sending goal request...") 25 | 26 | self._send_goal_future = self._action_client.send_goal_async( 27 | goal_msg, feedback_callback=self.feedback_callback 28 | ) 29 | 30 | self._send_goal_future.add_done_callback(self.goal_response_callback) 31 | 32 | def goal_response_callback(self, future): 33 | goal_handle = future.result() 34 | 35 | if not goal_handle.accepted: 36 | self.get_logger().info("Goal rejected") 37 | return 38 | 39 | self.get_logger().info("Goal accepted") 40 | 41 | self._get_result_future = goal_handle.get_result_async() 42 | self._get_result_future.add_done_callback(self.get_result_callback) 43 | 44 | def get_result_callback(self, future): 45 | result = future.result().result 46 | self.get_logger().info("Result: {0}".format(result)) 47 | rclpy.shutdown() 48 | 49 | def feedback_callback(self, feedback_msg): 50 | feedback = feedback_msg.feedback 51 | self.get_logger().info("Feedback: {0}".format(feedback.status)) 52 | 53 | 54 | def main(args=None): 55 | rclpy.init(args=args) 56 | 57 | action_client = JointSenderActionClientAsync() 58 | names =["shoulder_pan_joint","shoulder_lift_joint","elbow_joint","wrist_1_joint","wrist_2_joint","wrist_3_joint"] 59 | positions = [-4.0,-2.067,1.849,-1.428,-1.395,2.045] 60 | goal = JointState() 61 | goal.name = names 62 | goal.position = positions 63 | 64 | action_client.send_goal(goal) 65 | 66 | rclpy.spin(action_client) 67 | 68 | 69 | if __name__ == "__main__": 70 | main() 71 | -------------------------------------------------------------------------------- /examples/scripts/pose_sender_action_client_async.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rclpy 4 | from arm_api2_msgs.action import MoveCartesian 5 | from geometry_msgs.msg import PoseStamped 6 | from rclpy.action import ActionClient 7 | from rclpy.node import Node 8 | 9 | 10 | class PoseSenderActionClientAsync(Node): 11 | 12 | def __init__(self): 13 | super().__init__("pose_sender_action_client_async") 14 | self._action_client = ActionClient(self, MoveCartesian, "arm/move_to_pose") 15 | 16 | def send_goal(self, goal): 17 | goal_msg = MoveCartesian.Goal() 18 | goal_msg.goal = goal 19 | 20 | self.get_logger().info("Waiting for action server...") 21 | 22 | self._action_client.wait_for_server() 23 | 24 | self.get_logger().info("Sending goal request...") 25 | 26 | self._send_goal_future = self._action_client.send_goal_async( 27 | goal_msg, feedback_callback=self.feedback_callback 28 | ) 29 | 30 | self._send_goal_future.add_done_callback(self.goal_response_callback) 31 | 32 | def goal_response_callback(self, future): 33 | goal_handle = future.result() 34 | 35 | if not goal_handle.accepted: 36 | self.get_logger().info("Goal rejected") 37 | return 38 | 39 | self.get_logger().info("Goal accepted") 40 | 41 | self._get_result_future = goal_handle.get_result_async() 42 | self._get_result_future.add_done_callback(self.get_result_callback) 43 | 44 | def get_result_callback(self, future): 45 | result = future.result().result 46 | self.get_logger().info("Result: {0}".format(result)) 47 | rclpy.shutdown() 48 | 49 | def feedback_callback(self, feedback_msg): 50 | feedback = feedback_msg.feedback 51 | self.get_logger().info("Feedback: {0}".format(feedback.status)) 52 | 53 | 54 | def main(args=None): 55 | rclpy.init(args=args) 56 | 57 | action_client = PoseSenderActionClientAsync() 58 | 59 | goal = PoseStamped() 60 | goal.header.frame_id = "world" 61 | goal.pose.position.x = 0.1 62 | goal.pose.position.y = -0.7 63 | goal.pose.position.z = 1.4 64 | goal.pose.orientation.x = 1.0 65 | goal.pose.orientation.y = 0.0 66 | goal.pose.orientation.z = 0.0 67 | goal.pose.orientation.w = 0.0 68 | 69 | action_client.send_goal(goal) 70 | 71 | rclpy.spin(action_client) 72 | 73 | 74 | if __name__ == "__main__": 75 | main() 76 | -------------------------------------------------------------------------------- /src/arm_joy_node.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2024, Crobotic Solutions d.o.o. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | /* Title : moveit2_iface.cpp 35 | * Project : arm_api2 36 | * Created : 05/10/2024 37 | * Author : Filip Zoric 38 | * 39 | * Description : ROS 2 node wrapper for the arm_joy.cpp class 40 | */ 41 | #include "arm_api2/arm_joy.hpp" 42 | 43 | int main(int argc, char * argv []) 44 | { 45 | rclcpp::init(argc, argv); 46 | 47 | // Create node 48 | rclcpp::Node::SharedPtr node = std::make_shared(); 49 | 50 | // Add multithreaded executor 51 | rclcpp::executors::MultiThreadedExecutor executor; 52 | executor.add_node(node); 53 | executor.spin(); 54 | 55 | rclcpp::shutdown(); 56 | return 0; 57 | } -------------------------------------------------------------------------------- /src/moveit2_iface_node.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2024, Crobotic Solutions d.o.o. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | /* Title : moveit2_iface.cpp 35 | * Project : arm_api2 36 | * Created : 05/10/2024 37 | * Author : Filip Zoric 38 | * 39 | * Description : ROS 2 node wrapper for the moveit2_iface.cpp class 40 | */ 41 | 42 | #include "arm_api2/moveit2_iface.hpp" 43 | 44 | int main(int argc, char * argv []) 45 | { 46 | 47 | rclcpp::init(argc, argv); 48 | // Set node options 49 | rclcpp::NodeOptions node_options; 50 | node_options.automatically_declare_parameters_from_overrides(true); 51 | node_options.use_intra_process_comms(false); 52 | 53 | // Create node 54 | auto move_group_node = std::make_shared(node_options); 55 | rclcpp::spin(move_group_node); 56 | 57 | // Test like this :) [Without executors] 58 | //rclcpp::spin(node); 59 | rclcpp::shutdown(); 60 | return 0; 61 | 62 | } -------------------------------------------------------------------------------- /src/moveit2_simple_iface_node.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2025, Crobotic Solutions d.o.o. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | /* Title : moveit2_simple_iface_node.cpp 35 | * Project : arm_api2 36 | * Created : 05/10/2024 37 | * Author : Filip Zoric 38 | * 39 | * Description : ROS 2 node wrapper for the moveit2_iface.cpp class 40 | */ 41 | 42 | #include "arm_api2/moveit2_simple_iface.hpp" 43 | 44 | int main(int argc, char * argv []) 45 | { 46 | 47 | rclcpp::init(argc, argv); 48 | // Set node options 49 | rclcpp::NodeOptions node_options; 50 | node_options.automatically_declare_parameters_from_overrides(true); 51 | node_options.use_intra_process_comms(false); 52 | 53 | // Create node 54 | auto move_group_node = std::make_shared(node_options); 55 | rclcpp::spin(move_group_node); 56 | 57 | // Test like this :) [Without executors] 58 | //rclcpp::spin(node); 59 | rclcpp::shutdown(); 60 | return 0; 61 | 62 | } -------------------------------------------------------------------------------- /include/arm_api2/utils.hpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * 3 | * Copyright (c) 2024, Crobotic Solutions d.o.o. (www.crobotics.tech) 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright notice, this 10 | * list of conditions and the following disclaimer. 11 | * 12 | * * Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * * Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived from 18 | * this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *******************************************************************************/ 32 | 33 | /* Title : utils.hpp 34 | * Project : arm_api2 35 | * Created : 05/10/2024 36 | * Author : Filip Zoric 37 | * 38 | * Description : Header for the utils.cpp file 39 | */ 40 | 41 | #ifndef UTILS_HPP 42 | #define UTILS_HPP 43 | 44 | #include 45 | 46 | #include "tf2/LinearMath/Quaternion.h" 47 | #include "tf2/LinearMath/Matrix3x3.h" 48 | #include "Eigen/Dense" 49 | 50 | #include "geometry_msgs/msg/pose.hpp" 51 | #include "geometry_msgs/msg/pose_stamped.hpp" 52 | #include "geometry_msgs/msg/quaternion.hpp" 53 | #include "std_msgs/msg/string.hpp" 54 | 55 | namespace utils { 56 | 57 | bool comparePosition (geometry_msgs::msg::PoseStamped p1, geometry_msgs::msg::PoseStamped p2); 58 | bool compareOrientation (geometry_msgs::msg::PoseStamped p1, geometry_msgs::msg::PoseStamped p2); 59 | bool comparePose (geometry_msgs::msg::PoseStamped p1, geometry_msgs::msg::PoseStamped p2); 60 | std::vector createCartesianWaypoints (geometry_msgs::msg::Pose p1, geometry_msgs::msg::Pose p2, int n); 61 | geometry_msgs::msg::PoseStamped normalizeOrientation (geometry_msgs::msg::PoseStamped p); 62 | geometry_msgs::msg::PoseStamped convertIsometryToMsg (Eigen::Isometry3d pose); 63 | std_msgs::msg::String stateToMsg (int state); 64 | 65 | } // namespace utils 66 | 67 | #endif // UTILS_HPP -------------------------------------------------------------------------------- /include/arm_api2/arm_joy.hpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * 3 | * Copyright (c) 2024, Crobotic Solutions d.o.o. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright notice, this 10 | * list of conditions and the following disclaimer. 11 | * 12 | * * Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * * Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived from 18 | * this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *******************************************************************************/ 32 | 33 | /* Title : moveit2_iface.cpp 34 | * Project : arm_api2 35 | * Created : 05/10/2024 36 | * Author : Filip Zoric 37 | * 38 | * Description : Header for the joystick control class. 39 | */ 40 | 41 | 42 | #ifndef ARM_JOY_H 43 | #define ARM_JOY_H 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | //* ros 51 | #include "rclcpp/rclcpp.hpp" 52 | 53 | //* msgs 54 | #include "std_msgs/msg/string.hpp" 55 | #include "std_msgs/msg/bool.hpp" 56 | #include "sensor_msgs/msg/joy.hpp" 57 | #include "geometry_msgs/msg/twist_stamped.hpp" 58 | 59 | //* srvs 60 | #include "std_srvs/srv/empty.hpp" 61 | #include "std_srvs/srv/trigger.hpp" 62 | 63 | using namespace std::chrono_literals; 64 | using std::placeholders::_1; 65 | using std::placeholders::_2; 66 | 67 | class JoyCtl: public rclcpp::Node 68 | { 69 | public: 70 | JoyCtl(); 71 | 72 | private: 73 | 74 | // vars 75 | bool enableJoy_; 76 | mutable int scale_factor; 77 | rclcpp::Clock clock_; 78 | 79 | // publishers 80 | rclcpp::Publisher::SharedPtr cmdVelPub_; 81 | 82 | // subscribers 83 | rclcpp::Subscription::SharedPtr joySub_; 84 | 85 | // clients 86 | rclcpp::Client::SharedPtr jingleBellsClient_; // Could be used for initing all UAVs 87 | 88 | void init(); 89 | void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg); 90 | 91 | // Setting them as const to be usable by joy_callback which is also const 92 | void setScaleFactor(int value); 93 | int getScaleFactor() const; 94 | void setEnableJoy(bool val); 95 | bool getEnableJoy() const; 96 | 97 | // TODO: Add service to turn joystick on and off 98 | 99 | }; 100 | 101 | #endif -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(arm_api2) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(moveit_ros_planning_interface REQUIRED) 11 | find_package(moveit_ros REQUIRED) 12 | find_package(joy REQUIRED) 13 | find_package(moveit_servo REQUIRED) 14 | find_package(moveit_visual_tools REQUIRED) 15 | find_package(rclcpp REQUIRED) 16 | find_package(rosidl_default_generators REQUIRED) 17 | find_package(std_msgs REQUIRED) 18 | find_package(arm_api2_msgs REQUIRED) 19 | find_package(moveit_msgs REQUIRED) 20 | find_package(tf2 REQUIRED) 21 | find_package(rclpy REQUIRED) 22 | find_package(geometry_msgs REQUIRED) 23 | find_package(sensor_msgs REQUIRED) 24 | find_package(std_srvs REQUIRED) 25 | 26 | # include headers 27 | include_directories(include) 28 | 29 | # Build moveit2_iface 30 | add_executable(moveit2_iface src/moveit2_iface.cpp src/moveit2_iface_node.cpp src/utils.cpp) 31 | target_include_directories(moveit2_iface 32 | PUBLIC 33 | $ 34 | $ 35 | PRIVATE 36 | include/) 37 | 38 | # Build moveit2_iface 39 | target_compile_features(moveit2_iface PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 40 | ament_target_dependencies( 41 | moveit2_iface moveit_ros moveit_ros_planning_interface moveit_visual_tools rclcpp arm_api2_msgs moveit_servo sensor_msgs geometry_msgs 42 | ) 43 | 44 | # Install moveit2_iface 45 | install(TARGETS 46 | moveit2_iface 47 | DESTINATION lib/${PROJECT_NAME}) 48 | 49 | # Build moveit2_simple_iface 50 | add_executable(moveit2_simple_iface src/moveit2_simple_iface.cpp src/moveit2_simple_iface_node.cpp src/utils.cpp) 51 | target_include_directories(moveit2_simple_iface 52 | PUBLIC 53 | $ 54 | $ 55 | PRIVATE 56 | include/) 57 | 58 | target_compile_features(moveit2_simple_iface PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 59 | ament_target_dependencies( 60 | moveit2_simple_iface moveit_ros moveit_ros_planning_interface moveit_visual_tools rclcpp arm_api2_msgs moveit_servo sensor_msgs geometry_msgs 61 | ) 62 | 63 | # Install moveit2_simple_iface 64 | install(TARGETS 65 | moveit2_simple_iface 66 | DESTINATION lib/${PROJECT_NAME}) 67 | 68 | # Install launch directory 69 | install(DIRECTORY 70 | launch 71 | config 72 | utils 73 | DESTINATION share/${PROJECT_NAME} 74 | ) 75 | 76 | # Install C++ files 77 | add_executable(joy_ctl src/arm_joy_node.cpp src/arm_joy.cpp) 78 | 79 | target_include_directories(joy_ctl 80 | PRIVATE 81 | include/ 82 | ) 83 | 84 | # python dependencies 85 | ament_target_dependencies(joy_ctl 86 | rclcpp std_msgs geometry_msgs std_srvs sensor_msgs control_msgs) 87 | 88 | install(TARGETS 89 | joy_ctl 90 | DESTINATION lib/${PROJECT_NAME}) 91 | 92 | add_executable(keyboard_ctl src/servo_keyboard_input.cpp) 93 | 94 | ament_target_dependencies(keyboard_ctl 95 | rclcpp std_msgs geometry_msgs std_srvs sensor_msgs control_msgs rclcpp_action) 96 | 97 | install(TARGETS 98 | keyboard_ctl 99 | DESTINATION lib/${PROJECT_NAME}) 100 | 101 | # Install Python modules 102 | #ament_python_install_package(${PROJECT_NAME}) 103 | 104 | install(PROGRAMS 105 | examples/scripts/joint_sender_action_client_async.py 106 | examples/scripts/joint_sender_action_client.py 107 | examples/scripts/pose_sender_action_client_async.py 108 | examples/scripts/pose_sender_action_client.py 109 | examples/scripts/trajectory_sender_action_client_async.py 110 | examples/scripts/trajectory_sender_action_client.py 111 | examples/scripts/servo_twist_sender.py 112 | examples/scripts/servo_watchdog.py 113 | DESTINATION lib/${PROJECT_NAME} 114 | ) 115 | 116 | if(BUILD_TESTING) 117 | find_package(ament_lint_auto REQUIRED) 118 | # the following line skips the linter which checks for copyrights 119 | # comment the line when a copyright and license is added to all source files 120 | set(ament_cmake_copyright_FOUND TRUE) 121 | # the following line skips cpplint (only works in a git repo) 122 | # comment the line when this package is in a git repo and when 123 | # a copyright and license is added to all source files 124 | set(ament_cmake_cpplint_FOUND TRUE) 125 | ament_lint_auto_find_test_dependencies() 126 | endif() 127 | 128 | ament_package() 129 | -------------------------------------------------------------------------------- /CONTRIBUTE.md: -------------------------------------------------------------------------------- 1 | # Contributing 2 | 3 | When contributing to this repository, please first discuss the change you wish to make via issue, 4 | email, or any other method with the owners of this repository before making a change. 5 | 6 | Please note we have a code of conduct, please follow it in all your interactions with the project. 7 | 8 | ## Pull Request Process 9 | 10 | 1. Ensure any install or build dependencies are removed before the end of the layer when doing a 11 | build. 12 | 2. Update the README.md with details of changes to the interface, this includes new environment 13 | variables, exposed ports, useful file locations and container parameters. 14 | 3. Increase the version numbers in any examples files and the README.md to the new version that this 15 | Pull Request would represent. The versioning scheme we use is [SemVer](http://semver.org/). 16 | 4. You may merge the Pull Request in once you have the sign-off of two other developers, or if you 17 | do not have permission to do that, you may request the second reviewer to merge it for you. 18 | 19 | ## Code of Conduct 20 | 21 | ### Our Pledge 22 | 23 | In the interest of fostering an open and welcoming environment, we as 24 | contributors and maintainers pledge to making participation in our project and 25 | our community a harassment-free experience for everyone, regardless of age, body 26 | size, disability, ethnicity, gender identity and expression, level of experience, 27 | nationality, personal appearance, race, religion, or sexual identity and 28 | orientation. 29 | 30 | ### Our Standards 31 | 32 | Examples of behavior that contributes to creating a positive environment 33 | include: 34 | 35 | * Using welcoming and inclusive language 36 | * Being respectful of differing viewpoints and experiences 37 | * Gracefully accepting constructive criticism 38 | * Focusing on what is best for the community 39 | * Showing empathy towards other community members 40 | 41 | Examples of unacceptable behavior by participants include: 42 | 43 | * The use of sexualized language or imagery and unwelcome sexual attention or 44 | advances 45 | * Trolling, insulting/derogatory comments, and personal or political attacks 46 | * Public or private harassment 47 | * Publishing others' private information, such as a physical or electronic 48 | address, without explicit permission 49 | * Other conduct which could reasonably be considered inappropriate in a 50 | professional setting 51 | 52 | ### Our Responsibilities 53 | 54 | Project maintainers are responsible for clarifying the standards of acceptable 55 | behavior and are expected to take appropriate and fair corrective action in 56 | response to any instances of unacceptable behavior. 57 | 58 | Project maintainers have the right and responsibility to remove, edit, or 59 | reject comments, commits, code, wiki edits, issues, and other contributions 60 | that are not aligned to this Code of Conduct, or to ban temporarily or 61 | permanently any contributor for other behaviors that they deem inappropriate, 62 | threatening, offensive, or harmful. 63 | 64 | ### Scope 65 | 66 | This Code of Conduct applies both within project spaces and in public spaces 67 | when an individual is representing the project or its community. Examples of 68 | representing a project or community include using an official project e-mail 69 | (hello@crobotics.tech) address, posting via an official social media account, 70 | or acting as an appointed representative at an online or offline event. 71 | Representation of a project may be further defined and clarified by project maintainers. 72 | 73 | ### Enforcement 74 | 75 | Instances of abusive, harassing, or otherwise unacceptable behavior may be 76 | reported by contacting the project team at filip.zoric@crobotics.tech. All 77 | complaints will be reviewed and investigated and will result in a response that 78 | is deemed necessary and appropriate to the circumstances. The project team is 79 | obligated to maintain confidentiality with regard to the reporter of an incident. 80 | Further details of specific enforcement policies may be posted separately. 81 | 82 | Project maintainers who do not follow or enforce the Code of Conduct in good 83 | faith may face temporary or permanent repercussions as determined by other 84 | members of the project's leadership. 85 | 86 | ### Attribution 87 | 88 | This Code of Conduct is adapted from the [Contributor Covenant][homepage], version 1.4, 89 | available at [http://contributor-covenant.org/version/1/4][version] 90 | 91 | [homepage]: http://contributor-covenant.org 92 | [version]: http://contributor-covenant.org/version/1/4/ 93 | 94 | -------------------------------------------------------------------------------- /config/piper/piper_servo_sim.yaml: -------------------------------------------------------------------------------- 1 | ############################################### 2 | # Modify all parameters related to servoing here 3 | ############################################### 4 | 5 | # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) 6 | # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] 7 | 8 | ## Properties of outgoing commands 9 | publish_period: 0.05 # 1/Nominal publish rate [seconds] 10 | low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) 11 | 12 | command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s 13 | scale: 14 | # Scale parameters are only used if command_in_type=="unitless" 15 | linear: 1.0 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. 16 | rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. 17 | # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. 18 | joint: 0.5 19 | 20 | # What type of topic does your robot driver expect? 21 | # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory 22 | command_out_type: trajectory_msgs/JointTrajectory 23 | 24 | # What to publish? Can save some bandwidth as most robots only require positions or velocities 25 | publish_joint_positions: true 26 | publish_joint_velocities: true 27 | publish_joint_accelerations: true 28 | 29 | ## Plugins for smoothing outgoing commands 30 | use_smoothing: false 31 | smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" 32 | 33 | # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, 34 | # which other nodes can use as a source for information about the planning environment. 35 | # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), 36 | # then is_primary_planning_scene_monitor needs to be set to false. 37 | is_primary_planning_scene_monitor: false 38 | check_octomap_collisions: false # Check collision against the octomap (if a 3D sensor plugin is available) 39 | 40 | ## MoveIt properties 41 | move_group_name: arm # Often 'manipulator' or 'arm' 42 | planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world' 43 | 44 | ## Other frames 45 | ee_frame_name: link6 # The name of the end effector link, used to return the EE pose 46 | robot_link_command_frame: link6 # commands must be given in the frame of a robot link. Usually either the base or end effector 47 | 48 | ## Stopping behaviour 49 | incoming_command_timeout: 0.25 # Stop servoing if X seconds elapse without a new command 50 | # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. 51 | # Important because ROS may drop some messages and we need the robot to halt reliably. 52 | num_outgoing_halt_msgs_to_publish: 1 53 | 54 | ## Configure handling of singularities and joint limits 55 | lower_singularity_threshold: 50.0 # Start decelerating when the condition number hits this (close to singularity) 56 | hard_stop_singularity_threshold: 100.0 # Stop when the condition number hists this 57 | joint_limit_margin: 0.005 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. 58 | 59 | ## Topic names 60 | cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands 61 | joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands 62 | joint_topic: /joint_states_single # Get joint states from this tpoic 63 | status_topic: ~/servo/status # Publish status to this topic 64 | command_out_topic: /arm_controller/joint_trajectory # Publish outgoing commands here 65 | 66 | ## Collision checking for the entire robot body 67 | check_collisions: true # Check collisions? 68 | collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. 69 | # Two collision check algorithms are available: 70 | # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. 71 | # "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits 72 | collision_check_type: threshold_distance 73 | # Parameters for "threshold_distance"-type collision checking 74 | self_collision_proximity_threshold: 0.002 # Start decelerating when a self-collision is this far [m] 75 | scene_collision_proximity_threshold: 0.005 # Start decelerating when a scene collision is this far [m] 76 | # Parameters for "stop_distance"-type collision checking 77 | collision_distance_safety_factor: 1.0 # Must be >= 1. A large safety factor is recommended to account for latency 78 | min_allowable_collision_distance: 0.002 # Stop if a collision is closer than this [m] 79 | -------------------------------------------------------------------------------- /config/abb/abb_servo_sim.yaml: -------------------------------------------------------------------------------- 1 | ############################################### 2 | # Modify all parameters related to servoing here 3 | ############################################### 4 | 5 | # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) 6 | # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] 7 | 8 | ## Properties of outgoing commands 9 | publish_period: 0.05 # 1/Nominal publish rate [seconds] 10 | low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) 11 | 12 | command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s 13 | scale: 14 | # Scale parameters are only used if command_in_type=="unitless" 15 | linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. 16 | rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. 17 | # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. 18 | joint: 0.5 19 | 20 | # What type of topic does your robot driver expect? 21 | # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory 22 | command_out_type: trajectory_msgs/JointTrajectory 23 | 24 | # What to publish? Can save some bandwidth as most robots only require positions or velocities 25 | publish_joint_positions: true 26 | publish_joint_velocities: true 27 | publish_joint_accelerations: false 28 | 29 | ## Plugins for smoothing outgoing commands 30 | use_smoothing: true 31 | smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" 32 | 33 | # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, 34 | # which other nodes can use as a source for information about the planning environment. 35 | # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), 36 | # then is_primary_planning_scene_monitor needs to be set to false. 37 | is_primary_planning_scene_monitor: false 38 | check_octomap_collisions: false # Check collision against the octomap (if a 3D sensor plugin is available) 39 | 40 | ## MoveIt properties 41 | move_group_name: manipulator # Often 'manipulator' or 'arm' 42 | planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world' 43 | 44 | ## Other frames 45 | ee_frame_name: tool0 # The name of the end effector link, used to return the EE pose 46 | robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector 47 | 48 | ## Stopping behaviour 49 | incoming_command_timeout: 0.25 # Stop servoing if X seconds elapse without a new command 50 | # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. 51 | # Important because ROS may drop some messages and we need the robot to halt reliably. 52 | num_outgoing_halt_msgs_to_publish: 1 53 | 54 | ## Configure handling of singularities and joint limits 55 | lower_singularity_threshold: 20.0 # Start decelerating when the condition number hits this (close to singularity) 56 | hard_stop_singularity_threshold: 50.0 # Stop when the condition number hits this 57 | joint_limit_margin: 0.087267 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. 58 | 59 | ## Topic names 60 | cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands 61 | joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands 62 | joint_topic: /joint_states # Get joint states from this tpoic 63 | status_topic: ~/status # Publish status to this topic 64 | command_out_topic: /joint_trajectory_controller/joint_trajectory # Publish outgoing commands here 65 | 66 | ## Collision checking for the entire robot body 67 | check_collisions: true # Check collisions? 68 | collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. 69 | # Two collision check algorithms are available: 70 | # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. 71 | # "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits 72 | collision_check_type: threshold_distance 73 | # Parameters for "threshold_distance"-type collision checking 74 | self_collision_proximity_threshold: 0.03 # Start decelerating when a self-collision is this far [m] 75 | scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m] 76 | # Parameters for "stop_distance"-type collision checking 77 | collision_distance_safety_factor: 10.0 # Must be >= 1. A large safety factor is recommended to account for latency 78 | min_allowable_collision_distance: 0.025 # Stop if a collision is closer than this [m] 79 | -------------------------------------------------------------------------------- /config/franka/franka_servo_sim.yaml: -------------------------------------------------------------------------------- 1 | ############################################### 2 | # Modify all parameters related to servoing here 3 | ############################################### 4 | 5 | # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) 6 | # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] 7 | 8 | ## Properties of outgoing commands 9 | publish_period: 0.05 # 1/Nominal publish rate [seconds] 10 | low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) 11 | 12 | command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s 13 | scale: 14 | # Scale parameters are only used if command_in_type=="unitless" 15 | linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. 16 | rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. 17 | # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. 18 | joint: 0.5 19 | 20 | # What type of topic does your robot driver expect? 21 | # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory 22 | command_out_type: trajectory_msgs/JointTrajectory 23 | 24 | # What to publish? Can save some bandwidth as most robots only require positions or velocities 25 | publish_joint_positions: true 26 | publish_joint_velocities: true 27 | publish_joint_accelerations: false 28 | 29 | ## Plugins for smoothing outgoing commands 30 | use_smoothing: true 31 | smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" 32 | 33 | # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, 34 | # which other nodes can use as a source for information about the planning environment. 35 | # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), 36 | # then is_primary_planning_scene_monitor needs to be set to false. 37 | is_primary_planning_scene_monitor: false 38 | check_octomap_collisions: false # Check collision against the octomap (if a 3D sensor plugin is available) 39 | 40 | ## MoveIt properties 41 | move_group_name: arm # Often 'manipulator' or 'arm' 42 | planning_frame: panda_link0 # The MoveIt planning frame. Often 'base_link' or 'world' 43 | 44 | ## Other frames 45 | ee_frame_name: panda_hand_tcp # The name of the end effector link, used to return the EE pose 46 | robot_link_command_frame: panda_link0 # commands must be given in the frame of a robot link. Usually either the base or end effector 47 | 48 | ## Stopping behaviour 49 | incoming_command_timeout: 0.25 # Stop servoing if X seconds elapse without a new command 50 | # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. 51 | # Important because ROS may drop some messages and we need the robot to halt reliably. 52 | num_outgoing_halt_msgs_to_publish: 1 53 | 54 | ## Configure handling of singularities and joint limits 55 | lower_singularity_threshold: 20.0 # Start decelerating when the condition number hits this (close to singularity) 56 | hard_stop_singularity_threshold: 50.0 # Stop when the condition number hits this 57 | joint_limit_margin: 0.087267 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. 58 | 59 | ## Topic names 60 | cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands 61 | joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands 62 | joint_topic: /joint_states # Get joint states from this tpoic 63 | status_topic: ~/status # Publish status to this topic 64 | command_out_topic: /joint_trajectory_controller/joint_trajectory # Publish outgoing commands here 65 | 66 | ## Collision checking for the entire robot body 67 | check_collisions: true # Check collisions? 68 | collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. 69 | # Two collision check algorithms are available: 70 | # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. 71 | # "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits 72 | collision_check_type: threshold_distance 73 | # Parameters for "threshold_distance"-type collision checking 74 | self_collision_proximity_threshold: 0.03 # Start decelerating when a self-collision is this far [m] 75 | scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m] 76 | # Parameters for "stop_distance"-type collision checking 77 | collision_distance_safety_factor: 10.0 # Must be >= 1. A large safety factor is recommended to account for latency 78 | min_allowable_collision_distance: 0.025 # Stop if a collision is closer than this [m] -------------------------------------------------------------------------------- /config/kinova/kinova_servo_sim.yaml: -------------------------------------------------------------------------------- 1 | ############################################### 2 | # Modify all parameters related to servoing here 3 | ############################################### 4 | 5 | # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) 6 | # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] 7 | 8 | ## Properties of outgoing commands 9 | publish_period: 0.05 # 1/Nominal publish rate [seconds] 10 | low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) 11 | 12 | command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s 13 | scale: 14 | # Scale parameters are only used if command_in_type=="unitless" 15 | linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. 16 | rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. 17 | # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. 18 | joint: 0.5 19 | 20 | # What type of topic does your robot driver expect? 21 | # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory 22 | command_out_type: trajectory_msgs/JointTrajectory 23 | 24 | # What to publish? Can save some bandwidth as most robots only require positions or velocities 25 | publish_joint_positions: true 26 | publish_joint_velocities: true 27 | publish_joint_accelerations: false 28 | 29 | ## Plugins for smoothing outgoing commands 30 | use_smoothing: true 31 | smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" 32 | 33 | # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, 34 | # which other nodes can use as a source for information about the planning environment. 35 | # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), 36 | # then is_primary_planning_scene_monitor needs to be set to false. 37 | is_primary_planning_scene_monitor: false 38 | check_octomap_collisions: false # Check collision against the octomap (if a 3D sensor plugin is available) 39 | 40 | ## MoveIt properties 41 | move_group_name: manipulator # Often 'manipulator' or 'arm' 42 | planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world' 43 | 44 | ## Other frames 45 | ee_frame_name: end_effector_link # The name of the end effector link, used to return the EE pose 46 | robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector 47 | 48 | ## Stopping behaviour 49 | incoming_command_timeout: 0.25 # Stop servoing if X seconds elapse without a new command 50 | # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. 51 | # Important because ROS may drop some messages and we need the robot to halt reliably. 52 | num_outgoing_halt_msgs_to_publish: 1 53 | 54 | ## Configure handling of singularities and joint limits 55 | lower_singularity_threshold: 20.0 # Start decelerating when the condition number hits this (close to singularity) 56 | hard_stop_singularity_threshold: 50.0 # Stop when the condition number hits this 57 | joint_limit_margin: 0.087267 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. 58 | 59 | ## Topic names 60 | cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands 61 | joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands 62 | joint_topic: /joint_states # Get joint states from this tpoic 63 | status_topic: ~/status # Publish status to this topic 64 | command_out_topic: /joint_trajectory_controller/joint_trajectory # Publish outgoing commands here 65 | 66 | ## Collision checking for the entire robot body 67 | check_collisions: true # Check collisions? 68 | collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. 69 | # Two collision check algorithms are available: 70 | # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. 71 | # "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits 72 | collision_check_type: threshold_distance 73 | # Parameters for "threshold_distance"-type collision checking 74 | self_collision_proximity_threshold: 0.03 # Start decelerating when a self-collision is this far [m] 75 | scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m] 76 | # Parameters for "stop_distance"-type collision checking 77 | collision_distance_safety_factor: 10.0 # Must be >= 1. A large safety factor is recommended to account for latency 78 | min_allowable_collision_distance: 0.025 # Stop if a collision is closer than this [m] -------------------------------------------------------------------------------- /config/big_cell/big_cell_servo_sim.yaml: -------------------------------------------------------------------------------- 1 | ############################################### 2 | # Modify all parameters related to servoing here 3 | ############################################### 4 | 5 | # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) 6 | # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] 7 | 8 | ## Properties of outgoing commands 9 | publish_period: 0.005 # 1/Nominal publish rate [seconds] 10 | low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) 11 | 12 | command_in_type: speed_units # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s 13 | scale: 14 | # Scale parameters are only used if command_in_type=="unitless" 15 | linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. 16 | rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. 17 | # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. 18 | joint: 0.5 19 | 20 | # What type of topic does your robot driver expect? 21 | # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory 22 | command_out_type: std_msgs/Float64MultiArray 23 | 24 | # What to publish? Can save some bandwidth as most robots only require positions or velocities 25 | publish_joint_positions: true 26 | publish_joint_velocities: false 27 | publish_joint_accelerations: false 28 | 29 | ## Plugins for smoothing outgoing commands 30 | use_smoothing: true 31 | smoothing_filter_plugin_name: online_signal_smoothing::ButterworthFilterPlugin 32 | 33 | # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, 34 | # which other nodes can use as a source for information about the planning environment. 35 | # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), 36 | # then is_primary_planning_scene_monitor needs to be set to false. 37 | is_primary_planning_scene_monitor: false 38 | check_octomap_collisions: false # Check collision against the octomap (if a 3D sensor plugin is available) 39 | 40 | ## MoveIt properties 41 | move_group_name: manipulator # Often 'manipulator' or 'arm' 42 | planning_frame: world # The MoveIt planning frame. Often 'base_link' or 'world' 43 | 44 | ## Other frames 45 | ee_frame_name: tcp # The name of the end effector link, used to return the EE pose 46 | robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector 47 | 48 | ## Stopping behaviour 49 | incoming_command_timeout: 0.25 # Stop servoing if X seconds elapse without a new command 50 | # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. 51 | # Important because ROS may drop some messages and we need the robot to halt reliably. 52 | num_outgoing_halt_msgs_to_publish: 10 53 | 54 | ## Configure handling of singularities and joint limits 55 | lower_singularity_threshold: 20.0 # Start decelerating when the condition number hits this (close to singularity) 56 | hard_stop_singularity_threshold: 50.0 # Stop when the condition number hits this 57 | joint_limit_margin: 0.087267 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. 58 | 59 | ## Topic names 60 | cartesian_command_in_topic: /moveit2_iface_node/delta_twist_cmds # Topic for incoming Cartesian twist commands 61 | joint_command_in_topic: /moveit2_iface_node/delta_joint_cmds # Topic for incoming joint angle commands 62 | joint_topic: /joint_states # Get joint states from this tpoic 63 | status_topic: ~/status # Publish status to this topic 64 | command_out_topic: /forward_position_controller/commands # Publish outgoing commands here 65 | 66 | ## Collision checking for the entire robot body 67 | check_collisions: true # Check collisions? 68 | collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. 69 | # Two collision check algorithms are available: 70 | # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. 71 | # "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits 72 | collision_check_type: threshold_distance 73 | # Parameters for "threshold_distance"-type collision checking 74 | self_collision_proximity_threshold: 0.03 # Start decelerating when a self-collision is this far [m] 75 | scene_collision_proximity_threshold: 0.15 # Start decelerating when a scene collision is this far [m] 76 | # Parameters for "stop_distance"-type collision checking 77 | collision_distance_safety_factor: 10.0 # Must be >= 1. A large safety factor is recommended to account for latency 78 | min_allowable_collision_distance: 0.025 # Stop if a collision is closer than this [m] 79 | -------------------------------------------------------------------------------- /config/ur/ur_servo_sim.yaml: -------------------------------------------------------------------------------- 1 | ############################################### 2 | # Modify all parameters related to servoing here 3 | ############################################### 4 | 5 | # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) 6 | # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] 7 | 8 | ## Properties of outgoing commands 9 | publish_period: 0.005 # 1/Nominal publish rate [seconds] 10 | low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) 11 | 12 | command_in_type: speed_units # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s 13 | scale: 14 | # Scale parameters are only used if command_in_type=="unitless" 15 | linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. 16 | rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. 17 | # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. 18 | joint: 0.5 19 | 20 | # What type of topic does your robot driver expect? 21 | # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory 22 | command_out_type: std_msgs/Float64MultiArray 23 | 24 | # What to publish? Can save some bandwidth as most robots only require positions or velocities 25 | publish_joint_positions: true 26 | publish_joint_velocities: false 27 | publish_joint_accelerations: false 28 | 29 | ## Plugins for smoothing outgoing commands 30 | use_smoothing: true # Enables the use of smoothing plugins for joint trajectory smoothing 31 | smoothing_filter_plugin_name: online_signal_smoothing::ButterworthFilterPlugin 32 | 33 | # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, 34 | # which other nodes can use as a source for information about the planning environment. 35 | # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), 36 | # then is_primary_planning_scene_monitor needs to be set to false. 37 | is_primary_planning_scene_monitor: false 38 | check_octomap_collisions: false # Check collision against the octomap (if a 3D sensor plugin is available) 39 | 40 | ## MoveIt properties 41 | move_group_name: ur_manipulator # Often 'manipulator' or 'arm' 42 | planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world' 43 | 44 | ## Other frames 45 | ee_frame_name: tool0 # The name of the end effector link, used to return the EE pose 46 | robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector 47 | 48 | ## Stopping behaviour 49 | incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command 50 | # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. 51 | # Important because ROS may drop some messages and we need the robot to halt reliably. 52 | num_outgoing_halt_msgs_to_publish: 4 53 | 54 | ## Configure handling of singularities and joint limits 55 | lower_singularity_threshold: 20.0 # Start decelerating when the condition number hits this (close to singularity) 56 | hard_stop_singularity_threshold: 50.0 # Stop when the condition number hits this 57 | leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity 58 | joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. 59 | 60 | ## Topic names 61 | cartesian_command_in_topic: /moveit2_iface_node/delta_twist_cmds # Topic for incoming Cartesian twist commands 62 | joint_command_in_topic: /moveit2_iface_node/delta_joint_cmds # Topic for incoming joint angle commands 63 | joint_topic: /joint_states # Get joint states from this tpoic 64 | status_topic: ~/status # Publish status to this topic 65 | command_out_topic: /forward_position_controller/commands # Publish outgoing commands here 66 | 67 | ## Collision checking for the entire robot body 68 | check_collisions: true # Check collisions? 69 | collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. 70 | # Two collision check algorithms are available: 71 | # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. 72 | # "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits 73 | collision_check_type: threshold_distance 74 | # Parameters for "threshold_distance"-type collision checking 75 | self_collision_proximity_threshold: 0.03 # Start decelerating when a self-collision is this far [m] 76 | scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m] 77 | # Parameters for "stop_distance"-type collision checking 78 | collision_distance_safety_factor: 10.0 # Must be >= 1. A large safety factor is recommended to account for latency 79 | min_allowable_collision_distance: 0.025 # Stop if a collision is closer than this [m] 80 | -------------------------------------------------------------------------------- /arm_api2/scripts/trajectory_sender.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rclpy 4 | import csv 5 | import numpy as np 6 | import rclpy.duration 7 | from rclpy.node import Node 8 | from geometry_msgs.msg import PoseStamped, Pose 9 | from std_msgs.msg import Bool 10 | from scipy.spatial.transform import Rotation as R 11 | from arm_api2_msgs.msg import CartesianWaypoints 12 | 13 | # NOTE: Requirement transformations 14 | # Used: transformations-2024.5.24 15 | # installed with: pip install transformations 16 | 17 | class CreateAndPublishTrajectory(Node): 18 | 19 | def __init__(self): 20 | super().__init__('create_publish_trajectory') 21 | 22 | # Create subscribers 23 | self.curr_p_sub = self.create_subscription(PoseStamped, "/arm/state/current_pose", self.curr_p_cb, 1) 24 | self.trigger_sub = self.create_subscription(Bool, "/traj_trigger", self.trig_cb, 1) 25 | 26 | # Create publishers 27 | self.traj_pub = self.create_publisher(CartesianWaypoints, '/arm/cmd/traj', 1) 28 | 29 | # Create timer 30 | timer_period = 1.0 # seconds 31 | self.timer = self.create_timer(timer_period, self.run) 32 | 33 | # TODO: Add absolute path to the CSV files 34 | c_csv_pth = "/root/arm_ws/src/arm_api2/utils/CS_C.csv" 35 | s_csv_pth = "/root/arm_ws/src/arm_api2/utils/CS_S.csv" 36 | 37 | self.get_logger().info(f"C trajectory path is: {c_csv_pth}") 38 | self.get_logger().info(f"S trajectory path is: {s_csv_pth}") 39 | # Load positions from YAML file 40 | 41 | self.c_data = self.load_positions(c_csv_pth) 42 | self.s_data = self.load_positions(s_csv_pth) 43 | 44 | # Flags 45 | self.reciv_trig = False 46 | self.reciv_p = False 47 | self.i = 0 48 | 49 | def load_positions(self, csv_pth): 50 | p_data = [] 51 | with open(csv_pth, 'r') as file: 52 | reader = csv.DictReader(file) 53 | # TODO: Use this 54 | for row in reader: 55 | x = row['x']; y = row['y']; z = row['z'] 56 | p_data.append(np.array([float(x), float(y), float(z), 1]).T) 57 | self.get_logger().info("Loaded positions.") 58 | return p_data 59 | 60 | def trig_cb(self, msg): 61 | self.reciv_trig = True 62 | self.i+=1 63 | 64 | def curr_p_cb(self, msg): 65 | self.reciv_p = True 66 | self.curr_p = PoseStamped() 67 | self.curr_p.header = msg.header 68 | self.curr_p.pose.position = msg.pose.position 69 | self.curr_p.pose.orientation = msg.pose.orientation 70 | p, q = msg.pose.position, msg.pose.orientation 71 | x, y, z = p.x, p.y, p.z 72 | qx, qy, qz, qw = q.x, q.y, q.z, q.w 73 | #self.get_logger().info(f"x: {x}\t {y}\t {z}\t") 74 | # x, y, z, w 75 | R_ = R.from_quat([qx, qy, qz, qw]) 76 | r = R_.as_matrix() 77 | p_ = np.array([x, y, z]).T.reshape(3, 1) 78 | self.get_logger().debug(f"R shape is: {r.shape}") 79 | self.get_logger().debug(f"p shape is: {p_.shape}") 80 | T_ = np.hstack((r, p_)) 81 | self.T = np.vstack((T_, np.array([0, 0, 0, 1]))) 82 | self.get_logger().debug(f"Reciv T matrix is: {self.T}") 83 | 84 | def create_trajectory(self, letter): 85 | self.get_logger().info("Create trajectory!") 86 | 87 | if letter == 'S': data = self.s_data 88 | if letter == 'C': data = self.c_data 89 | ct = CartesianWaypoints() 90 | # TODO: remove c_data as hardcoding 91 | for i, p in enumerate(data): 92 | dt = 0.05 93 | self.get_logger().info(f"p is: {p}") 94 | self.get_logger().info(f"R is: {self.T}") 95 | p_ = np.matmul(self.T, p) 96 | r_ = R.from_matrix(self.T[:3, :3]); quat = r_.as_quat() 97 | rosp = Pose() 98 | rosp.position.x = p_[0]; rosp.position.y = p_[1]; rosp.position.z = p_[2] 99 | rosp.orientation.x = quat[0]; rosp.orientation.y = quat[1]; rosp.orientation.z = quat[2]; rosp.orientation.w = quat[3] 100 | ct.poses.append(rosp) 101 | return ct 102 | 103 | def publish_trajectory(self, msg): 104 | self.traj_pub.publish(msg) 105 | self.get_logger().info("Publish trajectory!") 106 | 107 | def run(self): 108 | if self.reciv_trig and self.reciv_p: 109 | self.get_logger().info("Sending trajectory!") 110 | if self.i == 0: 111 | traj = self.create_trajectory('C') 112 | if self.i == 1: 113 | traj = self.create_trajectory('S') 114 | self.publish_trajectory(traj) 115 | self.reciv_trig = False 116 | self.i = 0 117 | 118 | else: 119 | if self.reciv_p: 120 | self.get_logger().info("Recieved pose!") 121 | if self.reciv_trig: 122 | self.get_logger().info("Recieved trigger!") 123 | 124 | self.get_logger().info("Waiting for the trigger to publish trajectory!") 125 | 126 | def main(args=None): 127 | rclpy.init(args=args) 128 | CPT = CreateAndPublishTrajectory() 129 | rclpy.spin(CPT) 130 | CPT.destroy_node() 131 | rclpy.shutdown() 132 | 133 | if __name__ == '__main__': 134 | main() 135 | -------------------------------------------------------------------------------- /src/arm_joy.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2024, Crobotic Solutions d.o.o. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | /* Title : arm_joy.cpp 35 | * Project : arm_api2 36 | * Created : 05/10/2024 37 | * Author : Filip Zoric 38 | * 39 | * Description : Joystick control code. 40 | */ 41 | 42 | #include "arm_api2/arm_joy.hpp" 43 | #define X_I 4 44 | #define Y_I 3 45 | #define YAW_I 6 46 | #define Z_I 7 47 | 48 | // TODO: Add config file to modify this values based on the joystick/robot used 49 | // TODO: Enable joint space and task space control 50 | 51 | JoyCtl::JoyCtl(): Node("joy_ctl") 52 | { 53 | init(); 54 | 55 | setScaleFactor(1); 56 | this->set_parameter(rclcpp::Parameter("use_sim_time", false)); 57 | 58 | enableJoy_ = true; 59 | 60 | 61 | } 62 | 63 | void JoyCtl::init() 64 | { 65 | 66 | // publishers 67 | cmdVelPub_ = this->create_publisher("/moveit2_iface_node/delta_twist_cmds", 1); 68 | 69 | // subscribers 70 | joySub_ = this->create_subscription("/joy", 10, std::bind(&JoyCtl::joy_callback, this, _1)); 71 | 72 | RCLCPP_INFO(this->get_logger(), "Initialized joy_ctl"); 73 | } 74 | 75 | void JoyCtl::joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg) 76 | { 77 | float x_dir, y_dir, z_dir, yaw; 78 | std::vector axes_ = msg->axes; 79 | 80 | x_dir = axes_.at(X_I); 81 | y_dir = axes_.at(Y_I); 82 | z_dir = axes_.at(Z_I); 83 | yaw = axes_.at(YAW_I); 84 | 85 | // Enabling joystick functionality 86 | // R2 pressed --> joy on 87 | int LOG_JOY_STATE_T = 5000; 88 | if (msg->buttons.at(5) == 1) 89 | { 90 | RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), clock_, LOG_JOY_STATE_T, "ON"); 91 | setEnableJoy(true); 92 | } 93 | 94 | // R2 released --> joy off 95 | if (msg->buttons.at(5) == 0) 96 | { 97 | 98 | RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), clock_, LOG_JOY_STATE_T, "OFF"); 99 | setEnableJoy(false); 100 | } 101 | 102 | enableJoy_ = getEnableJoy(); 103 | 104 | float sF = getScaleFactor(); 105 | // https://www.quantstart.com/articles/Passing-By-Reference-To-Const-in-C/ 106 | if (msg->axes.at(1) == 1){ 107 | 108 | if (sF > 0 && sF < 100) 109 | { 110 | sF += 1; 111 | RCLCPP_INFO_STREAM(this->get_logger(), "Increasing scale factor: " << sF); 112 | } 113 | else{sF = 1;} 114 | } 115 | 116 | if (msg->axes.at(1) == -1){ 117 | if (sF > 0 && sF < 100) 118 | { 119 | sF -= 1; 120 | RCLCPP_INFO_STREAM(this->get_logger(), "Decreasing scale factor: " << sF); 121 | } 122 | else{sF = 1;} 123 | } 124 | 125 | /*if (msg->buttons.at(4) == 1) { 126 | RCLCPP_INFO_STREAM(this->get_logger(), "Calling jingle bells!"); 127 | auto req_ = std::make_shared(); 128 | jingleBellsClient_->async_send_request(req_); 129 | }*/ 130 | // Test scale fact (ADD C const to prevent large cmd) 131 | setScaleFactor(sF); 132 | 133 | 134 | // Create teleop msg 135 | auto teleop_msg = geometry_msgs::msg::TwistStamped(); 136 | teleop_msg.header.stamp = this->get_clock()->now(); 137 | teleop_msg.header.frame_id = "link6"; 138 | 139 | float C = 0.01; 140 | if (enableJoy_){ 141 | // Currently modified for the PIPER 142 | teleop_msg.twist.linear.z = x_dir * sF * C; 143 | teleop_msg.twist.linear.y = y_dir * sF * C; 144 | teleop_msg.twist.linear.x = - z_dir * sF * C; 145 | teleop_msg.twist.angular.z = yaw * sF * C; 146 | cmdVelPub_->publish(teleop_msg); 147 | } 148 | else{ 149 | teleop_msg.twist.linear.x = 0; 150 | teleop_msg.twist.linear.y = 0; 151 | teleop_msg.twist.linear.z = 0; 152 | teleop_msg.twist.angular.z = 0; 153 | cmdVelPub_->publish(teleop_msg); 154 | } 155 | 156 | 157 | } 158 | 159 | // Methods that set scale factor 160 | void JoyCtl::setScaleFactor(int value) 161 | { 162 | scale_factor = value; 163 | } 164 | 165 | int JoyCtl::getScaleFactor() const 166 | { 167 | return scale_factor; 168 | } 169 | 170 | void JoyCtl::setEnableJoy(bool val) 171 | { 172 | enableJoy_ = val; 173 | } 174 | 175 | bool JoyCtl::getEnableJoy() const 176 | { 177 | return enableJoy_; 178 | } -------------------------------------------------------------------------------- /examples/scripts/trajectory_sender_action_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import csv 4 | import threading 5 | 6 | import numpy as np 7 | import rclpy 8 | import rclpy.duration 9 | from ament_index_python.packages import get_package_share_directory 10 | from arm_api2_msgs.action import MoveCartesianPath 11 | from geometry_msgs.msg import PoseStamped 12 | from rclpy.action import ActionClient 13 | from rclpy.executors import MultiThreadedExecutor 14 | from rclpy.node import Node 15 | from scipy.spatial.transform import Rotation as R 16 | 17 | 18 | class CreateAndPublishTrajectory(Node): 19 | 20 | def __init__(self): 21 | super().__init__("create_publish_trajectory") 22 | 23 | # Create subscribers 24 | self.curr_p_sub = self.create_subscription( 25 | PoseStamped, "/arm/state/current_pose", self.curr_p_cb, 1 26 | ) 27 | # Create timer 28 | timer_period = 1.0 # seconds 29 | self.timer = self.create_timer(timer_period, self.run) 30 | 31 | 32 | # Create action client 33 | self._action_client = ActionClient( 34 | self, MoveCartesianPath, "arm/move_to_pose_path" 35 | ) 36 | 37 | c_csv_pth = get_package_share_directory('arm_api2') + "/utils/CS_C_easy.csv" 38 | s_csv_pth = get_package_share_directory('arm_api2') + "/utils/CS_S_easy.csv" 39 | 40 | 41 | self.get_logger().info(f"C trajectory path is: {c_csv_pth}") 42 | self.get_logger().info(f"S trajectory path is: {s_csv_pth}") 43 | # Load positions from YAML file 44 | 45 | self.c_data = self.load_positions(c_csv_pth) 46 | self.s_data = self.load_positions(s_csv_pth) 47 | 48 | # Flags 49 | self.receive_pose_flag = threading.Event() 50 | 51 | # Create a separate thread to handle user input 52 | self.user_input_thread = threading.Thread(target=self.handle_user_input) 53 | self.user_input_thread.start() 54 | 55 | def send_goal(self, goal_path): 56 | goal_msg = MoveCartesianPath.Goal() 57 | goal_msg.poses = goal_path 58 | 59 | self.get_logger().info("Waiting for action server...") 60 | 61 | self._action_client.wait_for_server() 62 | 63 | self.get_logger().info("Sending goal request...") 64 | 65 | response = self._action_client.send_goal(goal_msg) 66 | 67 | print(response.result) 68 | 69 | if response.result.success: 70 | self.get_logger().info("Result: Trajectory executed successfully!") 71 | else: 72 | self.get_logger().info("Result: Trajectory execution failed!") 73 | 74 | def load_positions(self, csv_pth): 75 | p_data = [] 76 | with open(csv_pth, "r") as file: 77 | reader = csv.DictReader(file) 78 | for row in reader: 79 | x = row["x"] 80 | y = row["y"] 81 | z = row["z"] 82 | p_data.append(np.array([float(x), float(y), float(z), 1]).T) 83 | self.get_logger().info("Loaded positions.") 84 | return p_data 85 | 86 | def curr_p_cb(self, msg): 87 | self.curr_p = PoseStamped() 88 | self.curr_p.header = msg.header 89 | self.curr_p.pose.position = msg.pose.position 90 | self.curr_p.pose.orientation = msg.pose.orientation 91 | p, q = msg.pose.position, msg.pose.orientation 92 | x, y, z = p.x, p.y, p.z 93 | qx, qy, qz, qw = q.x, q.y, q.z, q.w 94 | # self.get_logger().info(f"x: {x}\t {y}\t {z}\t") 95 | # x, y, z, w 96 | R_ = R.from_quat([qx, qy, qz, qw]) 97 | r = R_.as_matrix() 98 | p_ = np.array([x, y, z]).T.reshape(3, 1) 99 | self.get_logger().debug(f"R shape is: {r.shape}") 100 | self.get_logger().debug(f"p shape is: {p_.shape}") 101 | T_ = np.hstack((r, p_)) 102 | self.T = np.vstack((T_, np.array([0, 0, 0, 1]))) 103 | self.get_logger().debug(f"Reciv T matrix is: {self.T}") 104 | self.receive_pose_flag.set() 105 | 106 | def create_trajectory(self, letter): 107 | self.get_logger().info("Create trajectory!") 108 | 109 | if letter == "S": 110 | data = self.s_data 111 | if letter == "C": 112 | data = self.c_data 113 | path = [] 114 | for p in data: 115 | self.get_logger().info(f"p is: {p}") 116 | self.get_logger().info(f"T is: {self.T}") 117 | p_ = np.matmul(self.T, p) 118 | self.get_logger().info(f"p_ is: {p_}") 119 | r_ = R.from_matrix(self.T[:3, :3]) 120 | quat = r_.as_quat() 121 | rosp = PoseStamped() 122 | rosp.header.frame_id = self.curr_p.header.frame_id 123 | rosp.pose.position.x = p_[0] 124 | rosp.pose.position.y = p_[1] 125 | rosp.pose.position.z = p_[2] 126 | rosp.pose.orientation.x = quat[0] 127 | rosp.pose.orientation.y = quat[1] 128 | rosp.pose.orientation.z = quat[2] 129 | rosp.pose.orientation.w = quat[3] 130 | path.append(rosp) 131 | return path 132 | 133 | def handle_user_input(self): 134 | while rclpy.ok(): 135 | 136 | while not self.receive_pose_flag.is_set(): 137 | pass 138 | 139 | user_input = input( 140 | "Enter 's' for S trajectory, 'c' for C trajectory, enter 'q' to quit: " 141 | ) 142 | 143 | if user_input.lower() == "s": 144 | self.get_logger().info("Starting S trajectory...") 145 | traj = self.create_trajectory("S") 146 | self.send_goal(traj) 147 | 148 | elif user_input.lower() == "c": 149 | self.get_logger().info("Starting C trajectory...") 150 | traj = self.create_trajectory("C") 151 | self.send_goal(traj) 152 | 153 | if user_input.lower() == "q": 154 | self.get_logger().info("Shutting down.") 155 | rclpy.shutdown() 156 | break 157 | 158 | 159 | def main(args=None): 160 | rclpy.init(args=args) 161 | CPT = CreateAndPublishTrajectory() 162 | 163 | executor = MultiThreadedExecutor() 164 | executor.add_node(CPT) 165 | 166 | try: 167 | executor.spin() 168 | 169 | finally: 170 | CPT.destroy_node() 171 | 172 | 173 | if __name__ == "__main__": 174 | main() 175 | -------------------------------------------------------------------------------- /examples/scripts/servo_twist_sender.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | 4 | import numpy as np 5 | import transforms3d.quaternions as quat 6 | import copy 7 | 8 | import time 9 | import rclpy 10 | from geometry_msgs.msg import PoseStamped, Pose, TwistStamped, TransformStamped 11 | import math 12 | from rclpy.node import Node 13 | from rclpy.time import Time 14 | from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster 15 | 16 | 17 | 18 | class ServoTwistSender(Node): 19 | def __init__(self): 20 | super().__init__('servo_pose_sender') 21 | self.twist_pub = self.create_publisher(TwistStamped, '/moveit2_iface_node/delta_twist_cmds', 10) 22 | self.pose_sub = self.create_subscription(PoseStamped, '/arm/state/current_pose', self.pose_callback, 10) 23 | self.tf_static_broadcaster = StaticTransformBroadcaster(self) 24 | self.current_pose = None 25 | self.start_pose = None 26 | 27 | timer_period = 0.1 # seconds 28 | self.rate = self.create_timer(timer_period, self.run) 29 | 30 | 31 | def pose_callback(self, msg): 32 | self.current_pose = msg.pose 33 | 34 | def send_transform(self, name, pose): 35 | t = TransformStamped() 36 | 37 | t.header.stamp = self.get_clock().now().to_msg() 38 | t.header.frame_id = 'world' 39 | t.child_frame_id = name 40 | 41 | t.transform.translation.x = pose.position.x 42 | t.transform.translation.y = pose.position.y 43 | t.transform.translation.z = pose.position.z 44 | t.transform.rotation = pose.orientation 45 | 46 | 47 | self.tf_static_broadcaster.sendTransform(t) 48 | 49 | def run(self): 50 | if self.current_pose is None: 51 | return 52 | if self.start_pose is None: 53 | self.start_time = time.time() 54 | self.start_pose = copy.deepcopy(self.current_pose) 55 | 56 | q_delta = [0.9848078, 0, 0, 0.1736482] 57 | q_target = [self.start_pose.orientation.w, self.start_pose.orientation.x, self.start_pose.orientation.y, self.start_pose.orientation.z] 58 | self.get_logger().info('q_target 1: ' + str(q_target)) 59 | q_target = quat.qmult(q_target, q_delta) 60 | self.get_logger().info('q_target 2: ' + str(q_target)) 61 | q_target = quat.qmult(q_target, q_delta) 62 | self.get_logger().info('q_target 3: ' + str(q_target)) 63 | 64 | self.start_pose.orientation.w = q_target[0] 65 | self.start_pose.orientation.x = q_target[1] 66 | self.start_pose.orientation.y = q_target[2] 67 | self.start_pose.orientation.z = q_target[3] 68 | 69 | 70 | else: 71 | elapsed_time = (time.time() - self.start_time) 72 | angle = elapsed_time * 2 * math.pi / 20 # Complete a circle in 20 seconds 73 | offset_x = 0.1 * math.cos(angle) - 0.1 # 10 cm radius 74 | offset_y = 0.1 * math.sin(angle) 75 | 76 | gain = 15.0 77 | 78 | 79 | #self.get_logger().info('Elapsed time: %f, angle: %f, offset_x: %f, offset_y: %f' % (elapsed_time, angle, offset_x, offset_y)) 80 | 81 | target_pose = Pose() 82 | target_pose.position.x = self.start_pose.position.x + offset_x 83 | target_pose.position.y = self.start_pose.position.y + offset_y 84 | target_pose.position.z = self.start_pose.position.z 85 | target_pose.orientation = self.start_pose.orientation 86 | 87 | self.send_transform('target_pose', target_pose) 88 | 89 | 90 | 91 | # create twist from delta between current and target pose 92 | twist = TwistStamped() 93 | twist.header.stamp = Time().to_msg() 94 | twist.header.frame_id = 'world' 95 | twist.twist.linear.x = (target_pose.position.x - self.current_pose.position.x) * gain 96 | twist.twist.linear.y = (target_pose.position.y - self.current_pose.position.y) * gain 97 | twist.twist.linear.z = (target_pose.position.z - self.current_pose.position.z) * gain 98 | 99 | 100 | dt = 0.1 101 | q_current = [self.current_pose.orientation.w, self.current_pose.orientation.x, self.current_pose.orientation.y, self.current_pose.orientation.z] 102 | q_target = [target_pose.orientation.w, target_pose.orientation.x, target_pose.orientation.y, target_pose.orientation.z] 103 | angular_velocity = quaternion_to_angular_velocity(q_current, q_target, dt) 104 | self.get_logger().info('Angular velocity: ' + str(angular_velocity)) 105 | twist.twist.angular.x = angular_velocity[0] 106 | twist.twist.angular.y = angular_velocity[1] 107 | twist.twist.angular.z = angular_velocity[2] 108 | 109 | 110 | self.twist_pub.publish(twist) 111 | #self.get_logger().info('Published twist message') 112 | 113 | 114 | def quaternion_to_angular_velocity(q_current, q_target, dt): 115 | """ 116 | Computes the required angular velocity in global frame to rotate from q_current to q_target. 117 | 118 | Args: 119 | q_current (list or np.array): Current quaternion [w, x, y, z] 120 | q_target (list or np.array): Target quaternion [w, x, y, z] 121 | dt (float): Time step over which rotation is applied 122 | 123 | Returns: 124 | np.array: Angular velocity (wx, wy, wz) in global frame 125 | """ 126 | # Ensure numpy arrays 127 | q_current = np.array(q_current) 128 | q_target = np.array(q_target) 129 | 130 | # Compute relative quaternion: q_relative = q_target * q_current⁻¹ 131 | q_current_inv = quat.qinverse(q_current) 132 | q_relative = quat.qmult(q_target, q_current_inv) 133 | 134 | if q_relative[0] > 1.0: 135 | print('q_relative:' + str(q_relative)) 136 | return np.array([0.0, 0.0, 0.0]) 137 | 138 | # Extract angle and axis from q_relative 139 | theta = 2 * np.arccos(q_relative[0]) # q_relative[0] is the w-component 140 | 141 | # Avoid division by zero when theta is very small 142 | sin_half_theta = np.sqrt(1 - q_relative[0]**2) 143 | if np.isclose(sin_half_theta, 0): 144 | angular_velocity = np.array([0.0, 0.0, 0.0]) # No rotation needed 145 | else: 146 | axis = q_relative[-3:] / sin_half_theta # Normalize rotation axis 147 | angular_velocity = (theta / dt) * axis # ω = (θ/Δt) * axis 148 | 149 | return angular_velocity 150 | 151 | 152 | 153 | def main(args=None): 154 | rclpy.init(args=args) 155 | 156 | action_client = ServoTwistSender() 157 | 158 | rclpy.spin(action_client) 159 | 160 | 161 | if __name__ == "__main__": 162 | main() 163 | -------------------------------------------------------------------------------- /src/utils.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * 3 | * Copyright (c) 2024, Crobotic Solutions d.o.o. (www.crobotics.tech) 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright notice, this 10 | * list of conditions and the following disclaimer. 11 | * 12 | * * Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * * Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived from 18 | * this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *******************************************************************************/ 32 | 33 | /* Title : utils.cpp 34 | * Project : arm_api2 35 | * Created : 05/10/2024 36 | * Author : Filip Zoric 37 | * 38 | * Description : Contains all functional implementations not directly related to MoveIt! or ROS 2 39 | */ 40 | 41 | 42 | #include "arm_api2/utils.hpp" 43 | 44 | // TODO: move to utils 45 | std::vector utils::createCartesianWaypoints(geometry_msgs::msg::Pose p1, geometry_msgs::msg::Pose p2, int n) 46 | { 47 | std::vector result; 48 | geometry_msgs::msg::Pose pose_; 49 | if (n <= 1) { 50 | result.push_back(p1); 51 | return result; 52 | } 53 | double dX = (p2.position.x - p1.position.x) / (n - 1); 54 | double dY = (p2.position.y - p1.position.y) / (n - 1); 55 | double dZ = (p2.position.z - p1.position.z) / (n - 1); 56 | // Set i == 1 because start point doesn't have to be included into waypoint list 57 | // https://answers.ros.org/question/253004/moveit-problem-error-trajectory-message-contains-waypoints-that-are-not-strictly-increasing-in-time/ 58 | for (int i = 1; i < n; ++i) { 59 | pose_.position.x = p1.position.x + i * dX; 60 | pose_.position.y = p1.position.y + i * dY; 61 | pose_.position.z = p1.position.z + i * dZ; 62 | pose_.orientation.x = p1.orientation.x; 63 | pose_.orientation.y = p1.orientation.y; 64 | pose_.orientation.z = p1.orientation.z; 65 | pose_.orientation.w = p1.orientation.w; 66 | result.push_back(pose_); 67 | } 68 | return result; 69 | } 70 | 71 | 72 | // TODO: Move to utils 73 | bool utils::comparePosition(geometry_msgs::msg::PoseStamped p1, geometry_msgs::msg::PoseStamped p2) 74 | { 75 | // Returns false if different positions 76 | bool x_cond = false; bool y_cond = false; bool z_cond = false; 77 | double d = 0.01; 78 | 79 | if (std::abs(p1.pose.position.x - p2.pose.position.x) < d) x_cond = true; 80 | if (std::abs(p1.pose.position.y - p2.pose.position.y) < d) y_cond = true; 81 | if (std::abs(p1.pose.position.z - p2.pose.position.z) < d) z_cond = true; 82 | 83 | bool cond = x_cond && y_cond && z_cond; 84 | return cond; 85 | } 86 | 87 | // TODO: Move to utils 88 | bool utils::compareOrientation(geometry_msgs::msg::PoseStamped p1, geometry_msgs::msg::PoseStamped p2) 89 | { 90 | // Returns false if different positions 91 | bool p_cond = false; bool r_cond = false; bool y_cond = false; 92 | double d = 0.01; 93 | 94 | geometry_msgs::msg::Quaternion q1c, q2c; 95 | q1c.x = p1.pose.orientation.x; q2c.x = p2.pose.orientation.x; 96 | q1c.y = p1.pose.orientation.y; q2c.y = p2.pose.orientation.y; 97 | q1c.z = p1.pose.orientation.z; q2c.z = p2.pose.orientation.z; 98 | q1c.w = p1.pose.orientation.w; q2c.w = p2.pose.orientation.w; 99 | 100 | tf2::Quaternion q1(q1c.x, q1c.y, q1c.z, q1c.w); 101 | tf2::Quaternion q2(q2c.x, q2c.y, q2c.z, q2c.w); 102 | 103 | double r1, r2, pi1, pi2, y1, y2; 104 | tf2::Matrix3x3(q1).getEulerYPR(y1, pi1, r1); 105 | tf2::Matrix3x3(q2).getEulerYPR(y2, pi2, r2); 106 | 107 | d = 0.01; 108 | if (std::abs(pi1 - pi2) < d) p_cond = true; 109 | if (std::abs(r1 - r2) < d) r_cond = true; 110 | if (std::abs(y1 - y2) < d) y_cond = true; 111 | 112 | //TODO: Convert quat1 to roll & pitch & yaw 113 | bool cond = p_cond && r_cond && y_cond; 114 | return cond; 115 | } 116 | 117 | // TODO: move to utils 118 | bool utils::comparePose(geometry_msgs::msg::PoseStamped p1, geometry_msgs::msg::PoseStamped p2) 119 | { 120 | bool position, orientation; 121 | position = comparePosition(p1, p2); 122 | orientation = compareOrientation(p1, p2); 123 | return position && orientation; 124 | } 125 | 126 | //TODO: move to utils 127 | geometry_msgs::msg::PoseStamped utils::normalizeOrientation(geometry_msgs::msg::PoseStamped p) 128 | { 129 | geometry_msgs::msg::PoseStamped p_; 130 | tf2::Quaternion q(p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w); 131 | q.normalize(); 132 | p_.pose.orientation.x = q.x(); 133 | p_.pose.orientation.y = q.y(); 134 | p_.pose.orientation.z = q.z(); 135 | p_.pose.orientation.w = q.w(); 136 | p_.pose.position = p.pose.position; 137 | return p_; 138 | } 139 | 140 | // There's also built-in method in tf2 however, didn't manage to get it to work 141 | geometry_msgs::msg::PoseStamped utils::convertIsometryToMsg(Eigen::Isometry3d pose) 142 | { 143 | geometry_msgs::msg::PoseStamped p; 144 | p.pose.position.x = pose.translation().x(); 145 | p.pose.position.y = pose.translation().y(); 146 | p.pose.position.z = pose.translation().z(); 147 | Eigen::Quaterniond q(pose.rotation()); 148 | p.pose.orientation.x = q.x(); 149 | p.pose.orientation.y = q.y(); 150 | p.pose.orientation.z = q.z(); 151 | p.pose.orientation.w = q.w(); 152 | return p; 153 | } 154 | 155 | std_msgs::msg::String utils::stateToMsg(int state) 156 | { 157 | const char* stateNames[4] = {"IDLE", "JOINT_TRAJ_CTL", "CART_TRAJ_CTL", "SERVO_CTL"}; 158 | std_msgs::msg::String msg; 159 | 160 | if (state >= 0 && state < 4) { 161 | msg.data = stateNames[state]; 162 | } else { 163 | msg.data = "UNKNOWN"; 164 | } 165 | 166 | return msg; 167 | } -------------------------------------------------------------------------------- /launch/moveit2_iface.launch.py: -------------------------------------------------------------------------------- 1 | ###################################################################### 2 | # BSD 3-Clause License 3 | # 4 | # Copyright (c) 2024, Crobotic Solutions d.o.o. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # 13 | # * Redistributions in binary form must reproduce the above copyright notice, 14 | # this list of conditions and the following disclaimer in the documentation 15 | # and/or other materials provided with the distribution. 16 | # 17 | # * Neither the name of the copyright holder nor the names of its 18 | # contributors may be used to endorse or promote products derived from 19 | # this software without specific prior written permission.Need document name?RTICULAR PURPOSE 20 | # ARE 21 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ###################################################################### 29 | 30 | # Title : moveit2_iface.launch.py 31 | # Project : arm_api2 32 | # Created : 06/08/2025 33 | # Author : Filip Zoric 34 | # 35 | # Description : Launch file for moveit2_iface 36 | # 37 | 38 | from ament_index_python.packages import get_package_share_directory 39 | 40 | from launch import LaunchDescription, LaunchContext 41 | from launch.actions import OpaqueFunction, DeclareLaunchArgument, IncludeLaunchDescription 42 | from launch_ros.actions import Node 43 | from launch_param_builder import ParameterBuilder 44 | from launch.substitutions import LaunchConfiguration 45 | 46 | import yaml 47 | import os 48 | 49 | # TODO: Make this changeable without ERROR for wrong param type 50 | use_sim_time = True 51 | use_servo = True 52 | dt = 0.1 53 | 54 | def launch_setup(context, *args, **kwargs): 55 | 56 | launch_nodes_ = [] 57 | arg_robot_name = context.perform_substitution(LaunchConfiguration('robot_name')) 58 | arg_launch_joy = context.perform_substitution(LaunchConfiguration('launch_joy', default=True)) 59 | arg_launch_servo_watchdog = context.perform_substitution(LaunchConfiguration('launch_servo_watchdog', default=True)) 60 | print("arg_launch_joy: ", arg_launch_joy) 61 | 62 | # TODO: Swap between sim and real arg depending on the robot type 63 | robot_yaml = "{0}/{1}_sim.yaml".format(arg_robot_name, arg_robot_name) 64 | servo_yaml = "{0}/{1}_servo_sim.yaml".format(arg_robot_name, arg_robot_name) 65 | kinematics_yaml = "config/{0}/{1}_kinematics.yaml".format(arg_robot_name, arg_robot_name) 66 | 67 | # Arm params (ctl, servo) --> sent just as path 68 | # 3 different ways of loading and using yaml files, DISGUSTING [FIX ASAP] 69 | config_path = os.path.join( 70 | get_package_share_directory('arm_api2'), 71 | "config", 72 | robot_yaml 73 | ) 74 | 75 | # Servo params created with the help of ParameterBuilder 76 | servo_params = { 77 | "moveit_servo": ParameterBuilder("arm_api2") 78 | .yaml(f"config/{servo_yaml}") 79 | .to_dict() 80 | } 81 | 82 | # Load kinematic params 83 | kinematic_params = load_yaml("arm_api2", kinematics_yaml) 84 | 85 | launch_move_group = Node( 86 | package='arm_api2', 87 | executable='moveit2_iface', 88 | #prefix=['gdbserver localhost:3000'], # Used for debugging 89 | parameters=[{"use_sim_time": use_sim_time}, 90 | {"enable_servo": use_servo}, 91 | {"dt": dt}, 92 | {"config_path": config_path}, 93 | kinematic_params, 94 | servo_params,] 95 | ) 96 | 97 | launch_nodes_.append(launch_move_group) 98 | 99 | if str(arg_launch_joy).lower() == "true": # To avoid pkg not found 100 | # https://index.ros.org/p/joy/ --> joy node as joystick (Create subscriber that takes cmd_vel) 101 | joy_node = Node( 102 | package='joy', 103 | executable="joy_node", 104 | output="screen", 105 | arguments={'device_name':"js0"}.items() 106 | ) 107 | launch_nodes_.append(joy_node) 108 | 109 | if str(arg_launch_servo_watchdog).lower() == "true": 110 | launch_servo_watchdog = Node( 111 | package='arm_api2', 112 | executable="servo_watchdog.py", 113 | output='screen' 114 | ) 115 | launch_nodes_.append(launch_servo_watchdog) 116 | 117 | return launch_nodes_ 118 | 119 | #https://answers.ros.org/question/396345/ros2-launch-file-how-to-convert-launchargument-to-string/ 120 | def generate_launch_description(): 121 | 122 | declared_arguments = [] 123 | 124 | declared_arguments.append( 125 | DeclareLaunchArgument(name='robot_name', 126 | default_value='kinova', 127 | description='robot name') 128 | ) 129 | # TODO: THIS IS NOT CONVERTED TO FALSE WHEN SETUP! FIX IT! 130 | declared_arguments.append( 131 | DeclareLaunchArgument(name='launch_joy', 132 | default_value='true', 133 | description='launch joystick') 134 | ) 135 | 136 | declared_arguments.append( 137 | DeclareLaunchArgument(name='launch_servo_watchdog', 138 | default_value='true', 139 | description='launch servo_watchdog node') 140 | ) 141 | 142 | declared_arguments.append( 143 | DeclareLaunchArgument(name='dt', 144 | default_value='0.1', 145 | description='time step') 146 | ) 147 | 148 | declared_arguments.append( 149 | DeclareLaunchArgument(name='use_sim_time', 150 | default_value='false', 151 | description='use simulation time') 152 | ) 153 | 154 | return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) 155 | 156 | 157 | def load_yaml(package_name: str, file_path: str): 158 | """ 159 | Load yaml configuration based on package name and file path relative to its share. 160 | """ 161 | 162 | package_path = get_package_share_directory(package_name) 163 | absolute_file_path = os.path.join(package_path, file_path) 164 | return parse_yaml(absolute_file_path) 165 | 166 | def parse_yaml(absolute_file_path: str): 167 | """ 168 | Parse yaml from file, given its absolute file path. 169 | """ 170 | 171 | try: 172 | with open(absolute_file_path, "r") as file: 173 | return yaml.safe_load(file) 174 | except EnvironmentError: 175 | return None -------------------------------------------------------------------------------- /examples/scripts/trajectory_sender_action_client_async.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import csv 4 | import threading 5 | 6 | import numpy as np 7 | import rclpy 8 | import rclpy.duration 9 | from ament_index_python.packages import get_package_share_directory 10 | from arm_api2_msgs.action import MoveCartesianPath 11 | from geometry_msgs.msg import PoseStamped 12 | from rclpy.action import ActionClient 13 | from rclpy.executors import MultiThreadedExecutor 14 | from rclpy.node import Node 15 | from scipy.spatial.transform import Rotation as R 16 | 17 | 18 | class CreateAndPublishTrajectoryAsync(Node): 19 | 20 | def __init__(self): 21 | super().__init__("create_publish_trajectory_async") 22 | 23 | # Create subscribers 24 | self.curr_p_sub = self.create_subscription( 25 | PoseStamped, "/arm/state/current_pose", self.curr_p_cb, 1 26 | ) 27 | 28 | # Create action client 29 | self._action_client = ActionClient( 30 | self, MoveCartesianPath, "arm/move_to_pose_path" 31 | ) 32 | 33 | # Path to predefined trajectories 34 | c_csv_pth = get_package_share_directory("arm_api2") + "/utils/CS_C_easy.csv" 35 | s_csv_pth = get_package_share_directory("arm_api2") + "/utils/CS_S_easy.csv" 36 | 37 | self.get_logger().info(f"C trajectory path is: {c_csv_pth}") 38 | self.get_logger().info(f"S trajectory path is: {s_csv_pth}") 39 | # Load positions from YAML file 40 | 41 | self.c_data = self.load_positions(c_csv_pth) 42 | self.s_data = self.load_positions(s_csv_pth) 43 | 44 | # Flags 45 | self.send_traj_flag = threading.Event() 46 | self.receive_pose_flag = threading.Event() 47 | 48 | # Create a separate thread to handle user input 49 | self.user_input_thread = threading.Thread(target=self.handle_user_input) 50 | self.user_input_thread.start() 51 | 52 | def send_goal(self, goal_path): 53 | goal_msg = MoveCartesianPath.Goal() 54 | goal_msg.poses = goal_path 55 | 56 | self.get_logger().info("Waiting for action server...") 57 | 58 | self._action_client.wait_for_server() 59 | 60 | self.get_logger().info("Sending goal request...") 61 | 62 | self._send_goal_future = self._action_client.send_goal_async( 63 | goal_msg, feedback_callback=self.feedback_callback 64 | ) 65 | 66 | self._send_goal_future.add_done_callback(self.goal_response_callback) 67 | 68 | def goal_response_callback(self, future): 69 | goal_handle = future.result() 70 | 71 | if not goal_handle.accepted: 72 | self.get_logger().info("Goal rejected") 73 | return 74 | 75 | self.get_logger().info("Goal accepted") 76 | 77 | self._get_result_future = goal_handle.get_result_async() 78 | self._get_result_future.add_done_callback(self.get_result_callback) 79 | 80 | def get_result_callback(self, future): 81 | result = future.result().result 82 | if result.success: 83 | self.get_logger().info("Result: Trajectory executed successfully!") 84 | else: 85 | self.get_logger().info("Result: Trajectory execution failed!") 86 | self.send_traj_flag.clear() 87 | 88 | def feedback_callback(self, feedback_msg): 89 | feedback = feedback_msg.feedback 90 | self.get_logger().info("Feedback: {0}".format(feedback.status)) 91 | 92 | def load_positions(self, csv_pth): 93 | p_data = [] 94 | with open(csv_pth, "r") as file: 95 | reader = csv.DictReader(file) 96 | for row in reader: 97 | x = row["x"] 98 | y = row["y"] 99 | z = row["z"] 100 | p_data.append(np.array([float(x), float(y), float(z), 1]).T) 101 | self.get_logger().info("Loaded positions.") 102 | return p_data 103 | 104 | def curr_p_cb(self, msg): 105 | self.curr_p = PoseStamped() 106 | self.curr_p.header = msg.header 107 | self.curr_p.pose.position = msg.pose.position 108 | self.curr_p.pose.orientation = msg.pose.orientation 109 | p, q = msg.pose.position, msg.pose.orientation 110 | x, y, z = p.x, p.y, p.z 111 | qx, qy, qz, qw = q.x, q.y, q.z, q.w 112 | # self.get_logger().info(f"x: {x}\t {y}\t {z}\t") 113 | # x, y, z, w 114 | R_ = R.from_quat([qx, qy, qz, qw]) 115 | r = R_.as_matrix() 116 | p_ = np.array([x, y, z]).T.reshape(3, 1) 117 | self.get_logger().debug(f"R shape is: {r.shape}") 118 | self.get_logger().debug(f"p shape is: {p_.shape}") 119 | T_ = np.hstack((r, p_)) 120 | self.T = np.vstack((T_, np.array([0, 0, 0, 1]))) 121 | self.get_logger().debug(f"Reciv T matrix is: {self.T}") 122 | self.receive_pose_flag.set() 123 | 124 | def create_trajectory(self, letter): 125 | self.get_logger().info("Create trajectory!") 126 | 127 | if letter == "S": 128 | data = self.s_data 129 | if letter == "C": 130 | data = self.c_data 131 | path = [] 132 | for p in data: 133 | self.get_logger().info(f"p is: {p}") 134 | self.get_logger().info(f"T is: {self.T}") 135 | p_ = np.matmul(self.T, p) 136 | self.get_logger().info(f"p_ is: {p_}") 137 | r_ = R.from_matrix(self.T[:3, :3]) 138 | quat = r_.as_quat() 139 | rosp = PoseStamped() 140 | rosp.header.frame_id = self.curr_p.header.frame_id 141 | rosp.pose.position.x = p_[0] 142 | rosp.pose.position.y = p_[1] 143 | rosp.pose.position.z = p_[2] 144 | rosp.pose.orientation.x = quat[0] 145 | rosp.pose.orientation.y = quat[1] 146 | rosp.pose.orientation.z = quat[2] 147 | rosp.pose.orientation.w = quat[3] 148 | path.append(rosp) 149 | return path 150 | 151 | def handle_user_input(self): 152 | while rclpy.ok(): 153 | 154 | while self.send_traj_flag.is_set() or not self.receive_pose_flag.is_set(): 155 | pass 156 | 157 | user_input = input( 158 | "Enter 's' for S trajectory, 'c' for C trajectory, enter 'q' to quit: " 159 | ) 160 | 161 | if user_input.lower() == "s": 162 | self.get_logger().info("Starting S trajectory...") 163 | traj = self.create_trajectory("S") 164 | self.send_goal(traj) 165 | self.send_traj_flag.set() 166 | 167 | elif user_input.lower() == "c": 168 | self.get_logger().info("Starting C trajectory...") 169 | traj = self.create_trajectory("C") 170 | self.send_goal(traj) 171 | self.send_traj_flag.set() 172 | 173 | if user_input.lower() == "q": 174 | self.get_logger().info("Shutting down.") 175 | rclpy.shutdown() 176 | break 177 | 178 | 179 | def main(args=None): 180 | rclpy.init(args=args) 181 | CPT = CreateAndPublishTrajectoryAsync() 182 | 183 | executor = MultiThreadedExecutor() 184 | executor.add_node(CPT) 185 | 186 | try: 187 | executor.spin() 188 | 189 | finally: 190 | CPT.destroy_node() 191 | 192 | 193 | if __name__ == "__main__": 194 | main() 195 | -------------------------------------------------------------------------------- /launch/moveit2_simple_iface.launch.py: -------------------------------------------------------------------------------- 1 | ###################################################################### 2 | # BSD 3-Clause License 3 | # 4 | # Copyright (c) 2024, Crobotic Solutions d.o.o. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # 13 | # * Redistributions in binary form must reproduce the above copyright notice, 14 | # this list of conditions and the following disclaimer in the documentation 15 | # and/or other materials provided with the distribution. 16 | # 17 | # * Neither the name of the copyright holder nor the names of its 18 | # contributors may be used to endorse or promote products derived from 19 | # this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | # ARE 25 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | ###################################################################### 33 | 34 | # Title : moveit2_simple_iface.launch.py 35 | # Project : arm_api2 36 | # Created : 05/10/2024 37 | # Author : Filip Zoric 38 | # 39 | # Description : Launch file for moveit2_simple_iface 40 | # 41 | 42 | from ament_index_python.packages import get_package_share_directory 43 | 44 | from launch import LaunchDescription, LaunchContext 45 | from launch.actions import OpaqueFunction, DeclareLaunchArgument, IncludeLaunchDescription 46 | from launch_ros.actions import Node 47 | from launch_param_builder import ParameterBuilder 48 | from launch.substitutions import LaunchConfiguration 49 | 50 | import yaml 51 | import os 52 | 53 | # TODO: Make this changeable without ERROR for wrong param type 54 | use_sim_time = True 55 | use_servo = True 56 | dt = 0.1 57 | 58 | def launch_setup(context, *args, **kwargs): 59 | 60 | launch_nodes_ = [] 61 | arg_robot_name = context.perform_substitution(LaunchConfiguration('robot_name')) 62 | arg_launch_joy = context.perform_substitution(LaunchConfiguration('launch_joy', default=True)) 63 | arg_use_gdb = context.perform_substitution(LaunchConfiguration('use_gdb', default=False)) 64 | 65 | # TODO: Swap between sim and real arg depending on the robot type 66 | robot_yaml = "{0}/{1}_sim.yaml".format(arg_robot_name, arg_robot_name) 67 | servo_yaml = "{0}/{1}_servo_sim.yaml".format(arg_robot_name, arg_robot_name) 68 | kinematics_yaml = "config/{0}/{1}_kinematics.yaml".format(arg_robot_name, arg_robot_name) 69 | 70 | # Arm params (ctl, servo) --> sent just as path 71 | # 3 different ways of loading and using yaml files, DISGUSTING [FIX ASAP] 72 | config_path = os.path.join( 73 | get_package_share_directory('arm_api2'), 74 | "config", 75 | robot_yaml 76 | ) 77 | 78 | # Servo params created with the help of ParameterBuilder 79 | servo_params = { 80 | "moveit_servo": ParameterBuilder("arm_api2") 81 | .yaml(f"config/{servo_yaml}") 82 | .to_dict() 83 | } 84 | 85 | # Load kinematic params 86 | kinematic_params = load_yaml("arm_api2", kinematics_yaml) 87 | 88 | # Configure GDB prefix if debugging is enabled 89 | prefix_cmd = [] 90 | if arg_use_gdb.lower() == 'true': 91 | prefix_cmd =['xterm -e gdb -ex run --args'] 92 | 93 | launch_arm_api2 = Node( 94 | package='arm_api2', 95 | executable='moveit2_simple_iface', 96 | parameters=[{"use_sim_time": use_sim_time}, 97 | {"enable_servo": use_servo}, 98 | {"dt": dt}, 99 | {"config_path": config_path}, 100 | kinematic_params, 101 | servo_params,], 102 | prefix=prefix_cmd, 103 | output='screen' 104 | ) 105 | 106 | launch_nodes_.append(launch_arm_api2) 107 | 108 | if arg_launch_joy: 109 | 110 | # https://index.ros.org/p/joy/ --> joy node as joystick (Create subscriber that takes cmd_vel) 111 | # Example of demo joint_jog 112 | joy_node = Node( 113 | package='joy', 114 | executable="joy_node", 115 | output="screen", 116 | arguments={'device_name':'js0'}.items() 117 | ) 118 | launch_nodes_.append(joy_node) 119 | 120 | joy_ctl_node = Node( 121 | package="arm_api2", 122 | executable="joy_ctl", 123 | output="screen", 124 | parameters = [{"use_sim_time": use_sim_time}] 125 | ) 126 | 127 | launch_nodes_.append(joy_ctl_node) 128 | 129 | return launch_nodes_ 130 | 131 | #https://answers.ros.org/question/396345/ros2-launch-file-how-to-convert-launchargument-to-string/ 132 | def generate_launch_description(): 133 | 134 | declared_arguments = [] 135 | 136 | declared_arguments.append( 137 | DeclareLaunchArgument(name='robot_name', 138 | default_value='kinova', 139 | description='robot name') 140 | ) 141 | # TODO: THIS IS NOT CONVERTED TO FALSE WHEN SETUP! FIX IT! 142 | declared_arguments.append( 143 | DeclareLaunchArgument(name='launch_joy', 144 | default_value='true', 145 | description='launch joystick') 146 | ) 147 | 148 | declared_arguments.append( 149 | DeclareLaunchArgument(name='dt', 150 | default_value='0.1', 151 | description='time step') 152 | ) 153 | 154 | declared_arguments.append( 155 | DeclareLaunchArgument(name='use_sim_time', 156 | default_value='false', 157 | description='use simulation time') 158 | ) 159 | 160 | declared_arguments.append( 161 | DeclareLaunchArgument(name='use_gdb', 162 | default_value='false', 163 | description='Run node with GDB debugger') 164 | ) 165 | 166 | return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) 167 | 168 | 169 | def load_yaml(package_name: str, file_path: str): 170 | """ 171 | Load yaml configuration based on package name and file path relative to its share. 172 | """ 173 | 174 | package_path = get_package_share_directory(package_name) 175 | absolute_file_path = os.path.join(package_path, file_path) 176 | return parse_yaml(absolute_file_path) 177 | 178 | def parse_yaml(absolute_file_path: str): 179 | """ 180 | Parse yaml from file, given its absolute file path. 181 | """ 182 | 183 | try: 184 | with open(absolute_file_path, "r") as file: 185 | return yaml.safe_load(file) 186 | except EnvironmentError: 187 | return None -------------------------------------------------------------------------------- /include/arm_api2/grippers/robotiq_gripper.hpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * 3 | * Copyright (c) 2024, Crobotic Solutions d.o.o. (www.crobotics.tech) 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright notice, this 10 | * list of conditions and the following disclaimer. 11 | * 12 | * * Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * * Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived from 18 | * this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *******************************************************************************/ 32 | 33 | /* Title : utils.hpp 34 | * Project : arm_api2 35 | * Created : 01/19/2025 36 | * Author : Filip Zoric 37 | * 38 | * Description : Implementation of the gripper class for the Robotiq gripper 39 | */ 40 | 41 | #ifndef ROBOTIQ_GRIPPER_H 42 | #define ROBOTIQ_GRIPPER_H 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include "gripper.hpp" 49 | #include 50 | 51 | class RobotiqGripper: public Gripper { 52 | 53 | public: 54 | // Constructor 55 | RobotiqGripper(std::shared_ptr node) : node_(node){ 56 | isOpen = false; 57 | gripper_client_ = rclcpp_action::create_client(node_,"robotiq_2f_urcap_adapter/gripper_command"); 58 | }; 59 | ~RobotiqGripper(){}; 60 | 61 | // Method to open the gripper 62 | void open(){ 63 | send_gripper_command(0.0); // open gripper 64 | RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"), "Gripper opened."); 65 | isOpen = true; 66 | }; 67 | 68 | // Method to close the gripper 69 | void close(){ 70 | send_gripper_command(0.8); // close gripper 71 | RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"), "Gripper closed."); 72 | isOpen = false; 73 | }; 74 | 75 | bool send_gripper_command(double position, double max_effort = 140.0) 76 | { 77 | if (!gripper_client_->wait_for_action_server(std::chrono::seconds(5))) { 78 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Gripper action server not available!"); 79 | return false; 80 | } 81 | auto goal = control_msgs::action::GripperCommand::Goal(); 82 | goal.command.position = position; 83 | goal.command.max_effort = max_effort; 84 | 85 | RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sending gripper command ..."); 86 | /*auto result = gripper_client_->async_send_goal(goal); 87 | 88 | // Wait for the result 89 | if (rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result) != rclcpp::FutureReturnCode::SUCCESS) { 90 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to send gripper command"); 91 | return; 92 | } 93 | 94 | std::shared_ptr> response = result.get(); 95 | auto result_response = response->get_result(); 96 | float pos = result_response.position;*/ 97 | 98 | auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); 99 | send_goal_options.goal_response_callback = std::bind(&RobotiqGripper::goal_response_callback, this, std::placeholders::_1); 100 | send_goal_options.feedback_callback = std::bind(&RobotiqGripper::feedback_callback, this, std::placeholders::_1, std::placeholders::_2); 101 | send_goal_options.result_callback = std::bind(&RobotiqGripper::result_callback, this, std::placeholders::_1); 102 | 103 | gripper_client_->async_send_goal(goal, send_goal_options); 104 | 105 | is_done = false; 106 | 107 | while (!is_done) { 108 | //rclcpp::spin_some(node_); 109 | } 110 | 111 | RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Gripper moved to position %f", last_position); 112 | return success; 113 | }; 114 | 115 | float get_position(){ 116 | return last_position; 117 | }; 118 | 119 | float get_effort(){ 120 | return last_effort; 121 | }; 122 | 123 | bool is_stalled(){ 124 | return last_stalled; 125 | }; 126 | 127 | bool reached_goal(){ 128 | return last_reached_goal; 129 | }; 130 | 131 | private: 132 | std::shared_ptr node_; 133 | bool isOpen; 134 | 135 | float last_position = 0.0; 136 | float last_effort = 0.0; 137 | bool last_stalled = false; 138 | bool last_reached_goal = false; 139 | bool success = false; 140 | bool is_done = false; 141 | 142 | // client 143 | rclcpp_action::Client::SharedPtr gripper_client_; 144 | 145 | void goal_response_callback(std::shared_ptr> goal_handle) { 146 | if (!goal_handle) { 147 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Goal was rejected by server"); 148 | } else { 149 | RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Goal accepted by server, waiting for result"); 150 | } 151 | } 152 | 153 | void feedback_callback( 154 | std::shared_ptr> goal_handle, 155 | const std::shared_ptr feedback) { 156 | (void)goal_handle; 157 | RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Got feedback: position = %f, effort = %f, stalled = %d, reached_goal = %d", 158 | feedback->position, feedback->effort, feedback->stalled, feedback->reached_goal); 159 | } 160 | 161 | void result_callback(const rclcpp_action::ClientGoalHandle::WrappedResult & result) { 162 | switch (result.code) { 163 | case rclcpp_action::ResultCode::SUCCEEDED: 164 | RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Goal succeeded!"); 165 | RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Result: position = %f, effort = %f, stalled = %d, reached_goal = %d", 166 | result.result->position, result.result->effort, result.result->stalled, result.result->reached_goal); 167 | last_position = result.result->position; 168 | last_effort = result.result->effort; 169 | last_stalled = result.result->stalled; 170 | last_reached_goal = result.result->reached_goal; 171 | success = true; 172 | break; 173 | case rclcpp_action::ResultCode::ABORTED: 174 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Goal was aborted"); 175 | success = false; 176 | break; 177 | case rclcpp_action::ResultCode::CANCELED: 178 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Goal was canceled"); 179 | success = false; 180 | break; 181 | default: 182 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Unknown result code"); 183 | break; 184 | } 185 | 186 | 187 | is_done = true; 188 | } 189 | 190 | }; 191 | 192 | #endif // ROBOTIQ_GRIPPER_H -------------------------------------------------------------------------------- /utils/demo_sem_rdsc.txt: -------------------------------------------------------------------------------- 1 | String value is: 5 | -------------------------------------------------------------------------------- /utils/demo_rdsc.txt: -------------------------------------------------------------------------------- 1 | String value is: mock_components/GenericSystem0.00.0-0.7850.00.00.0-2.3560.00.00.01.5710.00.7850.0mock_components/GenericSystem0.00.0panda_finger_joint110.00.0 2 | -------------------------------------------------------------------------------- /examples/scripts/servo_watchdog.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import threading 4 | import rclpy 5 | from rclpy.node import Node 6 | from rclpy.time import Time 7 | from rclpy.qos import QoSProfile 8 | 9 | from typing import List 10 | from geometry_msgs.msg import TwistStamped 11 | from control_msgs.msg import JointJog 12 | from std_msgs.msg import String 13 | 14 | from controller_manager_msgs.srv import SwitchController 15 | 16 | 17 | from arm_api2_msgs.srv import ChangeState 18 | 19 | class ServoWatchdog(Node): 20 | def __init__(self): 21 | super().__init__('servo_watchdog_node') 22 | 23 | qos_profile = QoSProfile(depth=10) 24 | qos_profile.avoid_ros_namespace_conventions = True 25 | 26 | # Create a publisher for the twist topic with zero velocity 27 | self.pub_twist = self.create_publisher(TwistStamped, '/moveit2_iface_node/delta_twist_cmds', qos_profile) 28 | self.pub_jog = self.create_publisher(JointJog, '/moveit2_iface_node/delta_joint_cmds', qos_profile) 29 | 30 | # Create a subscriber for the twist topic to check if messages are being published 31 | self.sub_twist = self.create_subscription( 32 | TwistStamped, 33 | '/moveit2_iface_node/delta_twist_cmds', 34 | self.twist_callback, 35 | 10 36 | ) 37 | 38 | self.sub_jog = self.create_subscription( 39 | JointJog, 40 | '/moveit2_iface_node/delta_joint_cmds', 41 | self.jog_callback, 42 | 10 43 | ) 44 | self.sub_arm_api2_state = self.create_subscription( 45 | String, 46 | '/arm/state/current_state', 47 | self.arm_api2_state_callback, 48 | 10 49 | ) 50 | 51 | self._service_client_switch_controller = self.create_client( 52 | SwitchController, "controller_manager/switch_controller") 53 | self._service_client_state_change = self.create_client( 54 | ChangeState, "arm/change_state") 55 | 56 | # Set a timer to publish a zero velocity twist message every second 57 | self.timer = self.create_timer(1.0, self.publish_zero_twist) 58 | 59 | # Flag to track whether we received a message 60 | self.received_message = False 61 | self.joint_names = [] 62 | self.watchdog_active = False 63 | 64 | self.watchdog_counter = 0 65 | 66 | self.get_logger().info('Servo watchdog node started') 67 | 68 | def twist_callback(self, msg): 69 | """Callback that gets triggered when a new Twist message is received.""" 70 | #self.get_logger().info('Received Twist message!') 71 | self.received_message = True # Message received, reset the flag 72 | 73 | def jog_callback(self, msg): 74 | """Callback that gets triggered when a new Twist message is received.""" 75 | #self.get_logger().info('Received JointJog message!') 76 | self.joint_names = msg.joint_names 77 | self.received_message = True # Message received, reset the flag 78 | 79 | def arm_api2_state_callback(self, msg): 80 | """Callback that gets triggered when a new Twist message is received.""" 81 | #self.get_logger().info('Received Twist message!') 82 | if msg.data == "SERVO_CTL": 83 | if self.watchdog_active == False: 84 | self.get_logger().info('arm_api2 is in SERVO_CTL mode, starting watchdog') 85 | self.watchdog_active = True 86 | else: 87 | if self.watchdog_active == True: 88 | self.get_logger().info('arm_api2 is not in SERVO_CTL mode anymore, stopping watchdog') 89 | self.watchdog_active = False 90 | self.watchdog_counter = 0 91 | 92 | def publish_zero_twist(self): 93 | """Publish a zero velocity twist message if no message was received in the last second.""" 94 | if self.watchdog_active: 95 | 96 | if not self.received_message and self.watchdog_active: 97 | self.get_logger().info('No Twist/JointJog message received, publishing zero velocity message. Counter: ' + str(self.watchdog_counter)) 98 | zero_twist = TwistStamped() 99 | zero_twist.header.stamp = Time().to_msg() 100 | zero_twist.header.frame_id = 'world' 101 | zero_twist.twist.linear.x = 0.0 102 | zero_twist.twist.linear.y = 0.0 103 | zero_twist.twist.linear.z = 0.0 104 | zero_twist.twist.angular.x = 0.0 105 | zero_twist.twist.angular.y = 0.0 106 | zero_twist.twist.angular.z = 0.0 107 | self.pub_twist.publish(zero_twist) # Publish zero velocity 108 | 109 | zero_jog = JointJog() 110 | zero_jog.header.stamp = Time().to_msg() 111 | zero_jog.joint_names = self.joint_names 112 | zero_jog.velocities = [0.0] * len(self.joint_names) 113 | self.pub_jog.publish(zero_jog) 114 | 115 | self.watchdog_counter += 1 116 | if self.watchdog_counter > 5: # 5 seconds of no messages 117 | self.get_logger().info('No Twist/JointJog message received for 5 seconds, switching to CART_TRAJ_CTL' + str(self.watchdog_counter)) 118 | thread = threading.Thread(target=self.change_state_to_cartesian_ctl) 119 | thread.start() 120 | else: 121 | #self.get_logger().info('Twist/JointJog message received in the last second') 122 | # Reset the if a message was received in the last second 123 | self.watchdog_counter = 0 124 | self.received_message = False 125 | else: 126 | #self.get_logger().info('Watchdog is not active') 127 | pass 128 | 129 | def switch_controller(self, start_controllers: List[str], stop_controllers: List[str]): 130 | """ 131 | Sends a request to the service server to switch controllers. 132 | 133 | !!! IMPORTANT: This function is blocking until the service server response is received 134 | This function must be not called in the main thread. Otherwise will cause a deadlock. 135 | 136 | reference about available controllers in UR ROS2 driver: 137 | https://docs.universal-robots.com/Universal_Robots_ROS2_Documentation/doc/ur_robot_driver/ur_robot_driver/doc/usage/controllers.html#commanding-controllers 138 | 139 | Args: 140 | start_controllers (List[str]): The controllers to start. 141 | stop_controllers (List[str]): The controllers to stop. 142 | 143 | Returns: 144 | bool: True if the controllers were switched, False otherwise. 145 | """ 146 | 147 | 148 | if len(start_controllers) == 0 and len(stop_controllers) == 0: 149 | self.get_logger().info("No controllers to switch") 150 | return True 151 | 152 | request = SwitchController.Request() 153 | request.start_controllers = start_controllers 154 | request.stop_controllers = stop_controllers 155 | request.strictness = 2 # STRICT=2, BEST_EFFORT=1 156 | 157 | self.get_logger().info("Waiting for service server...") 158 | 159 | self._service_client_switch_controller.wait_for_service() 160 | 161 | self.get_logger().info( 162 | "switch_controller request sent, waiting for response...") 163 | 164 | response = self._service_client_switch_controller.call(request) 165 | 166 | if response.ok: 167 | self.get_logger().info(f"Controllers switched") 168 | else: 169 | self.get_logger().info(f"Switching controllers failed") 170 | 171 | return response.ok 172 | 173 | def change_state_to(self, state: str): 174 | """ 175 | Sends a request to the service server to change the arm state to the specified state. 176 | 177 | !!! IMPORTANT: This function is blocking until the service server response is received 178 | This function must be not called in the main thread. Otherwise will cause a deadlock. 179 | 180 | Args: 181 | state (str): The state to change the arm to. One of "JOINT_TRAJ_CTL", "CART_TRAJ_CTL", "SERVO_CTL" 182 | 183 | Returns: 184 | bool: True if the service server response was received, False otherwise. 185 | """ 186 | request = ChangeState.Request() 187 | request.state = state 188 | 189 | self.get_logger().info("Waiting for service server...") 190 | 191 | self._service_client_state_change.wait_for_service() 192 | 193 | self.get_logger().info( 194 | "change_state request sent, waiting for response...") 195 | 196 | response = self._service_client_state_change.call(request) 197 | 198 | if response.success: 199 | self.get_logger().info(f"State changed to {state}") 200 | else: 201 | self.get_logger().info(f"State change to {state} failed") 202 | 203 | return response.success 204 | 205 | def change_state_to_cartesian_ctl(self): 206 | """ 207 | Sends a request to the service server to change the arm state to cartesian control and switch controllers. 208 | 209 | !!! IMPORTANT: This function is blocking until the service server response is received 210 | This function must be not called in the main thread. Otherwise will cause a deadlock. 211 | 212 | Returns: 213 | bool: True if the service server response was received, False otherwise. 214 | """ 215 | stop_controllers = ["forward_position_controller"] 216 | start_controllers = ["scaled_joint_trajectory_controller"] 217 | res1 = self.switch_controller(start_controllers, stop_controllers) 218 | res2 = self.change_state_to("CART_TRAJ_CTL") 219 | return res1 and res2 220 | 221 | 222 | def main(args=None): 223 | rclpy.init(args=args) 224 | node = ServoWatchdog() 225 | 226 | try: 227 | rclpy.spin(node) 228 | except KeyboardInterrupt: 229 | pass 230 | finally: 231 | node.destroy_node() 232 | rclpy.shutdown() 233 | 234 | 235 | if __name__ == '__main__': 236 | main() 237 | -------------------------------------------------------------------------------- /include/arm_api2/moveit2_simple_iface.hpp: -------------------------------------------------------------------------------- 1 | 2 | /******************************************************************************* 3 | * 4 | * Copyright (c) 2024, Crobotic Solutions d.o.o. (www.crobotics.tech) 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | /* Title : moveit2_simple_iface.hpp 35 | * Project : arm_api2 36 | * Created : 05/10/2024 37 | * Author : Filip Zoric 38 | * 39 | * Description : The core robot manipulator and MoveIt2! ROS 2 interfacing header class. 40 | */ 41 | 42 | #ifndef MOVEIT2_SIMPLE_IFACE_HPP 43 | #define MOVEIT2_SIMPLE_IFACE_HPP 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | //* yaml params 52 | #include 53 | 54 | //* ros 55 | #include 56 | #include "tf2/LinearMath/Quaternion.h" 57 | #include "tf2/LinearMath/Matrix3x3.h" 58 | 59 | //* moveit 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | #include 69 | #include 70 | #include 71 | 72 | //* msgs 73 | #include "geometry_msgs/msg/pose_stamped.hpp" 74 | #include "geometry_msgs/msg/pose.hpp" 75 | #include "sensor_msgs/msg/joint_state.hpp" 76 | #include "std_msgs/msg/string.hpp" 77 | #include "arm_api2_msgs/msg/cartesian_waypoints.hpp" 78 | #include "moveit_msgs/msg/collision_object.hpp" 79 | #include "shape_msgs/msg/solid_primitive.hpp" 80 | 81 | //* srvs 82 | #include "arm_api2_msgs/srv/change_state.hpp" 83 | #include "arm_api2_msgs/srv/set_vel_acc.hpp" 84 | #include "arm_api2_msgs/srv/set_string_param.hpp" 85 | #include "arm_api2_msgs/srv/add_collision_object.hpp" 86 | #include "std_srvs/srv/trigger.hpp" 87 | 88 | // utils 89 | #include "arm_api2/utils.hpp" 90 | 91 | // For starters just include robotiq_gripper 92 | // TODO: Think of a way to include different gripper based on the gripper type 93 | #include "arm_api2/grippers/gripper.hpp" 94 | #include "arm_api2/grippers/robotiq_gripper.hpp" 95 | 96 | #define stringify( name ) #name 97 | 98 | // *std placeholders 99 | using namespace std::chrono_literals; 100 | using std::placeholders::_1; 101 | using std::placeholders::_2; 102 | 103 | class m2SimpleIface: public rclcpp::Node 104 | { 105 | 106 | public: 107 | 108 | m2SimpleIface(const rclcpp::NodeOptions &options); 109 | //~m2SimpleIface(); 110 | 111 | /* namespace param, maybe redundant */ 112 | std::string ns_; 113 | 114 | private: 115 | 116 | /* node related stuff */ 117 | rclcpp::Node::SharedPtr node_; 118 | rclcpp::Executor::SharedPtr executor_; 119 | std::thread executor_thread_; 120 | 121 | /* Thread safety */ 122 | std::mutex pose_cmd_mutex_; 123 | 124 | /* gripper */ 125 | RobotiqGripper gripper; 126 | 127 | /* arm_definition */ 128 | std::string PLANNING_GROUP; 129 | std::string EE_LINK_NAME; 130 | std::string ROBOT_DESC; 131 | std::string PLANNING_SCENE; 132 | std::string PLANNING_FRAME; 133 | std::string MOVE_GROUP_NS; 134 | std::string JOINT_STATES; 135 | int NUM_CART_PTS; 136 | bool ENABLE_SERVO; 137 | 138 | /* timers */ 139 | rclcpp::TimerBase::SharedPtr timer_; 140 | 141 | /* parameters */ 142 | std::string config_path; 143 | bool enable_servo; 144 | float dt; 145 | float max_vel_scaling_factor; 146 | float max_acc_scaling_factor; 147 | 148 | /* config_file */ 149 | YAML::Node config; 150 | 151 | /*config*/ 152 | YAML::Node init_config(std::string yaml_path); 153 | 154 | /* init methods */ 155 | void init_subscribers(); 156 | void init_publishers(); 157 | void init_services(); 158 | void init_moveit(); 159 | std::unique_ptr init_servo(); 160 | 161 | /* subs */ 162 | rclcpp::Subscription::SharedPtr pose_cmd_sub_; 163 | rclcpp::Subscription::SharedPtr joint_state_sub_; 164 | rclcpp::Subscription::SharedPtr ctraj_cmd_sub_; 165 | 166 | /* pubs */ 167 | rclcpp::Publisher::SharedPtr pose_state_pub_; 168 | rclcpp::Publisher::SharedPtr robot_state_pub_; 169 | 170 | /* srvs */ 171 | rclcpp::Service::SharedPtr change_state_srv_; 172 | rclcpp::Service::SharedPtr set_vel_acc_srv_; 173 | rclcpp::Service::SharedPtr set_planner_srv_; 174 | rclcpp::Service::SharedPtr open_gripper_srv_; 175 | rclcpp::Service::SharedPtr close_gripper_srv_; 176 | rclcpp::Service::SharedPtr add_collision_object_srv_; 177 | /* topic callbacks */ 178 | void pose_cmd_cb(const geometry_msgs::msg::PoseStamped::SharedPtr msg); 179 | void cart_poses_cb(const arm_api2_msgs::msg::CartesianWaypoints::SharedPtr msg); 180 | void joint_state_cb(const sensor_msgs::msg::JointState::SharedPtr msg); 181 | 182 | /* srv callbacks*/ 183 | void change_state_cb(const std::shared_ptr req, 184 | const std::shared_ptr res); 185 | void set_vel_acc_cb(const std::shared_ptr req, 186 | const std::shared_ptr res); 187 | void set_planner_cb(const std::shared_ptr req, 188 | const std::shared_ptr res); 189 | void open_gripper_cb(const std::shared_ptr req, 190 | const std::shared_ptr res); 191 | void close_gripper_cb(const std::shared_ptr req, 192 | const std::shared_ptr res); 193 | void add_collision_object_cb(const std::shared_ptr req, 194 | const std::shared_ptr res); 195 | bool run(); 196 | 197 | /* setters */ 198 | bool setMoveGroup(rclcpp::Node::SharedPtr nodePtr, std::string groupName, std::string moveNs); 199 | bool setRobotModel(rclcpp::Node::SharedPtr nodePtr); 200 | bool setPlanningSceneMonitor(rclcpp::Node::SharedPtr nodePtr, std::string name); 201 | 202 | /* getters */ 203 | void getArmState(); 204 | 205 | /* funcs */ 206 | void execPlan(bool async); 207 | void execMove(bool async); 208 | void execCartesian(bool async); 209 | void planExecCartesian(bool async); 210 | void execTrajectory(moveit_msgs::msg::RobotTrajectory trajectory, bool async); 211 | 212 | // Simple state machine 213 | enum state{ 214 | IDLE = 0, 215 | JOINT_TRAJ_CTL = 1, 216 | CART_TRAJ_CTL = 2, 217 | SERVO_CTL = 3 218 | }; 219 | 220 | // stateNames 221 | const char* stateNames[4] = 222 | { 223 | stringify (IDLE), 224 | stringify (JOINT_TRAJ_CTL), 225 | stringify (CART_TRAJ_CTL), 226 | stringify (SERVO_CTL) 227 | }; 228 | 229 | // robot state 230 | enum state robotState = IDLE; 231 | 232 | /* flags*/ 233 | bool moveGroupInit = false; 234 | bool robotModelInit = false; 235 | bool pSceneMonitorInit = false; 236 | bool gripperInit = false; 237 | bool nodeInit = false; 238 | bool recivCmd = false; 239 | bool recivTraj = false; 240 | bool servoEntered = false; 241 | bool async = true; 242 | 243 | /* planner info */ 244 | std::string current_planner_id_ = "pilz_industrial_motion_planner"; 245 | std::string current_planner_type_ = "LIN"; 246 | 247 | /* ros vars */ 248 | geometry_msgs::msg::PoseStamped m_currPoseCmd; 249 | geometry_msgs::msg::PoseStamped m_pubCurrPoseCmd; 250 | geometry_msgs::msg::PoseStamped m_oldPoseCmd; 251 | geometry_msgs::msg::PoseStamped m_currPoseState; 252 | sensor_msgs::msg::JointState m_currJointState; 253 | std::vector m_cartesianWaypoints; 254 | 255 | // Store plan and trajectory for async execution to prevent premature destruction 256 | std::shared_ptr m_async_plan_ptr; 257 | std::shared_ptr m_async_trajectory_ptr; 258 | 259 | moveit::planning_interface::MoveGroupInterfacePtr m_moveGroupPtr; 260 | moveit::core::RobotStatePtr m_robotStatePtr; 261 | moveit::core::RobotModelPtr kinematic_model; 262 | std::shared_ptr m_pSceneMonitorPtr; 263 | moveit::planning_interface::PlanningSceneInterface m_planningSceneInterface; 264 | std::unique_ptr servoPtr; 265 | 266 | }; 267 | 268 | #endif 269 | -------------------------------------------------------------------------------- /utils/CS_S.csv: -------------------------------------------------------------------------------- 1 | x,y,z 2 | 0.0,0.0,0 3 | -0.0010000000000000009,0.0010000000000000009,0 4 | -0.002999999999999975,0.0010000000000000009,0 5 | -0.003999999999999976,0.0020000000000000018,0 6 | -0.005999999999999978,0.0020000000000000018,0 7 | -0.0069999999999999785,0.0030000000000000027,0 8 | -0.00799999999999998,0.0030000000000000027,0 9 | -0.00899999999999998,0.0040000000000000036,0 10 | -0.009999999999999981,0.0040000000000000036,0 11 | -0.015999999999999986,0.010000000000000009,0 12 | -0.015999999999999986,0.01100000000000001,0 13 | -0.017999999999999988,0.013000000000000012,0 14 | -0.017999999999999988,0.013999999999999999,0 15 | -0.01899999999999999,0.015,0 16 | -0.01899999999999999,0.017,0 17 | -0.01999999999999999,0.018000000000000002,0 18 | -0.01999999999999999,0.020000000000000004,0 19 | -0.02099999999999999,0.021000000000000005,0 20 | -0.02099999999999999,0.032000000000000015,0 21 | -0.021999999999999992,0.033000000000000015,0 22 | -0.021999999999999992,0.03800000000000002,0 23 | -0.022999999999999993,0.03899999999999999,0 24 | -0.022999999999999993,0.040999999999999995,0 25 | -0.023999999999999994,0.041999999999999996,0 26 | -0.023999999999999994,0.044,0 27 | -0.024999999999999994,0.045,0 28 | -0.024999999999999994,0.047,0 29 | -0.026999999999999996,0.049,0 30 | -0.026999999999999996,0.05,0 31 | -0.027999999999999997,0.051000000000000004,0 32 | -0.027999999999999997,0.052000000000000005,0 33 | -0.028999999999999998,0.053000000000000005,0 34 | -0.028999999999999998,0.054000000000000006,0 35 | -0.032,0.05700000000000001,0 36 | -0.032,0.05800000000000001,0 37 | -0.04099999999999998,0.06700000000000002,0 38 | -0.04199999999999998,0.06700000000000002,0 39 | -0.043999999999999984,0.06900000000000002,0 40 | -0.044999999999999984,0.06900000000000002,0 41 | -0.046999999999999986,0.071,0 42 | -0.04799999999999999,0.071,0 43 | -0.04899999999999999,0.072,0 44 | -0.04999999999999999,0.072,0 45 | -0.05099999999999999,0.073,0 46 | -0.05199999999999999,0.073,0 47 | -0.05299999999999999,0.074,0 48 | -0.05399999999999999,0.074,0 49 | -0.05499999999999999,0.075,0 50 | -0.056999999999999995,0.075,0 51 | -0.057999999999999996,0.076,0 52 | -0.061,0.076,0 53 | -0.062,0.077,0 54 | -0.06899999999999998,0.077,0 55 | -0.06999999999999998,0.078,0 56 | -0.07899999999999999,0.078,0 57 | -0.07999999999999999,0.079,0 58 | -0.08199999999999999,0.079,0 59 | -0.08299999999999999,0.08,0 60 | -0.08399999999999999,0.08,0 61 | -0.08499999999999999,0.081,0 62 | -0.086,0.081,0 63 | -0.087,0.082,0 64 | -0.088,0.082,0 65 | -0.094,0.08800000000000001,0 66 | -0.094,0.08900000000000001,0 67 | -0.096,0.09100000000000001,0 68 | -0.096,0.09200000000000001,0 69 | -0.09699999999999998,0.09300000000000001,0 70 | -0.09699999999999998,0.09500000000000001,0 71 | -0.09799999999999998,0.09600000000000002,0 72 | -0.09799999999999998,0.09900000000000002,0 73 | -0.09899999999999998,0.10000000000000002,0 74 | -0.09899999999999998,0.109,0 75 | -0.09799999999999998,0.11,0 76 | -0.09799999999999998,0.113,0 77 | -0.09699999999999998,0.114,0 78 | -0.09699999999999998,0.116,0 79 | -0.096,0.117,0 80 | -0.096,0.11800000000000001,0 81 | -0.094,0.12000000000000001,0 82 | -0.094,0.12100000000000001,0 83 | -0.088,0.127,0 84 | -0.087,0.127,0 85 | -0.08499999999999999,0.129,0 86 | -0.08299999999999999,0.129,0 87 | -0.08199999999999999,0.13,0 88 | -0.07999999999999999,0.13,0 89 | -0.07899999999999999,0.131,0 90 | -0.07499999999999998,0.131,0 91 | -0.07399999999999998,0.132,0 92 | -0.063,0.132,0 93 | -0.062,0.133,0 94 | -0.059,0.133,0 95 | -0.057999999999999996,0.134,0 96 | -0.055999999999999994,0.134,0 97 | -0.05499999999999999,0.135,0 98 | -0.05299999999999999,0.135,0 99 | -0.05199999999999999,0.136,0 100 | -0.05099999999999999,0.136,0 101 | -0.04999999999999999,0.137,0 102 | -0.04899999999999999,0.137,0 103 | -0.04799999999999999,0.138,0 104 | -0.046999999999999986,0.138,0 105 | -0.044999999999999984,0.14,0 106 | -0.043999999999999984,0.14,0 107 | -0.04099999999999998,0.14300000000000002,0 108 | -0.03999999999999998,0.14300000000000002,0 109 | -0.032,0.15100000000000002,0 110 | -0.032,0.15200000000000002,0 111 | -0.03,0.15400000000000003,0 112 | -0.03,0.15500000000000003,0 113 | -0.027999999999999997,0.15700000000000003,0 114 | -0.027999999999999997,0.15800000000000003,0 115 | -0.026999999999999996,0.15900000000000003,0 116 | -0.026999999999999996,0.16000000000000003,0 117 | -0.024999999999999994,0.16200000000000003,0 118 | -0.024999999999999994,0.16400000000000003,0 119 | -0.023999999999999994,0.16500000000000004,0 120 | -0.023999999999999994,0.16700000000000004,0 121 | -0.022999999999999993,0.16800000000000004,0 122 | -0.022999999999999993,0.17000000000000004,0 123 | -0.021999999999999992,0.17100000000000004,0 124 | -0.021999999999999992,0.17600000000000005,0 125 | -0.02099999999999999,0.17700000000000005,0 126 | -0.02099999999999999,0.188,0 127 | -0.01999999999999999,0.189,0 128 | -0.01999999999999999,0.191,0 129 | -0.01899999999999999,0.192,0 130 | -0.01899999999999999,0.194,0 131 | -0.017999999999999988,0.195,0 132 | -0.017999999999999988,0.196,0 133 | -0.016999999999999987,0.197,0 134 | -0.016999999999999987,0.198,0 135 | -0.009999999999999981,0.20500000000000002,0 136 | -0.00899999999999998,0.20500000000000002,0 137 | -0.00799999999999998,0.20600000000000002,0 138 | -0.0069999999999999785,0.20600000000000002,0 139 | -0.005999999999999978,0.20700000000000002,0 140 | -0.004999999999999977,0.20700000000000002,0 141 | -0.003999999999999976,0.20800000000000002,0 142 | -0.001999999999999974,0.20800000000000002,0 143 | -0.0010000000000000009,0.20900000000000002,0 144 | 0.01200000000000001,0.20900000000000002,0 145 | 0.013000000000000012,0.20800000000000002,0 146 | 0.015000000000000013,0.20800000000000002,0 147 | 0.016000000000000014,0.20700000000000002,0 148 | 0.018000000000000016,0.20700000000000002,0 149 | 0.019000000000000017,0.20600000000000002,0 150 | 0.020000000000000018,0.20600000000000002,0 151 | 0.02200000000000002,0.20400000000000001,0 152 | 0.02300000000000002,0.20400000000000001,0 153 | 0.028000000000000025,0.199,0 154 | 0.028000000000000025,0.198,0 155 | 0.029000000000000026,0.197,0 156 | 0.029000000000000026,0.196,0 157 | 0.030000000000000027,0.195,0 158 | 0.030000000000000027,0.194,0 159 | 0.031000000000000028,0.193,0 160 | 0.031000000000000028,0.191,0 161 | 0.03200000000000003,0.19,0 162 | 0.03200000000000003,0.187,0 163 | 0.03300000000000003,0.186,0 164 | 0.03300000000000003,0.17900000000000005,0 165 | 0.03200000000000003,0.17800000000000005,0 166 | 0.03200000000000003,0.17500000000000004,0 167 | 0.031000000000000028,0.17400000000000004,0 168 | 0.031000000000000028,0.17100000000000004,0 169 | 0.030000000000000027,0.17000000000000004,0 170 | 0.030000000000000027,0.16900000000000004,0 171 | 0.028000000000000025,0.16700000000000004,0 172 | 0.028000000000000025,0.16600000000000004,0 173 | 0.02200000000000002,0.16000000000000003,0 174 | 0.02100000000000002,0.16000000000000003,0 175 | 0.020000000000000018,0.15900000000000003,0 176 | 0.019000000000000017,0.15900000000000003,0 177 | 0.018000000000000016,0.15800000000000003,0 178 | 0.017000000000000015,0.15800000000000003,0 179 | 0.016000000000000014,0.15700000000000003,0 180 | 0.014000000000000012,0.15700000000000003,0 181 | 0.013000000000000012,0.15600000000000003,0 182 | 0.009000000000000008,0.15600000000000003,0 183 | 0.008000000000000007,0.15500000000000003,0 184 | -0.002999999999999975,0.15500000000000003,0 185 | -0.003999999999999976,0.15400000000000003,0 186 | -0.0069999999999999785,0.15400000000000003,0 187 | -0.00799999999999998,0.15300000000000002,0 188 | -0.010999999999999982,0.15300000000000002,0 189 | -0.011999999999999983,0.15200000000000002,0 190 | -0.012999999999999984,0.15200000000000002,0 191 | -0.013999999999999985,0.15100000000000002,0 192 | -0.014999999999999986,0.15100000000000002,0 193 | -0.015999999999999986,0.15000000000000002,0 194 | -0.016999999999999987,0.15000000000000002,0 195 | -0.017999999999999988,0.14900000000000002,0 196 | -0.01899999999999999,0.14900000000000002,0 197 | -0.02099999999999999,0.14700000000000002,0 198 | -0.021999999999999992,0.14700000000000002,0 199 | -0.023999999999999994,0.14500000000000002,0 200 | -0.024999999999999994,0.14500000000000002,0 201 | -0.033999999999999975,0.136,0 202 | -0.033999999999999975,0.135,0 203 | -0.03699999999999998,0.132,0 204 | -0.03699999999999998,0.131,0 205 | -0.03799999999999998,0.13,0 206 | -0.03799999999999998,0.129,0 207 | -0.03899999999999998,0.128,0 208 | -0.03899999999999998,0.127,0 209 | -0.03999999999999998,0.126,0 210 | -0.03999999999999998,0.125,0 211 | -0.04099999999999998,0.12400000000000001,0 212 | -0.04099999999999998,0.12300000000000001,0 213 | -0.04199999999999998,0.12200000000000001,0 214 | -0.04199999999999998,0.12100000000000001,0 215 | -0.04299999999999998,0.12000000000000001,0 216 | -0.04299999999999998,0.117,0 217 | -0.043999999999999984,0.116,0 218 | -0.043999999999999984,0.112,0 219 | -0.044999999999999984,0.111,0 220 | -0.044999999999999984,0.09800000000000002,0 221 | -0.043999999999999984,0.09700000000000002,0 222 | -0.043999999999999984,0.09300000000000001,0 223 | -0.04299999999999998,0.09200000000000001,0 224 | -0.04299999999999998,0.08900000000000001,0 225 | -0.04199999999999998,0.08800000000000001,0 226 | -0.04199999999999998,0.08700000000000001,0 227 | -0.04099999999999998,0.08600000000000001,0 228 | -0.04099999999999998,0.085,0 229 | -0.03999999999999998,0.084,0 230 | -0.03999999999999998,0.083,0 231 | -0.03899999999999998,0.082,0 232 | -0.03899999999999998,0.081,0 233 | -0.03799999999999998,0.08,0 234 | -0.03799999999999998,0.079,0 235 | -0.035999999999999976,0.077,0 236 | -0.035999999999999976,0.076,0 237 | -0.032999999999999974,0.073,0 238 | -0.032999999999999974,0.072,0 239 | -0.025999999999999995,0.06500000000000002,0 240 | -0.024999999999999994,0.06500000000000002,0 241 | -0.021999999999999992,0.06200000000000001,0 242 | -0.02099999999999999,0.06200000000000001,0 243 | -0.01999999999999999,0.06100000000000001,0 244 | -0.01899999999999999,0.06100000000000001,0 245 | -0.016999999999999987,0.05900000000000001,0 246 | -0.015999999999999986,0.05900000000000001,0 247 | -0.014999999999999986,0.05800000000000001,0 248 | -0.013999999999999985,0.05800000000000001,0 249 | -0.012999999999999984,0.05700000000000001,0 250 | -0.010999999999999982,0.05700000000000001,0 251 | -0.009999999999999981,0.05600000000000001,0 252 | -0.00799999999999998,0.05600000000000001,0 253 | -0.0069999999999999785,0.05500000000000001,0 254 | -0.002999999999999975,0.05500000000000001,0 255 | -0.001999999999999974,0.054000000000000006,0 256 | 0.009000000000000008,0.054000000000000006,0 257 | 0.010000000000000009,0.053000000000000005,0 258 | 0.013000000000000012,0.053000000000000005,0 259 | 0.014000000000000012,0.052000000000000005,0 260 | 0.017000000000000015,0.052000000000000005,0 261 | 0.018000000000000016,0.051000000000000004,0 262 | 0.019000000000000017,0.051000000000000004,0 263 | 0.02100000000000002,0.049,0 264 | 0.02200000000000002,0.049,0 265 | 0.028000000000000025,0.043,0 266 | 0.028000000000000025,0.041999999999999996,0 267 | 0.030000000000000027,0.039999999999999994,0 268 | 0.030000000000000027,0.03899999999999999,0 269 | 0.031000000000000028,0.03800000000000002,0 270 | 0.031000000000000028,0.03600000000000002,0 271 | 0.03200000000000003,0.03500000000000002,0 272 | 0.03200000000000003,0.031000000000000014,0 273 | 0.03300000000000003,0.030000000000000013,0 274 | 0.03300000000000003,0.023000000000000007,0 275 | 0.03200000000000003,0.022000000000000006,0 276 | 0.03200000000000003,0.019000000000000003,0 277 | 0.031000000000000028,0.018000000000000002,0 278 | 0.031000000000000028,0.016,0 279 | 0.030000000000000027,0.015,0 280 | 0.030000000000000027,0.013999999999999999,0 281 | 0.029000000000000026,0.013000000000000012,0 282 | 0.029000000000000026,0.01200000000000001,0 283 | 0.027000000000000024,0.010000000000000009,0 284 | 0.027000000000000024,0.009000000000000008,0 285 | 0.02300000000000002,0.0050000000000000044,0 286 | 0.02200000000000002,0.0050000000000000044,0 287 | 0.020000000000000018,0.0030000000000000027,0 288 | 0.019000000000000017,0.0030000000000000027,0 289 | 0.018000000000000016,0.0020000000000000018,0 290 | 0.016000000000000014,0.0020000000000000018,0 291 | 0.015000000000000013,0.0010000000000000009,0 292 | 0.013000000000000012,0.0010000000000000009,0 293 | 0.01200000000000001,0.0,0 294 | -------------------------------------------------------------------------------- /include/arm_api2/moveit2_iface.hpp: -------------------------------------------------------------------------------- 1 | 2 | /******************************************************************************* 3 | * 4 | * Copyright (c) 2024, Crobotic Solutions d.o.o. (www.crobotics.tech) 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | /* Title : arm2_action_iface.hpp 35 | * Project : arm_api2 36 | * Created : 08/06/2025 37 | * Author : Filip Zoric 38 | * 39 | * Description : The core robot manipulator and MoveIt2! ROS 2 interfacing header class. 40 | */ 41 | 42 | #ifndef MOVEIT2_IFACE_HPP 43 | #define MOVEIT2_IFACE_HPP 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | //* yaml params 52 | #include 53 | 54 | //* ros 55 | #include 56 | #include "rclcpp_action/rclcpp_action.hpp" 57 | #include "rclcpp_components/register_node_macro.hpp" 58 | #include "tf2/LinearMath/Quaternion.h" 59 | #include "tf2/LinearMath/Matrix3x3.h" 60 | 61 | //* moveit 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | #include 69 | #include 70 | #include 71 | #include 72 | #include 73 | #include 74 | 75 | //* msgs 76 | #include "geometry_msgs/msg/pose_stamped.hpp" 77 | #include "geometry_msgs/msg/pose.hpp" 78 | #include "sensor_msgs/msg/joint_state.hpp" 79 | #include "arm_api2_msgs/msg/cartesian_waypoints.hpp" 80 | #include "moveit_msgs/msg/collision_object.hpp" 81 | #include "shape_msgs/msg/solid_primitive.hpp" 82 | 83 | //* srvs 84 | #include "arm_api2_msgs/srv/change_state.hpp" 85 | #include "arm_api2_msgs/srv/add_collision_object.hpp" 86 | #include "arm_api2_msgs/srv/set_vel_acc.hpp" 87 | #include "arm_api2_msgs/srv/set_string_param.hpp" 88 | #include "control_msgs/action/gripper_command.hpp" 89 | #include "arm_api2_msgs/action/move_cartesian.hpp" 90 | #include "arm_api2_msgs/action/move_joint.hpp" 91 | #include "arm_api2_msgs/action/move_cartesian_path.hpp" 92 | #include "std_srvs/srv/trigger.hpp" 93 | #include "std_srvs/srv/set_bool.hpp" 94 | 95 | // utils 96 | #include "arm_api2/utils.hpp" 97 | 98 | // For starters just include robotiq_gripper 99 | // TODO: Think of a way to include different gripper based on the gripper type 100 | #include "arm_api2/grippers/gripper.hpp" 101 | #include "arm_api2/grippers/robotiq_gripper.hpp" 102 | 103 | #define stringify( name ) #name 104 | 105 | // *std placeholders 106 | using namespace std::chrono_literals; 107 | using std::placeholders::_1; 108 | using std::placeholders::_2; 109 | 110 | class m2Iface: public rclcpp::Node 111 | { 112 | 113 | public: 114 | 115 | m2Iface(const rclcpp::NodeOptions &options); 116 | //~m2Iface(); 117 | 118 | /* namespace param, maybe redundant */ 119 | std::string ns_; 120 | 121 | private: 122 | 123 | /* node related stuff */ 124 | rclcpp::Node::SharedPtr node_; 125 | rclcpp::Executor::SharedPtr executor_; 126 | std::thread executor_thread_; 127 | 128 | /* gripper */ 129 | RobotiqGripper gripper; 130 | 131 | /* arm_definition */ 132 | std::string PLANNING_GROUP; 133 | std::string EE_LINK_NAME; 134 | std::string ROBOT_DESC; 135 | std::string PLANNING_SCENE; 136 | std::string PLANNING_FRAME; 137 | std::string MOVE_GROUP_NS; 138 | std::string JOINT_STATES; 139 | bool WITH_PLANNER; 140 | bool ENABLE_SERVO; 141 | float INIT_VEL_SCALING; 142 | float INIT_ACC_SCALING; 143 | 144 | /* timers */ 145 | rclcpp::TimerBase::SharedPtr timer_; 146 | 147 | /* parameters */ 148 | std::string config_path; 149 | bool enable_servo; 150 | bool eager_execution; 151 | float dt; 152 | float max_vel_scaling_factor; 153 | float max_acc_scaling_factor; 154 | 155 | /* planner info */ 156 | std::string current_planner_id_ = "pilz_industrial_motion_planner"; 157 | std::string current_planner_type_ = "LIN"; 158 | 159 | /* config_file */ 160 | YAML::Node config; 161 | /*config*/ 162 | YAML::Node init_config(std::string yaml_path); 163 | 164 | /* init methods */ 165 | void init_subscribers(); 166 | void init_publishers(); 167 | void init_services(); 168 | void init_actionservers(); 169 | void init_moveit(); 170 | std::unique_ptr init_servo(); 171 | 172 | /* subs */ 173 | rclcpp::Subscription::SharedPtr joint_state_sub_; 174 | 175 | /* pubs */ 176 | rclcpp::Publisher::SharedPtr pose_state_pub_; 177 | rclcpp::Publisher::SharedPtr robot_state_pub_; 178 | 179 | /* srvs */ 180 | rclcpp::Service::SharedPtr change_state_srv_; 181 | rclcpp::Service::SharedPtr set_vel_acc_srv_; 182 | rclcpp::Service::SharedPtr set_planner_srv_; 183 | rclcpp::Service::SharedPtr set_eelink_srv_; 184 | rclcpp::Service::SharedPtr set_plan_only_srv_; 185 | rclcpp::Service::SharedPtr add_collision_object_srv_; 186 | 187 | /* actions */ 188 | rclcpp_action::Server::SharedPtr move_to_joint_as_; 189 | rclcpp_action::Server::SharedPtr move_to_pose_as_; 190 | rclcpp_action::Server::SharedPtr move_to_pose_path_as_; 191 | rclcpp_action::Server::SharedPtr gripper_control_as_; 192 | 193 | /* topic callbacks */ 194 | void joint_state_cb(const sensor_msgs::msg::JointState::SharedPtr msg); 195 | 196 | /* srv callbacks*/ 197 | void change_state_cb(const std::shared_ptr req, 198 | const std::shared_ptr res); 199 | void set_vel_acc_cb(const std::shared_ptr req, 200 | const std::shared_ptr res); 201 | void set_planner_cb(const std::shared_ptr req, 202 | const std::shared_ptr res); 203 | void set_eelink_cb(const std::shared_ptr req, 204 | const std::shared_ptr res); 205 | void set_plan_only_cb(const std::shared_ptr req, 206 | const std::shared_ptr res); 207 | void add_collision_object_cb(const std::shared_ptr req, 208 | const std::shared_ptr res); 209 | 210 | /* action callbacks */ 211 | rclcpp_action::GoalResponse move_to_joint_goal_cb( 212 | const rclcpp_action::GoalUUID &uuid, 213 | std::shared_ptr goal); 214 | rclcpp_action::CancelResponse move_to_joint_cancel_cb( 215 | const std::shared_ptr> goal_handle); 216 | void move_to_joint_accepted_cb( 217 | std::shared_ptr> goal_handle); 218 | 219 | rclcpp_action::GoalResponse move_to_pose_goal_cb( 220 | const rclcpp_action::GoalUUID &uuid, 221 | std::shared_ptr goal); 222 | rclcpp_action::CancelResponse move_to_pose_cancel_cb( 223 | const std::shared_ptr> goal_handle); 224 | void move_to_pose_accepted_cb( 225 | std::shared_ptr> goal_handle); 226 | 227 | rclcpp_action::GoalResponse move_to_pose_path_goal_cb( 228 | const rclcpp_action::GoalUUID &uuid, 229 | std::shared_ptr goal); 230 | rclcpp_action::CancelResponse move_to_pose_path_cancel_cb( 231 | const std::shared_ptr> goal_handle); 232 | void move_to_pose_path_accepted_cb( 233 | std::shared_ptr> goal_handle); 234 | 235 | rclcpp_action::GoalResponse gripper_control_goal_cb( 236 | const rclcpp_action::GoalUUID &uuid, 237 | std::shared_ptr goal); 238 | rclcpp_action::CancelResponse gripper_control_cancel_cb( 239 | const std::shared_ptr> goal_handle); 240 | void gripper_control_accepted_cb( 241 | std::shared_ptr> goal_handle); 242 | 243 | bool run(); 244 | 245 | /* setters */ 246 | bool setMoveGroup(rclcpp::Node::SharedPtr nodePtr, std::string groupName, std::string moveNs); 247 | bool setRobotModel(rclcpp::Node::SharedPtr nodePtr); 248 | bool setPlanningSceneMonitor(rclcpp::Node::SharedPtr nodePtr, std::string name); 249 | 250 | /* getters */ 251 | void getArmState(); 252 | 253 | /* funcs */ 254 | void planAndExecJoint(); 255 | void planAndExecPose(); 256 | void planAndExecPosePath(); 257 | void printTimestamps(const moveit_msgs::msg::RobotTrajectory &trajectory); 258 | void addTimestampsToTrajectory(moveit_msgs::msg::RobotTrajectory &trajectory); 259 | bool planWithPlanner(moveit::planning_interface::MoveGroupInterface::Plan &plan, bool eagerExecution = true); 260 | 261 | // Simple state machine 262 | enum state{ 263 | IDLE = 0, 264 | JOINT_TRAJ_CTL = 1, 265 | CART_TRAJ_CTL = 2, 266 | SERVO_CTL = 3 267 | }; 268 | 269 | // stateNames 270 | const char* stateNames[4] = 271 | { 272 | stringify (IDLE), 273 | stringify (JOINT_TRAJ_CTL), 274 | stringify (CART_TRAJ_CTL), 275 | stringify (SERVO_CTL) 276 | }; 277 | 278 | // robot state 279 | enum state robotState = IDLE; 280 | 281 | /* flags*/ 282 | bool moveGroupInit = false; 283 | bool robotModelInit = false; 284 | bool pSceneMonitorInit = false; 285 | bool nodeInit = false; 286 | bool recivCmd = false; 287 | bool recivTraj = false; 288 | bool recivGripperCmd = false; 289 | bool servoEntered = false; 290 | bool async = false; 291 | bool planOnly = false; 292 | 293 | /* goal handles */ 294 | std::shared_ptr> m_moveToJointGoalHandle_; 295 | std::shared_ptr> m_moveToPoseGoalHandle_; 296 | std::shared_ptr> m_moveToPosePathGoalHandle_; 297 | std::shared_ptr> m_gripperControlGoalHandle_; 298 | 299 | /* ros vars */ 300 | std::vector m_currJointPosition; 301 | geometry_msgs::msg::PoseStamped m_currPoseState; 302 | std::vector m_cartesianWaypoints; 303 | 304 | 305 | moveit::planning_interface::MoveGroupInterfacePtr m_moveGroupPtr; 306 | moveit::core::RobotStatePtr m_robotStatePtr; 307 | moveit::core::RobotModelPtr kinematic_model; 308 | std::shared_ptr m_pSceneMonitorPtr; 309 | std::unique_ptr servoPtr; 310 | moveit::planning_interface::PlanningSceneInterfacePtr m_planningSceneInterface; 311 | 312 | }; 313 | 314 | #endif 315 | --------------------------------------------------------------------------------