From f7fa24b8ad790a05c4c33dd229b0cabf636f0391 Mon Sep 17 00:00:00 2001
From: Simon Linux <simon.wolf1999@gmail.com>
Date: Thu, 29 Aug 2024 07:15:42 +0200
Subject: [PATCH] changed ITA setup for data collection
---
.../custompanda/pathplanning.py | 2 +-
.../custompanda/scenesetup.py | 46 ++++++++++---------
.../scripts/panda_auto_dynamics_v1.py | 15 +++++-
3 files changed, 38 insertions(+), 25 deletions(-)
diff --git a/catkin_ws/src/panda_autodynamics/custompanda/pathplanning.py b/catkin_ws/src/panda_autodynamics/custompanda/pathplanning.py
index 329c309..8b9497e 100755
--- a/catkin_ws/src/panda_autodynamics/custompanda/pathplanning.py
+++ b/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
diff --git a/catkin_ws/src/panda_autodynamics/custompanda/scenesetup.py b/catkin_ws/src/panda_autodynamics/custompanda/scenesetup.py
index f42e515..1517196 100755
--- a/catkin_ws/src/panda_autodynamics/custompanda/scenesetup.py
+++ b/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
diff --git a/catkin_ws/src/panda_autodynamics/scripts/panda_auto_dynamics_v1.py b/catkin_ws/src/panda_autodynamics/scripts/panda_auto_dynamics_v1.py
index c38b68e..e83a718 100755
--- a/catkin_ws/src/panda_autodynamics/scripts/panda_auto_dynamics_v1.py
+++ b/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(
--
GitLab