Configure ada_moveit for Articulable Fork

ada_imu/config/imu_params.yaml

@@ -15,7 +15,7 @@ imu_jointstate_publisher:
       - -232.50333333333336
       - 40.96
 
-    serial_port: '/dev/ttyUSB0'
+    serial_port: '/dev/ttyUSB1'
     velocity_thresh: 0.05 # radians per second
 
     # weights used in exponential rolling averages, between 0 and 1

ada_moveit/config/ada.ros2_control.xacro

@@ -133,4 +133,53 @@
 
         </ros2_control>
     </xacro:macro>
+
+    <xacro:macro name="af_ros2_control" params="name initial_positions_file sim">
+      <xacro:property name="initial_positions" value="${xacro.load_yaml(initial_positions_file)['initial_positions']}"/>
+
+      <ros2_control name="${name}" type="system">
+        <hardware>
+          <xacro:if value="${sim == 'mock'}">
+            <plugin>mock_components/GenericSystem</plugin>
+            <param name="mock_sensor_commands">false</param>
+            <param name="state_following_offset">0.0</param>
+          </xacro:if>
+          <xacro:if value="${sim == 'real'}">
+            <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">
+          <param name="id">1</param>
+          <command_interface name="position" />
+          <command_interface name="velocity" />
+          <state_interface name="position">
+            <param name="initial_value">${initial_positions['af_joint_1']}</param>
+          </state_interface>
+          <state_interface name="velocity">
+              <param name="initial_value">0.0</param>
+          </state_interface>
+          <state_interface name="effort">
+              <param name="initial_value">0.0</param>
+          </state_interface>
+        </joint>
+        <joint name="af_joint_2">
+          <param name="id">2</param>
+          <command_interface name="position" />
+          <command_interface name="velocity" />
+          <state_interface name="position">
+            <param name="initial_value">${initial_positions['af_joint_2']}</param>
+          </state_interface>
+          <state_interface name="velocity">
+              <param name="initial_value">0.0</param>
+          </state_interface>
+          <state_interface name="effort">
+              <param name="initial_value">0.0</param>
+          </state_interface>
+        </joint>
+      </ros2_control>
+
+    </xacro:macro>
 </robot>

ada_moveit/config/ada.srdf

@@ -3,14 +3,17 @@
     This is a format for representing semantic information about the robot structure.
     A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
 -->
-<robot name="ada">
+<robot name="ada" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:arg name="end_effector_tool" default="fork" />
+    <xacro:property name="end_effector_tool" value="$(arg end_effector_tool)" />
+
     <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
     <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
     <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
     <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
     <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
     <group name="jaco_arm">
-        <chain base_link="j2n6s200_link_base" tip_link="forkTip"/>
+        <chain base_link="j2n6s200_link_base" tip_link="j2n6s200_end_effector"/>
     </group>
     <group name="hand">
         <link name="j2n6s200_end_effector"/>
@@ -21,6 +24,14 @@
         <joint name="j2n6s200_joint_finger_1"/>
         <joint name="j2n6s200_joint_finger_2"/>
     </group>
+    <xacro:if value="${end_effector_tool == 'articulable_fork'}">
+      <group name="articulable_fork">
+          <chain base_link="af_link_base" tip_link="forkTip"/>
+      </group>
+    </xacro:if>
+    <group name="ada">
+        <chain base_link="j2n6s200_link_base" tip_link="forkTip"/>
+    </group>
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="Home" group="jaco_arm">
         <joint name="j2n6s200_joint_1" value="-1.47568"/>
@@ -50,6 +61,36 @@
         <joint name="j2n6s200_joint_finger_1" value="1.46977"/>
         <joint name="j2n6s200_joint_finger_2" value="1.46977"/>
     </group_state>
+    <xacro:if value="${end_effector_tool == 'articulable_fork'}">
+      <group_state name="Home" group="articulable_fork">
+          <joint name="af_joint_1" value="0.0"/>
+          <joint name="af_joint_2" value="0.0"/>
+      </group_state>
+    </xacro:if>
+    <group_state name="Home" group="ada">
+        <joint name="j2n6s200_joint_1" value="-1.47568"/>
+        <joint name="j2n6s200_joint_2" value="2.92779"/>
+        <joint name="j2n6s200_joint_3" value="1.00845"/>
+        <joint name="j2n6s200_joint_4" value="-2.0847"/>
+        <joint name="j2n6s200_joint_5" value="1.43588"/>
+        <joint name="j2n6s200_joint_6" value="1.32575"/>
+        <xacro:if value="${end_effector_tool == 'articulable_fork'}">
+          <joint name="af_joint_1" value="0.0"/>
+          <joint name="af_joint_2" value="0.0"/>
+        </xacro:if>
+    </group_state>
+    <group_state name="Candle" group="ada">
+        <joint name="j2n6s200_joint_1" value="0.0"/>
+        <joint name="j2n6s200_joint_2" value="3.14159"/>
+        <joint name="j2n6s200_joint_3" value="3.14159"/>
+        <joint name="j2n6s200_joint_4" value="0.0"/>
+        <joint name="j2n6s200_joint_5" value="0.0"/>
+        <joint name="j2n6s200_joint_6" value="0.0"/>
+        <xacro:if value="${end_effector_tool == 'articulable_fork'}">
+          <joint name="af_joint_1" value="0.0"/>
+          <joint name="af_joint_2" value="0.0"/>
+        </xacro:if>
+    </group_state>
     <!--END EFFECTOR: Purpose: Delineate group(s) as end-effectors for other group(s).-->
     <end_effector name="hand" parent_link="j2n6s200_link_6" group="hand" parent_group="jaco_arm"/>
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
@@ -266,4 +307,39 @@
     <disable_collisions link1="uncalibrated_camera_link" link2="screwHeadLeft" reason="Adjacent"/>
     <disable_collisions link1="uncalibrated_camera_link" link2="screwHeadRight" reason="Adjacent"/>
 
+    <!-- Disable collisions for Articulable Fork links -->
+    <xacro:if value="${end_effector_tool == 'articulable_fork'}">
+      <disable_collisions link1="af_link_base" link2="forkHandle" reason="Adjacent"/>
+      <disable_collisions link1="af_link_base" link2="af_link_1" reason="Adjacent"/>
+      <disable_collisions link1="af_link_base" link2="af_link_2" reason="Adjacent"/>
+      <disable_collisions link1="af_link_base" link2="af_ft_adapter_link" reason="Adjacent"/>
+      <disable_collisions link1="af_link_base" link2="af_motor_fixture_link" reason="Adjacent"/>
+      <disable_collisions link1="af_link_base" link2="forkTine" reason="Never"/>
+      <disable_collisions link1="af_link_base" link2="forque" reason="Never"/>
+      <disable_collisions link1="af_link_1" link2="af_link_base" reason="Adjacent"/>
+      <disable_collisions link1="af_link_1" link2="af_link_2" reason="Adjacent"/>
+      <disable_collisions link1="af_link_1" link2="af_ft_adapter_link" reason="Adjacent"/>
+      <disable_collisions link1="af_link_1" link2="af_motor_fixture_link" reason="Adjacent"/>
+      <disable_collisions link1="af_link_1" link2="forkTine" reason="Adjacent"/>
+      <disable_collisions link1="af_link_1" link2="forque" reason="Adjacent"/>
+      <disable_collisions link1="af_link_2" link2="af_link_1" reason="Adjacent"/>
+      <disable_collisions link1="af_link_2" link2="af_link_base" reason="Adjacent"/>
+      <disable_collisions link1="af_link_2" link2="af_ft_adapter_link" reason="Adjacent"/>
+      <disable_collisions link1="af_link_2" link2="af_motor_fixture_link" reason="Adjacent"/>
+      <disable_collisions link1="af_link_2" link2="forkTine" reason="Adjacent"/>
+      <disable_collisions link1="af_link_2" link2="forque" reason="Adjacent"/>
+      <disable_collisions link1="af_ft_adapter_link" link2="af_link_1" reason="Adjacent"/>
+      <disable_collisions link1="af_ft_adapter_link" link2="af_link_2" reason="Adjacent"/>
+      <disable_collisions link1="af_ft_adapter_link" link2="af_link_base" reason="Adjacent"/>
+      <disable_collisions link1="af_ft_adapter_link" link2="af_motor_fixture_link" reason="Adjacent"/>
+      <disable_collisions link1="af_ft_adapter_link" link2="forkTine" reason="Adjacent"/>
+      <disable_collisions link1="af_ft_adapter_link" link2="forque" reason="Adjacent"/>
+      <disable_collisions link1="af_motor_fixture_link" link2="af_link_1" reason="Adjacent"/>
+      <disable_collisions link1="af_motor_fixture_link" link2="af_link_2" reason="Adjacent"/>
+      <disable_collisions link1="af_motor_fixture_link" link2="af_ft_adapter_link" reason="Adjacent"/>
+      <disable_collisions link1="af_motor_fixture_link" link2="af_link_base" reason="Adjacent"/>
+      <disable_collisions link1="af_motor_fixture_link" link2="forkTine" reason="Adjacent"/>
+      <disable_collisions link1="af_motor_fixture_link" link2="forque" reason="Adjacent"/>
+    </xacro:if>
+
 </robot>

ada_moveit/config/ada.urdf.xacro

@@ -2,10 +2,14 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="ada">
     <xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
     <xacro:arg name="sim" default="real" />
+    <xacro:arg name="end_effector_tool" default="fork" />
 
     <!-- Import ada urdf file -->
     <xacro:include filename="$(find ada_description)/urdf/ada.xacro" />
 
+    <xacro:property name="end_effector_tool" value="$(arg end_effector_tool)" />
+    <xacro:ada end_effector_tool="${end_effector_tool}" />
+
     <!-- Import control_xacro -->
     <xacro:include filename="ada.ros2_control.xacro" />
 
@@ -14,4 +18,10 @@
         initial_positions_file="$(arg initial_positions_file)"
         sim="$(arg sim)" />
 
+    <xacro:if value="${end_effector_tool == 'articulable_fork'}">
+      <xacro:af_ros2_control name="AFHardware"
+          initial_positions_file="$(arg initial_positions_file)"
+          sim="$(arg sim)" />
+    </xacro:if>
+
 </robot>

ada_moveit/config/initial_positions.yaml

@@ -23,3 +23,5 @@ initial_positions:
   j2n6s200_joint_6: 1.32575
   j2n6s200_joint_finger_1: 1.317
   j2n6s200_joint_finger_2: 1.317
+  af_joint_1: 0.0
+  af_joint_2: 0.0

ada_moveit/config/joint_limits.yaml

@@ -51,3 +51,13 @@ joint_limits:
     max_velocity: 1.0
     has_acceleration_limits: false
     max_acceleration: 0
+  af_joint_1:
+    has_velocity_limits: true
+    max_velocity: 0.83775804095727813
+    has_acceleration_limits: false
+    max_acceleration: 0
+  af_joint_2:
+    has_velocity_limits: true
+    max_velocity: 0.83775804095727813
+    has_acceleration_limits: false
+    max_acceleration: 0

ada_moveit/config/kinematics.yaml

@@ -15,3 +15,31 @@ jaco_arm:
   cost_threshold: 0.001
   minimal_displacement_weight: 0.0
   gd_step_size: 0.0001
+
+ada:
+  kinematics_solver: pick_ik/PickIkPlugin
+  kinematics_solver_timeout: 0.05
+  kinematics_solver_attempts: 3
+  mode: global
+  stop_optimization_on_valid_solution: true
+  position_scale: 1.0
+  rotation_scale: 0.5
+  position_threshold: 0.001
+  orientation_threshold: 0.01
+  cost_threshold: 0.001
+  minimal_displacement_weight: 0.0
+  gd_step_size: 0.0001
+
+articulable_fork:
+  kinematics_solver: pick_ik/PickIkPlugin
+  kinematics_solver_timeout: 0.05
+  kinematics_solver_attempts: 3
+  mode: global
+  stop_optimization_on_valid_solution: true
+  position_scale: 1.0
+  rotation_scale: 0.5
+  position_threshold: 0.001
+  orientation_threshold: 0.01
+  cost_threshold: 0.001
+  minimal_displacement_weight: 0.0
+  gd_step_size: 0.0001

ada_moveit/config/mock_controllers.yaml

@@ -21,12 +21,21 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
+
     jaco_arm_cartesian_controller:
       type: cartesian_twist_controller/CartesianTwistController
 
+
+    af_controller:
+      type: force_gate_controller/ForceGateController
+
+
+    af_servo_controller:
+      type: force_gate_controller/ForceGatePositionController
+
 jaco_arm_cartesian_controller:
     ros__parameters:
-      end_effector_link: "forkTip"
+      end_effector_link: "j2n6s200_end_effector"
       robot_base_link: "j2n6s200_link_base"
       ik_solver: "selectively_damped_least_squares"
       joints:
@@ -85,6 +94,32 @@ jaco_arm_servo_controller:
       - j2n6s200_joint_5
       - j2n6s200_joint_6
 
+af_controller:
+  ros__parameters:
+    joints:
+      - af_joint_1
+      - af_joint_2
+    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
+    af_joint_1:
+      goal: 0.02
+      trajectory: 0.05
+    af_joint_2:
+      goal: 0.02
+      trajectory: 0.05
+
+af_servo_controller:
+  ros__parameters:
+    joints:
+      - af_joint_1
+      - af_joint_2
+
 hand_controller:
   ros__parameters:
     joints:

ada_moveit/config/mock_servo.yaml

@@ -47,10 +47,11 @@ servo_node:
 
       ## MoveIt properties
       move_group_name:  jaco_arm  # Often 'manipulator' or 'arm'
+
       planning_frame: j2n6s200_link_base  # The MoveIt planning frame. Often 'base_link' or 'world'
 
       ## Other frames
-      ee_frame_name: forkTip  # The name of the end effector link, used to return the EE pose
+      ee_frame_name: j2n6s200_end_effector  # The name of the end effector link, used to return the EE pose
       robot_link_command_frame:  j2n6s200_link_base  # commands must be given in the frame of a robot link. Usually either the base or end effector
 
       ## Stopping behaviour

ada_moveit/config/moveit_controllers.yaml

@@ -10,6 +10,8 @@ moveit_simple_controller_manager:
     - jaco_arm_controller
     - jaco_arm_cartesian_controller
     - jaco_arm_servo_controller
+    - af_controller
+    - af_servo_controller
     - hand_controller
 
   jaco_arm_controller:
@@ -47,6 +49,23 @@ moveit_simple_controller_manager:
       - j2n6s200_joint_4
       - j2n6s200_joint_5
       - j2n6s200_joint_6
+
+  af_controller:
+    type: FollowJointTrajectory
+    action_ns: follow_joint_trajectory
+    default: true
+    joints:
+      - af_joint_1
+      - af_joint_2
+
+  af_servo_controller:
+    type: ""
+    action_ns: commands
+    default: false
+    joints:
+      - af_joint_1
+      - af_joint_2
+
   hand_controller:
     type: FollowJointTrajectory
     action_ns: follow_joint_trajectory

ada_moveit/config/ompl_planning.yaml

@@ -64,6 +64,21 @@ planner_configs:
     max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
   PRMstarkConfigDefault:
     type: geometric::PRMstar
+ada:
+  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
 jaco_arm:
   enforce_constrained_state_space: true
   projection_evaluator: joints(j2n6s200_joint_1,j2n6s200_joint_2)
@@ -79,6 +94,21 @@ jaco_arm:
     - TRRTkConfigDefault
     - PRMkConfigDefault
     - PRMstarkConfigDefault
+articulable_fork:
+  enforce_constrained_state_space: true
+  projection_evaluator: joints(af_joint_1,af_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)