Sync V6.5 to main (#29)

* Syncing 6.5 to main

src/fanuc_sim/CMakeLists.txt

@@ -14,7 +14,6 @@ set(DEST_DIR "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/description/")
 install(DIRECTORY "${PICKNIK_ACCESSORIES_SHARE_DIR}"
         DESTINATION "${DEST_DIR}"
         FILES_MATCHING PATTERN "*")
-
 install(
   DIRECTORY
     config

src/lab_sim/config/config.yaml

@@ -42,7 +42,6 @@ objectives:
       - "moveit_studio::behaviors::MTCCoreBehaviorsLoader"
       - "moveit_studio::behaviors::ServoBehaviorsLoader"
       - "moveit_studio::behaviors::VisionBehaviorsLoader"
-      - "example_behaviors::ExampleBehaviorsLoader"
   # Specify source folder for objectives
   # [Required]
   objective_library_paths:

src/lab_sim/objectives/pick_and_place_example.xml

@@ -24,6 +24,7 @@
           task="{mtc_pick_task}"
         />
         <Action ID="SetupMTCCurrentState" task="{mtc_pick_task}" />
+        <!-- The following behavior can be found in the example_behaviors package. Remove the COLCON_IGNORE file and `moveit_pro build user_workspace` to use. -->
         <Action
           ID="SetupMtcPickFromPose"
           grasp_pose="{pick_pose}"
@@ -51,6 +52,7 @@
           orientation_xyzw="0;1;0;0"
           stamped_pose="{place_pose}"
         />
+        <!-- The following behavior can be found in the example_behaviors package. Remove the COLCON_IGNORE file and `moveit_pro build user_workspace` to use. -->
         <Action
           ID="SetupMtcPlaceFromPose"
           place_pose="{place_pose}"

src/lab_sim/objectives/plan_and_save_trajectory.xml

@@ -39,6 +39,7 @@
       />
       <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}" />
       <Action ID="WaitForUserTrajectoryApproval" />
+      <!-- The following behavior can be found in the example_behaviors package. Remove the COLCON_IGNORE file and `moveit_pro build user_workspace` to use. -->
       <Action
         ID="ConvertMtcSolutionToJointTrajectory"
         joint_trajectory="{joint_trajectory_msg}"

src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/kortex_robot.xacro

@@ -0,0 +1,97 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="load_robot" params="
+    parent
+    *origin
+    prefix
+    arm
+    gripper
+    gripper_joint_name
+    dof
+    vision
+    robot_ip
+    username
+    password
+    port
+    port_realtime
+    session_inactivity_timeout_ms
+    connection_inactivity_timeout_ms
+    use_internal_bus_gripper_comm:=false
+    use_fake_hardware:=false
+    fake_sensor_commands:=false
+    sim_gazebo:=false
+    sim_ignition:=false
+    sim_isaac:=false
+    isaac_joint_commands:=/isaac_joint_commands
+    isaac_joint_states:=/isaac_joint_states
+    use_external_cable:=false
+    initial_positions:=${dict(joint_1=0.0,joint_2=0.0,joint_3=0.0,joint_4=0.0,joint_5=0.0,joint_6=0.0,joint_7=0.0)}
+    gripper_max_velocity:=100.0
+    gripper_max_force:=100.0
+    gripper_com_port:=/dev/ttyUSB0
+    moveit_active:=false">
+
+    <!-- Include and load arm macro files -->
+    <xacro:include filename="$(find kortex_description)/arms/${arm}/${dof}dof/urdf/${arm}_macro.xacro" />
+
+    <!-- Load the arm -->
+    <xacro:load_arm
+      parent="${parent}"
+      dof="${dof}"
+      vision="${vision}"
+      robot_ip="${robot_ip}"
+      username="${username}"
+      password="${password}"
+      port="${port}"
+      port_realtime="${port_realtime}"
+      session_inactivity_timeout_ms="${session_inactivity_timeout_ms}"
+      connection_inactivity_timeout_ms="${connection_inactivity_timeout_ms}"
+      prefix="${prefix}"
+      use_internal_bus_gripper_comm="${use_internal_bus_gripper_comm}"
+      use_fake_hardware="${use_fake_hardware}"
+      fake_sensor_commands="${fake_sensor_commands}"
+      sim_gazebo="${sim_gazebo}"
+      sim_ignition="${sim_ignition}"
+      sim_isaac="${sim_isaac}"
+      gripper_joint_name="${gripper_joint_name}"
+      gripper_max_velocity="${gripper_max_velocity}"
+      gripper_max_force="${gripper_max_force}"
+      use_external_cable="${use_external_cable}"
+      initial_positions="${initial_positions}">
+      <xacro:insert_block name="origin" />
+    </xacro:load_arm>
+
+    <!-- If no gripper, define tool frame here -->
+    <xacro:if value="${not gripper}">
+      <link name="${prefix}tool_frame"/>
+      <joint name="${prefix}tool_frame_joint" type="fixed">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <parent link="${prefix}${last_arm_link}" />
+        <child link="${prefix}tool_frame" />
+        <axis xyz="0 0 0" />
+      </joint>
+    </xacro:if>
+
+    <!-- Include and load the gripper if defined -->
+    <xacro:unless value="${not gripper}">
+      <xacro:include filename="$(find kinova_gen3_base_config)/description/robotiq_2f_85_macro.xacro" />
+      <!-- last_arm_link is defined in "$(find kortex_description)/arms/${arm}/urdf/${arm}_macro.xacro" -->
+      <xacro:load_gripper
+        parent="${prefix}${last_arm_link}"
+        prefix="${prefix}"
+        use_fake_hardware="${use_fake_hardware}"
+        fake_sensor_commands="${fake_sensor_commands}"
+        sim_ignition="${sim_ignition}"
+        sim_isaac="${sim_isaac}"
+        isaac_joint_commands="${isaac_joint_commands}"
+        isaac_joint_states="${isaac_joint_states}"
+        com_port="${gripper_com_port}"
+        use_internal_bus_gripper_comm="${use_internal_bus_gripper_comm}"
+        moveit_active="${moveit_active}"/>
+    </xacro:unless>
+
+  </xacro:macro>
+
+</robot>

src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/picknik_kinova_gen3.xacro

@@ -26,7 +26,7 @@
     <xacro:arg name="wrist_realsense" default="true" />
 
     <!-- Import macros for main hardware components -->
-    <xacro:include filename="$(find kortex_description)/robots/kortex_robot.xacro" />
+    <xacro:include filename="$(find kinova_gen3_base_config)/description/kortex_robot.xacro" />
     <xacro:include filename="$(find picknik_accessories)/descriptions/sensors/realsense_d415.urdf.xacro"/>
 
     <!-- Import environment macros -->

src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/robotiq_2f_85_macro.xacro

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<robot name="robotiq_2f_85_model" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="load_gripper" params="
+    parent
+    prefix
+    use_fake_hardware:=false
+    fake_sensor_commands:=false
+    sim_ignition:=false
+    sim_isaac:=false
+    isaac_joint_commands:=/isaac_joint_commands
+    isaac_joint_states:=/isaac_joint_states
+    use_internal_bus_gripper_comm:=false
+    com_port:=/dev/ttyUSB0
+    moveit_active:=false">
+    <xacro:include filename="$(find robotiq_description)/urdf/robotiq_2f_85_macro.urdf.xacro" />
+
+    <!-- Hardware talks directly to the gripper so we don't need ros2_control unless we are simulating -->
+    <xacro:property name="include_ros2_control" value="false"/>
+    <xacro:if value="${sim_ignition or sim_isaac or use_fake_hardware or not use_internal_bus_gripper_comm}">
+      <xacro:property name="include_ros2_control" value="true"/>
+    </xacro:if>
+
+    <xacro:robotiq_gripper
+        name="RobotiqGripperHardwareInterface"
+        prefix="${prefix}"
+        parent="${parent}"
+        include_ros2_control="${include_ros2_control}"
+        com_port="${com_port}"
+        use_fake_hardware="${use_fake_hardware}"
+        mock_sensor_commands="${fake_sensor_commands}"
+        sim_ignition="${sim_ignition}"
+        sim_isaac="${sim_isaac}"
+        isaac_joint_commands="${isaac_joint_commands}"
+        isaac_joint_states="${isaac_joint_states}">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:robotiq_gripper>
+  </xacro:macro>
+</robot>

src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/00_solution_-_move_to_initial_pose.xml

@@ -1,4 +1,3 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root
   BTCPP_format="4"
   main_tree_to_execute="00 Solution - Move to Initial Pose"
@@ -7,7 +6,7 @@
   <BehaviorTree
     ID="00 Solution - Move to Initial Pose"
     _description="Move to a pre-defined waypoint"
-    _favorite="true"
+    _favorite="false"
     _hardcoded="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">

src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/01_solution_-_create_a_pose.xml

@@ -1,10 +1,9 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="01 Solution - Create a Pose">
   <!--//////////-->
   <BehaviorTree
     ID="01 Solution - Create a Pose"
     _description="Creates a pose relative to a frame"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <Action

src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/02_solution_-_visualize_a_pose.xml

@@ -1,10 +1,9 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="02 Solution - Visualize a Pose">
   <!--//////////-->
   <BehaviorTree
     ID="02 Solution - Visualize a Pose"
     _description="Creates a pose and visualizes it"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <Action

src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/03_solution_-_pose_2m_above_the_robot.xml

@@ -1,4 +1,3 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root
   BTCPP_format="4"
   main_tree_to_execute="03 Solution - Pose 2m above the robot"
@@ -7,7 +6,7 @@
   <BehaviorTree
     ID="03 Solution - Pose 2m above the robot"
     _description="Creates a visualizes a pose 2m right above the robot 'base_link'"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <Action

src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/05_solution_-_define_a_path.xml

@@ -1,10 +1,9 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="05 Solution - Define a path">
   <!--//////////-->
   <BehaviorTree
     ID="05 Solution - Define a path"
     _description="Creates a simple path with two waypoints"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <Action

src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/06_solution_-_visualize_a_path.xml

@@ -1,10 +1,9 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="06 Solution - Visualize a path">
   <!--//////////-->
   <BehaviorTree
     ID="06 Solution - Visualize a path"
     _description="How to visualize a path"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <Action

src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/07_solution_-_plan_motion_for_a_path.xml

@@ -1,4 +1,3 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root
   BTCPP_format="4"
   main_tree_to_execute="07 Solution - Plan motion for a path"
@@ -7,7 +6,7 @@
   <BehaviorTree
     ID="07 Solution - Plan motion for a path"
     _description="Given a path, plans the motion for the robot joints"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <Action

src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/08_solution_-_execute_path_plan.xml

@@ -1,10 +1,9 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="08 Solution - Execute path plan">
   <!--//////////-->
   <BehaviorTree
     ID="08 Solution - Execute path plan"
     _description="Plans joint-space motion to track a path, and executes it"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <Action

src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/09_solution_-_an_infeasible_path.xml

@@ -1,10 +1,9 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="09 Solution - An infeasible path">
   <!--//////////-->
   <BehaviorTree
     ID="09 Solution - An infeasible path"
     _description="An example of a path that is not feasible"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <Action

src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/10_solution_-_visualize_path_error.xml

@@ -1,4 +1,3 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root
   BTCPP_format="4"
   main_tree_to_execute="10 Solution - Visualize path error"
@@ -7,7 +6,7 @@
   <BehaviorTree
     ID="10 Solution - Visualize path error"
     _description="How to visualize what's going on with an infeasible path"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <Action

src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/12_solution_-_constrain_orientation.xml

@@ -1,4 +1,3 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root
   BTCPP_format="4"
   main_tree_to_execute="12 Solution - Constrain orientation"
@@ -7,7 +6,7 @@
   <BehaviorTree
     ID="12 Solution - Constrain orientation"
     _description="How to constrain end-effector orientation along paths"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <Action

src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/13_solution_-_load_a_path_from_file.xml

@@ -1,4 +1,3 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root
   BTCPP_format="4"
   main_tree_to_execute="13 Solution - Load a path from file"
@@ -7,7 +6,7 @@
   <BehaviorTree
     ID="13 Solution - Load a path from file"
     _description="Loads a complex path from a file, and visualizes it"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <Action ID="LoadPoseStampedVectorFromYaml" file_path="path.yaml" />

src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/15_solution_-_a_path_that_collides.xml

@@ -1,4 +1,3 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root
   BTCPP_format="4"
   main_tree_to_execute="15 Solution - A path that collides"
@@ -7,7 +6,7 @@
   <BehaviorTree
     ID="15 Solution - A path that collides"
     _description="An example of a path kinematically feasible, but colliding"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <Action

src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/16_solution_-_checking_for_collisions.xml

@@ -1,4 +1,3 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root
   BTCPP_format="4"
   main_tree_to_execute="16 Solution - Checking for collisions"
@@ -7,7 +6,7 @@
   <BehaviorTree
     ID="16 Solution - Checking for collisions"
     _description="How to check if a plan will collide"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <Action

src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/17_solution_-_execute_the_feasible_path.xml

@@ -1,4 +1,3 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root
   BTCPP_format="4"
   main_tree_to_execute="17 Solution - Execute the feasible path"
@@ -7,7 +6,7 @@
   <BehaviorTree
     ID="17 Solution - Execute the feasible path"
     _description=""
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <Action

src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/18_solution_-_define_a_coverage_path.xml

@@ -1,4 +1,3 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root
   BTCPP_format="4"
   main_tree_to_execute="18 Solution - Define a coverage path"
@@ -7,7 +6,7 @@
   <BehaviorTree
     ID="18 Solution - Define a coverage path"
     _description="Automatically creates a path to cover a rectangular area"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <Action