Squashed 'src/fanuc_sim/' content from commit 0b1e054

git-subtree-dir: src/fanuc_sim
git-subtree-split: 0b1e054ccb6a4fa20b2658ad318ebc1acfd64bb6

CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.22)
+project(fanuc_sim)
+
+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()

CONTRIBUTING.md

@@ -0,0 +1,7 @@
+Any contribution that you make to this repository will
+be under the 3-Clause BSD License, as dictated by that
+[license](https://opensource.org/licenses/BSD-3-Clause).
+
+# Contributing to this Repository
+
+Thanks for getting involved! If you want to add to this repository, please reach out to [email protected].

LICENSE

@@ -0,0 +1,25 @@
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+
+   * Neither the name of the copyright holder nor the names of its
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.

README.md

@@ -0,0 +1,5 @@
+# Fanuc sim config
+
+A MoveIt Pro base configuration for a Fanuc LR Mate 200iD.
+
+For detailed documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/)

config/config.yaml

@@ -0,0 +1,152 @@
+###############################################################
+#
+#  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: "fanuc_sim"
+      path: "description/picknik_fanuc.xacro"
+    srdf:
+      package: "fanuc_sim"
+      path: "config/moveit/picknik_fanuc.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:
+        # Use "mock", "mujoco", or "real"
+      - hardware_interface: "mujoco"
+
+# 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: "fanuc_sim"
+    path: "config/moveit/ompl_planning.yaml"
+  stomp_planning:
+    package: "fanuc_sim"
+    path: "config/moveit/stomp_planning.yaml"
+  pilz_planning:
+    package: "fanuc_sim"
+    path: "config/moveit/pilz_industrial_motion_planner_planning.yaml"
+  kinematics:
+    package: "fanuc_sim"
+    path: "config/moveit/trac_ik_kinematics_distance.yaml"
+  servo:
+    package: "fanuc_sim"
+    path: "config/moveit/servo.yaml"
+  servo_kinematics:
+    package: "fanuc_sim"
+    path: "config/moveit/trac_ik_kinematics_speed.yaml"
+  joint_limits:
+    package: "fanuc_sim"
+    path: "config/moveit/joint_limits.yaml"
+  servo_joint_limits:
+    package: "fanuc_sim"
+    path: "config/moveit/joint_limits.yaml"
+  pilz_cartesian_limits:
+    package: "fanuc_sim"
+    path: "config/moveit/pilz_cartesian_limits.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: "fanuc_sim"
+    path: "config/control/picknik_fanuc.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_trajectory_controller"
+    - "servo_controller"
+    - "joint_state_broadcaster"
+    # - "servo_controller"
+  # Load but do not start these controllers so they can be activated later if needed.
+  # [Optional, default=[]]
+  controllers_inactive_at_startup: []
+  # Any controllers here will not be spawned by MoveIt Pro.
+  # [Optional, default=[]]
+  controllers_not_managed: []
+    # - "joint_trajectory_controller"
+    # - "servo_controller"
+    # - "joint_state_broadcaster"
+  # Optionally configure remapping rules to let multiple controllers receive commands on the same topic.
+  # [Optional, default=[]]
+  controller_shared_topics: []
+
+# 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"
+  # Specify source folder for objectives
+  # [Required]
+  objective_library_paths:
+    core:
+      package_name: "fanuc_sim"
+      relative_path: "objectives"
+  # Specify the location of the saved waypoints file.
+  # [Required]
+  waypoints_file:
+    package_name: "fanuc_sim"
+    relative_path: "waypoints/waypoints.yaml"

config/control/picknik_fanuc.ros2_control.yaml

@@ -0,0 +1,78 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 600  # Hz
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+    servo_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+joint_state_broadcaster:
+  ros__parameters:
+    use_local_topics: false
+    joints:
+      - joint_1
+      - joint_2
+      - joint_3
+      - joint_4
+      - joint_5
+      - joint_6
+    interfaces:
+      - position
+      - velocity
+
+joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - joint_1
+      - joint_2
+      - joint_3
+      - joint_4
+      - joint_5
+      - joint_6
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+
+servo_controller:
+  ros__parameters:
+    joints:
+      - joint_1
+      - joint_2
+      - joint_3
+      - joint_4
+      - joint_5
+      - joint_6
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    command_joints:
+      - joint_1
+      - joint_2
+      - joint_3
+      - joint_4
+      - joint_5
+      - joint_6
+    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_1:
+        goal: 0.05
+      joint_2:
+        goal: 0.05
+      joint_3:
+        goal: 0.05
+      joint_4:
+        goal: 0.05
+      joint_5:
+        goal: 0.05
+      joint_6:
+        goal: 0.05

config/initial_positions.yaml

@@ -0,0 +1,9 @@
+# Default initial positions for fanuc_lrmate200id's ros2_control fake system
+
+initial_positions:
+  joint_1: 0
+  joint_2: 0
+  joint_3: 0
+  joint_4: 0
+  joint_5: 0
+  joint_6: 0

config/moveit/bioik_kinematics.yaml

@@ -0,0 +1,8 @@
+manipulator:
+  kinematics_solver: bio_ik/BioIKKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.005
+  kinematics_solver_attempts: 3
+  mode: gd_c
+  dpos: 0.0001
+  drot: 0.0001

config/moveit/joint_limits.yaml

@@ -0,0 +1,40 @@
+# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+
+# For beginners, we downscale velocity and acceleration limits.
+# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
+default_velocity_scaling_factor: 0.1
+default_acceleration_scaling_factor: 0.1
+
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+joint_limits:
+  joint_1:
+    has_velocity_limits: true
+    max_velocity: 7.8539816339744828
+    has_acceleration_limits: true
+    max_acceleration: 10.0
+  joint_2:
+    has_velocity_limits: true
+    max_velocity: 6.6322511575784526
+    has_acceleration_limits: true
+    max_acceleration: 10.0
+  joint_3:
+    has_velocity_limits: true
+    max_velocity: 9.0757121103705138
+    has_acceleration_limits: true
+    max_acceleration: 10.0
+  joint_4:
+    has_velocity_limits: true
+    max_velocity: 9.5993108859688121
+    has_acceleration_limits: true
+    max_acceleration: 10.0
+  joint_5:
+    has_velocity_limits: true
+    max_velocity: 9.5120444233690957
+    has_acceleration_limits: true
+    max_acceleration: 10.0
+  joint_6:
+    has_velocity_limits: true
+    max_velocity: 17.453292519943297
+    has_acceleration_limits: true
+    max_acceleration: 10.0