changed ITA setup for data collection

catkin_ws/src/panda_autodynamics/custompanda/pathplanning.py

@@ -27,7 +27,7 @@ def plan_cartesian_path(
     group: moveit_commander.MoveGroupCommander,
 ) -> "tuple[moveit_commander.RobotTrajectory,float]":
     waypoints = []
-    factor = 0.001
+    factor = - 0.0005
     start_pose = group.get_current_pose().pose
 
     # Point 11

catkin_ws/src/panda_autodynamics/custompanda/scenesetup.py

@@ -8,7 +8,7 @@ import os
 import geometry_msgs.msg
 import moveit_commander
 import numpy as np
-
+import sys
 
 def get_env_variable(var_name):
     value = os.environ.get(var_name)
@@ -22,8 +22,9 @@ try:
 
 except EnvironmentError as e:
     print(e)
+    sys.exit(0)
     # Handle the error appropriately, e.g., exit the program or set default values
-    ROBOT_UUID = None
+    #ROBOT_UUID = None
 
 
 def create_scene(
@@ -196,20 +197,33 @@ def create_scene(
         scene.add_box("Other Robot", p, size=(0.02, 2, 1))  # (x, z / höhe, y)
     if ROBOT_UUID == "c9ff52e1-1733-4829-a209-ebd1586a8697":  # ITA
         # Wall (steht jetzt gerade)
+        # umgedreht
+        # ehemals minus 14 cm 
         p = geometry_msgs.msg.PoseStamped()
         p.header.frame_id = planning_frame
-        p.pose.position.x = 0.0
-        p.pose.position.y = 0.5
+        p.pose.position.x = -1
+        p.pose.position.y = -0.5
         p.pose.position.z = 0.0
         p.pose.orientation.x = 1 / np.sqrt(2)
         p.pose.orientation.y = 0
         p.pose.orientation.z = 0
         p.pose.orientation.w = 1 / np.sqrt(2)
         scene.add_box("wall", p, size=(3, 3, 0.2))  # (x, z / höhe, y)
+        # wand 2
+        p = geometry_msgs.msg.PoseStamped()
+        p.header.frame_id = planning_frame
+        p.pose.position.x = 0.25
+        p.pose.position.y = 0.0
+        p.pose.position.z = 0.0
+        p.pose.orientation.x = 1 / np.sqrt(2)
+        p.pose.orientation.y = 0
+        p.pose.orientation.z = 0
+        p.pose.orientation.w = 1 / np.sqrt(2)
+        scene.add_box("wall2", p, size=(0.1, 3, 1.6))  # (x, z / höhe, y)
         # UR5e
         p = geometry_msgs.msg.PoseStamped()
         p.header.frame_id = planning_frame
-        p.pose.position.x = 1.1
+        p.pose.position.x = -1.25
         p.pose.position.y = 0.0
         p.pose.position.z = 0.30
         p.pose.orientation.x = 1 / np.sqrt(2)
@@ -217,29 +231,17 @@ def create_scene(
         p.pose.orientation.z = 0
         p.pose.orientation.w = 1 / np.sqrt(2)
         scene.add_box("ur5e", p, size=(0.6, 0.6, 0.6))  # (x, z / höhe, y)
-        # Single ITEM Profile
+        # Activation
         p = geometry_msgs.msg.PoseStamped()
         p.header.frame_id = planning_frame
-        p.pose.position.x = -0.55
-        p.pose.position.y = -0.55
+        p.pose.position.x = -0.09
+        p.pose.position.y = 0.38
         p.pose.position.z = 0.0
         p.pose.orientation.x = 1 / np.sqrt(2)
         p.pose.orientation.y = 0
         p.pose.orientation.z = 0
         p.pose.orientation.w = 1 / np.sqrt(2)
-        scene.add_box(
-            "Single ITEM Profile ", p, size=(0.15, 2, 0.15)
-        )  # (x, z / höhe, y)
-        # Textile Form
-        # p = geometry_msgs.msg.PoseStamped()
-        # p.header.frame_id = planning_frame
-        # p.pose.position.x = 0.35
-        # p.pose.position.y = 0.0
-        # p.pose.position.z = 0.05
-        # p.pose.orientation.x = 1 / np.sqrt(2)
-        # p.pose.orientation.y = 0
-        # p.pose.orientation.z = 0
-        # p.pose.orientation.w = 1 / np.sqrt(2)
-        # scene.add_box("Textile Form", p, size=(0.6, 0.1, 0.2)) # (x, z / höhe, y)
+        scene.add_box("activation_emergency-stop", p, size=(0.2, 0.2, 0.15))  # (x, z / höhe, y)
+
 
     return None

catkin_ws/src/panda_autodynamics/scripts/panda_auto_dynamics_v1.py

@@ -63,6 +63,16 @@ except EnvironmentError as e:
     DATASET_TYPE = False
 
 
+try:
+    ROBOT_UUID = get_env_variable("ROBOT_UUID")  # Get ROBOT_UUID from environment
+
+except EnvironmentError as e:
+    rospy.logwarn(e)
+    sys.exit(0)
+    # Handle the error appropriately, e.g., exit the program or set default values
+    # ROBOT_UUID = None
+    
+
 def state_listener_callback(data):
     global t_meas, q_meas, qd_meas, tau_meas, U_l1n, I1, P1
     t_meas = np.append(t_meas, data.time)
@@ -215,10 +225,11 @@ def main():
                     if (
                         ROBOT_UUID == "c9ff52e1-1733-4829-a209-ebd1586a8697"
                     ):  # For ITA move start position to the other side
-                        move_group.set_pose_target([-0.18, -0.6, 0.2, -np.pi, 0, 0])
+                        rospy.loginfo("GOT HERE")
+                        move_group.set_pose_target([-0.2, 0, 0.2, -np.pi, 0, 0])
                     if TEST == "true":  # LLT
                         move_group.set_pose_target(
-                            [0.25, 0, 0.2, -np.pi, 0, 0]
+                            [-0.15, 0.25, 0.2, -np.pi, 0, 0]
                         )  # TODO test 0.6,0,0.2
                     else:
                         move_group.set_pose_target(