Add "Joint Diagnostic" Objective (#48)

PickNikRobotics · Jun 27, 2023 · 297396b · 297396b
1 parent 523ffcb
commit 297396b
Show file tree

Hide file tree

Showing 4 changed files with 102 additions and 0 deletions.
diff --git a/src/picknik_ur_base_config/waypoints/ur5e_waypoints.yaml b/src/picknik_ur_base_config/waypoints/ur5e_waypoints.yaml
@@ -193,3 +193,25 @@
     position: [2.89725, -2.0944, -1.50098, -1.0472, 1.69297, 0.20944]
     velocity: []
     effort: []
+- name: Wrist 2 Min
+  joint_state:
+    header:
+      frame_id: world
+      stamp:
+        sec: 0
+        nanosec: 0
+    name: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
+    position: [-2.2335834056138996e-05, -0.7800858658974525, -0.5100288585397881, -2.399985082258936, -3.141592653589793, 2.282092208042741e-05]
+    velocity: []
+    effort: []
+- name: Wrist 2 Max
+  joint_state:
+    header:
+      frame_id: world
+      stamp:
+        sec: 0
+        nanosec: 0
+    name: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
+    position: [-2.2335834056138996e-05, -0.7800858658974525, -0.5100288585397881, -2.399985082258936, 3.141592653589793, 2.282092208042741e-05]
+    velocity: []
+    effort: []
diff --git a/src/picknik_ur_gazebo_config/objectives/joint_diagnostic.xml b/src/picknik_ur_gazebo_config/objectives/joint_diagnostic.xml
@@ -0,0 +1,15 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Joint Diagnostic">
+  <!-- ////////// -->
+  <BehaviorTree ID="Joint Diagnostic" _description="Move to a known pose and cycle through the min and max limits of a joint repeatedly">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="MoveToJointState" waypoint_name="Home" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+      <Decorator ID="KeepRunningUntilFailure">
+        <Control ID="Sequence">
+          <Action ID="MoveToJointState" waypoint_name="Wrist 2 Max" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+          <Action ID="MoveToJointState" waypoint_name="Wrist 2 Min" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+        </Control>
+      </Decorator>
+    </Control>
+  </BehaviorTree>
+</root>
diff --git a/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml b/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml
@@ -248,3 +248,53 @@
       - 0.6922835016976157
     velocity: []
   name: Pick
+- description: ''
+  favorite: false
+  joint_state:
+    effort: []
+    header:
+      frame_id: world
+      stamp:
+        nanosec: 0
+        sec: 0
+    name:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    position:
+      - -2.2335834056138996e-05
+      - -0.7800858658974525
+      - -0.5100288585397881
+      - -2.399985082258936
+      - -3.141592653589793
+      - 2.282092208042741e-05
+    velocity: []
+  name: Wrist 2 Min
+- description: ''
+  favorite: false
+  joint_state:
+    effort: []
+    header:
+      frame_id: world
+      stamp:
+        nanosec: 0
+        sec: 0
+    name:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    position:
+      - -2.2335834056138996e-05
+      - -0.7800858658974525
+      - -0.5100288585397881
+      - -2.399985082258936
+      - 3.141592653589793
+      - 2.282092208042741e-05
+    velocity: []
+  name: Wrist 2 Max
diff --git a/src/picknik_ur_site_config/objectives/joint_diagnostic.xml b/src/picknik_ur_site_config/objectives/joint_diagnostic.xml
@@ -0,0 +1,15 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Joint Diagnostic">
+  <!-- ////////// -->
+  <BehaviorTree ID="Joint Diagnostic" _description="Move to a known pose and cycle through the min and max limits of a joint repeatedly">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="MoveToJointState" waypoint_name="Home" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+      <Decorator ID="KeepRunningUntilFailure">
+        <Control ID="Sequence">
+          <Action ID="MoveToJointState" waypoint_name="Wrist 2 Max" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+          <Action ID="MoveToJointState" waypoint_name="Wrist 2 Min" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+        </Control>
+      </Decorator>
+    </Control>
+  </BehaviorTree>
+</root>