Add af_velocity_controller and af_joint_trajectory_controller, add hy…

…brid control mode

ada_moveit/config/ada.ros2_control.xacro

@@ -17,6 +17,11 @@
                 <xacro:if value="${sim == 'real'}">
                   <plugin>ada_hardware/Jaco2</plugin>
                 </xacro:if>
+                <xacro:if value="${sim == 'hybrid'}">
+                  <plugin>mock_components/GenericSystem</plugin>
+                  <param name="mock_sensor_commands">false</param>
+                  <param name="state_following_offset">0.0</param>
+                </xacro:if>
               </hardware>
             <joint name="j2n6s200_joint_1">
                 <command_interface name="position"/>
@@ -149,6 +154,11 @@
             <param name="usb_port">/dev/ttyUSB0</param>
             <param name="baud_rate">1000000</param>
           </xacro:if>
+          <xacro:if value="${sim == 'hybrid'}">
+            <plugin>dynamixel_hardware/DynamixelHardware</plugin>
+            <param name="usb_port">/dev/ttyUSB0</param>
+            <param name="baud_rate">1000000</param>
+          </xacro:if>
         </hardware>
 
         <joint name="af_joint_1">

ada_moveit/config/hybrid_controllers.yaml

@@ -0,0 +1,126 @@
+# This config file is used by ros2_control
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    jaco_arm_controller:
+      type: force_gate_controller/ForceGateController
+
+
+    jaco_arm_servo_controller:
+      type: force_gate_controller/ForceGatePositionController
+
+
+    hand_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+
+    jaco_arm_cartesian_controller:
+      type: cartesian_twist_controller/CartesianTwistController
+
+
+    af_velocity_controller:
+      type: velocity_controllers/JointGroupVelocityController
+
+    af_joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+jaco_arm_cartesian_controller:
+    ros__parameters:
+      end_effector_link: "j2n6s200_end_effector"
+      robot_base_link: "j2n6s200_link_base"
+      ik_solver: "selectively_damped_least_squares"
+      joints:
+        - j2n6s200_joint_1
+        - j2n6s200_joint_2
+        - j2n6s200_joint_3
+        - j2n6s200_joint_4
+        - j2n6s200_joint_5
+        - j2n6s200_joint_6
+      command_interfaces:
+        - position
+
+jaco_arm_controller:
+  ros__parameters:
+    joints:
+      - j2n6s200_joint_1
+      - j2n6s200_joint_2
+      - j2n6s200_joint_3
+      - j2n6s200_joint_4
+      - j2n6s200_joint_5
+      - j2n6s200_joint_6
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+    allow_partial_joints_goal: false
+    open_loop_control: false
+    stopped_velocity_tolerance: 0.01
+    # No gains on position interface
+    j2n6s200_joint_1:
+      goal: 0.02
+      trajectory: 0.05
+    j2n6s200_joint_2:
+      goal: 0.02
+      trajectory: 0.05
+    j2n6s200_joint_3:
+      goal: 0.02
+      trajectory: 0.05
+    j2n6s200_joint_4:
+      goal: 0.02
+      trajectory: 0.05
+    j2n6s200_joint_5:
+      goal: 0.02
+      trajectory: 0.05
+    j2n6s200_joint_6:
+      goal: 0.02
+      trajectory: 0.05
+
+jaco_arm_servo_controller:
+  ros__parameters:
+    joints:
+      - j2n6s200_joint_1
+      - j2n6s200_joint_2
+      - j2n6s200_joint_3
+      - j2n6s200_joint_4
+      - j2n6s200_joint_5
+      - j2n6s200_joint_6
+
+af_velocity_controller:
+  ros__parameters:
+    joints:
+      - af_joint_1
+      - af_joint_2
+
+af_joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - af_joint_1
+      - af_joint_2
+
+    command_interfaces:
+      - position
+
+    state_interfaces:
+      - position
+
+hand_controller:
+  ros__parameters:
+    joints:
+      - j2n6s200_joint_finger_1
+      - j2n6s200_joint_finger_2
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+    allow_partial_joints_goal: false
+    open_loop_control: false
+    constraints:
+      stopped_velocity_tolerance: 0.01
+    gains: # Required because we're controlling a velocity interface
+      j2n6s200_joint_finger_1: {p: 1.0,  d: 0.0, i: 0.0, i_clamp: 1.0}
+      j2n6s200_joint_finger_2: {p: 1.0,  d: 0.0, i: 0.0, i_clamp: 1.0}

ada_moveit/config/moveit_controllers.yaml

@@ -7,8 +7,8 @@ moveit_simple_controller_manager:
     - jaco_arm_controller
     - jaco_arm_cartesian_controller
     - jaco_arm_servo_controller
-    - articulable_fork_controller
-    - articulable_fork_cartesian_controller
+    - af_velocity_controller
+    - af_joint_trajectory_controller
     - hand_controller
 
   jaco_arm_controller:
@@ -47,15 +47,15 @@ moveit_simple_controller_manager:
       - j2n6s200_joint_5
       - j2n6s200_joint_6
 
-  articulable_fork_controller:
+  af_velocity_controller:
     type: FollowJointTrajectory
     action_ns: follow_joint_trajectory
     default: true
     joints:
       - af_joint_1
       - af_joint_2
 
-  articulable_fork_cartesian_controller:
+  af_joint_trajectory_controller:
     type: ""
     action_ns: cart_commands
     default: false

ada_moveit/config/ompl_planning.yaml

@@ -76,21 +76,6 @@ jaco_arm:
     - TRRTkConfigDefault
     - PRMkConfigDefault
     - PRMstarkConfigDefault
-articulable_fork:
-  enforce_constrained_state_space: true
-  projection_evaluator: joints(j2n6s200_joint_1,j2n6s200_joint_2)
-  planner_configs:
-    - SBLkConfigDefault
-    - ESTkConfigDefault
-    - LBKPIECEkConfigDefault
-    - BKPIECEkConfigDefault
-    - KPIECEkConfigDefault
-    - RRTkConfigDefault
-    - RRTConnectkConfigDefault
-    - RRTstarkConfigDefault
-    - TRRTkConfigDefault
-    - PRMkConfigDefault
-    - PRMstarkConfigDefault
 hand:
   enforce_constrained_state_space: true
   projection_evaluator: joints(j2n6s200_joint_1,j2n6s200_joint_2)

ada_moveit/config/real_controllers.yaml

@@ -23,13 +23,11 @@ controller_manager:
       type: cartesian_twist_controller/CartesianTwistController
 
 
-    articulable_fork_controller:
-      type: force_gate_controller/ForceGateController
-
-
-    articulable_fork_cartesian_controller:
-      type: cartesian_twist_controller/CartesianTwistController
+    af_velocity_controller:
+      type: velocity_controllers/JointGroupVelocityController
 
+    af_joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
 
 jaco_arm_cartesian_controller:
     ros__parameters:
@@ -108,43 +106,23 @@ jaco_arm_servo_controller:
       topic: /wireless_ft/ftSensor3
       fMag: 1.0
 
-articulable_fork_cartesian_controller:
-    ros__parameters:
-      end_effector_link: "forkTip"
-      robot_base_link: "af_link_base"
-      ik_solver: "selectively_damped_least_squares"
-      joints:
-        - af_joint_1
-        - af_joint_2
-      command_interfaces:
-        - velocity
+af_velocity_controller:
+  ros__parameters:
+    joints:
+      - af_joint_1
+      - af_joint_2
 
-articulable_fork_controller:
+af_joint_trajectory_controller:
   ros__parameters:
     joints:
       - af_joint_1
       - af_joint_2
+
     command_interfaces:
-      - velocity
+      - position
+
     state_interfaces:
       - position
-      - velocity
-    allow_partial_joints_goal: false
-    open_loop_control: false
-    stopped_velocity_tolerance: 0.01
-    gains: # Required because we're controlling a velocity interface
-      j2n6s200_joint_1: {p: 4.0,  d: 0.0, i: 0.0, i_clamp: 1.0, normalize_error: true}
-      j2n6s200_joint_2: {p: 4.0,  d: 0.0, i: 0.0, i_clamp: 1.0, normalize_error: true}
-      j2n6s200_joint_3: {p: 4.0,  d: 0.0, i: 0.0, i_clamp: 1.0, normalize_error: true}
-      j2n6s200_joint_4: {p: 4.0,  d: 0.0, i: 0.0, i_clamp: 1.0, normalize_error: true}
-      j2n6s200_joint_5: {p: 4.0,  d: 0.0, i: 0.0, i_clamp: 1.0, normalize_error: true}
-      j2n6s200_joint_6: {p: 4.0,  d: 0.0, i: 0.0, i_clamp: 1.0, normalize_error: true}
-    af_joint_1:
-      goal: 0.02
-      trajectory: 0.05
-    af_joint_2:
-      goal: 0.02
-      trajectory: 0.05
 
 hand_controller:
   ros__parameters: