diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..3e738c59f --- /dev/null +++ b/.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 diff --git a/src/cybertech_mock/CMakeLists.txt b/src/cybertech_mock/CMakeLists.txt new file mode 100644 index 000000000..ea411af10 --- /dev/null +++ b/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() diff --git a/src/cybertech_mock/README.md b/src/cybertech_mock/README.md new file mode 100644 index 000000000..63efae21f --- /dev/null +++ b/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/) diff --git a/src/cybertech_mock/config/config.yaml b/src/cybertech_mock/config/config.yaml new file mode 100644 index 000000000..0b1faaa32 --- /dev/null +++ b/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" diff --git a/src/cybertech_mock/config/control/ros2_control.yaml b/src/cybertech_mock/config/control/ros2_control.yaml new file mode 100644 index 000000000..353d0aa40 --- /dev/null +++ b/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 diff --git a/src/cybertech_mock/config/moveit/joint_limits.yaml b/src/cybertech_mock/config/moveit/joint_limits.yaml new file mode 100644 index 000000000..67a777a69 --- /dev/null +++ b/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 diff --git a/src/cybertech_mock/config/moveit/joint_limits_servo.yaml b/src/cybertech_mock/config/moveit/joint_limits_servo.yaml new file mode 100644 index 000000000..95449b432 --- /dev/null +++ b/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 diff --git a/src/cybertech_mock/config/moveit/kinematics.yaml b/src/cybertech_mock/config/moveit/kinematics.yaml new file mode 100644 index 000000000..454b152c3 --- /dev/null +++ b/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 diff --git a/src/cybertech_mock/config/moveit/kuka_cybertech.srdf b/src/cybertech_mock/config/moveit/kuka_cybertech.srdf new file mode 100644 index 000000000..03ca7b0a3 --- /dev/null +++ b/src/cybertech_mock/config/moveit/kuka_cybertech.srdf @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/cybertech_mock/config/moveit/moveit_controllers.yaml b/src/cybertech_mock/config/moveit/moveit_controllers.yaml new file mode 100644 index 000000000..780fb8f63 --- /dev/null +++ b/src/cybertech_mock/config/moveit/moveit_controllers.yaml @@ -0,0 +1,7 @@ +# MoveIt uses this configuration for controller management +trajectory_execution: + allowed_execution_duration_scaling: 1.2 + allowed_goal_duration_margin: 0.5 + allowed_start_tolerance: 0.01 + +moveit_controller_manager: moveit_ros_control_interface/Ros2ControlManager diff --git a/src/cybertech_mock/config/moveit/ompl_planning.yaml b/src/cybertech_mock/config/moveit/ompl_planning.yaml new file mode 100644 index 000000000..925027a7e --- /dev/null +++ b/src/cybertech_mock/config/moveit/ompl_planning.yaml @@ -0,0 +1,171 @@ +planning_plugins: + - ompl_interface/OMPLPlanner +# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath + +planner_configs: + AnytimePathShortening: + type: geometric::AnytimePathShortening + shortcut: 1 # Attempt to shortcut all new solution paths + hybridize: 1 # Compute hybrid solution trajectories + max_hybrid_paths: 36 # Number of hybrid paths generated per iteration + num_planners: 8 # The number of default planners to use for planning + planners: "RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 +manipulator: + default_planner_config: RRTConnect + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(joint_a1,joint_a2) + longest_valid_segment_fraction: 0.01 diff --git a/src/cybertech_mock/config/moveit/pose_ik_distance.yaml b/src/cybertech_mock/config/moveit/pose_ik_distance.yaml new file mode 100644 index 000000000..69e4c7c6f --- /dev/null +++ b/src/cybertech_mock/config/moveit/pose_ik_distance.yaml @@ -0,0 +1,5 @@ +manipulator: + kinematics_solver: pose_ik_plugin/PoseIKPlugin + target_tolerance: 0.001 + solve_mode: "optimize_distance" + optimization_timeout: 0.005 diff --git a/src/cybertech_mock/config/moveit/pose_jog.yaml b/src/cybertech_mock/config/moveit/pose_jog.yaml new file mode 100644 index 000000000..5ba7f244a --- /dev/null +++ b/src/cybertech_mock/config/moveit/pose_jog.yaml @@ -0,0 +1,4 @@ +# Planning groups to use in PoseJog, and their corresponding VFC controllers. +# The number of elements in `planning_groups` and `controllers` must match. +planning_groups: ['manipulator'] +controllers: ['velocity_force_controller'] diff --git a/src/cybertech_mock/config/moveit/servo.yaml b/src/cybertech_mock/config/moveit/servo.yaml new file mode 100644 index 000000000..2c0124297 --- /dev/null +++ b/src/cybertech_mock/config/moveit/servo.yaml @@ -0,0 +1,64 @@ +############################################### +# Modify all parameters related to servoing here +############################################### + +# Enable dynamic parameter updates +enable_parameter_update: true + +## Properties of incoming commands +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 +scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.8 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. + joint: 0.5 + +# Default to driving the arm at 50% maximum speed. +# override_velocity_scaling_factor: 0.5 + +## Properties of outgoing commands +publish_period: 0.01 # 1/Nominal publish rate [seconds] +max_expected_latency: 0.1 # delay between sending a command and the robot executing it [seconds] + +# What type of topic does your robot driver expect? +# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory +command_out_type: trajectory_msgs/JointTrajectory + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: true +publish_joint_accelerations: false + +## Plugins for smoothing outgoing commands +use_smoothing: true +smoothing_filter_plugin_name: "online_signal_smoothing::AccelerationLimitedPlugin" +acceleration_filter_update_period: 0.01 # [seconds] Must match the publish_period parameter + +## MoveIt properties +move_group_name: manipulator # Often 'manipulator' or 'arm' +acceleration_filter_planning_group_name: manipulator # Often 'manipulator' or 'arm' +is_primary_planning_scene_monitor: false # The MoveGroup node maintains the planning scene, so Servo needs to get its world info from there. +monitored_planning_scene_topic: /monitored_planning_scene + +## Stopping behaviour +incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command + +## Configure handling of singularities and joint limits +lower_singularity_threshold: 30.0 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 50.0 # Stop when the condition number hits this +joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # added as a buffer to joint limits [radians]. If moving quickly, make this larger. + +## Topic names +cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands +joint_topic: /joint_states +status_topic: ~/status # Publish status to this topic +command_out_topic: /servo_controller/joint_trajectory # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: true # Check collisions? +check_octomap_collisions: false # Check collisions with octomap? +collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. +self_collision_proximity_threshold: 0.006 # Start decelerating when a self-collision is this far [m] +scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] diff --git a/src/cybertech_mock/config/moveit/stomp_planning.yaml b/src/cybertech_mock/config/moveit/stomp_planning.yaml new file mode 100644 index 000000000..dc6fd444d --- /dev/null +++ b/src/cybertech_mock/config/moveit/stomp_planning.yaml @@ -0,0 +1,22 @@ +planning_plugins: + - stomp_moveit/StompPlanner +# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath + +stomp_moveit: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + exponentiated_cost_sensitivity: 0.8 + control_cost_weight: 0.1 + delta_t: 0.1 diff --git a/src/cybertech_mock/description/kuka_cybertech.urdf.xacro b/src/cybertech_mock/description/kuka_cybertech.urdf.xacro new file mode 100644 index 000000000..25481cc18 --- /dev/null +++ b/src/cybertech_mock/description/kuka_cybertech.urdf.xacro @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/cybertech_mock/launch/agent_bridge.launch.xml b/src/cybertech_mock/launch/agent_bridge.launch.xml new file mode 100644 index 000000000..b2aa75434 --- /dev/null +++ b/src/cybertech_mock/launch/agent_bridge.launch.xml @@ -0,0 +1,6 @@ + + + + diff --git a/src/cybertech_mock/launch/view_robot.launch.py b/src/cybertech_mock/launch/view_robot.launch.py new file mode 100644 index 000000000..b66d661ba --- /dev/null +++ b/src/cybertech_mock/launch/view_robot.launch.py @@ -0,0 +1,81 @@ +from launch import LaunchDescription +from launch.conditions import IfCondition +from launch.actions import DeclareLaunchArgument +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "standalone", + default_value="false", + description="Whether to start in standalone mode", + ) + ) + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("cybertech_mock"), + "description", + "kuka_cybertech.urdf.xacro", + ] + ), + " robot_name:=", + "kuka_cybertech", + " description_package:=", + "kuka_kr16_support", + " description_macro_file:=", + "kr16_2_macro.xacro", + " use_mock_hardware:=", + "true", + ] + ) + robot_description = {"robot_description": robot_description_content} + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("cybertech_mock"), "launch", "view_robot.rviz"] + ) + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[robot_description], + condition=IfCondition(LaunchConfiguration("standalone")), + ) + + joint_state_publisher_gui = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + name="joint_state_publisher_gui", + condition=IfCondition(LaunchConfiguration("standalone")), + ) + + rviz = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen", + ) + + nodes_to_start = [ + robot_state_publisher, + joint_state_publisher_gui, + rviz, + ] + + return LaunchDescription(declared_arguments + nodes_to_start) diff --git a/src/cybertech_mock/launch/view_robot.rviz b/src/cybertech_mock/launch/view_robot.rviz new file mode 100644 index 000000000..8a3272141 --- /dev/null +++ b/src/cybertech_mock/launch/view_robot.rviz @@ -0,0 +1,267 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 90 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1/Frames1 + Splitter Ratio: 0.5 + Tree Height: 515 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + grasp_link: + Alpha: 1 + Show Axes: false + Show Trail: false + link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base: + Value: false + base_link: + Value: false + flange: + Value: false + grasp_link: + Value: true + link_1: + Value: false + link_2: + Value: false + link_3: + Value: false + link_4: + Value: false + link_5: + Value: false + link_6: + Value: false + tool0: + Value: false + world: + Value: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + world: + base_link: + base: + {} + link_1: + link_2: + link_3: + link_4: + link_5: + link_6: + flange: + grasp_link: + {} + tool0: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7853981852531433 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000163000002a2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000047000002a2000000f900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000047000002a2000000c500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000030300fffffffb0000000800540069006d006501000000000000045000000000000000000000022e000002a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 60 + Y: 60 diff --git a/src/cybertech_mock/objectives/cycle.xml b/src/cybertech_mock/objectives/cycle.xml new file mode 100644 index 000000000..7695556f2 --- /dev/null +++ b/src/cybertech_mock/objectives/cycle.xml @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/src/cybertech_mock/package.xml b/src/cybertech_mock/package.xml new file mode 100644 index 000000000..ade04fd20 --- /dev/null +++ b/src/cybertech_mock/package.xml @@ -0,0 +1,34 @@ + + + cybertech_mock + 0.1.0 + + + Configuration package for the KUKA CYBERTECH manipulator family using mock + hardware. + + + MoveIt Pro Maintainer + + BSD-3-Clause + + ament_cmake + + kuka_experimental + + moveit_studio_agent + moveit_studio_behavior + + ament_lint_auto + + ament_clang_format + ament_clang_tidy + ament_cmake_copyright + ament_cmake_lint_cmake + picknik_ament_copyright + ament_flake8 + + + ament_cmake + + diff --git a/src/cybertech_mock/waypoints/waypoints.yaml b/src/cybertech_mock/waypoints/waypoints.yaml new file mode 100644 index 000000000..9eb673962 --- /dev/null +++ b/src/cybertech_mock/waypoints/waypoints.yaml @@ -0,0 +1,74 @@ +- description: '' + favorite: false + joint_group_names: + - manipulator + joint_state: + effort: [] + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + name: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + position: + - 0 + - -1.57 + - 1.57 + - 0 + - 1.57 + - 0 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Home +- description: '' + favorite: false + joint_group_names: + - manipulator + joint_state: + effort: [] + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + name: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + position: + - 0 + - -1.57 + - 0 + - 0 + - 0 + - 0 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Up diff --git a/src/external_dependencies/kuka_experimental b/src/external_dependencies/kuka_experimental new file mode 160000 index 000000000..2a48afa8d --- /dev/null +++ b/src/external_dependencies/kuka_experimental @@ -0,0 +1 @@ +Subproject commit 2a48afa8d0dd925712d2720b710772e3db3d49a5