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 new file mode 100644 index 0000000..b4c7e16 --- /dev/null +++ b/journal_experiments/TODO.md @@ -0,0 +1,26 @@ +# 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 +- Twist Controller: orientation control correct? + +- No PointCloud! + - lateral_nozzle_pose_override is set to 0 in control_ur + +## 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 new file mode 100644 index 0000000..1819eac --- /dev/null +++ b/journal_experiments/launch/mur_sim.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + \ No newline at end of file diff --git a/journal_experiments/launch/start_experiment.launch b/journal_experiments/launch/start_experiment.launch index 1d5091e..5f5be2e 100644 --- a/journal_experiments/launch/start_experiment.launch +++ b/journal_experiments/launch/start_experiment.launch @@ -1,5 +1,36 @@ + + + + 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" + + 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/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/rviz/mur620c_monitoring.rviz b/journal_experiments/rviz/mur620c_monitoring.rviz index ce39f87..1fdd832 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: 589 - 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: + mur620c/UR10_l/tool0: Value: false - mur620c/UR10_r/tool0: - Value: true - mur620c/UR10_r/upper_arm_link: + mur620c/UR10_l/upper_arm_link: Value: false - mur620c/UR10_r/wrist_1_link: + mur620c/UR10_l/wrist_1_link: Value: false - mur620c/UR10_r/wrist_2_link: + mur620c/UR10_l/wrist_2_link: Value: false - mur620c/UR10_r/wrist_3_link: + mur620c/UR10_l/wrist_3_link: Value: false - mur620c/mir/back_laser_link: - Value: false - mur620c/mir/base_footprint: - 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 @@ -615,8 +622,8 @@ Visualization Manager: Pose Style: None 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: 000000ff00000000fd00000004000000000000017600000324fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000002d60000006700000371000003d1fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004b000003240000010300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f00000324fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004b00000324000000d200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000005efc0100000002fb0000000800540069006d0065010000000000000738000004cb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004530000032400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -732,6 +750,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 3732 - X: 7788 - Y: 54 + Width: 1848 + X: 712 + Y: 27 diff --git a/journal_experiments/scripts/calibrated_pose_fake.py b/journal_experiments/scripts/calibrated_pose_fake.py new file mode 100755 index 0000000..e241326 --- /dev/null +++ b/journal_experiments/scripts/calibrated_pose_fake.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python3 + +import rospy +from geometry_msgs.msg import PoseStamped, Pose + +class RemapUrEeTopic: + def __init__(self) -> None: + ns = rospy.get_param("~namespace", "mur620c/UR10_r/") + self.pose = PoseStamped() + self.pose.header.frame_id = ns+"base_link" + + 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() + 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..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] + '/mir/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/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) @@ -76,7 +78,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..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 + "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_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] @@ -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..109aa7a 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(): @@ -12,6 +13,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,24 +38,25 @@ 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] 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) @@ -61,16 +66,32 @@ 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 + + # 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) 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..deebdd0 100755 --- a/journal_experiments/scripts/helper_nodes/control_ur_helper.py +++ b/journal_experiments/scripts/helper_nodes/control_ur_helper.py @@ -12,10 +12,11 @@ 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.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/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..22222cc 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/move_ur_to_start_pose.py b/journal_experiments/scripts/move_ur_to_start_pose.py index 2845939..0e2c4ab 100755 --- a/journal_experiments/scripts/move_ur_to_start_pose.py +++ b/journal_experiments/scripts/move_ur_to_start_pose.py @@ -2,11 +2,10 @@ 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 +import numpy as np class MoveURToStartPose(): @@ -19,7 +18,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 +50,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 +70,35 @@ 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 - - if e_phi > math.pi: - e_phi = e_phi - 2 * math.pi - elif e_phi < -math.pi: - e_phi = e_phi + 2 * math.pi + # 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 # 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 +107,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 +154,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 fc3e147..cf4b5b8 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,17 +104,32 @@ 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)) - - ur_start_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - ur_start_pose[0] = relative_positions_x_local + lin[0] - ur_start_pose[1] = relative_positions_y_local + lin[1] + 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[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] + + # 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)