Feat/movegroups

urc_arm_moveit_config/config/WalliiArmV3.ros2_control.xacro

@@ -5,10 +5,11 @@
 
         <ros2_control name="${name}" type="system">
             <hardware>
-
-                <!-- gazebo_ros2_control/GazeboSystem -->
-                <!-- mock_components/GenericSystem -->
-                <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+              <!-- TODO make a launch param to switch between real/fake hardware. You can only use ONE. -->
+              <!-- Use for functionality in Gazebo -->
+              <plugin>gazebo_ros2_control/GazeboSystem</plugin> 
+              <!--Use for functionality in RViz-->
+              <!-- <plugin>mock_components/GenericSystem</plugin> -->
             </hardware>
             <joint name="shoulderjoint">
                 <command_interface name="position"/>

urc_arm_moveit_config/config/WalliiArmV3.urdf.xacro

@@ -1,9 +1,13 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="WalliiArmV3">
     <xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
+    <!-- mock_components/GenericSystem -->
+    <xacro:arg name="use_hardware" default="gazebo_ros2_control/GazeboSystem"/>
 
     <!-- Import WalliiArmV3 urdf file -->
-    <xacro:include filename="$(find urc_gazebo)/urdf/wallii.xacro" />
+    <!-- wallii.xacro -->
+    <!-- WalliiArmV3.urdf -->
+    <xacro:include filename="$(find urc_gazebo)/urdf/WalliiArmV3.urdf" />
 
     <!-- Import control_xacro -->
     <xacro:include filename="WalliiArmV3.ros2_control.xacro" />

urc_arm_moveit_config/config/WalliiArmV3_simulated_config.yaml

@@ -39,7 +39,7 @@ is_primary_planning_scene_monitor: false
 
 ## MoveIt properties
 move_group_name:  arm  # 'arm' for arm, 'gripper' for gripper
-planning_frame: world  # The MoveIt planning frame. Often 'base_link' or 'world'
+planning_frame: world # world  # The MoveIt planning frame. Often 'base_link' or 'world'
 
 ## Other frames
 ee_frame_name: leftgripper  # The name of the end effector link, used to return the EE pose

urc_arm_moveit_config/config/chomp_planning.yaml

@@ -0,0 +1,12 @@
+planning_plugin: chomp_interface/CHOMPPlanner
+enable_failure_recovery: true
+jiggle_fraction: 0.05
+request_adapters: >-
+    default_planner_request_adapters/AddTimeOptimalParameterization
+    default_planner_request_adapters/ResolveConstraintFrames
+    default_planner_request_adapters/FixWorkspaceBounds
+    default_planner_request_adapters/FixStartStateBounds
+    default_planner_request_adapters/FixStartStateCollision
+    default_planner_request_adapters/FixStartStatePathConstraints
+ridge_factor: 0.01
+start_state_max_bounds_error: 0.1

urc_arm_moveit_config/config/moveit.rviz

@@ -27,7 +27,7 @@ Visualization Manager:
         Robot Alpha: 0.5
       Value: true
   Global Options:
-    Fixed Frame: world
+    Fixed Frame: arm_base_link
   Tools:
     - Class: rviz_default_plugins/Interact
     - Class: rviz_default_plugins/MoveCamera
@@ -43,7 +43,7 @@ Visualization Manager:
         Z: 0.30
       Name: Current View
       Pitch: 0.5
-      Target Frame: world
+      Target Frame: arm_base_link
       Yaw: -0.623
 Window Geometry:
   Height: 975

urc_arm_moveit_config/launch/demo.launch.py

@@ -4,5 +4,5 @@
 
 def generate_launch_description():
     moveit_config = MoveItConfigsBuilder("WalliiArmV3",
-                                         package_name="walliiArm").to_moveit_configs()
+                                         package_name="urc_arm_moveit_config").to_moveit_configs()
     return generate_demo_launch(moveit_config)

urc_arm_moveit_config/launch/move_group.launch.py

@@ -4,5 +4,5 @@
 
 def generate_launch_description():
     moveit_config = MoveItConfigsBuilder("WalliiArmV3",
-                                         package_name="walliiArm").to_moveit_configs()
+                                         package_name="urc_arm_moveit_config").to_moveit_configs()
     return generate_move_group_launch(moveit_config)

urc_arm_moveit_config/launch/moveit_rviz.launch.py

@@ -4,5 +4,5 @@
 
 def generate_launch_description():
     moveit_config = MoveItConfigsBuilder("WalliiArmV3",
-                                         package_name="walliiArm").to_moveit_configs()
+                                         package_name="urc_arm_moveit_config").to_moveit_configs()
     return generate_moveit_rviz_launch(moveit_config)

urc_arm_moveit_config/launch/rsp.launch.py

@@ -4,5 +4,5 @@
 
 def generate_launch_description():
     moveit_config = MoveItConfigsBuilder("WalliiArmV3",
-                                         package_name="walliiArm").to_moveit_configs()
+                                         package_name="urc_arm_moveit_config").to_moveit_configs()
     return generate_rsp_launch(moveit_config)

urc_arm_moveit_config/launch/setup_assistant.launch.py

@@ -4,5 +4,5 @@
 
 def generate_launch_description():
     moveit_config = MoveItConfigsBuilder("WalliiArmV3",
-                                         package_name="walliiArm").to_moveit_configs()
+                                         package_name="urc_arm_moveit_config").to_moveit_configs()
     return generate_setup_assistant_launch(moveit_config)

urc_arm_moveit_config/launch/spawn_controllers.launch.py

@@ -4,5 +4,5 @@
 
 def generate_launch_description():
     moveit_config = MoveItConfigsBuilder("WalliiArmV3",
-                                         package_name="walliiArm").to_moveit_configs()
+                                         package_name="urc_arm_moveit_config").to_moveit_configs()
     return generate_spawn_controllers_launch(moveit_config)

urc_arm_moveit_config/launch/static_virtual_joint_tfs.launch.py

@@ -4,5 +4,5 @@
 
 def generate_launch_description():
     moveit_config = MoveItConfigsBuilder("WalliiArmV3",
-                                         package_name="walliiArm").to_moveit_configs()
+                                         package_name="urc_arm_moveit_config").to_moveit_configs()
     return generate_static_virtual_joint_tfs_launch(moveit_config)

urc_arm_moveit_config/launch/warehouse_db.launch.py

@@ -4,5 +4,5 @@
 
 def generate_launch_description():
     moveit_config = MoveItConfigsBuilder("WalliiArmV3",
-                                         package_name="walliiArm").to_moveit_configs()
+                                         package_name="urc_arm_moveit_config").to_moveit_configs()
     return generate_warehouse_db_launch(moveit_config)

urc_arm_moveit_config/package.xml

@@ -18,14 +18,22 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>moveit_common</depend>
   <depend>controller_manager</depend>
   <depend>controller_manager_msgs</depend>
   <exec_depend>moveit_core</exec_depend>
-  <exec_depend>moveit_ros_move_group</exec_depend>
+  <exec_depend>moveit_ros_occupancy_map_monitor</exec_depend>
+  <exec_depend>moveit_ros_perception</exec_depend>
   <exec_depend>moveit_ros_planning</exec_depend>
   <exec_depend>moveit_kinematics</exec_depend>
   <exec_depend>moveit_planners</exec_depend>
+  <exec_depend>moveit_planners_chomp</exec_depend>
   <exec_depend>moveit_simple_controller_manager</exec_depend>
+  <exec_depend>moveit_configs_utils</exec_depend>
+  <exec_depend>moveit_ros_move_group</exec_depend>
+  <exec_depend>moveit_ros_visualization</exec_depend>
+  <exec_depend>moveit_ros_warehouse</exec_depend>
+  <exec_depend>moveit_setup_assistant</exec_depend>
   <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>joint_state_publisher_gui</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
@@ -34,11 +42,6 @@
        We don't include them by default to prevent installing gazebo and all its dependencies. -->
   <!-- <exec_depend>joint_trajectory_controller</exec_depend> -->
   <!-- <exec_depend>gazebo_ros_control</exec_depend> -->
-  <exec_depend>moveit_configs_utils</exec_depend>
-  <exec_depend>moveit_ros_move_group</exec_depend>
-  <exec_depend>moveit_ros_visualization</exec_depend>
-  <exec_depend>moveit_ros_warehouse</exec_depend>
-  <exec_depend>moveit_setup_assistant</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>rviz2</exec_depend>
   <exec_depend>rviz_common</exec_depend>

urc_gazebo/urdf/WalliiArmV3.urdf

@@ -555,24 +555,17 @@
   <gazebo reference="rightgripperjoint">
     <implicitSpringDamper>true</implicitSpringDamper>
   </gazebo>
-  <!-- mock_components/GenericSystem -->
-  <!-- gazebo_ros2_control/GazeboSystem -->
-
-
-    <ros2_control name="arm_control" type="system">
+
+  <ros2_control name="arm_control" type="system">
     <hardware>
+      <!-- TODO make a launch param to switch between real/fake hardware. You can only use ONE. -->
+      <!--Use for functionality in Gazebo-->
       <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+      <!--Use for functionality in RViz-->
+      <!-- <plugin>mock_components/GenericSystem</plugin> -->
       <plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
     </hardware>
-    <joint name ="arm_joint">
-        <command_interface name="position" />
-        <state_interface name="position">
-          <param name="initial_value">0.0</param>
-        </state_interface>
-        <state_interface name="velocity"/>
-        <state_interface name="effort"/>
-      </joint>
-
+
       <joint name ="shoulderjoint">
         <command_interface name="position" />
         <state_interface name="position">

urc_gazebo/urdf/walli_prop.xacro

@@ -2,6 +2,8 @@
  -->
 <robot name="walli_prop" xmlns:xacro="http://ros.org/wiki/xacro">
 
+  <xacro:property name="realhardware" value="false"/>
+
    <!-- Degree-to-radian conversions -->
   <xacro:property name="degrees_45" value="0.785398163"/>
   <xacro:property name="degrees_90" value="1.57079633"/>