From da3fb9edc4a06ae6f08eae86872b5ce1ec50cfab Mon Sep 17 00:00:00 2001 From: Hauke Date: Tue, 8 Aug 2023 18:50:23 +0200 Subject: [PATCH 1/8] First commit simulation --- journal_experiments/launch/mur_sim.launch | 9 +++++++ .../launch/start_experiment.launch | 8 ++++++ journal_experiments/launch/twist_sim.launch | 25 +++++++++++++++++++ .../scripts/calibrated_pose_fake.py | 23 +++++++++++++++++ journal_experiments/scripts/control_mir.py | 6 ++--- journal_experiments/scripts/control_ur.py | 6 ++--- journal_experiments/scripts/create_path.py | 11 ++++++-- .../scripts/helper_nodes/control_ur_helper.py | 4 +-- journal_experiments/scripts/move_mir.py | 2 +- .../scripts/move_to_start_pose.py | 13 +++++----- journal_experiments/scripts/state_machine.py | 7 +++--- 11 files changed, 94 insertions(+), 20 deletions(-) create mode 100644 journal_experiments/launch/mur_sim.launch create mode 100644 journal_experiments/launch/twist_sim.launch create mode 100755 journal_experiments/scripts/calibrated_pose_fake.py diff --git a/journal_experiments/launch/mur_sim.launch b/journal_experiments/launch/mur_sim.launch new file mode 100644 index 0000000..84ff700 --- /dev/null +++ b/journal_experiments/launch/mur_sim.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/journal_experiments/launch/start_experiment.launch b/journal_experiments/launch/start_experiment.launch index 1d5091e..1599bce 100644 --- a/journal_experiments/launch/start_experiment.launch +++ b/journal_experiments/launch/start_experiment.launch @@ -1,5 +1,13 @@ + + + + mur620c/mir_cmd_vel_topic: "/mur620c/mobile_base_controller/cmd_vel" + move_ur_to_start_pose/ur_command_topic: "/mur620c/UR10_l/twist_controller/command_safe" + + + diff --git a/journal_experiments/launch/twist_sim.launch b/journal_experiments/launch/twist_sim.launch new file mode 100644 index 0000000..b447445 --- /dev/null +++ b/journal_experiments/launch/twist_sim.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + ur_command_topic: "/mur620c/UR10_l/twist_command" + mir_pose_topic: "/mur620c/mir_pose_simple" + mir_cmd_vel_topic: "/mur620c/mobile_base_controller/cmd_vel" + + + \ No newline at end of file diff --git a/journal_experiments/scripts/calibrated_pose_fake.py b/journal_experiments/scripts/calibrated_pose_fake.py new file mode 100755 index 0000000..0e12811 --- /dev/null +++ b/journal_experiments/scripts/calibrated_pose_fake.py @@ -0,0 +1,23 @@ +#!/usr/bin/env python3 + +import rospy +from geometry_msgs.msg import PoseStamped, Pose + +class RemapUrEeTopic: + def __init__(self) -> None: + self.pose = PoseStamped() + self.pose.header.frame_id = "mur620c/UR10_r/base_link" + + self.pub = rospy.Publisher("/mur620c/UR10_r/ur_calibrated_pose", PoseStamped, queue_size=1) + rospy.Subscriber("/mur620c/UR10_r/tcp_pose", Pose, self.cb_ee_pose) + + def cb_ee_pose(self, msg: Pose): + self.pose.header.stamp = rospy.Time.now() + self.pose.pose = msg + self.pub.publish(self.pose) + +if __name__ == "__main__": + rospy.init_node("ur_calibrated_pose") + RemapUrEeTopic() + rospy.spin() + \ No newline at end of file diff --git a/journal_experiments/scripts/control_mir.py b/journal_experiments/scripts/control_mir.py index fbbb785..78a3671 100755 --- a/journal_experiments/scripts/control_mir.py +++ b/journal_experiments/scripts/control_mir.py @@ -41,10 +41,10 @@ def __init__(self): for i in range(len(self.robot_names)): self.robot_path_publishers.append(rospy.Publisher(self.robot_names[i] + '/robot_path', Path, queue_size=1)) - self.robot_twist_publishers.append(rospy.Publisher(self.robot_names[i] + '/mir/cmd_vel', Twist, queue_size=1)) + self.robot_twist_publishers.append(rospy.Publisher(self.robot_names[i] + '/cmd_vel', Twist, queue_size=1)) for i in range(len(self.robot_names)): - rospy.Subscriber(self.robot_names[i] + '/mir/mir_pose_simple', Pose, self.mir_pose_callback, i) + rospy.Subscriber(self.robot_names[i] + '/mir_pose_simple', Pose, self.mir_pose_callback, i) if self.external_control: rospy.Subscriber('path_index', Int32, self.path_index_callback) @@ -76,7 +76,7 @@ def run(self): # wait for mocap to be ready - rospy.wait_for_message(self.robot_names[0] + '/mir/mir_pose_simple', Pose) + rospy.wait_for_message(self.robot_names[0] + '/mir_pose_simple', Pose) diff --git a/journal_experiments/scripts/control_ur.py b/journal_experiments/scripts/control_ur.py index 3abb8ce..bffccfd 100755 --- a/journal_experiments/scripts/control_ur.py +++ b/journal_experiments/scripts/control_ur.py @@ -142,8 +142,8 @@ def compute_path_speed_and_distance(self,ur_command): def get_mir_ur_transform(self): tf_listener = tf.TransformListener() # wait for transform - tf_listener.waitForTransform(self.tf_prefix + "mir/base_link", self.tf_prefix + "UR10_r/base_link", rospy.Time(0), rospy.Duration(4.0)) - lin, ang = tf_listener.lookupTransform(self.tf_prefix + "mir/base_link", self.tf_prefix + "UR10_r/base_link", rospy.Time(0)) + tf_listener.waitForTransform(self.tf_prefix + "base_link", self.tf_prefix + "UR10_r/base_link", rospy.Time(0), rospy.Duration(4.0)) + lin, ang = tf_listener.lookupTransform(self.tf_prefix + "base_link", self.tf_prefix + "UR10_r/base_link", rospy.Time(0)) self.mir_ur_transform.translation.x = lin[0] self.mir_ur_transform.translation.y = lin[1] @@ -224,7 +224,7 @@ def compute_ur_target_pose_local(self, ur_target_pose_global): ur_target_pose_local.position.z = ur_target_pose_global.position.z # broadcast target pose - self.ur_target_pose_broadcaster.sendTransform((ur_target_pose_local.position.x, ur_target_pose_local.position.y, ur_target_pose_local.position.z), (ur_target_pose_global.orientation.x, ur_target_pose_global.orientation.y, ur_target_pose_global.orientation.z, ur_target_pose_global.orientation.w), rospy.Time.now(), "ur_local_target_pose", "mur620c/mir/base_link") + self.ur_target_pose_broadcaster.sendTransform((ur_target_pose_local.position.x, ur_target_pose_local.position.y, ur_target_pose_local.position.z), (ur_target_pose_global.orientation.x, ur_target_pose_global.orientation.y, ur_target_pose_global.orientation.z, ur_target_pose_global.orientation.w), rospy.Time.now(), "ur_local_target_pose", "mur620c/base_link") # UR is mounted backwards, so we need to invert the x-axis and y-axis diff --git a/journal_experiments/scripts/create_path.py b/journal_experiments/scripts/create_path.py index 09670e9..69353cb 100755 --- a/journal_experiments/scripts/create_path.py +++ b/journal_experiments/scripts/create_path.py @@ -64,13 +64,20 @@ def main(self): ur_pose.pose.position.x = 1.0*math.cos(i/self.point_per_meter) + 0.05 * math.sin(6*i/self.point_per_meter) ur_pose.pose.position.y = 1.0*math.sin(i/self.point_per_meter) + 0.05 * math.cos(6*i/self.point_per_meter) ur_pose.pose.position.z = 0.5 + + ur_pose.pose.orientation = pose.pose.orientation ur_path.poses.append(ur_pose) rospy.sleep(1.1) - self.path_pub_ur.publish(ur_path) - rospy.sleep(1.1) + rospy.loginfo("publishing MirPath") self.path_pub.publish(path) + # rospy.wait_for_message("/mir_path", Path) + rospy.sleep(1.1) + rospy.loginfo("publishing URPath") + self.path_pub_ur.publish(ur_path) + # rospy.wait_for_message("/ur_path", Path) + rospy.loginfo("paths published") rospy.sleep(1.1) print("path published") diff --git a/journal_experiments/scripts/helper_nodes/control_ur_helper.py b/journal_experiments/scripts/helper_nodes/control_ur_helper.py index e931695..56ede17 100755 --- a/journal_experiments/scripts/helper_nodes/control_ur_helper.py +++ b/journal_experiments/scripts/helper_nodes/control_ur_helper.py @@ -12,10 +12,10 @@ def __init__(self,base_node): self.base_node.ur_command_topic = rospy.get_param("~ur_command_topic", "/mur620c/UR10_r/twist_controller/command_safe") self.base_node.ur_pose_topic = rospy.get_param("~ur_pose_topic", "/mur620c/UR10_r/ur_calibrated_pose") - self.base_node.mir_pose_topic = rospy.get_param("~mir_pose_topic", "/mur620c/mir/robot_pose") + self.base_node.mir_pose_topic = rospy.get_param("~mir_pose_topic", "/mur620c/robot_pose") self.lateral_nozzle_pose_override_topic = rospy.get_param("~lateral_nozzle_pose_override_topic", "/lateral_nozzle_pose_override") self.base_node.ur_base_link_frame_id = rospy.get_param("~ur_base_link_frame_id", "mur620c/UR10_r/base_link") - self.base_node.mir_cmd_vel_topic = rospy.get_param("~mir_cmd_vel_topic", "/mur620c/mir/cmd_vel") + self.base_node.mir_cmd_vel_topic = rospy.get_param("~mir_cmd_vel_topic", "/mur620c/cmd_vel") self.base_node.tf_prefix = rospy.get_param("~tf_prefix", "mur620c/") self.base_node.ur_twist_publisher = rospy.Publisher(self.base_node.ur_command_topic, Twist, queue_size=1) self.base_node.ur_target_pose_broadcaster = tf.TransformBroadcaster() diff --git a/journal_experiments/scripts/move_mir.py b/journal_experiments/scripts/move_mir.py index cd08d12..3744892 100755 --- a/journal_experiments/scripts/move_mir.py +++ b/journal_experiments/scripts/move_mir.py @@ -28,7 +28,7 @@ def __init__(self): self.listener = listener.TransformListener() self.config() self.ur_command_old = Twist() - self.cmd_vel_pub = rospy.Publisher("/mur620b/mir/cmd_vel", Twist, queue_size=1) + self.cmd_vel_pub = rospy.Publisher("/mur620b/cmd_vel", Twist, queue_size=1) self.ur_twist_pub = rospy.Publisher("/UR10_r/twist_controller/command", Twist, queue_size=1) rospy.Subscriber("/qualisys/mur620b/pose", PoseStamped, self.pose_callback) diff --git a/journal_experiments/scripts/move_to_start_pose.py b/journal_experiments/scripts/move_to_start_pose.py index 74ca6c8..c043c4b 100755 --- a/journal_experiments/scripts/move_to_start_pose.py +++ b/journal_experiments/scripts/move_to_start_pose.py @@ -19,18 +19,19 @@ def __init__(self): self.linear_vel_limit = 0.08 self.angular_vel_limit = 0.2 self.linear_tolerance = 0.15 - + self.cmd_vel_topic = rospy.get_param("mir_cmd_vel_topic", "/mur620c/cmd_vel") + rospy.loginfo("cmd_vel_topic: %s", self.cmd_vel_topic) self.move_base_simple_goal_pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=1) - self.cmd_vel_pub = rospy.Publisher('/mur620c/mir/cmd_vel', Twist, queue_size=1) - rospy.Subscriber('/mur620c/mir/mir_pose_simple', Pose, self.mir_pose_callback) + self.cmd_vel_pub = rospy.Publisher(self.cmd_vel_topic, Twist, queue_size=1) + rospy.Subscriber('/mur620c/mir_pose_simple', Pose, self.mir_pose_callback) rospy.sleep(0.1) def run(self): # wait for pose - rospy.loginfo('Waiting for first message from /mur620c/mir/mir_pose_simple...') - rospy.wait_for_message('/mur620c/mir/mir_pose_simple', Pose) + rospy.loginfo('Waiting for first message from /mur620c/mir_pose_simple...') + rospy.wait_for_message('/mur620c/mir_pose_simple', Pose) # send the robot to the target pose using move base action server @@ -133,7 +134,7 @@ def turn_to_target_orientation(self): # stop the robot self.target_vel.angular.z = 0 self.cmd_vel_pub.publish(self.target_vel) - rospy.loginfo('Robot turned to the target pose') + rospy.loginfo('Robot turned to the target orientation') def move_to_target_pose(self): rate = rospy.Rate(self.control_rate) diff --git a/journal_experiments/scripts/state_machine.py b/journal_experiments/scripts/state_machine.py index fc3e147..d0fa158 100755 --- a/journal_experiments/scripts/state_machine.py +++ b/journal_experiments/scripts/state_machine.py @@ -95,10 +95,11 @@ def execute(self, userdata): # get transformation between ur and mir tf_listener = TransformListener() # wait for transform - tf_listener.waitForTransform(robot_names[0] + "/mir/base_link", robot_names[0] + "/UR10_r/base_link", rospy.Time(0), rospy.Duration(4.0)) - lin, ang = tf_listener.lookupTransform(robot_names[0] + "/mir/base_link", robot_names[0] + "/UR10_r/base_link", rospy.Time(0)) + tf_listener.waitForTransform(robot_names[0] + "/base_link", robot_names[0] + "/UR10_r/base_link", rospy.Time(0), rospy.Duration(4.0)) + lin, ang = tf_listener.lookupTransform(robot_names[0] + "/base_link", robot_names[0] + "/UR10_r/base_link", rospy.Time(0)) - ur_start_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + rospy.loginfo(f"UR start pose orientation: {ur_path.poses[1].pose.orientation}") + ur_start_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] ur_start_pose[0] = relative_positions_x_local + lin[0] ur_start_pose[1] = relative_positions_y_local + lin[1] ur_start_pose[2] = ur_path.poses[1].pose.position.z - lin[2] From 6dbb7eab2d85f5ed1a94fd12e5b44e163a41c9c6 Mon Sep 17 00:00:00 2001 From: Hauke Date: Wed, 9 Aug 2023 17:07:38 +0200 Subject: [PATCH 2/8] sim: all states are running --- journal_experiments/TODO.md | 21 + journal_experiments/launch/mur_sim.launch | 4 +- .../launch/start_experiment.launch | 25 +- .../rviz/mur620c_monitoring.rviz | 566 +++++++++--------- .../scripts/calibrated_pose_fake.py | 7 +- journal_experiments/scripts/control_mir.py | 6 +- journal_experiments/scripts/create_path.py | 16 +- .../scripts/helper_nodes/control_ur_helper.py | 1 + .../scripts/move_to_start_pose.py | 2 +- .../scripts/move_ur_to_start_pose.py | 58 +- journal_experiments/scripts/state_machine.py | 43 +- 11 files changed, 438 insertions(+), 311 deletions(-) create mode 100644 journal_experiments/TODO.md diff --git a/journal_experiments/TODO.md b/journal_experiments/TODO.md new file mode 100644 index 0000000..061352d --- /dev/null +++ b/journal_experiments/TODO.md @@ -0,0 +1,21 @@ +# TODO for SImualtion of journal_experiments + +## Launch + +```bash +roslaunch journal_experiments mur_sim.launch +roslaunch journal_experiments start_experiment.launch +``` + +wait for URs to reach home pose, then: + +```bash +roslaunch journal_experiments twist_sim.launch +``` + +## Problems + +- No PointCloud! + - lateral_nozzle_pose_override is set to 0 in control_ur +- ur_control is not working correctly while printing + - false transformations? diff --git a/journal_experiments/launch/mur_sim.launch b/journal_experiments/launch/mur_sim.launch index 84ff700..825c19c 100644 --- a/journal_experiments/launch/mur_sim.launch +++ b/journal_experiments/launch/mur_sim.launch @@ -1,5 +1,7 @@ - + + + diff --git a/journal_experiments/launch/start_experiment.launch b/journal_experiments/launch/start_experiment.launch index 1599bce..8fc975e 100644 --- a/journal_experiments/launch/start_experiment.launch +++ b/journal_experiments/launch/start_experiment.launch @@ -3,10 +3,33 @@ - mur620c/mir_cmd_vel_topic: "/mur620c/mobile_base_controller/cmd_vel" + move_to_start_pose/mir_cmd_vel_topic: "/mur620c/mobile_base_controller/cmd_vel" move_ur_to_start_pose/ur_command_topic: "/mur620c/UR10_l/twist_controller/command_safe" + move_ur_to_start_pose/ur_pose_topic: "/mur620c/UR10_l/ur_calibrated_pose" + move_ur_to_start_pose/ur_base_link_frame_id: "mur620c/UR10_l/base_link" + + move_ur_to_start_pose/ur_target_tolerance_trans: 0.1 + move_ur_to_start_pose/ur_target_tolerance_rot: 0.5 + + move_ur_to_start_pose/Kpx: 0.5 + move_ur_to_start_pose/Kpy: 0.5 + move_ur_to_start_pose/Kpz: 0.5 + move_ur_to_start_pose/Kp_phi: 0.1 + + control_ur/mir_cmd_vel_topic: "/mur620c/mobile_base_controller/cmd_vel" + control_ur/ur_command_topic: "/mur620c/UR10_l/twist_controller/command_safe" + control_ur/mir_pose_topic : /mur620c/mir_pose_simple + control_ur/ur_pose_topic: "/mur620c/UR10_l/ur_calibrated_pose" + control_ur/ur_base_link_frame_id: "mur620c/UR10_l/base_link" + + control_mir/mir_cmd_vel_topic_relative: "mobile_base_controller/cmd_vel" + + calibrated_pose_fake/namespace: "mur620c/UR10_l/" + + + diff --git a/journal_experiments/rviz/mur620c_monitoring.rviz b/journal_experiments/rviz/mur620c_monitoring.rviz index ce39f87..3aef2a0 100644 --- a/journal_experiments/rviz/mur620c_monitoring.rviz +++ b/journal_experiments/rviz/mur620c_monitoring.rviz @@ -6,16 +6,11 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /RobotModel1 - - /TF1 - /TF1/Frames1 - - /Path1 - - /RobotModel2 - - /Path2 - - /PointCloud21 - - /RobotModel3 - Splitter Ratio: 0.8283433318138123 - Tree Height: 1522 + - /Pose1/Status1 + - /Pose2 + Splitter Ratio: 0.6205882430076599 + Tree Height: 627 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -33,7 +28,7 @@ Panels: - Class: rviz/Time Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: "" Preferences: PromptSaveOnExit: true Toolbars: @@ -79,6 +74,126 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + UR10_l/base: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/flange: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/gripper: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/base: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/flange: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/gripper: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true back_laser_link: Alpha: 1 Show Axes: false @@ -235,8 +350,8 @@ Visualization Manager: Show Axes: false Show Trail: false Name: RobotModel - Robot Description: /mur620c/mir/robot_description - TF Prefix: mur620c/mir + Robot Description: /mur620c/robot_description + TF Prefix: mur620c Update Interval: 0 Value: true Visual Enabled: true @@ -247,148 +362,130 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: false - actual_pose_0: - Value: true - base: - Value: false map: Value: false - mir_global_target_pose: - Value: true - mocap: - Value: true - mur620b/UR10_r/base_link: + mur620c/UR10_l/base: Value: false - mur620b/UR10_r/calibrated_ee_pose: + mur620c/UR10_l/base_link: + Value: true + mur620c/UR10_l/base_link_inertia: Value: false - mur620c/UR10_r/base: + mur620c/UR10_l/flange: Value: false - mur620c/UR10_r/base_link: + mur620c/UR10_l/forearm_link: Value: false - mur620c/UR10_r/base_link_inertia: + mur620c/UR10_l/gripper: Value: false - mur620c/UR10_r/flange: + mur620c/UR10_l/shoulder_link: Value: false - mur620c/UR10_r/forearm_link: + mur620c/UR10_l/tcp: Value: false - mur620c/UR10_r/shoulder_link: - Value: false - mur620c/UR10_r/tool0: - Value: true - mur620c/UR10_r/upper_arm_link: - Value: false - mur620c/UR10_r/wrist_1_link: + mur620c/UR10_l/tool0: Value: false - mur620c/UR10_r/wrist_2_link: + mur620c/UR10_l/upper_arm_link: Value: false - mur620c/UR10_r/wrist_3_link: + mur620c/UR10_l/wrist_1_link: Value: false - mur620c/mir/back_laser_link: + mur620c/UR10_l/wrist_2_link: Value: false - mur620c/mir/base_footprint: + mur620c/UR10_l/wrist_3_link: Value: false - mur620c/mir/base_link: + mur620c/UR10_r/base: Value: false - mur620c/mir/bl_caster_rotation_link: + mur620c/UR10_r/base_link: Value: false - mur620c/mir/bl_caster_wheel_link: + mur620c/UR10_r/base_link_inertia: Value: false - mur620c/mir/br_caster_rotation_link: + mur620c/UR10_r/flange: Value: false - mur620c/mir/br_caster_wheel_link: + mur620c/UR10_r/forearm_link: Value: false - mur620c/mir/camera_bottom_screw_frame: + mur620c/UR10_r/gripper: Value: false - mur620c/mir/camera_color_frame: + mur620c/UR10_r/shoulder_link: Value: false - mur620c/mir/camera_color_optical_frame: + mur620c/UR10_r/tcp: Value: false - mur620c/mir/camera_depth_frame: + mur620c/UR10_r/tool0: Value: false - mur620c/mir/camera_depth_optical_frame: + mur620c/UR10_r/upper_arm_link: Value: false - mur620c/mir/camera_floor_left_color_frame: + mur620c/UR10_r/wrist_1_link: Value: false - mur620c/mir/camera_floor_left_color_optical_frame: + mur620c/UR10_r/wrist_2_link: Value: false - mur620c/mir/camera_floor_left_depth_optical_frame: + mur620c/UR10_r/wrist_3_link: Value: false - mur620c/mir/camera_floor_left_link: + mur620c/back_laser_link: Value: false - mur620c/mir/camera_floor_right_color_frame: + mur620c/base_footprint: Value: false - mur620c/mir/camera_floor_right_color_optical_frame: + mur620c/base_link: + Value: true + mur620c/bl_caster_rotation_link: Value: false - mur620c/mir/camera_floor_right_depth_optical_frame: + mur620c/bl_caster_wheel_link: Value: false - mur620c/mir/camera_floor_right_link: + mur620c/br_caster_rotation_link: Value: false - mur620c/mir/camera_infra1_frame: + mur620c/br_caster_wheel_link: Value: false - mur620c/mir/camera_infra1_optical_frame: + mur620c/camera_bottom_screw_frame: Value: false - mur620c/mir/camera_infra2_frame: + mur620c/camera_color_frame: Value: false - mur620c/mir/camera_infra2_optical_frame: + mur620c/camera_color_optical_frame: Value: false - mur620c/mir/camera_link: + mur620c/camera_depth_frame: Value: false - mur620c/mir/fl_caster_rotation_link: + mur620c/camera_depth_optical_frame: Value: false - mur620c/mir/fl_caster_wheel_link: + mur620c/camera_infra1_frame: Value: false - mur620c/mir/fr_caster_rotation_link: + mur620c/camera_infra1_optical_frame: Value: false - mur620c/mir/fr_caster_wheel_link: + mur620c/camera_infra2_frame: Value: false - mur620c/mir/front_laser_link: + mur620c/camera_infra2_optical_frame: Value: false - mur620c/mir/imu_frame: + mur620c/camera_link: Value: false - mur620c/mir/imu_link: + mur620c/fl_caster_rotation_link: Value: false - mur620c/mir/left_lift_bottom: + mur620c/fl_caster_wheel_link: Value: false - mur620c/mir/left_wheel_link: + mur620c/fr_caster_rotation_link: Value: false - mur620c/mir/odom: + mur620c/fr_caster_wheel_link: Value: false - mur620c/mir/right_lift_bottom: + mur620c/front_laser_link: Value: false - mur620c/mir/right_lift_top: + mur620c/imu_frame: Value: false - mur620c/mir/right_wheel_link: + mur620c/imu_link: Value: false - mur620c/mir/surface: + mur620c/left_lift_bottom: Value: false - mur620c/mir/top_module: + mur620c/left_lift_top: Value: false - mur620c/mir/us_1_frame: + mur620c/left_wheel_link: Value: false - mur620c/mir/us_2_frame: + mur620c/odom: Value: false - mur620c/target_point: - Value: true - sensor_frame: + mur620c/right_lift_bottom: Value: false - sensor_optical_frame: + mur620c/right_lift_top: Value: false - target_point: + mur620c/right_wheel_link: Value: false - target_pose_0: + mur620c/surface: Value: false - target_pose_controll0: - Value: true - tool0_controller: + mur620c/top_module: Value: false - ur_global_target_pose: - Value: true - ur_local_target_pose: + mur620c/us_1_frame: Value: false - ur_target_pose: + mur620c/us_2_frame: Value: false - world: - Value: true Marker Alpha: 1 Marker Scale: 1 Name: TF @@ -396,73 +493,70 @@ Visualization Manager: Show Axes: true Show Names: true Tree: - base: - tool0_controller: - {} map: - actual_pose_0: - {} - mir_global_target_pose: - {} - mur620c/mir/odom: - mur620c/mir/base_footprint: - mur620c/mir/base_link: - mur620c/mir/back_laser_link: + mur620c/odom: + mur620c/base_footprint: + mur620c/base_link: + mur620c/back_laser_link: {} - mur620c/mir/bl_caster_rotation_link: - mur620c/mir/bl_caster_wheel_link: + mur620c/bl_caster_rotation_link: + mur620c/bl_caster_wheel_link: {} - mur620c/mir/br_caster_rotation_link: - mur620c/mir/br_caster_wheel_link: + mur620c/br_caster_rotation_link: + mur620c/br_caster_wheel_link: {} - mur620c/mir/camera_bottom_screw_frame: - mur620c/mir/camera_link: - mur620c/mir/camera_color_frame: - mur620c/mir/camera_color_optical_frame: + mur620c/camera_bottom_screw_frame: + mur620c/camera_link: + mur620c/camera_color_frame: + mur620c/camera_color_optical_frame: {} - mur620c/mir/camera_depth_frame: - mur620c/mir/camera_depth_optical_frame: + mur620c/camera_depth_frame: + mur620c/camera_depth_optical_frame: {} - mur620c/mir/camera_floor_left_link: - mur620c/mir/camera_floor_left_color_frame: - mur620c/mir/camera_floor_left_color_optical_frame: - {} - mur620c/mir/camera_floor_left_depth_optical_frame: - {} - mur620c/mir/camera_infra1_frame: - mur620c/mir/camera_infra1_optical_frame: - {} - mur620c/mir/camera_infra2_frame: - mur620c/mir/camera_infra2_optical_frame: - {} - mur620c/mir/camera_floor_right_link: - mur620c/mir/camera_floor_right_color_frame: - mur620c/mir/camera_floor_right_color_optical_frame: - {} - mur620c/mir/camera_floor_right_depth_optical_frame: - {} - mur620c/mir/fl_caster_rotation_link: - mur620c/mir/fl_caster_wheel_link: + mur620c/camera_infra1_frame: + mur620c/camera_infra1_optical_frame: + {} + mur620c/camera_infra2_frame: + mur620c/camera_infra2_optical_frame: + {} + mur620c/fl_caster_rotation_link: + mur620c/fl_caster_wheel_link: {} - mur620c/mir/fr_caster_rotation_link: - mur620c/mir/fr_caster_wheel_link: + mur620c/fr_caster_rotation_link: + mur620c/fr_caster_wheel_link: {} - mur620c/mir/front_laser_link: + mur620c/front_laser_link: {} - mur620c/mir/imu_link: - mur620c/mir/imu_frame: + mur620c/imu_link: + mur620c/imu_frame: {} - mur620c/mir/left_wheel_link: + mur620c/left_wheel_link: {} - mur620c/mir/right_wheel_link: + mur620c/right_wheel_link: {} - mur620c/mir/surface: + mur620c/surface: {} - mur620c/mir/top_module: - mur620c/mir/left_lift_bottom: - {} - mur620c/mir/right_lift_bottom: - mur620c/mir/right_lift_top: + mur620c/top_module: + mur620c/left_lift_bottom: + mur620c/left_lift_top: + mur620c/UR10_l/base_link: + mur620c/UR10_l/base: + {} + mur620c/UR10_l/base_link_inertia: + mur620c/UR10_l/shoulder_link: + mur620c/UR10_l/upper_arm_link: + mur620c/UR10_l/forearm_link: + mur620c/UR10_l/wrist_1_link: + mur620c/UR10_l/wrist_2_link: + mur620c/UR10_l/wrist_3_link: + mur620c/UR10_l/flange: + mur620c/UR10_l/gripper: + mur620c/UR10_l/tcp: + {} + mur620c/UR10_l/tool0: + {} + mur620c/right_lift_bottom: + mur620c/right_lift_top: mur620c/UR10_r/base_link: mur620c/UR10_r/base: {} @@ -474,35 +568,15 @@ Visualization Manager: mur620c/UR10_r/wrist_2_link: mur620c/UR10_r/wrist_3_link: mur620c/UR10_r/flange: + mur620c/UR10_r/gripper: + mur620c/UR10_r/tcp: + {} mur620c/UR10_r/tool0: {} - sensor_optical_frame: - sensor_frame: - {} - target_point: - {} - ur_target_pose: - {} - mur620c/mir/us_1_frame: - {} - mur620c/mir/us_2_frame: + mur620c/us_1_frame: {} - ur_local_target_pose: + mur620c/us_2_frame: {} - target_pose_controll0: - {} - ur_global_target_pose: - {} - world: - {} - mocap: - mur620c/target_point: - {} - target_pose_0: - {} - mur620b/UR10_r/base_link: - mur620b/UR10_r/calibrated_ee_pose: - {} Update Interval: 0 Value: true - Alpha: 1 @@ -529,80 +603,13 @@ Visualization Manager: Topic: /mir_path Unreliable: false Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link_inertia: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - flange: - Alpha: 1 - Show Axes: false - Show Trail: false - forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - shoulder_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wrist_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wrist_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wrist_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: /mur620c/UR10_r/robot_description - TF Prefix: mur620c/UR10_r - Update Interval: 0 - Value: true - Visual Enabled: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 164; 0; 0 Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 + Head Diameter: 0.0005000000237487257 + Head Length: 0.0005000000237487257 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 @@ -612,11 +619,11 @@ Visualization Manager: Y: 0 Z: 0 Pose Color: 255; 85; 255 - Pose Style: None + Pose Style: Arrows Queue Size: 10 Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 + Shaft Diameter: 0.00019999999494757503 + Shaft Length: 9.000000136438757e-05 Topic: /ur_path Unreliable: false Value: true @@ -632,7 +639,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 @@ -647,28 +654,39 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true + Value: false + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 0.10000000149011612 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /start_pose_ur + Unreliable: false Value: true - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 252; 233; 79 Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - sensor_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: /lj_v7200_robot_description - TF Prefix: "" - Update Interval: 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /ur_path_start + Unreliable: false Value: true - Visual Enabled: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -697,7 +715,7 @@ Visualization Manager: Views: Current: Class: rviz/ThirdPersonFollower - Distance: 2.102748155593872 + Distance: 8.133821487426758 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -705,25 +723,25 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 52.59535598754883 - Y: 43.36806106567383 - Z: -8.135128155117854e-06 + X: 1.0051915645599365 + Y: 1.4709643125534058 + Z: -8.909988537197933e-06 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.8503982424736023 + Pitch: 1.2303980588912964 Target Frame: - Yaw: 0.20040541887283325 + Yaw: 0.4254046082496643 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 2032 + Height: 1016 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000003b6000006eafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000371000003d1fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000006ea0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000006eafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000006ea0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e940000005efc0100000002fb0000000800540069006d0065010000000000000e94000006dc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000967000006ea00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001560000033afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000002d60000006700000371000003d1fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000033a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f0000033afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000033a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000005efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004770000033a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -732,6 +750,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 3732 - X: 7788 - Y: 54 + Width: 1848 + X: 798 + Y: 27 diff --git a/journal_experiments/scripts/calibrated_pose_fake.py b/journal_experiments/scripts/calibrated_pose_fake.py index 0e12811..e241326 100755 --- a/journal_experiments/scripts/calibrated_pose_fake.py +++ b/journal_experiments/scripts/calibrated_pose_fake.py @@ -5,11 +5,12 @@ class RemapUrEeTopic: def __init__(self) -> None: + ns = rospy.get_param("~namespace", "mur620c/UR10_r/") self.pose = PoseStamped() - self.pose.header.frame_id = "mur620c/UR10_r/base_link" + self.pose.header.frame_id = ns+"base_link" - self.pub = rospy.Publisher("/mur620c/UR10_r/ur_calibrated_pose", PoseStamped, queue_size=1) - rospy.Subscriber("/mur620c/UR10_r/tcp_pose", Pose, self.cb_ee_pose) + self.pub = rospy.Publisher(ns + "ur_calibrated_pose", PoseStamped, queue_size=1) + rospy.Subscriber(ns + "tcp_pose", Pose, self.cb_ee_pose) def cb_ee_pose(self, msg: Pose): self.pose.header.stamp = rospy.Time.now() diff --git a/journal_experiments/scripts/control_mir.py b/journal_experiments/scripts/control_mir.py index 78a3671..a37c1bd 100755 --- a/journal_experiments/scripts/control_mir.py +++ b/journal_experiments/scripts/control_mir.py @@ -13,6 +13,8 @@ class Control_mir_node(): def __init__(self): + self.mir_cmd_vel_topic_relative = rospy.get_param("~mir_cmd_vel_topic_relative", "cmd_vel") + self.mir_pose_topic_relative = rospy.get_param("~mir_pose_topic_relative", "mir_pose_simple") self.external_control = rospy.get_param("~external_control", True) self.path_array = rospy.get_param("~path_array", []) self.relative_positions_x = rospy.get_param("~relative_positions_x", [0]) @@ -41,10 +43,10 @@ def __init__(self): for i in range(len(self.robot_names)): self.robot_path_publishers.append(rospy.Publisher(self.robot_names[i] + '/robot_path', Path, queue_size=1)) - self.robot_twist_publishers.append(rospy.Publisher(self.robot_names[i] + '/cmd_vel', Twist, queue_size=1)) + self.robot_twist_publishers.append(rospy.Publisher(self.robot_names[i] +'/'+self.mir_cmd_vel_topic_relative, Twist, queue_size=1)) for i in range(len(self.robot_names)): - rospy.Subscriber(self.robot_names[i] + '/mir_pose_simple', Pose, self.mir_pose_callback, i) + rospy.Subscriber(self.robot_names[i] + '/'+self.mir_pose_topic_relative, Pose, self.mir_pose_callback, i) if self.external_control: rospy.Subscriber('path_index', Int32, self.path_index_callback) diff --git a/journal_experiments/scripts/create_path.py b/journal_experiments/scripts/create_path.py index 69353cb..f61c7bc 100755 --- a/journal_experiments/scripts/create_path.py +++ b/journal_experiments/scripts/create_path.py @@ -12,6 +12,9 @@ def config(self): self.target_pose.position.x = 2.5 self.point_per_meter = 100 self.dist = 100 + self.r_ur = 1.0 + self.r_mir=2.0 + self.shift_ur = 0.1 # in meter pass def main(self): @@ -34,18 +37,17 @@ def main(self): ur_path = Path() ur_path.header.frame_id = "map" - for i in range(0,int(self.dist*self.point_per_meter)): pose = PoseStamped() pose.header.frame_id = "map" pose.header.stamp = rospy.Time.now() pose.header.seq = i # plan circle - pose.pose.position.x = 2.0*math.cos(i/self.point_per_meter) - pose.pose.position.y = 2.0*math.sin(i/self.point_per_meter) + pose.pose.position.x = self.r_mir*math.cos(i/self.point_per_meter) + pose.pose.position.y = self.r_mir*math.sin(i/self.point_per_meter) next_pose = Pose() - next_pose.position.x = 2.0*math.cos((i+1)/self.point_per_meter) - next_pose.position.y = 2.0*math.sin((i+1)/self.point_per_meter) + next_pose.position.x = self.r_mir*math.cos((i+1)/self.point_per_meter) + next_pose.position.y = self.r_mir*math.sin((i+1)/self.point_per_meter) orientation = math.atan2(next_pose.position.y - pose.pose.position.y, next_pose.position.x - pose.pose.position.x) q = tf.transformations.quaternion_from_euler(0, 0, orientation) pose.pose.orientation.x = q[0] @@ -61,8 +63,8 @@ def main(self): ur_pose.header.frame_id = "map" ur_pose.header.stamp = rospy.Time.now() ur_pose.header.seq = i - ur_pose.pose.position.x = 1.0*math.cos(i/self.point_per_meter) + 0.05 * math.sin(6*i/self.point_per_meter) - ur_pose.pose.position.y = 1.0*math.sin(i/self.point_per_meter) + 0.05 * math.cos(6*i/self.point_per_meter) + ur_pose.pose.position.x = self.r_ur*math.cos((i+self.shift_ur)/self.point_per_meter) + 0.05 * math.sin(6*i/self.point_per_meter) + ur_pose.pose.position.y = self.r_ur*math.sin((i+self.shift_ur)/self.point_per_meter) + 0.05 * math.cos(6*i/self.point_per_meter) ur_pose.pose.position.z = 0.5 ur_pose.pose.orientation = pose.pose.orientation diff --git a/journal_experiments/scripts/helper_nodes/control_ur_helper.py b/journal_experiments/scripts/helper_nodes/control_ur_helper.py index 56ede17..deebdd0 100755 --- a/journal_experiments/scripts/helper_nodes/control_ur_helper.py +++ b/journal_experiments/scripts/helper_nodes/control_ur_helper.py @@ -14,6 +14,7 @@ def __init__(self,base_node): self.base_node.ur_pose_topic = rospy.get_param("~ur_pose_topic", "/mur620c/UR10_r/ur_calibrated_pose") self.base_node.mir_pose_topic = rospy.get_param("~mir_pose_topic", "/mur620c/robot_pose") self.lateral_nozzle_pose_override_topic = rospy.get_param("~lateral_nozzle_pose_override_topic", "/lateral_nozzle_pose_override") + self.base_node.lateral_nozzle_pose_override = 0.0 self.base_node.ur_base_link_frame_id = rospy.get_param("~ur_base_link_frame_id", "mur620c/UR10_r/base_link") self.base_node.mir_cmd_vel_topic = rospy.get_param("~mir_cmd_vel_topic", "/mur620c/cmd_vel") self.base_node.tf_prefix = rospy.get_param("~tf_prefix", "mur620c/") diff --git a/journal_experiments/scripts/move_to_start_pose.py b/journal_experiments/scripts/move_to_start_pose.py index c043c4b..22222cc 100755 --- a/journal_experiments/scripts/move_to_start_pose.py +++ b/journal_experiments/scripts/move_to_start_pose.py @@ -19,7 +19,7 @@ def __init__(self): self.linear_vel_limit = 0.08 self.angular_vel_limit = 0.2 self.linear_tolerance = 0.15 - self.cmd_vel_topic = rospy.get_param("mir_cmd_vel_topic", "/mur620c/cmd_vel") + self.cmd_vel_topic = rospy.get_param("~mir_cmd_vel_topic", "/mur620c/cmd_vel") rospy.loginfo("cmd_vel_topic: %s", self.cmd_vel_topic) self.move_base_simple_goal_pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=1) diff --git a/journal_experiments/scripts/move_ur_to_start_pose.py b/journal_experiments/scripts/move_ur_to_start_pose.py index 2845939..f63c2eb 100755 --- a/journal_experiments/scripts/move_ur_to_start_pose.py +++ b/journal_experiments/scripts/move_ur_to_start_pose.py @@ -7,6 +7,7 @@ from geometry_msgs.msg import PoseStamped, Pose, Twist import math from tf import transformations +import numpy as np class MoveURToStartPose(): @@ -19,7 +20,8 @@ def config(self): self.Kpy = rospy.get_param("~Kpy", 0.3) self.Kpz = rospy.get_param("~Kpz", 0.1) self.Kp_phi = rospy.get_param("~Kp_phi", 0.3) - self.ur_target_tolerance = rospy.get_param("~ur_target_tolerance", 0.01) + self.ur_target_tolerance_trans = rospy.get_param("~ur_target_tolerance_trans", 0.01) + self.ur_target_tolerance_rot = rospy.get_param("~ur_target_tolerance_rot", 0.01) self.ur_scanner_angular_offset = rospy.get_param("~ur_scanner_angular_offset", -math.pi) self.mir_angle = rospy.get_param("~mir_angle", 0.0) pass @@ -50,6 +52,14 @@ def __init__(self): self.config() + # For Debugging: + robot_names = rospy.get_param("~robot_names", ["mur620c"]) + pub_start_pose = rospy.Publisher("/start_pose_ur", PoseStamped, queue_size=1, latch=True) + pose_pub = PoseStamped() + pose_pub.header.frame_id = robot_names[0] + "/UR10_l/base_link" + # pose_pub.header.frame_id = robot_names[0] + "/base_link" + pose_pub.pose = self.ur_start_pose + pub_start_pose.publish(pose_pub) def move_ur_to_start_pose(self): @@ -62,30 +72,43 @@ def move_ur_to_start_pose(self): # broadcast start pose self.broadcast_target_pose(self.ur_start_pose) - # the ur is mounted backwards, so we have to invert the x and y axis - ur_start_pose_x = - self.ur_start_pose.position.x - ur_start_pose_y = - self.ur_start_pose.position.y + # for UR_r_ the ur is mounted backwards, so we have to invert the x and y axis (ONLY IF goal pose is calculated wrong before...) + ur_start_pose_x = self.ur_start_pose.position.x + ur_start_pose_y = self.ur_start_pose.position.y # compute ur phi angle ur_current_phi = transformations.euler_from_quaternion([self.ur_pose_current.orientation.x, self.ur_pose_current.orientation.y, self.ur_pose_current.orientation.z, self.ur_pose_current.orientation.w])[2] ur_target_phi = transformations.euler_from_quaternion([self.ur_start_pose.orientation.x, self.ur_start_pose.orientation.y, self.ur_start_pose.orientation.z, self.ur_start_pose.orientation.w])[2] - + # compute all orientation errors: + ur_current_euler = np.array(transformations.euler_from_quaternion([self.ur_pose_current.orientation.x, self.ur_pose_current.orientation.y, self.ur_pose_current.orientation.z, self.ur_pose_current.orientation.w])) + ur_target_euler = np.array(transformations.euler_from_quaternion([self.ur_start_pose.orientation.x, self.ur_start_pose.orientation.y, self.ur_start_pose.orientation.z, self.ur_start_pose.orientation.w])) # calculate error e_x = ur_start_pose_x - self.ur_pose_current.position.x e_y = ur_start_pose_y - self.ur_pose_current.position.y e_z = self.ur_start_pose.position.z - self.ur_pose_current.position.z - e_phi = ur_target_phi - self.mir_angle - ur_current_phi + self.ur_scanner_angular_offset + # e_phi = ur_target_phi - self.mir_angle - ur_current_phi + self.ur_scanner_angular_offset + ur_euler_error = ur_target_euler - ur_current_euler + ur_euler_error[2] = ur_euler_error[2] - self.mir_angle + self.ur_scanner_angular_offset + for i, e_angle in enumerate(ur_euler_error): + if e_angle > math.pi: + ur_euler_error[i] = e_angle - 2 * math.pi + elif e_angle < -math.pi: + ur_euler_error[i] = e_angle + 2 * math.pi + ur_euler_error*=self.Kp_phi - if e_phi > math.pi: - e_phi = e_phi - 2 * math.pi - elif e_phi < -math.pi: - e_phi = e_phi + 2 * math.pi + # if e_phi > math.pi: + # e_phi = e_phi - 2 * math.pi + # elif e_phi < -math.pi: + # e_phi = e_phi + 2 * math.pi # calculate command self.ur_command.linear.x = e_x * self.Kpx self.ur_command.linear.y = e_y * self.Kpy self.ur_command.linear.z = e_z * self.Kpz - self.ur_command.angular.z = e_phi * self.Kp_phi + # self.ur_command.angular.z = e_phi * self.Kp_phi + self.ur_command.angular.x = ur_euler_error[0] + self.ur_command.angular.y = ur_euler_error[1] + self.ur_command.angular.z = ur_euler_error[2] # limit velocity ur_command = self.limit_velocity(self.ur_command, self.ur_command_old) @@ -94,11 +117,12 @@ def move_ur_to_start_pose(self): self.ur_twist_publisher.publish(ur_command) # check if target is reached - if abs(e_x) < self.ur_target_tolerance and abs(e_y) < self.ur_target_tolerance and abs(e_z) < self.ur_target_tolerance and abs(e_phi) < self.ur_target_tolerance : + if abs(e_x) < self.ur_target_tolerance_trans and abs(e_y) < self.ur_target_tolerance_trans and abs(e_z) < self.ur_target_tolerance_trans and all(abs(ur_euler_error) < self.ur_target_tolerance_rot) : self.ur_command.linear.x = 0 self.ur_command.linear.y = 0 self.ur_command.linear.z = 0 self.ur_twist_publisher.publish(self.ur_command) + rospy.loginfo("UR reached start pose") rospy.sleep(0.1) break @@ -140,14 +164,24 @@ def limit_velocity(self, ur_command, ur_command_old): ur_command.linear.z = ur_command.linear.z * vel_scale # limit angular velocity + if abs(ur_command.angular.x) > self.ur_angular_velocity_limit: + vel_scale = self.ur_angular_velocity_limit / abs(ur_command.angular.x) + if abs(ur_command.angular.y) > self.ur_angular_velocity_limit: + vel_scale = self.ur_angular_velocity_limit / abs(ur_command.angular.y) if abs(ur_command.angular.z) > self.ur_angular_velocity_limit: vel_scale = self.ur_angular_velocity_limit / abs(ur_command.angular.z) # limit angular acceleration + if abs(ur_command.angular.x - ur_command_old.angular.x) > self.ur_angular_acceleration_limit: + vel_scale = self.ur_angular_acceleration_limit / abs(ur_command.angular.x - ur_command_old.angular.x) + if abs(ur_command.angular.y - ur_command_old.angular.y) > self.ur_angular_acceleration_limit: + vel_scale = self.ur_angular_acceleration_limit / abs(ur_command.angular.y - ur_command_old.angular.y) if abs(ur_command.angular.z - ur_command_old.angular.z) > self.ur_angular_acceleration_limit: vel_scale = self.ur_angular_acceleration_limit / abs(ur_command.angular.z - ur_command_old.angular.z) # apply vel_scale + ur_command.angular.x = ur_command.angular.x * vel_scale + ur_command.angular.y = ur_command.angular.y * vel_scale ur_command.angular.z = ur_command.angular.z * vel_scale return ur_command diff --git a/journal_experiments/scripts/state_machine.py b/journal_experiments/scripts/state_machine.py index d0fa158..c4211a9 100755 --- a/journal_experiments/scripts/state_machine.py +++ b/journal_experiments/scripts/state_machine.py @@ -9,7 +9,8 @@ import math from copy import deepcopy from std_msgs.msg import String -from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseStamped, Quaternion +import numpy as np # define global variables path = Path(); @@ -82,6 +83,14 @@ def __init__(self): def execute(self, userdata): + + # Just for debugging: + pub_ur_path_start = rospy.Publisher("/ur_path_start", PoseStamped, queue_size=1, latch=True) + pose_start = PoseStamped() + pose_start.header.frame_id = "map" + pose_start.pose = ur_path.poses[1].pose + pub_ur_path_start.publish(pose_start) + # Just for debugging end relative_positions_x_global = path.poses[1].pose.position.x - ur_path.poses[0].pose.position.x relative_positions_y_global = path.poses[1].pose.position.y - ur_path.poses[0].pose.position.y @@ -95,18 +104,32 @@ def execute(self, userdata): # get transformation between ur and mir tf_listener = TransformListener() # wait for transform - tf_listener.waitForTransform(robot_names[0] + "/base_link", robot_names[0] + "/UR10_r/base_link", rospy.Time(0), rospy.Duration(4.0)) - lin, ang = tf_listener.lookupTransform(robot_names[0] + "/base_link", robot_names[0] + "/UR10_r/base_link", rospy.Time(0)) - + tf_listener.waitForTransform(robot_names[0] + "/base_link", robot_names[0] + "/UR10_l/base_link", rospy.Time(0), rospy.Duration(4.0)) + lin, ang = tf_listener.lookupTransform(robot_names[0] + "/base_link", robot_names[0] + "/UR10_l/base_link", rospy.Time(0)) + rospy.loginfo(f"UR start pose orientation: {ur_path.poses[1].pose.orientation}") + + # rotate around x so that the gripper is pointing down + q_rot = transformations.quaternion_from_euler(np.pi, 0, 0) + q_ur=transformations.quaternion_multiply(q_rot, ur_path.poses[1].pose.orientation.__reduce__()[2]) + q_rot = transformations.quaternion_from_euler(0, 0, np.pi/2) + q_ur=transformations.quaternion_multiply(q_rot, q_ur) + q_ur = q_ur.tolist() # because param cant handle numpy types + ur_start_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] - ur_start_pose[0] = relative_positions_x_local + lin[0] - ur_start_pose[1] = relative_positions_y_local + lin[1] + ur_start_pose[0] = -relative_positions_x_local - lin[0] + ur_start_pose[1] = -relative_positions_y_local - lin[1] ur_start_pose[2] = ur_path.poses[1].pose.position.z - lin[2] - ur_start_pose[3] = ur_path.poses[1].pose.orientation.x - ur_start_pose[4] = ur_path.poses[1].pose.orientation.y - ur_start_pose[5] = ur_path.poses[1].pose.orientation.z - ur_start_pose[6] = ur_path.poses[1].pose.orientation.w + # ur_start_pose[3] = ur_path.poses[1].pose.orientation.x + # ur_start_pose[4] = ur_path.poses[1].pose.orientation.y + # ur_start_pose[5] = ur_path.poses[1].pose.orientation.z + # ur_start_pose[6] = ur_path.poses[1].pose.orientation.w + ur_start_pose[3] = q_ur[0] + ur_start_pose[4] = q_ur[1] + ur_start_pose[5] = q_ur[2] + ur_start_pose[6] = q_ur[3] + + # TODO: ADD MIR ANGLE TO ORIENTATION? (right now done by move_ur_start_pose.py) rospy.loginfo('Executing state Move_UR_to_start_pose') process = launch_ros_node("move_ur_to_start_pose","journal_experiments","move_ur_to_start_pose.py", "", "", ur_start_pose=ur_start_pose, mir_angle = mir_angle) From 4f29769bf7123514d8a5622345316fc7d8bb4c41 Mon Sep 17 00:00:00 2001 From: Hauke Date: Tue, 15 Aug 2023 15:08:12 +0200 Subject: [PATCH 3/8] fixed creation of path --- journal_experiments/scripts/create_path.py | 18 ++++++++++---- journal_experiments/scripts/state_machine.py | 26 ++++++++++---------- 2 files changed, 26 insertions(+), 18 deletions(-) diff --git a/journal_experiments/scripts/create_path.py b/journal_experiments/scripts/create_path.py index f61c7bc..ff3f009 100755 --- a/journal_experiments/scripts/create_path.py +++ b/journal_experiments/scripts/create_path.py @@ -2,8 +2,9 @@ import rospy from nav_msgs.msg import Path from geometry_msgs.msg import PoseStamped, Pose -import math import tf +import numpy as np +import math class Create_path(): @@ -50,10 +51,17 @@ def main(self): next_pose.position.y = self.r_mir*math.sin((i+1)/self.point_per_meter) orientation = math.atan2(next_pose.position.y - pose.pose.position.y, next_pose.position.x - pose.pose.position.x) q = tf.transformations.quaternion_from_euler(0, 0, orientation) - pose.pose.orientation.x = q[0] - pose.pose.orientation.y = q[1] - pose.pose.orientation.z = q[2] - pose.pose.orientation.w = q[3] + + # rotate around x so that the gripper is pointing down + q_rot = tf.transformations.quaternion_from_euler(np.pi, 0, 0) + q_ur=tf.transformations.quaternion_multiply(q_rot, q) + q_rot = tf.transformations.quaternion_from_euler(0, 0, np.pi/2) + q_ur=tf.transformations.quaternion_multiply(q_rot, q_ur) + + pose.pose.orientation.x = q_ur[0] + pose.pose.orientation.y = q_ur[1] + pose.pose.orientation.z = q_ur[2] + pose.pose.orientation.w = q_ur[3] #pose.pose.position.x = self.start_pose.position.x + i/self.point_per_meter #pose.pose.position.y = pose.pose.position.x*0.5 + 0.5 * math.sin(pose.pose.position.x) path.poses.append(pose) diff --git a/journal_experiments/scripts/state_machine.py b/journal_experiments/scripts/state_machine.py index c4211a9..cf4b5b8 100755 --- a/journal_experiments/scripts/state_machine.py +++ b/journal_experiments/scripts/state_machine.py @@ -110,24 +110,24 @@ def execute(self, userdata): rospy.loginfo(f"UR start pose orientation: {ur_path.poses[1].pose.orientation}") # rotate around x so that the gripper is pointing down - q_rot = transformations.quaternion_from_euler(np.pi, 0, 0) - q_ur=transformations.quaternion_multiply(q_rot, ur_path.poses[1].pose.orientation.__reduce__()[2]) - q_rot = transformations.quaternion_from_euler(0, 0, np.pi/2) - q_ur=transformations.quaternion_multiply(q_rot, q_ur) - q_ur = q_ur.tolist() # because param cant handle numpy types + # q_rot = transformations.quaternion_from_euler(np.pi, 0, 0) + # q_ur=transformations.quaternion_multiply(q_rot, ur_path.poses[1].pose.orientation.__reduce__()[2]) + # q_rot = transformations.quaternion_from_euler(0, 0, np.pi/2) + # q_ur=transformations.quaternion_multiply(q_rot, q_ur) + # q_ur = q_ur.tolist() # because param cant handle numpy types ur_start_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] ur_start_pose[0] = -relative_positions_x_local - lin[0] ur_start_pose[1] = -relative_positions_y_local - lin[1] ur_start_pose[2] = ur_path.poses[1].pose.position.z - lin[2] - # ur_start_pose[3] = ur_path.poses[1].pose.orientation.x - # ur_start_pose[4] = ur_path.poses[1].pose.orientation.y - # ur_start_pose[5] = ur_path.poses[1].pose.orientation.z - # ur_start_pose[6] = ur_path.poses[1].pose.orientation.w - ur_start_pose[3] = q_ur[0] - ur_start_pose[4] = q_ur[1] - ur_start_pose[5] = q_ur[2] - ur_start_pose[6] = q_ur[3] + ur_start_pose[3] = ur_path.poses[1].pose.orientation.x + ur_start_pose[4] = ur_path.poses[1].pose.orientation.y + ur_start_pose[5] = ur_path.poses[1].pose.orientation.z + ur_start_pose[6] = ur_path.poses[1].pose.orientation.w + # ur_start_pose[3] = q_ur[0] + # ur_start_pose[4] = q_ur[1] + # ur_start_pose[5] = q_ur[2] + # ur_start_pose[6] = q_ur[3] # TODO: ADD MIR ANGLE TO ORIENTATION? (right now done by move_ur_start_pose.py) From 5f511fe8116f7d112d53c3706a468e9f9537b19d Mon Sep 17 00:00:00 2001 From: Hauke Date: Tue, 15 Aug 2023 15:08:41 +0200 Subject: [PATCH 4/8] removed unnecessary lines --- journal_experiments/scripts/move_ur_to_start_pose.py | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/journal_experiments/scripts/move_ur_to_start_pose.py b/journal_experiments/scripts/move_ur_to_start_pose.py index f63c2eb..0e2c4ab 100755 --- a/journal_experiments/scripts/move_ur_to_start_pose.py +++ b/journal_experiments/scripts/move_ur_to_start_pose.py @@ -2,8 +2,6 @@ import rospy import tf -from std_msgs.msg import Int32 -from nav_msgs.msg import Path from geometry_msgs.msg import PoseStamped, Pose, Twist import math from tf import transformations @@ -76,9 +74,6 @@ def move_ur_to_start_pose(self): ur_start_pose_x = self.ur_start_pose.position.x ur_start_pose_y = self.ur_start_pose.position.y - # compute ur phi angle - ur_current_phi = transformations.euler_from_quaternion([self.ur_pose_current.orientation.x, self.ur_pose_current.orientation.y, self.ur_pose_current.orientation.z, self.ur_pose_current.orientation.w])[2] - ur_target_phi = transformations.euler_from_quaternion([self.ur_start_pose.orientation.x, self.ur_start_pose.orientation.y, self.ur_start_pose.orientation.z, self.ur_start_pose.orientation.w])[2] # compute all orientation errors: ur_current_euler = np.array(transformations.euler_from_quaternion([self.ur_pose_current.orientation.x, self.ur_pose_current.orientation.y, self.ur_pose_current.orientation.z, self.ur_pose_current.orientation.w])) ur_target_euler = np.array(transformations.euler_from_quaternion([self.ur_start_pose.orientation.x, self.ur_start_pose.orientation.y, self.ur_start_pose.orientation.z, self.ur_start_pose.orientation.w])) @@ -95,11 +90,6 @@ def move_ur_to_start_pose(self): elif e_angle < -math.pi: ur_euler_error[i] = e_angle + 2 * math.pi ur_euler_error*=self.Kp_phi - - # if e_phi > math.pi: - # e_phi = e_phi - 2 * math.pi - # elif e_phi < -math.pi: - # e_phi = e_phi + 2 * math.pi # calculate command self.ur_command.linear.x = e_x * self.Kpx From 19c03106d273fc6c6021a05e77b9972f1738bbe2 Mon Sep 17 00:00:00 2001 From: Tobias Recker Date: Tue, 15 Aug 2023 16:29:46 +0200 Subject: [PATCH 5/8] now running in sim fixed false transform --- journal_experiments/TODO.md | 3 +++ journal_experiments/launch/start_experiment.launch | 2 +- journal_experiments/scripts/control_ur.py | 4 ++-- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/journal_experiments/TODO.md b/journal_experiments/TODO.md index 061352d..2060ecb 100644 --- a/journal_experiments/TODO.md +++ b/journal_experiments/TODO.md @@ -19,3 +19,6 @@ roslaunch journal_experiments twist_sim.launch - lateral_nozzle_pose_override is set to 0 in control_ur - ur_control is not working correctly while printing - false transformations? + +## Future +- All in namespace like move_to_start_pose diff --git a/journal_experiments/launch/start_experiment.launch b/journal_experiments/launch/start_experiment.launch index 8fc975e..5f5be2e 100644 --- a/journal_experiments/launch/start_experiment.launch +++ b/journal_experiments/launch/start_experiment.launch @@ -3,7 +3,7 @@ - move_to_start_pose/mir_cmd_vel_topic: "/mur620c/mobile_base_controller/cmd_vel" + mur620c/move_to_start_pose/mir_cmd_vel_topic: "/mur620c/mobile_base_controller/cmd_vel" move_ur_to_start_pose/ur_command_topic: "/mur620c/UR10_l/twist_controller/command_safe" move_ur_to_start_pose/ur_pose_topic: "/mur620c/UR10_l/ur_calibrated_pose" move_ur_to_start_pose/ur_base_link_frame_id: "mur620c/UR10_l/base_link" diff --git a/journal_experiments/scripts/control_ur.py b/journal_experiments/scripts/control_ur.py index bffccfd..319b01e 100755 --- a/journal_experiments/scripts/control_ur.py +++ b/journal_experiments/scripts/control_ur.py @@ -142,8 +142,8 @@ def compute_path_speed_and_distance(self,ur_command): def get_mir_ur_transform(self): tf_listener = tf.TransformListener() # wait for transform - tf_listener.waitForTransform(self.tf_prefix + "base_link", self.tf_prefix + "UR10_r/base_link", rospy.Time(0), rospy.Duration(4.0)) - lin, ang = tf_listener.lookupTransform(self.tf_prefix + "base_link", self.tf_prefix + "UR10_r/base_link", rospy.Time(0)) + tf_listener.waitForTransform(self.tf_prefix + "base_link", self.tf_prefix + "UR10_l/base_link", rospy.Time(0), rospy.Duration(4.0)) + lin, ang = tf_listener.lookupTransform(self.tf_prefix + "base_link", self.tf_prefix + "UR10_l/base_link", rospy.Time(0)) self.mir_ur_transform.translation.x = lin[0] self.mir_ur_transform.translation.y = lin[1] From 1f11beb5b1f9fd9c32873d54638a7e756f7747e9 Mon Sep 17 00:00:00 2001 From: Tobias Recker Date: Tue, 15 Aug 2023 16:46:06 +0200 Subject: [PATCH 6/8] fixed false oreintation mir path --- .gitignore | 2 ++ journal_experiments/TODO.md | 3 +-- journal_experiments/scripts/create_path.py | 26 +++++++++++++--------- 3 files changed, 18 insertions(+), 13 deletions(-) diff --git a/.gitignore b/.gitignore index 9492000..a2f60b9 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,5 @@ +.vscode/ +*.pyc match_amc/scripts/path_files/__pycache__/circle_mirx.cpython-38.pyc match_amc/scripts/path_files/__pycache__/circle_miry.cpython-38.pyc match_amc/scripts/path_files/__pycache__/circle_urx.cpython-38.pyc diff --git a/journal_experiments/TODO.md b/journal_experiments/TODO.md index 2060ecb..7a5cdaa 100644 --- a/journal_experiments/TODO.md +++ b/journal_experiments/TODO.md @@ -14,11 +14,10 @@ roslaunch journal_experiments twist_sim.launch ``` ## Problems +- Twist Controller: orientation control correct? - No PointCloud! - lateral_nozzle_pose_override is set to 0 in control_ur -- ur_control is not working correctly while printing - - false transformations? ## Future - All in namespace like move_to_start_pose diff --git a/journal_experiments/scripts/create_path.py b/journal_experiments/scripts/create_path.py index ff3f009..109aa7a 100755 --- a/journal_experiments/scripts/create_path.py +++ b/journal_experiments/scripts/create_path.py @@ -52,16 +52,11 @@ def main(self): orientation = math.atan2(next_pose.position.y - pose.pose.position.y, next_pose.position.x - pose.pose.position.x) q = tf.transformations.quaternion_from_euler(0, 0, orientation) - # rotate around x so that the gripper is pointing down - q_rot = tf.transformations.quaternion_from_euler(np.pi, 0, 0) - q_ur=tf.transformations.quaternion_multiply(q_rot, q) - q_rot = tf.transformations.quaternion_from_euler(0, 0, np.pi/2) - q_ur=tf.transformations.quaternion_multiply(q_rot, q_ur) - - pose.pose.orientation.x = q_ur[0] - pose.pose.orientation.y = q_ur[1] - pose.pose.orientation.z = q_ur[2] - pose.pose.orientation.w = q_ur[3] + pose.pose.orientation.x = q[0] + pose.pose.orientation.y = q[1] + pose.pose.orientation.z = q[2] + pose.pose.orientation.w = q[3] + #pose.pose.position.x = self.start_pose.position.x + i/self.point_per_meter #pose.pose.position.y = pose.pose.position.x*0.5 + 0.5 * math.sin(pose.pose.position.x) path.poses.append(pose) @@ -75,7 +70,16 @@ def main(self): ur_pose.pose.position.y = self.r_ur*math.sin((i+self.shift_ur)/self.point_per_meter) + 0.05 * math.cos(6*i/self.point_per_meter) ur_pose.pose.position.z = 0.5 - ur_pose.pose.orientation = pose.pose.orientation + # rotate around x so that the gripper is pointing down + q_rot = tf.transformations.quaternion_from_euler(np.pi, 0, 0) + q_ur=tf.transformations.quaternion_multiply(q_rot, q) + q_rot = tf.transformations.quaternion_from_euler(0, 0, np.pi/2) + q_ur=tf.transformations.quaternion_multiply(q_rot, q_ur) + + ur_pose.pose.orientation.x = q_ur[0] + ur_pose.pose.orientation.y = q_ur[1] + ur_pose.pose.orientation.z = q_ur[2] + ur_pose.pose.orientation.w = q_ur[3] ur_path.poses.append(ur_pose) From c9f0a124b9fa7c0129564814a4409cb1d458555c Mon Sep 17 00:00:00 2001 From: --global <--global> Date: Tue, 22 Aug 2023 17:15:15 +0200 Subject: [PATCH 7/8] update --- journal_experiments/TODO.md | 3 +++ journal_experiments/launch/mur_sim.launch | 4 +++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/journal_experiments/TODO.md b/journal_experiments/TODO.md index 7a5cdaa..b4c7e16 100644 --- a/journal_experiments/TODO.md +++ b/journal_experiments/TODO.md @@ -21,3 +21,6 @@ roslaunch journal_experiments twist_sim.launch ## Future - All in namespace like move_to_start_pose +- Include speeds + - calc speed mir/ur in Parse Path + - change ur_control accordingly diff --git a/journal_experiments/launch/mur_sim.launch b/journal_experiments/launch/mur_sim.launch index 825c19c..1819eac 100644 --- a/journal_experiments/launch/mur_sim.launch +++ b/journal_experiments/launch/mur_sim.launch @@ -1,6 +1,8 @@ + + - + From 991eaa6ff410dd9225ea07ee5fae09dd4c07967b Mon Sep 17 00:00:00 2001 From: Hauke Heeren Date: Mon, 4 Sep 2023 12:10:27 +0200 Subject: [PATCH 8/8] path visualization doesnt crash rviz --- journal_experiments/rviz/mur620c_monitoring.rviz | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/journal_experiments/rviz/mur620c_monitoring.rviz b/journal_experiments/rviz/mur620c_monitoring.rviz index 3aef2a0..1fdd832 100644 --- a/journal_experiments/rviz/mur620c_monitoring.rviz +++ b/journal_experiments/rviz/mur620c_monitoring.rviz @@ -10,7 +10,7 @@ Panels: - /Pose1/Status1 - /Pose2 Splitter Ratio: 0.6205882430076599 - Tree Height: 627 + Tree Height: 589 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -619,7 +619,7 @@ Visualization Manager: Y: 0 Z: 0 Pose Color: 255; 85; 255 - Pose Style: Arrows + Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.00019999999494757503 @@ -741,7 +741,7 @@ Window Geometry: Height: 1016 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001560000033afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000002d60000006700000371000003d1fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000033a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f0000033afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000033a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000005efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004770000033a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000017600000324fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000002d60000006700000371000003d1fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004b000003240000010300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f00000324fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004b00000324000000d200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000005efc0100000002fb0000000800540069006d0065010000000000000738000004cb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004530000032400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -751,5 +751,5 @@ Window Geometry: Views: collapsed: false Width: 1848 - X: 798 + X: 712 Y: 27