Merge remote-tracking branch 'origin/v2.4'

PickNikRobotics · Jul 19, 2023 · c577349 · c577349
2 parents f7f9774 + 4a07327
commit c577349
Show file tree

Hide file tree

Showing 16 changed files with 76 additions and 61 deletions.
diff --git a/src/moveit_studio_agent_examples/package.xml b/src/moveit_studio_agent_examples/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>moveit_studio_agent_examples</name>
-  <version>2.3.0</version>
+  <version>2.4.0</version>
   <description>Package containing scripts for interacting with MoveIt Studio Agent</description>
 
   <maintainer email="chance.cardona@picknik.ai">Chance Cardona</maintainer>

diff --git a/src/picknik_ur_base_config/config/initial_positions.yaml b/src/picknik_ur_base_config/config/initial_positions.yaml
@@ -1,6 +1,6 @@
 shoulder_pan_joint: 0.0
-shoulder_lift_joint: -0.6
-elbow_joint: -1.4
-wrist_1_joint: -1.2
-wrist_2_joint: 1.7
+shoulder_lift_joint: -1.41
+elbow_joint: -0.7
+wrist_1_joint: -2.12
+wrist_2_joint: 1.67
 wrist_3_joint: 0.0
diff --git a/src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml b/src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
   <root BTCPP_format="4" main_tree_to_execute="Cycle Between Waypoints">
-    <BehaviorTree ID="Cycle Between Waypoints" _description="Cycles between two waypoints until failure" _favorite="true">
+    <BehaviorTree ID="Cycle Between Waypoints" _description="Cycles between two waypoints until failure">
         <Decorator ID="KeepRunningUntilFailure">
             <Control ID="Sequence">
                 <Action ID="MoveToJointState" waypoint_name="Right Corner" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>

diff --git a/src/picknik_ur_base_config/objectives/joint_diagnostic.xml b/src/picknik_ur_base_config/objectives/joint_diagnostic.xml
@@ -1,7 +1,7 @@
 <?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">
+  <BehaviorTree ID="Joint Diagnostic" _description="Move to a known pose and cycle through the min and max limits of a joint repeatedly" _favorite="true">
     <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">

diff --git a/src/picknik_ur_base_config/objectives/looping_pick_and_place_object.xml b/src/picknik_ur_base_config/objectives/looping_pick_and_place_object.xml
@@ -10,7 +10,7 @@
 
             <!-- Open the gripper and move to a consistent state. -->
             <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
-            <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+            <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
 
             <!-- This loops forever because the behavior tree under this decorator never returns FAILURE. -->
             <Decorator ID="KeepRunningUntilFailure">
@@ -69,22 +69,22 @@
                                     </Control>
                                 </Decorator>
                                 <!-- Move home. -->
-                                <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                                <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
 
                                 <!-- Move to the current place waypoint set by IterateThroughPlaceWaypoints, then open the gripper. -->
                                 <Action ID="MoveToJointState" waypoint_name="{place_waypoint}" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                                 <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
 
                                 <!-- Finally, return home. -->
-                                <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                                <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                             </Control>
                         </Decorator>
                     </Decorator>
 
                     <!-- TryGraspOuter fallback condition. Reset scene objects, move home, and then wait a short while before triggering a retry. -->
                     <Control ID="Sequence">
                         <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/>
-                        <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="WaitForDuration" delay_duration="5"/>
                         <Action ID="AlwaysSuccess"/>
                     </Control>

diff --git a/src/picknik_ur_base_config/objectives/push_button.xml b/src/picknik_ur_base_config/objectives/push_button.xml
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <root BTCPP_format="4" main_tree_to_execute="Push Button">
-    <BehaviorTree ID="Push Button" _description="Push a button with the end effector" _favorite="true">
+    <BehaviorTree ID="Push Button" _description="Push a button with the end effector">
         <Control ID="Sequence">
             <Action ID="LoadObjectiveParameters" config_file_name="push_along_axis_config.yaml" parameters="{parameters}" />
             <SubTree ID="CloseGripper" />

diff --git a/src/picknik_ur_base_config/package.xml b/src/picknik_ur_base_config/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>picknik_ur_base_config</name>
-  <version>2.3.0</version>
+  <version>2.4.0</version>
 
   <description>Base config package for a UR5e that can be simulated with mock hardware</description>
 

diff --git a/src/picknik_ur_gazebo_config/config/initial_positions.yaml b/src/picknik_ur_gazebo_config/config/initial_positions.yaml
diff --git a/src/picknik_ur_gazebo_config/description/picknik_ur_gazebo.xacro b/src/picknik_ur_gazebo_config/description/picknik_ur_gazebo.xacro
@@ -98,7 +98,7 @@
   <joint name="external_camera_joint" type="fixed">
     <parent link="world" />
     <child link="external_camera_link" />
-    <origin xyz="0.3 -0.3 1.0" rpy="0.0 0.4 3.14" />
+    <origin xyz="1.1 -1.1 1.3" rpy="0.0 0.4 2.4" />
   </joint>
 
   <xacro:realsense_d435 parent="external_camera_link" name="scene_camera" visible="false" simulate_depth="true">

diff --git a/src/picknik_ur_gazebo_config/objectives/3_waypoint_pick_and_place.xml b/src/picknik_ur_gazebo_config/objectives/3_waypoint_pick_and_place.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <root BTCPP_format="4" main_tree_to_execute="3 Waypoints Pick and Place">
     <!-- ////////// -->
-    <BehaviorTree ID="3 Waypoints Pick and Place" _description="Repeatedly pick up a small object, place it at desired destination, and then navigate to specified waypoint until failure" _favorite="true">
+    <BehaviorTree ID="3 Waypoints Pick and Place" _description="Repeatedly pick up a small object, place it at desired destination, and then navigate to specified waypoint until failure">
         <Control ID="Sequence">
             <!-- Clear snapshot, move to center pose, and open gripper -->
             <Action ID="ClearSnapshot" />

diff --git a/src/picknik_ur_gazebo_config/objectives/inspect_surface.xml b/src/picknik_ur_gazebo_config/objectives/inspect_surface.xml
diff --git a/src/picknik_ur_gazebo_config/objectives/open_door_affordance.xml b/src/picknik_ur_gazebo_config/objectives/open_door_affordance.xml
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <root BTCPP_format="4" main_tree_to_execute="Open Cabinet Door">
-    <BehaviorTree ID="Open Cabinet Door" _description="Open a cabinet door by grasping and pulling its handle" _favorite="true">
+    <BehaviorTree ID="Open Cabinet Door" _description="Open a cabinet door by grasping and pulling its handle">
         <Control ID="Sequence" name="Setup">
             <Action ID="LoadObjectiveParameters" config_file_name="open_door_affordance_config.yaml" parameters="{parameters}" />
             <Action ID="GetPoseFromUser" parameter_name="process_door_selection.hinge_axis_pose_end" parameter_value="{hinge_axis_pose_end}" />

diff --git a/src/picknik_ur_gazebo_config/objectives/take_snapshot.xml b/src/picknik_ur_gazebo_config/objectives/take_snapshot.xml
diff --git a/src/picknik_ur_gazebo_config/package.xml b/src/picknik_ur_gazebo_config/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>picknik_ur_gazebo_config</name>
-  <version>2.3.0</version>
+  <version>2.4.0</version>
 
   <description>Site config package for the simulated UR5e in PickNik's space station world simulated by Gazebo.</description>
 

diff --git a/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml b/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml
@@ -49,7 +49,7 @@
     velocity: []
   name: Home
 - description: ''
-  favorite: true
+  favorite: false
   joint_state:
     effort: []
     header:
@@ -74,7 +74,7 @@
     velocity: []
   name: Left Corner
 - description: ''
-  favorite: true
+  favorite: false
   joint_state:
     effort: []
     header:
@@ -99,7 +99,7 @@
     velocity: []
   name: Right Corner
 - description: ''
-  favorite: true
+  favorite: false
   joint_state:
     effort: []
     header:
@@ -165,11 +165,11 @@
       - wrist_2_joint
       - wrist_3_joint
     position:
-      - 3.03
-      - -0.78
-      - -0.51
-      - -2.39
-      - 1.65
+      - 0
+      - -1.41
+      - -0.7
+      - -2.12
+      - 1.67
       - 0
     velocity: []
   name: Look at Pick and Place Zone
@@ -298,3 +298,53 @@
       - 2.282092208042741e-05
     velocity: []
   name: Wrist 2 Max
+- description: Move here and run Take Snapshot again!
+  favorite: true
+  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:
+      - -1.5113688855545728
+      - -0.849676772994894
+      - -1.7900954114547076
+      - -0.6712599508612579
+      - 1.6807113853283657
+      - -0.010627009718118157
+    velocity: []
+  name: Right Wall
+- description: ''
+  favorite: true
+  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:
+      - 0.0003205429823013286
+      - -0.3419419371606998
+      - -0.5134107235570269
+      - -2.3994585463037112
+      - 1.6501763241696035
+      - 3.878003640148636e-05
+    velocity: []
+  name: Monitor View
diff --git a/src/picknik_ur_site_config/package.xml b/src/picknik_ur_site_config/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>picknik_ur_site_config</name>
-  <version>2.3.0</version>
+  <version>2.4.0</version>
 
   <description>Site config package for a UR5e that can be simulated with mock hardware</description>