Cybertech mock config

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "src/external_dependencies/kuka_experimental"]
+	path = src/external_dependencies/kuka_experimental
+	url = https://github.com/b-robotized-forks/kuka_experimental.git

src/cybertech_mock/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.22)
+project(cybertech_mock)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY
+    config
+    description
+    launch
+    objectives
+    waypoints
+  DESTINATION
+    share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/cybertech_mock/README.md

@@ -0,0 +1,9 @@
+# KUKA CYBERTECH Mock Config
+
+A MoveIt Pro mock configuration for a KUKA CYBERTECH robot.
+
+To change the model of robot, simply change the `robot_model` urdf parameter  in the `config.yaml` file to a model described in `kuka_experimental`.
+
+This config is for running Moveit Pro with ROS2 control mock hardware and does not start up with a GPIO controller.
+
+For detailed documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/)

src/cybertech_mock/config/config.yaml

@@ -0,0 +1,149 @@
+###############################################################
+#
+#  This configures the robot to work with MoveIt Pro
+#
+###############################################################
+
+# Baseline hardware configuration parameters for MoveIt Pro.
+# [Required]
+hardware:
+  # Set simulated to false if you are using this as a configuration for real hardware.
+  # This allows users to switch between mock and real hardware by changing a single parameter with config inheritance.
+  # [Required]
+  simulated: true
+  # If simulated is false, launch the following file:
+  # [Optional, defaults to a blank launch file if not specified]
+  robot_driver_persist_launch_file:
+    package: "moveit_studio_agent"
+    path: "launch/blank.launch.py"
+
+  # If the MoveIt Pro Agent should launch the ros2 controller node.
+  # [Optional, default=True]
+  launch_control_node: True
+
+  # If the MoveIt Pro Agent should launch the robot state publisher.
+  # This should be false if you are launching the robot state publisher as part of drivers.
+  # [Optional, default=True]
+  launch_robot_state_publisher: True
+
+
+  # Parameters used to configure the robot description through XACRO.
+  # A URDF and SRDF are both required.
+  # [Required]
+  robot_description:
+    urdf:
+      package: "cybertech_mock"
+      path: "description/kuka_cybertech.urdf.xacro"
+    srdf:
+      package: "cybertech_mock"
+      path: "config/moveit/kuka_cybertech.srdf"
+    # Specify any additional parameters required for the URDF.
+    # Many of these are specific to the descriptions packages, and can be customized as needed.
+    # [Optional]
+    urdf_params:
+      # If you'd like to use a different CYBERTECH model, enter it here!
+      - robot_name: "kuka_cybertech"
+      - description_package: "kuka_kr16_support"
+      - description_macro_file: "kr16_2_macro.xacro"
+      - use_mock_hardware: "true"
+
+# Sets ROS global params for launch.
+# [Optional]
+ros_global_params:
+  # Whether or not to use simulated time.
+  # [Optional, default=False]
+  use_sim_time: False
+
+# Configuration files for MoveIt.
+# For more information, refer to https://moveit.picknik.ai/main/doc/how_to_guides/moveit_configuration/moveit_configuration_tutorial.html
+# [Required]
+moveit_params:
+  # Used by the Waypoint Manager to save joint states from this joint group.
+  joint_group_name: "manipulator"
+
+  ompl_planning:
+    package: "cybertech_mock"
+    path: "config/moveit/ompl_planning.yaml"
+  stomp_planning:
+    package: "cybertech_mock"
+    path: "config/moveit/stomp_planning.yaml"
+  kinematics:
+    package: "cybertech_mock"
+    path: "config/moveit/pose_ik_distance.yaml"
+  servo:
+    package: "cybertech_mock"
+    path: "config/moveit/servo.yaml"
+  joint_limits:
+    package: "cybertech_mock"
+    path: "config/moveit/joint_limits.yaml"
+  servo_joint_limits:
+    package: "cybertech_mock"
+    path: "config/moveit/joint_limits_servo.yaml"
+  pose_jog:
+    package: "cybertech_mock"
+    path: "config/moveit/pose_jog.yaml"
+
+  publish:
+    planning_scene: True
+    geometry_updates: True
+    state_updates: True
+    transforms_updates: True
+
+  trajectory_execution:
+    manage_controllers: True
+    allowed_execution_duration_scaling: 2.0
+    allowed_goal_duration_margin: 5.0
+    allowed_start_tolerance: 0.01
+
+# Configuration for launching ros2_control processes.
+# [Required, if using ros2_control]
+ros2_control:
+  config:
+    package: "cybertech_mock"
+    path: "config/control/ros2_control.yaml"
+  # MoveIt Pro will load and activate these controllers at start up to ensure they are available.
+  # If not specified, it is up to the user to ensure the appropriate controllers are active and available
+  # for running the application.
+  # [Optional, default=[]]
+  controllers_active_at_startup:
+    - "joint_state_broadcaster"
+    - "joint_trajectory_controller"
+  # Load but do not start these controllers so they can be activated later if needed.
+  # [Optional, default=[]]
+  controllers_inactive_at_startup:
+    - "velocity_force_controller"
+    - "servo_controller"
+# Configuration for loading behaviors and objectives.
+# [Required]
+objectives:
+  # List of plugins for loading custom behaviors.
+  # [Required]
+  behavior_loader_plugins:
+    # This plugin will load the core MoveIt Pro Behaviors.
+    # Add additional plugin loaders as needed.
+    core:
+      - "moveit_studio::behaviors::CoreBehaviorsLoader"
+      - "moveit_studio::behaviors::MTCCoreBehaviorsLoader"
+      - "moveit_studio::behaviors::ServoBehaviorsLoader"
+      - "moveit_studio::behaviors::VisionBehaviorsLoader"
+      - "moveit_studio::behaviors::ConverterBehaviorsLoader"
+  # Specify source folder for objectives
+  # [Required]
+  objective_library_paths:
+    core_objectives:
+      package_name: "moveit_pro_objectives"
+      relative_path: "objectives/core"
+    motion_objectives:
+      package_name: "moveit_pro_objectives"
+      relative_path: "objectives/motion"
+    perception_objectives:
+      package_name: "moveit_pro_objectives"
+      relative_path: "objectives/perception"
+    mock_objectives:
+      package_name: "cybertech_mock"
+      relative_path: "objectives"
+  # Specify the location of the saved waypoints file.
+  # [Required]
+  waypoints_file:
+    package_name: "cybertech_mock"
+    relative_path: "waypoints/waypoints.yaml"

src/cybertech_mock/config/control/ros2_control.yaml

@@ -0,0 +1,133 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+    servo_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+    velocity_force_controller:
+      type: velocity_force_controller/VelocityForceController
+
+joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - joint_a1
+      - joint_a2
+      - joint_a3
+      - joint_a4
+      - joint_a5
+      - joint_a6
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    # Set to true because the goal we send when joint jogging sends the current velocity (which can be small values e.g. 2e-6). These small velocities are small enough that they get clipped for the actual end goal.
+    allow_nonzero_velocity_at_trajectory_end: true
+    constraints:
+      stopped_velocity_tolerance: 0.0
+      goal_time: 0.0
+      joint_a1:
+        goal: 0.05
+        trajectory: 0.25
+      joint_a2:
+        goal: 0.05
+        trajectory: 0.25
+      joint_a3:
+        goal: 0.05
+        trajectory: 0.25
+      joint_a4:
+        goal: 0.05
+        trajectory: 0.25
+      joint_a5:
+        goal: 0.05
+        trajectory: 0.25
+      joint_a6:
+        goal: 0.05
+        trajectory: 0.25
+
+servo_controller:
+  ros__parameters:
+    joints:
+      - joint_a1
+      - joint_a2
+      - joint_a3
+      - joint_a4
+      - joint_a5
+      - joint_a6
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.0
+      goal_time: 0.0
+      joint_a1:
+        goal: 0.05
+      joint_a2:
+        goal: 0.05
+      joint_a3:
+        goal: 0.05
+      joint_a4:
+        goal: 0.05
+      joint_a5:
+        goal: 0.05
+      joint_a6:
+        goal: 0.05
+
+velocity_force_controller:
+  ros__parameters:
+    # Joint group to control.
+    planning_group_name: manipulator
+    # The tip link of the kinematic chain, i.e. the frame that will be controlled.
+    ee_frame: grasp_link
+    # The frame where the force / torque sensor reading is given.
+    # (Needs to exist in the kinematic chain).
+    sensor_frame: grasp_link
+    # Maximum joint-space velocities.
+    max_joint_velocity:
+      - 1.0
+      - 1.0
+      - 1.0
+      - 1.0
+      - 1.0
+      - 1.0
+    # Maximum joint-space accelerations.
+    max_joint_acceleration:
+      - 2.0
+      - 2.0
+      - 2.0
+      - 2.0
+      - 2.0
+      - 2.0
+    # Maximum Cartesian-space velocities.
+    max_cartesian_velocity:
+      - 0.25
+      - 0.25
+      - 0.25
+      - 1.0
+      - 1.0
+      - 1.0
+    # Maximum Cartesian-space accelerations.
+    max_cartesian_acceleration:
+      - 2.0
+      - 2.0
+      - 2.0
+      - 4.0
+      - 4.0
+      - 4.0
+    # Rate in Hz at which the controller will publish the state.
+    state_publish_rate: 10
+    # Damping coefficient for the Jacobian damped least-squares inverse.
+    jacobian_damping: 0.005
+    # Timeout in seconds after which the controller will stop motion if no new commands are received.
+    command_timeout: 0.2
+    # Padding (in radians) to add to joint position limits as a safety margin.
+    joint_limit_position_tolerance: 0.02

src/cybertech_mock/config/moveit/joint_limits.yaml

@@ -0,0 +1,39 @@
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+joint_limits:
+  joint_a1:
+    has_velocity_limits: true
+    max_velocity: 1.832596
+    has_acceleration_limits: true
+    max_acceleration: 3.0
+    max_deceleration: 3.0
+  joint_a2:
+    has_velocity_limits: true
+    max_velocity: 1.640609
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+    max_deceleration: 5.0
+  joint_a3:
+    has_velocity_limits: true
+    max_velocity: 1.745329
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+    max_deceleration: 5.0
+  joint_a4:
+    has_velocity_limits: true
+    max_velocity: 2.373648
+    has_acceleration_limits: true
+    max_acceleration: 3.0
+    max_deceleration: 3.0
+  joint_a5:
+    has_velocity_limits: true
+    max_velocity: 2.251475
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+    max_deceleration: 5.0
+  joint_a6:
+    has_velocity_limits: true
+    max_velocity: 3.595378
+    has_acceleration_limits: true
+    max_acceleration: 7.0
+    max_deceleration: 7.0

src/cybertech_mock/config/moveit/joint_limits_servo.yaml

@@ -0,0 +1,33 @@
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+joint_limits:
+  joint_a1:
+    has_velocity_limits: true
+    max_velocity: 1.0
+    has_acceleration_limits: true
+    max_acceleration: 0.5
+  joint_a2:
+    has_velocity_limits: true
+    max_velocity: 1.0
+    has_acceleration_limits: true
+    max_acceleration: 0.5
+  joint_a3:
+    has_velocity_limits: true
+    max_velocity: 1.0
+    has_acceleration_limits: true
+    max_acceleration: 0.5
+  joint_a4:
+    has_velocity_limits: true
+    max_velocity: 1.5
+    has_acceleration_limits: true
+    max_acceleration: 0.5
+  joint_a5:
+    has_velocity_limits: true
+    max_velocity: 1.5
+    has_acceleration_limits: true
+    max_acceleration: 0.5
+  joint_a6:
+    has_velocity_limits: true
+    max_velocity: 1.5
+    has_acceleration_limits: true
+    max_acceleration: 0.5

src/cybertech_mock/config/moveit/kinematics.yaml

@@ -0,0 +1,4 @@
+manipulator:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.005