diff --git a/.gitignore b/.gitignore index 8f66b2d..e07231a 100644 --- a/.gitignore +++ b/.gitignore @@ -61,3 +61,19 @@ qtcreator-* _build _autosummary +# reachability test result files +aurmr_tasks/scripts/successScores_970-970_360-410.txt +aurmr_tasks/scripts/successScores1_832-832_412-412_0.txt +aurmr_tasks/scripts/successScores1_832-832_412-412.txt +aurmr_tasks/scripts/successScores1_old_720-960-10_360-580-10.txt +aurmr_tasks/scripts/successScores2_832-832_412-412_0.txt +aurmr_tasks/scripts/successScores2_832-832_412-412.txt +aurmr_tasks/scripts/successScores24_720-960-10_360-580-10.txt +aurmr_tasks/scripts/successScores24_780-930-10_360-580-10.txt +# reachability test result plots +aurmr_tasks/scripts/pod1_reach_brg.png +aurmr_tasks/scripts/pod1_reach_rainbow.png +aurmr_tasks/scripts/pod2_reach_brg.png +aurmr_tasks/scripts/pod2_reach_rainbow.png +aurmr_tasks/scripts/1.py +universal_robot diff --git a/aurmr_perception/src/aurmr_perception/visualization.py b/aurmr_perception/src/aurmr_perception/visualization.py index aa69f05..b50cf54 100644 --- a/aurmr_perception/src/aurmr_perception/visualization.py +++ b/aurmr_perception/src/aurmr_perception/visualization.py @@ -14,9 +14,18 @@ def create_gripper_pose_markers(poses, color, ns="gripper_poses", tf_buffer=None # These are expensive. You really should pass one in. tf_buffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tf_buffer) - # We're drawing the bulky part of the gripper, which is notably different in orientation and offset + + # (Old comment for default gripper 2F85) We're drawing the bulky part of the gripper, which is notably different in orientation and offset # than say, arm_tool0. Figure out how to get from the given frame to the gripper_base_link frame - transform = tf_buffer.lookup_transform("gripper_base_link", "gripper_equilibrium_grasp", rospy.Time(0), + + # (New comment for gripper robotiq_epick) "gripper_equilibrium_grasp" is aligned with "arm_tool0" orientation and placed at the base of suction cup + # Notice that "epick_end_effector" and "gripper_equilibrium_grasp" are displaced from each other at distance of the suction cup height (10 mm), with the same orientations + # "gripper_equilibrium_grasp" is rotated by 90deg around z-axis of "gripper_base_link" + # Model in robotiq_epick_full.stl has origin reference frame the same as "gripper_base_link" + # As I (Sanjar Normuradov) understood, "tf_buffer.lookup_transform().transform" tries to transform from source ("gripper_base_link") representation to target ("epick_end_effector"), + # i.e. see in RViz how object pose(position, orientation) would look like when the frame "gripper_base_link" is aligned with a given frame + # and when the frame "epick_end_effector" is aligned the given frame + transform = tf_buffer.lookup_transform("epick_end_effector", "gripper_base_link", rospy.Time(0), rospy.Duration(1)).transform markers = [] @@ -35,19 +44,29 @@ def create_gripper_pose_markers(poses, color, ns="gripper_poses", tf_buffer=None marker.id = i marker.type = Marker.MESH_RESOURCE marker.action = Marker.ADD - marker.scale.x = 1 - marker.scale.y = 1 - marker.scale.z = 1 - marker.color.r = colors[i][0] - marker.color.g = colors[i][1] - marker.color.b = colors[i][2] - marker.color.a = colors[i][3] - marker.mesh_resource = "package://robotiq_2f_85_gripper_visualization/meshes/visual/full_opened.stl" + # marker.scale.x = 1 + # marker.scale.y = 1 + # marker.scale.z = 1 + marker.color.r = color[i][0] + marker.color.g = color[i][1] + marker.color.b = color[i][2] + marker.color.a = color[i][3] + # marker.mesh_resource = "package://robotiq_2f_85_gripper_visualization/meshes/visual/full_opened.stl" + + # NOTICE: robotiq_epick_full and robotiq_epick_tcp files has different origin reference frames!!! + # robotiq_epick_full has at its center of contact surface with "coupling" (Look at aurmr_tahoma/tahoma_description/urdf/tahoma.xacro) + # robotiq_epick_tcp has at its TCP (Tool Contact Point) or EOT (End of Tool), i.e. where suction cup + marker.mesh_resource = "package://robotiq_epick_visualization/meshes/visual/robotiq_epick_full.stl" + marker.scale.x = 0.001 + marker.scale.y = 0.001 + marker.scale.z = 0.001 + #"package://robotiq_epick_visualization/meshes/visual/robotiq_epick.dae" + #full_opened.stl" transformed_pose = deepcopy(pose.pose) rotated_quat = transformations.quaternion_multiply(quat_msg_to_vec(pose.pose.orientation), quat_msg_to_vec(transform.rotation)) # This grasp is the pose between the fingers. Push the marker back along the z axis to have the fingers over the grasp point - offset = qv_mult(quat_msg_to_vec(pose.pose.orientation), (-transform.translation.x, -transform.translation.y, -transform.translation.z)) + offset = qv_mult(quat_msg_to_vec(pose.pose.orientation), (transform.translation.x, transform.translation.y, transform.translation.z)) transformed_pose.position.x += offset[0] transformed_pose.position.y += offset[1] transformed_pose.position.z += offset[2] diff --git a/aurmr_tasks/config/analyze_workspace.rviz b/aurmr_tasks/config/analyze_workspace.rviz index 89ddf29..3751269 100644 --- a/aurmr_tasks/config/analyze_workspace.rviz +++ b/aurmr_tasks/config/analyze_workspace.rviz @@ -5,8 +5,8 @@ Panels: Property Tree Widget: Expanded: - /Tahoma1 - Splitter Ratio: 0.5 - Tree Height: 1168 + Splitter Ratio: 0.4369918704032898 + Tree Height: 467 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -53,7 +53,7 @@ Visualization Manager: Value: true - Alpha: 0.6000000238418579 Class: rviz/RobotModel - Collision Enabled: false + Collision Enabled: true Enabled: true Links: All Links Enabled: true @@ -131,196 +131,391 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - cantilever: + camera_lower_left_accel_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - control_box: + camera_lower_left_accel_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - control_box_mount: + camera_lower_left_back_to_bottom_shim: Alpha: 1 Show Axes: false Show Trail: false - Value: true - coupling: + camera_lower_left_bottom_screw_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - front_left_caster: + camera_lower_left_color_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - front_right_caster: + camera_lower_left_color_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - gripper_base_link: + camera_lower_left_depth_frame: Alpha: 1 Show Axes: false Show Trail: false - gripper_left_inner_finger: + camera_lower_left_depth_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - gripper_left_inner_finger_pad: + camera_lower_left_gyro_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_gyro_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_infra_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_infra_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - gripper_left_inner_knuckle: + camera_lower_left_lower_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_mount: Alpha: 1 Show Axes: false Show Trail: false Value: true - gripper_left_outer_finger: + camera_lower_left_mount_attach_point: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_upper_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_accel_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_accel_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_back_to_bottom_shim: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_gyro_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_gyro_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_infra_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_infra_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - gripper_left_outer_knuckle: + camera_lower_right_lower_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_mount: Alpha: 1 Show Axes: false Show Trail: false Value: true - gripper_right_inner_finger: + camera_lower_right_mount_attach_point: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_upper_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_accel_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_accel_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_back_to_bottom_shim: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_gyro_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_gyro_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_infra_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_infra_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - gripper_right_inner_finger_pad: + camera_upper_lower_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_upper_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_back_to_bottom_shim: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_infra1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_infra1_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_infra2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_infra2_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - gripper_right_inner_knuckle: + cantilever: Alpha: 1 Show Axes: false Show Trail: false Value: true - gripper_right_outer_finger: + control_box: Alpha: 1 Show Axes: false Show Trail: false Value: true - gripper_right_outer_knuckle: + control_box_mount: Alpha: 1 Show Axes: false Show Trail: false Value: true - gripper_robotiq_arg2f_base_link: + coupling: Alpha: 1 Show Axes: false Show Trail: false Value: true - left_light_panel: + front_left_caster: Alpha: 1 Show Axes: false Show Trail: false Value: true - left_light_panel_emitter: + front_right_caster: Alpha: 1 Show Axes: false Show Trail: false Value: true - mid_camera_mount: + gripper_base_link: Alpha: 1 Show Axes: false Show Trail: false - right_light_panel: + gripper_camera_mount: Alpha: 1 Show Axes: false Show Trail: false - Value: true - right_light_panel_emitter: + gripper_camera_mount_attach_point: Alpha: 1 Show Axes: false Show Trail: false - Value: true - robot_plate: + gripper_equilibrium_grasp: Alpha: 1 Show Axes: false Show Trail: false - Value: true - stand: + gripper_left_bar: Alpha: 1 Show Axes: false Show Trail: false Value: true - stand_brace: + gripper_left_distal_phalanx: Alpha: 1 Show Axes: false Show Trail: false Value: true - stand_camera_accel_frame: + gripper_left_outer_knuckle: Alpha: 1 Show Axes: false Show Trail: false - stand_camera_accel_optical_frame: + Value: true + gripper_left_proximal_phalanx: Alpha: 1 Show Axes: false Show Trail: false - stand_camera_bottom_screw_frame: + Value: true + gripper_right_bar: Alpha: 1 Show Axes: false Show Trail: false - stand_camera_color_frame: + Value: true + gripper_right_distal_phalanx: Alpha: 1 Show Axes: false Show Trail: false - stand_camera_color_optical_frame: + Value: true + gripper_right_outer_knuckle: Alpha: 1 Show Axes: false Show Trail: false - stand_camera_depth_frame: + Value: true + gripper_right_proximal_phalanx: Alpha: 1 Show Axes: false Show Trail: false - stand_camera_depth_optical_frame: + Value: true + gripper_robotiq_arg2f_base_link: Alpha: 1 Show Axes: false Show Trail: false - stand_camera_gyro_frame: + Value: true + left_light_panel: Alpha: 1 Show Axes: false Show Trail: false - stand_camera_gyro_optical_frame: + Value: true + left_light_panel_emitter: Alpha: 1 Show Axes: false Show Trail: false - stand_camera_infra_frame: + Value: true + right_light_panel: Alpha: 1 Show Axes: false Show Trail: false - stand_camera_infra_optical_frame: + Value: true + right_light_panel_emitter: Alpha: 1 Show Axes: false Show Trail: false - stand_camera_link: + Value: true + robot_plate: Alpha: 1 Show Axes: false Show Trail: false Value: true - stand_camera_lower_mount: + stand: Alpha: 1 Show Axes: false Show Trail: false - stand_camera_upper_mount: + Value: true + stand_brace: Alpha: 1 Show Axes: false Show Trail: false + Value: true stand_upright: Alpha: 1 Show Axes: false Show Trail: false Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false Name: Tahoma Robot Description: robot_description TF Prefix: "" @@ -329,7 +524,7 @@ Visualization Manager: Visual Enabled: true - Alpha: 1 Class: rviz/RobotModel - Collision Enabled: false + Collision Enabled: true Enabled: true Links: All Links Enabled: true @@ -2177,13 +2372,1005 @@ Visualization Manager: Value: true Visual Enabled: true - Class: rviz/MarkerArray - Enabled: true + Enabled: false Marker Topic: /reachability Name: Reachability Namespaces: - tahoma_gripper_viz: true + {} Queue Size: 100 + Value: false + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + arm_base: + Alpha: 1 + Show Axes: false + Show Trail: false + arm_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + arm_base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_flange: + Alpha: 1 + Show Axes: false + Show Trail: false + arm_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + arm_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + back_left_caster: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + back_right_caster: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_beam_lower: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_lower_left_accel_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_accel_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_back_to_bottom_shim: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_gyro_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_gyro_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_infra_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_infra_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_lower_left_lower_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_lower_left_mount_attach_point: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_upper_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_accel_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_accel_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_back_to_bottom_shim: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_gyro_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_gyro_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_infra_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_infra_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_lower_right_lower_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_lower_right_mount_attach_point: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_upper_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_accel_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_accel_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_back_to_bottom_shim: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_gyro_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_gyro_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_infra_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_infra_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_upper_lower_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_upper_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_back_to_bottom_shim: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_infra1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_infra1_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_infra2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_infra2_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + cantilever: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + control_box: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + control_box_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + coupling: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_caster: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_caster: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + gripper_camera_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + gripper_camera_mount_attach_point: + Alpha: 1 + Show Axes: false + Show Trail: false + gripper_equilibrium_grasp: + Alpha: 1 + Show Axes: false + Show Trail: false + gripper_left_bar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left_distal_phalanx: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left_outer_knuckle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left_proximal_phalanx: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right_bar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right_distal_phalanx: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right_outer_knuckle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right_proximal_phalanx: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_robotiq_arg2f_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_light_panel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_light_panel_emitter: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_light_panel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_light_panel_emitter: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_plate: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + stand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + stand_brace: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + stand_upright: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: true + Show Robot Visual: true + Show Trail: true + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: move_group/display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: manipulator + Query Goal State: true + Query Start State: true + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + arm_base: + Alpha: 1 + Show Axes: false + Show Trail: false + arm_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + arm_base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_flange: + Alpha: 1 + Show Axes: false + Show Trail: false + arm_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + arm_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + back_left_caster: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + back_right_caster: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_beam_lower: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_lower_left_accel_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_accel_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_back_to_bottom_shim: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_gyro_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_gyro_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_infra_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_infra_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_lower_left_lower_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_lower_left_mount_attach_point: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_left_upper_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_accel_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_accel_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_back_to_bottom_shim: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_gyro_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_gyro_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_infra_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_infra_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_lower_right_lower_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_lower_right_mount_attach_point: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_lower_right_upper_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_accel_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_accel_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_back_to_bottom_shim: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_gyro_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_gyro_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_infra_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_infra_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_upper_lower_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_upper_upper_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_back_to_bottom_shim: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_infra1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_infra1_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_infra2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_infra2_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_wrist_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + cantilever: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + control_box: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + control_box_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + coupling: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_caster: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_caster: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + gripper_camera_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + gripper_camera_mount_attach_point: + Alpha: 1 + Show Axes: false + Show Trail: false + gripper_equilibrium_grasp: + Alpha: 1 + Show Axes: false + Show Trail: false + gripper_left_bar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left_distal_phalanx: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left_outer_knuckle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left_proximal_phalanx: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right_bar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right_distal_phalanx: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right_outer_knuckle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right_proximal_phalanx: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_robotiq_arg2f_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_light_panel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_light_panel_emitter: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_light_panel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_light_panel_emitter: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_plate: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + stand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + stand_brace: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + stand_upright: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: true + Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 0.1 Enabled: true Global Options: Background Color: 48; 48; 48 @@ -2212,7 +3399,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 3.7608654499053955 + Distance: 2.346240282058716 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -2228,9 +3415,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5149996876716614 + Pitch: 0.1749996542930603 Target Frame: pod_base_link - Yaw: 2.1807963848114014 + Yaw: 4.582606792449951 Saved: - Class: rviz/FrameAligned Enable Stereo Rendering: @@ -2274,10 +3461,14 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 1319 + Height: 1016 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001ee000004cdfc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000026100000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004cd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002f2000000c90000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000218000001a10000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f000004cdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004cd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006330000003efc0100000002fb0000000800540069006d0065000000000000000633000004f300fffffffb0000000800540069006d006501000000000000045000000000000000000000032a000004cd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + MotionPlanning: + collapsed: false + MotionPlanning - Slider: + collapsed: false + QMainWindow State: 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 Selection: collapsed: false Time: @@ -2286,6 +3477,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1587 - X: 493 + Width: 1848 + X: 72 Y: 27 diff --git a/aurmr_tasks/scripts/analyze_workspace b/aurmr_tasks/scripts/analyze_workspace index adb2a03..337d8b3 100755 --- a/aurmr_tasks/scripts/analyze_workspace +++ b/aurmr_tasks/scripts/analyze_workspace @@ -1,59 +1,508 @@ #!/usr/bin/env python + +import os +import sys +import rospkg + +# print("\n-------------------") +# print(F"[ANALYZE_WORKSPACE] {sys.path}") +# print("-------------------\n") + +import math import random import std_msgs -import visualization_msgs.msg - import rospy +import copy +import numpy as np + from smach import State +from visualization_msgs.msg import Marker, MarkerArray +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster +from gazebo_msgs.srv import SpawnModel, DeleteModel +from gazebo_msgs.msg import LinkStates +from tf_conversions import transformations +from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion, TransformStamped +from tf2_msgs.msg import TFMessage from aurmr_tasks.common.tahoma import Tahoma - -import numpy as np -from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion -from tf_conversions import transformations +# from aurmr_tasks.common.tahoma import Tahoma from aurmr_perception.visualization import create_gripper_pose_markers +# from aurmr_perception.visualization import create_gripper_pose_markers -from aurmr_tasks.pod import BIN_IDS +from aurmr_tasks.pod import BIN_IDS_1, BIN_IDS_2 +# from aurmr_tasks.pod import BIN_IDS_1, BIN_IDS_2 -def main(): - rospy.loginfo('Getting robot resources') - rospy.init_node("analyze_workspace") - simulation = rospy.get_param("/use_sim_time", False) - State.simulation = simulation - robot = Tahoma(simulation) +def main(TEST_MODE:int, + pod_id:str, + BIN_POSE_RAND_N:int, + dRoll:float, dPitch:float, dYaw:float, + SSL:int, vis_path:bool, vis_pose:bool, + planning_attempts:int, + planning_time:float, + replan_attempts:int, + joint_tolerance:float, + position_tolerance:float, + orientation_tolerance:float, + goal_type:str): + + # print('\n--------') + # print(robot.scene.get_known_object_names()) + # print(robot.scene.get_object_poses(['back_frame'])) + # print('---------\n') roslaunch fc.launch x_start:=970 x_stop:=971 x_step:=1 y_start:=360 y_stop:=411 y_step:=10 bin_randN:=1 pod_id:=1 + + # for creating histogram to manually set manipulability threshold + # file = open("manipData.txt", "w") + + dYaw *= np.pi / 180 * 1/2 + dRoll *= np.pi / 180 * 1/2 + dPitch *= np.pi / 180 * 1/2 + # # Redefine dRoll to use sampling weight function (sigmoids) + # dRoll = dRoll/2 if dRoll != 0 else 0.001 + # # Redefine dRoll to use sampling weight function (sigmoids) + # dPitch = dPitch/2 if dPitch != 0 else 0.001 + pod_id = int(pod_id[0]) + POD_BIN_HEIGHTS = robot.pod_sizes[f"pod{pod_id}_bin_heights"] + POD_BIN_FLAP_HEIGHT = robot.pod_sizes[f"pod{pod_id}_bin_flap_height"] + POD_BIN_WIDTH = robot.pod_sizes[f"pod{pod_id}_bin_width"] + POD_BIN_WALL_THICKNESS = robot.pod_sizes[f"pod{pod_id}_bin_wall_thickness"] + POD_BIN_BOTTOM_THICKNESS = robot.pod_sizes[f"pod{pod_id}_bin_bottom_thickness"] + POD_BIN_LENGTH = robot.pod_sizes[f"pod{pod_id}_bin_depth"] + POD_BRACE_FRAME_WIDTH = robot.pod_sizes[f"pod{pod_id}_brace_frame_width"] + NUM_COLUMNS = math.ceil(robot.pod_sizes[f"pod{pod_id}_brace_width"] / POD_BIN_WIDTH) + LOW_ROW = 4 if pod_id == 1 else 3 + UP_ROW = 8 if pod_id == 1 else 6 + BIN_IDS = BIN_IDS_1 if pod_id == 1 else BIN_IDS_2 + pod_bin_row_id = 0 + + EEF_RADIUS = robot.end_effector["epick_end_effect_R"] + + ''' + x could be x,z pre-grasp position Cartesian coordinates relative to pod_bin reference frames + -POD_BIN_WIDTH/2 < x < POD_BIN_WIDTH/2 | -POD_BIN_HEIGHT/2 < z < POD_BIN_HEIGHT/2 + angle: dRoll (defines change in z), dPitch (defines change in x) + ''' + # # Posititon Weight Sampling Function + # # Both functions are symetrical to each other, so that's why we use math.abs(x) + # # But, in general functions look like: + # # If sgn(x) = (+) -> (1 + math.exp(-1)) / (1 + math.exp(+angle * (x - 1/angle))) + # # If sgn(x) = (-) -> (1 + math.exp(-1)) / (1 + math.exp(-angle * (x + 1/angle))) + # POSITION_WEIGHT_SAMPLING_FUNC = np.vecto=position_tolerancerize(lambda x, angle: (1 + math.exp(-1)) / (1 + math.exp(angle * (abs(x) - 1/angle)))) + + # # Orientation Weight Sampling Function + # # Both functions are the same: + # # But, in general functions look like: + # # If sgn(x) = (+) -> (1 - math.atan2(2 * (+angle) * math.exp(-angle * x), (1 + math.exp(-angle * x)**2)) / math.atan2(+angle, 2)) + # # If sgn(x) = (-) -> (1 - math.atan2(2 * (-angle) * math.exp(+angle * x), (1 + math.exp(+angle * x)**2)) / math.atan2(-angle, 2)) + # ORIENTATION_SAMPLING_FUNC = np.vectorize(lambda x, angle: sgn(-x) * (1 - math.atan2(2 * (-angle) * math.exp(angle * x), (1 + math.exp(angle * x)**2)) / math.atan2(-angle, 2))) + + # # UR16e pose repeatability 0.05mm, from official technical specification + # UR16e_POSE_REPEATABILITY = 0.0005 + pod_side_bin_half_width = (POD_BIN_WIDTH - POD_BRACE_FRAME_WIDTH - 2 * EEF_RADIUS - POD_BIN_WALL_THICKNESS)/2 + pod_midl_bin_half_width = (POD_BIN_WIDTH - 2 * EEF_RADIUS - POD_BIN_WALL_THICKNESS)/2 + # # POSITIONS_X_SIDE_BIN and POSITIONS_X_MIDL_BIN has the same size, although not the same step + # positions_x_side_bin = np.linspace(start=-pod_side_bin_half_width, stop=pod_side_bin_half_width, num=int(pod_side_bin_half_width/UR16e_POSE_REPEATABILITY)) / pod_side_bin_half_width + # positions_x_midl_bin = np.linspace(start=-pod_midl_bin_half_width, stop=pod_midl_bin_half_width, num=int(pod_side_bin_half_width/UR16e_POSE_REPEATABILITY)) / pod_midl_bin_half_width - align_to_bin_orientation = transformations.quaternion_from_euler(-1.57, 0, 0) + # # np.array (2, POSITIONS_X_NUM) of positions x for side (1st row) and middle (2nd row) bins + # position_sampling_weights_x = np.vstack((POSITION_WEIGHT_SAMPLING_FUNC(positions_x_side_bin, dPitch), + # POSITION_WEIGHT_SAMPLING_FUNC(positions_x_midl_bin, dPitch))) + # # Normalize weights for each row + # position_sampling_weights_x /= np.sum(position_sampling_weights_x, axis=1).reshape(-1, 1) + + MAX_ANGLE_POS = np.vectorize( + lambda x, max_angle, half_width_or_height: + (1 - x) * max_angle + SSL * int((math.atan((1 - x) * half_width_or_height/POD_BIN_LENGTH) - (1 - x) * max_angle) < 0) * (math.atan((1 - x) * half_width_or_height/POD_BIN_LENGTH) - (1 - x) * max_angle) + ) + MAX_ANGLE_NEG = np.vectorize( + lambda x, max_angle, half_width_or_height: + (x + 1) * max_angle + SSL * int((math.atan((x + 1) * half_width_or_height/POD_BIN_LENGTH) - (x + 1) * max_angle) < 0) * (math.atan((x + 1) * half_width_or_height/POD_BIN_LENGTH) - (x + 1) * max_angle) + ) + + align_to_bin_orientation = transformations.quaternion_from_euler(-np.pi/2, 0, 0, axes='rxyz') + # align_to_bin_orientation = transformations.quaternion_from_euler(0, 0, 0) align_to_bin_quat = Quaternion(x=align_to_bin_orientation[0], y=align_to_bin_orientation[1], z=align_to_bin_orientation[2], w=align_to_bin_orientation[3]) + BIN_APPROACH_POSES = [] BIN_INNER_POSES = [] - for bin_id in BIN_IDS: + for column_id, bin_id in enumerate(BIN_IDS, start=1): frame_name = f"pod_bin_{bin_id}" - BIN_APPROACH_POSES.append(PoseStamped(header=std_msgs.msg.Header(frame_id=frame_name), pose=Pose(position=Point(x=.125, y=-.12,z=.07), orientation=align_to_bin_quat))) - for j in range(10): - # Get some randomly offset poses in the bin to see if we'll be able to reach the inside - perturbed_quat = [align_to_bin_quat.x, align_to_bin_quat.y, align_to_bin_quat.z, align_to_bin_quat.w] + np.random.uniform(-.2, .2, 4) - perturbed_quat /= np.linalg.norm(perturbed_quat) + + # Use different heights for bins in different rows + pod_bin_height = POD_BIN_HEIGHTS[pod_bin_row_id] + pod_bin_row_id += 1 if column_id % NUM_COLUMNS == 0 else 0 + + # Set initial pose at the center* (pod height - pod flap height) of each (x,z) bin with perpendicular orientation (quat) + # Take into account the brace frame width for side bins + X_OFFSET = (POD_BIN_WIDTH - POD_BRACE_FRAME_WIDTH * int((column_id % NUM_COLUMNS == 0) or (column_id % NUM_COLUMNS == 1))) / 2 + POD_BRACE_FRAME_WIDTH * int(column_id % NUM_COLUMNS == 1) + # Adjust y-offset if necessary + Y_OFFSET = -0.02 + Z_OFFSET = (pod_bin_height - POD_BIN_FLAP_HEIGHT) / 2 + POD_BIN_FLAP_HEIGHT + + BIN_APPROACH_POSES.append(PoseStamped(header=std_msgs.msg.Header(frame_id=frame_name), pose=Pose(position=Point(x=X_OFFSET, y=Y_OFFSET, z=Z_OFFSET), orientation=align_to_bin_quat))) + + if column_id % NUM_COLUMNS == 1: + # Set bin's height once for each row + pod_bin_half_height = (pod_bin_height - POD_BIN_BOTTOM_THICKNESS - 2 * EEF_RADIUS - POD_BIN_FLAP_HEIGHT)/2 + # positions_z = np.arange(start=-pod_bin_half_height, stop=pod_bin_half_height, step=2*UR16e_POSE_REPEATABILITY) / pod_bin_half_height + # position_sampling_weights_z = POSITION_WEIGHT_SAMPLING_FUNC(positions_z, dRoll) + # # Give some extra priority to positions close to the bottom of bins (negative z) + # position_sampling_weights_z[:int(pod_bin_half_height/(2*UR16e_POSE_REPEATABILITY))] += np.linspace(start=1.0, stop=0.0, num=int(pod_bin_half_height/(2*UR16e_POSE_REPEATABILITY))) + # # Normalize weights + # position_sampling_weights_z /= np.sum(position_sampling_weights_z) + + # # Generate random set of x with given weights position_sampling_weights_x over given range positions_x_side_bin/positions_x_midl_bin + # # Don't forget to multiply to pod_side_bin_half_width/pod_midl_bin_half_width as range positions_x_side_bin/positions_x_midl_bin lies within [-1, 1] + # if (column_id % NUM_COLUMNS == 0) or (column_id % NUM_COLUMNS == 1): + # rand_positions_x = np.random.choice(positions_x_side_bin, size=BIN_POSE_RAND_N, p=position_sampling_weights_x[0]) + # else: + # rand_positions_x = np.random.choice(positions_x_midl_bin, size=BIN_POSE_RAND_N, p=position_sampling_weights_x[1]) + # # Generate random set of z with given weights position_sampling_weights_z over given range positions_z + # # Don't forget to multiply to pod_bin_half_height as range positions_z lies within [-1, 1] + # rand_positions_z = np.random.choice(positions_z, size=BIN_POSE_RAND_N, p=position_sampling_weights_z) + + # # Compute rand orientation (roll, pitch) according to ORIENTATION_SAMPLING_FUNC + # # Don't forgert to scale rand orientations to given max angle (dRoll, dPitch) as ORIENTATION_SAMPLING_FUNC lies within [-1, 1] + # rand_orientation_pitch = ORIENTATION_SAMPLING_FUNC(rand_positions_x, dPitch) * dPitch * np.pi / 180 + # rand_orientation_roll = ORIENTATION_SAMPLING_FUNC(rand_positions_z, dRoll ) * dRoll * np.pi / 180 + # # Don't forget to scale rand positions to given max size (positions_x_side_bin/positions_x_midl_bin, pod_bin_half_height) as they lie within [-1, 1] + # rand_positions_x *= pod_side_bin_half_width if ((column_id % NUM_COLUMNS == 0) or (column_id % NUM_COLUMNS == 1)) else pod_midl_bin_half_width + # rand_positions_z *= pod_bin_half_height + + if (column_id % NUM_COLUMNS == 0) or (column_id % NUM_COLUMNS == 1): + rand_positions_x = np.random.uniform(low=-pod_side_bin_half_width, high=pod_side_bin_half_width, size=BIN_POSE_RAND_N) + max_pos_pitch = MAX_ANGLE_POS(rand_positions_x/pod_side_bin_half_width, dPitch, pod_side_bin_half_width) + max_neg_pitch = MAX_ANGLE_NEG(rand_positions_x/pod_side_bin_half_width, dPitch, pod_side_bin_half_width) + else: + rand_positions_x = np.random.uniform(low=-pod_midl_bin_half_width, high=pod_midl_bin_half_width, size=BIN_POSE_RAND_N) + max_pos_pitch = MAX_ANGLE_POS(rand_positions_x/pod_midl_bin_half_width, dPitch, pod_midl_bin_half_width) + max_neg_pitch = MAX_ANGLE_NEG(rand_positions_x/pod_midl_bin_half_width, dPitch, pod_midl_bin_half_width) + + rand_positions_z = np.random.uniform(low=-pod_bin_half_height, high=pod_bin_half_height, size=BIN_POSE_RAND_N) + max_pos_roll = MAX_ANGLE_POS(rand_positions_z/pod_bin_half_height, dRoll, pod_bin_half_height) + max_neg_roll = MAX_ANGLE_NEG(rand_positions_z/pod_bin_half_height, dRoll, pod_bin_half_height) + + rand_orientation_roll = np.random.uniform(low=-max_neg_roll, high=max_pos_roll, size=BIN_POSE_RAND_N) + rand_orientation_pitch = np.random.uniform(low=-max_neg_pitch, high=max_pos_pitch, size=BIN_POSE_RAND_N) + + for j in range(BIN_POSE_RAND_N): + perturbed_quat = transformations.quaternion_from_euler(-np.pi/2 - rand_orientation_roll[j], rand_orientation_pitch[j], np.random.uniform(-dYaw, dYaw), axes='rxyz') perturbed_quat_msg = Quaternion(x=perturbed_quat[0], y=perturbed_quat[1], z=perturbed_quat[2], w=perturbed_quat[3]) - BIN_INNER_POSES.append(PoseStamped(header=std_msgs.msg.Header(frame_id=frame_name), pose=Pose(position=Point(x=.125 + random.uniform(-.1, .1), y=random.uniform(-0,.1),z=.07 + random.uniform(0, .05)), orientation=perturbed_quat_msg))) - print("Determining approachable bins...") - reachability_pub = rospy.Publisher("~/reachability", visualization_msgs.msg.MarkerArray, queue_size=1, latch=True) - REACHABLE_BIN_APPROACHES = [] + BIN_INNER_POSES.append(PoseStamped(header=std_msgs.msg.Header(frame_id=frame_name), + pose=Pose(position=Point(x=X_OFFSET + rand_positions_x[j], y=Y_OFFSET, z=Z_OFFSET + rand_positions_z[j]), + orientation=perturbed_quat_msg))) + + + # storing info variables + # change flag to run manipulabilitytest + reachability = True if not TEST_MODE else False + reachability_pub = rospy.Publisher("~/reachability", MarkerArray, queue_size=1, latch=True) + REACHABLE_BIN_APPROACHES = [] # total poses = bin_random_pose_num * NUM_COLUMNS (bins in a row) * 13 (rows) [for pod_id=1, or 8 rows for pod_id=2] marker_colors = [] - all_poses = BIN_APPROACH_POSES + BIN_INNER_POSES - for i, pose in enumerate(all_poses): - solution = robot.compute_ik(pose, rospy.Duration(0)) - if solution: - REACHABLE_BIN_APPROACHES.append(pose) - marker_colors.append((0, 1, 0, 1)) + # Clear the scene from previous poses + marker = Marker() + marker.action = Marker.DELETEALL + reachability_pub.publish([marker, ]) + + #reachability test for bin inner poses + if (reachability): + print("\nDetermining reachable bins...\n") + input() + for i, pose in enumerate(BIN_INNER_POSES[LOW_ROW*NUM_COLUMNS*BIN_POSE_RAND_N : UP_ROW*NUM_COLUMNS*BIN_POSE_RAND_N]): + row_id = i // (NUM_COLUMNS * BIN_POSE_RAND_N) + 1 + LOW_ROW + column_id = (i // BIN_POSE_RAND_N) % NUM_COLUMNS + 1 + rand_id = i % BIN_POSE_RAND_N + 1 + # solution = robot.compute_ik(pose, rospy.Duration(0)) + solution = robot.compute_path(pose, allowed_planning_time=planning_time, + num_planning_attempts=planning_attempts, + replan_attempts=replan_attempts, + joint_tolerance=joint_tolerance, + position_tolerance=position_tolerance, + orientation_tolerance=orientation_tolerance, + startpoint='home', + goal_type=goal_type, + collision_free=False) + if solution is not None: + print(f"\n[row{row_id:02d}, column{column_id}] pose{rand_id:03d} has solution") + path, plan_time = solution + print(f"\t\t plan_time: {plan_time:2.4f}\n") + REACHABLE_BIN_APPROACHES.append(pose) + gripper_color = (0, 1, 0, 1) + if vis_path: + # while not robot.move_to_joint_angles(robot.home_state.joint_state, startpoint='current'): # robot.execute_path() has an option to exectue full-cycle (return to start pose) + # pass + reachability_pub.publish(create_gripper_pose_markers([pose,], [gripper_color,])) + robot.execute_path(path, pose, full_cycle=True) + else: + print(f"\n[row{row_id:02d}, column{column_id}] pose{rand_id:03d} not reachable") + gripper_color = (1, 0, 0, 1) + marker_colors.append(gripper_color) + + print(f"#Reachable: {len(REACHABLE_BIN_APPROACHES)} / {len(BIN_INNER_POSES[LOW_ROW*NUM_COLUMNS*BIN_POSE_RAND_N : UP_ROW*NUM_COLUMNS*BIN_POSE_RAND_N])}") + + ratio = len(REACHABLE_BIN_APPROACHES)/len(BIN_INNER_POSES[LOW_ROW*NUM_COLUMNS*BIN_POSE_RAND_N : UP_ROW*NUM_COLUMNS*BIN_POSE_RAND_N]) + print("Reachability Score: " + str(ratio * 100)) + + reachability_pub.publish(create_gripper_pose_markers(BIN_INNER_POSES[LOW_ROW*NUM_COLUMNS*BIN_POSE_RAND_N : UP_ROW*NUM_COLUMNS*BIN_POSE_RAND_N], marker_colors)) + if vis_pose: + rospy.spin() + return (ratio * 100) + + # manipulability test for bin approach poses + else : + print("Determining manipulable bins...") + MANIPULABLE_APPROACHES = [] + + # cartesian manipulability test for bin approach poses + counter = 0 + for approach in enumerate(BIN_APPROACH_POSES): + print('-------------------------------------------') + pose = copy.deepcopy(approach[1]) + manip = robot.compute_ik_cartesian(pose, pose.pose, rospy.Duration(0)) + + # not reachable or no viable path to bin pose + if (len(manip) == 0): + print(str(counter)) + marker_colors.append((1, 0, 0, 1)) #red + else: + print("cond num " + str(counter) + " has a solution.") + REACHABLE_BIN_APPROACHES.append(approach) + for i in manip: + file.write(str(i) + ", ") + + sorted = copy.deepcopy(manip) + sorted.sort() + + # how skewed is the data towards infinitiy (singularity)? how safely manipulatable is the object? + manipThreshold = 400 + + if (sorted[len(sorted)-1] >= manipThreshold): + print("solution but above manipulability threshold") + marker_colors.append((1, 1, 1, 1)) #white + else : + marker_colors.append((0, 1, 0, 1)) #green + MANIPULABLE_APPROACHES.append(approach) + + counter = counter + 1 + + file.close() + + if (len(REACHABLE_BIN_APPROACHES) == 0) : + print("no bins reachable") + else : + ratio = len(MANIPULABLE_APPROACHES)/len(REACHABLE_BIN_APPROACHES) + print("Manipulability Score : " + str(ratio * 100)) + + print("Manipulatable Points (Green) : " + str(len(MANIPULABLE_APPROACHES))) + print("Reachable but not Manipulatable Points (White) : " + str(len(REACHABLE_BIN_APPROACHES) - len(MANIPULABLE_APPROACHES))) + + reachability_pub.publish(create_gripper_pose_markers(BIN_APPROACH_POSES, marker_colors)) + + + # print("Published visualization data. Spinning to keep topic open.") + # rospy.spin() + +def sgn(x): + if x < 0: + return -1 + elif x > 0: + return 1 + else: + return 0 + +# def callback(data, arg): +# pose_pub = arg +# transforms = data.transforms +# if transforms[0].header.frame_id == "base_link": +# print(transforms[0].transform.translation.x) +# transforms[0].transform.translation.x = 0.4 +# pose_pub.publish(transforms) + +# def pose(): +# rospy.init_node("analyze_workspace") +# pose_pub = rospy.Publisher('/tf_static', TFMessage, queue_size=1) +# pose_sub = rospy.Subscriber("/tf_static", TFMessage, callback, (pose_pub)) +# rospy.spin() + + +def pod_pose_sub_callback(TFmessage): + for trnsfrm in TFmessage.transforms: + if trnsfrm.child_frame_id == "pod_base_link": + # tf_static rostopic message + print("\nPod's position relative to robot base") + print(f"x: {trnsfrm.transform.translation.x}, y: {trnsfrm.transform.translation.y}\n") + + +"""def transform_msg(x=0.832, y=0.4125): + transform = TransformStamped() + transform.header.stamp = rospy.Time.now() + transform.header.frame_id = 'base_link' + transform.child_frame_id = 'pod_base_link' + transform.transform.translation.x = x + transform.transform.translation.y = y + transform.transform.translation.z = 0.0 + transform.transform.rotation.x = 0.0 + transform.transform.rotation.y = 0.0 + transform.transform.rotation.z = -0.706825181105366 + transform.transform.rotation.w = 0.7073882691671998 + return transform""" + + +def spawn_model(model_name: str='pod', x: float=0.832, y: float=0.4125, podface_id: str='1A', include_flaps: str="true"): + import subprocess + # load xacro file and parse it to create urdf file + rospack = rospkg.RosPack() + xacro_path = os.path.join(rospack.get_path('aurmr_gazebo'), 'robots/pod.gazebo.xacro') + + xacro_args = f"id:={podface_id} include_flaps:={include_flaps}" + command = f"xacro {xacro_path} {xacro_args}" + process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True) + model_description, err = process.communicate() + model_description = model_description.decode('utf-8') + + try: + # spawn model in gazebo + rospy.wait_for_service('gazebo/spawn_urdf_model') + spawn_model_prox = rospy.ServiceProxy('gazebo/spawn_urdf_model', SpawnModel) + + pose = Pose() + pose.position.x = x + pose.position.y = y + pose.position.z = 0.0 + pose.orientation.x = 0.0 + pose.orientation.y = 0.0 + pose.orientation.z = -0.706825181105366 + pose.orientation.w = 0.7073882691671998 + + response = spawn_model_prox(model_name, model_description, 'aurmr_gazebo', pose, 'world') + if response.success: + print(f"[ANALYZE_WORKSPACE] Successfully spawned model {model_name}") + else: + print(f"[ANALYZE_WORKSPACE] Failed to spawn model {model_name}: {response.status_message}") + + # ServiceException (unavailable service, communication erros) + # ROSSerializationException (invalid model name) + # Gazebo errors (delete model that doesn't exist or has already been deleted) + except rospy.service.ServiceException as e: + print(f"Service call failed: {e}") + except Exception as e: # General exceptions, such as ROSInterruptException when the node is shut down while waiting for the service + print(f"An unexpected error occurred: {e}") + + # broadcast static tf + broadcaster = StaticTransformBroadcaster() + + transform = TransformStamped() + transform.header.stamp = rospy.Time.now() + transform.header.frame_id = 'base_link' + transform.child_frame_id = 'pod_base_link' + transform.transform.translation.x = x + transform.transform.translation.y = y + transform.transform.translation.z = 0.0 + transform.transform.rotation.x = 0.0 + transform.transform.rotation.y = 0.0 + transform.transform.rotation.z = -0.706825181105366 + transform.transform.rotation.w = 0.7073882691671998 + + broadcaster.sendTransform(transform) + + +def delete_model(model_name='pod'): + try: + rospy.wait_for_service('gazebo/delete_model') + delete_model_prox = rospy.ServiceProxy('gazebo/delete_model', DeleteModel) + response = delete_model_prox(model_name) + if response.success: + print(f"[ANALYZE_WORKSPACE] Successfully deleted model {model_name}") else: - marker_colors.append((1, 0, 0, 1)) + print(f"[ANALYZE_WORKSPACE] Failed to delete model {model_name}: {response.status_message}") - reachability_pub.publish(create_gripper_pose_markers(all_poses, marker_colors)) - print("Published visualization data. Spinning to keep topic open.") - rospy.spin() + # ServiceException (unavailable service, communication erros) + # ROSSerializationException (invalid model name) + # Gazebo errors (delete model that doesn't exist or has already been deleted) + except rospy.service.ServiceException as e: + print(f"[ANALYZE_WORKSPACE] Service call failed: {e}") + except Exception as e: # General exceptions, such as ROSInterruptException when the node is shut down while waiting for the service + print(f"[ANALYZE_WORKSPACE] An unexpected error occurred: {e}") +# robot position relative to bin gets hardcoded in fc.launch file if __name__ == '__main__': - main() \ No newline at end of file + rospy.init_node("analyze_workspace") + rospy.loginfo("[ANALYZE_WORKSPACE] Getting robot resources") + simulation = rospy.get_param("/use_sim_time", False) + State.simulation = simulation + robot = Tahoma(simulation) + pod_pose_sub = rospy.Subscriber('/tf_static', TFMessage, pod_pose_sub_callback) + + + print("[ANALYZE_WORKSPACE] Getting parameters(test_mode, random_pose_number, RPY, ssl)...") + test_mode = rospy.get_param("~test_mode", 0) # test_mode = 0: Reachibility | test_mode = 1: Manipulability + bin_pose_randN = rospy.get_param("~bin_pose_randN", 2) # Number of random pre-grasp poses for each bin + dRoll = rospy.get_param("~dRoll", 8) # Roll change (overall) in random pre-grasp pose generation (degrees) + dPitch = rospy.get_param("~dPitch", 8) # Pitch change (overall) in random pre-grasp pose generation (degrees) + dYaw = rospy.get_param("~dYaw", 720) # Yaw change (overall) in random pre-grasp pose generation (degrees) + # (Set Size Limits) + # SSL = 1: if dRoll(or dPitch) > atan2(pod_bin_height(or _width), pod_bin_length), then max_angle is set to atan2(pod_bin_height(or _width), pod_bin_length) + # SSL = 0: if dRoll(or dPitch) > atan2(pod_bin_height(or _width), pod_bin_length), then max_angle is set to dRoll(or dPitch) + # SSL = 0 or 1: if dRoll(or dPitch) < atan2(pod_bin_height(or _width), pod_bin_length), then max_angle is set to dRoll(or dPitch) + SSL = rospy.get_param("~ssl", False) + + planning_attempts = rospy.get_param("~planning_attempts", 10) # Number of planning attempts for each pre-graps pose joint configuration + planning_time = rospy.get_param("~planning_timfalsee", 10.0) # Allowed planning time, in seconds + replan_attempts = rospy.get_param("~replan_attempts", 5) # Number of attempts to compute joint configuration (IK) for each pre-graps pose + joint_tolerance = rospy.get_param("~joint_tolerance", 0.001) # Target pose joint values tolerance, in ? + position_tolerance = rospy.get_param("~position_tolerance", 0.001) # Target pose position values tolerance, in meters + orientation_tolerance = rospy.get_param("~orientation_tolerance", 0.001) # Target pose orientation values tolerance, in radians + + # goal_type: 'pose' - MoveIt would try to compute path for given pose (collision avoidance not guaranteed) + # 'joint' - MoveIt would try to compute path for given joint configuration (collision avoidance guaranteed) + goal_type = rospy.get_param("~goal_type", 'pose') + vis_path = rospy.get_param("~vis_path", False) # vis_path = True: Visualize path from 'home' pose to pre-grasp pose. 'home' - gripped object drop pose + vis_pose = rospy.get_param("~vis_pose", False) + + podface_id = rospy.get_param("~podface_id", '1A') + include_flaps = rospy.get_param("~include_flaps", '1A') + # Pod positon in centimeters + xStart = rospy.get_param("~x_start", 60) + xStop = rospy.get_param("~x_stop", 70) + xStep = rospy.get_param("~x_step", 10) + yStart = rospy.get_param("~y_start", 41) + yStop = rospy.get_param("~y_stop", 51) + yStep = rospy.get_param("~y_step", 10) + + rospack = rospkg.RosPack() + file_path = rospack.get_path('aurmr_tasks') + f'/scripts/successScores_{xStart}-{xStop-1}_{yStart}-{yStop-1}.txt' + try: + with open(file_path, 'w') as file: + for pod_x in range(xStart, xStop, xStep): + for pod_y in range(yStart, yStop, yStep): + print("[ANALYZE_WORKSPACE] Spawning model...") + spawn_model(model_name='pod', x=pod_x/1000, y=pod_y/1000, podface_id=podface_id, include_flaps=include_flaps) + rospy.sleep(1) + robot.add_pod_collision_geometry(podface_id, include_backwall=False, include_flaps=False) + input() + robot.move_to_joint_angles(joints=robot.home_state.joint_state, startpoint='current') + score = main(TEST_MODE=test_mode, + pod_id=podface_id, + BIN_POSE_RAND_N=bin_pose_randN, + dRoll=dRoll, dPitch=dPitch, dYaw=dYaw, + SSL=int(SSL), vis_path=vis_path, vis_pose=vis_pose, + planning_attempts=planning_attempts, + planning_time=planning_time, + replan_attempts=replan_attempts, + joint_tolerance=joint_tolerance, + position_tolerance=position_tolerance, + orientation_tolerance=orientation_tolerance, + goal_type=goal_type) + file.write(f"[{(pod_x / 1000):5.3f}, {(pod_y / 1000):5.3f}] : {score:7.4f}\n") + file.flush() # Ensure the data is written to the file + os.fsync(file.fileno()) # Ensure the data is flushed to disk + + print("[ANALYZE_WORKSPACE] Deleting model...") + delete_model(model_name='pod') + rospy.sleep(1) + except Exception as e: + print(f"[ANALYZE_WORKSPACE] Failed to write to file: {e}") + + rospy.signal_shutdown('Shut down all nodes to stop launch file execution') diff --git a/aurmr_tasks/scripts/plot_score.py b/aurmr_tasks/scripts/plot_score.py new file mode 100644 index 0000000..682479d --- /dev/null +++ b/aurmr_tasks/scripts/plot_score.py @@ -0,0 +1,74 @@ +import matplotlib.pyplot as plt +import numpy as np +from mpl_toolkits.mplot3d import Axes3D +import rospkg +from copy import copy +import os + + +def plot_score(x:np.array, y:np.array, z:np.array, x_step, y_step): + ''' Generate 3D barplot + args: + x, y - coordinates of the pod (in meters) + z - success score in range[0.0000, 1.0000] ''' + x, y, z = x.ravel(), y.ravel(), z.ravel() + fig = plt.figure(figsize=(6, 6), dpi=300) + ax = fig.add_subplot(111, projection='3d') + + # Normalize data for colormap + norm = plt.Normalize(min(z), max(z)) + colors = plt.cm.rainbow(norm(z)) # viridis, rainbow, brg + + bar = ax.bar3d(x, y, np.zeros_like(z), x_step/2, y_step/2, z, shade=True, color=colors) # Create the 3D bars + # bar.set(color=color[i], edgecolor='black', linewidth=0.5) + mappable = plt.cm.ScalarMappable(norm=norm, cmap=plt.cm.rainbow) + mappable.set_array(z) + cbar = plt.colorbar(mappable, ax=ax, location='left', aspect=50, format='%7.4f') + cbar.set_label(label='Success score', fontsize='xx-small') + cbar.set_ticks(np.linspace(min(z), max(z), 10, endpoint=True)) + cbar.ax.tick_params(labelsize=5) + + ax.view_init(elev=45, azim=135) # Set view point in degrees + + # Set labels for x, y, and z axes + ax.set_title(label="Reachability score as a function of pod's positions", fontsize='x-small') + ax.set_xlabel(xlabel="Pod's X-coordinate", fontsize='xx-small') + ax.set_ylabel(ylabel="Pod's Y-coordinate", fontsize='xx-small') + # ax.set_zlabel(zlabel="Success score", fontsize='xx-small') + + # Change tick properties (labelsize, interval) + ax.tick_params(axis='x', labelsize=3.5, rotation=45.0) + ax.tick_params(axis='y', labelsize=3.5, rotation=-45.0) + # ax.tick_params(axis='z', labelsize=0) + ax.set_xticks(ticks=np.arange(x.min(), x.max() + 0.001, x_step)) + ax.set_yticks(ticks=np.arange(y.min(), y.max() + 0.001, y_step)) + ax.set_zticks(ticks=[]) + + ax.text(x=0.52, y=0.2, z=2.2, s='Current [x, y, score]:\n[0.832, 0.423, 0.9844]', fontsize='xx-small', va='top', ha='right') + ax.text(x=0.515, y=0.195, z=2, s='Highest [x, y, score]:\n[0.750, 0.420, 0.9994]\n[0.950, 0.470, 0.9981]\n[0.750, 0.540, 0.9975]\n[0.840, 0.470, 0.9975]\n[0.920, 0.400, 0.9975]', + fontsize='xx-small', va='top', ha='right') + + # ax.text(x=0.52, y=0.2, z=2.2, s='Highest [x, y, score]:\n[0.850, 0.380, 0.9978]\n[0.910, 0.420, 0.9978]\n[0.920, 0.370, 0.9978]\n[0.970, 0.390, 0.9978]\n[0.910, 0.410, 0.9963]\n[0.930, 0.370, 0.9963]', + # fontsize='xx-small', va='top', ha='right') + + + plt.show() + + +if __name__ == '__main__': + x_step = 0.010 + y_step = 0.010 + x = [] + y = [] + scores = [] + rospack = rospkg.RosPack() + filename = './successScores1_720-960-10_360-580-10' # './successScores23_720-970-10_360-580-10' + with open(filename + '.txt', 'r') as file: + for line in file: + if len(line.rstrip()) != 0: # Skip '\n' lines + x.append(float(line[1 : line.find(',')])) + y.append(float(line[line.find(',') + 1 : line.find(']')])) + scores.append(float(line[line.find(':') + 1 : line.find('n')])/100) + + plot_score(x=np.array(x), y=np.array(y), z=np.array(scores), + x_step=x_step, y_step=y_step) diff --git a/aurmr_tasks/scripts/pod1_reach_viridis.png b/aurmr_tasks/scripts/pod1_reach_viridis.png new file mode 100644 index 0000000..b6ec54b Binary files /dev/null and b/aurmr_tasks/scripts/pod1_reach_viridis.png differ diff --git a/aurmr_tasks/scripts/pod2_reach_viridis.png b/aurmr_tasks/scripts/pod2_reach_viridis.png new file mode 100644 index 0000000..82c40e7 Binary files /dev/null and b/aurmr_tasks/scripts/pod2_reach_viridis.png differ diff --git a/aurmr_tasks/scripts/successScores1_720-960-10_360-580-10.txt b/aurmr_tasks/scripts/successScores1_720-960-10_360-580-10.txt new file mode 100644 index 0000000..a3223e5 --- /dev/null +++ b/aurmr_tasks/scripts/successScores1_720-960-10_360-580-10.txt @@ -0,0 +1,601 @@ +[0.720, 0.360] : 98.3750 +[0.720, 0.370] : 98.4375 +[0.720, 0.380] : 98.0000 +[0.720, 0.390] : 98.4375 +[0.720, 0.400] : 97.9375 +[0.720, 0.410] : 98.5000 +[0.720, 0.420] : 97.7500 +[0.720, 0.430] : 97.3125 +[0.720, 0.440] : 97.8750 +[0.720, 0.450] : 97.8125 +[0.720, 0.460] : 98.0625 +[0.720, 0.470] : 98.5000 +[0.720, 0.480] : 97.7500 +[0.720, 0.490] : 96.8125 +[0.720, 0.500] : 97.5000 +[0.720, 0.510] : 97.1875 +[0.720, 0.520] : 97.1875 +[0.720, 0.530] : 97.3125 +[0.720, 0.540] : 97.5625 +[0.720, 0.550] : 96.7500 +[0.720, 0.560] : 96.7500 +[0.720, 0.570] : 96.8750 +[0.720, 0.580] : 96.8125 + +[0.730, 0.360] : 98.7500 +[0.730, 0.370] : 98.2500 +[0.730, 0.380] : 98.1250 +[0.730, 0.390] : 98.5000 +[0.730, 0.400] : 97.6875 +[0.730, 0.410] : 99.4375 +[0.730, 0.420] : 98.8750 +[0.730, 0.430] : 97.8125 +[0.730, 0.440] : 97.5625 +[0.730, 0.450] : 97.6875 +[0.730, 0.460] : 98.3125 +[0.730, 0.470] : 98.6250 +[0.730, 0.480] : 98.0625 +[0.730, 0.490] : 97.8750 +[0.730, 0.500] : 97.2500 +[0.730, 0.510] : 97.2500 +[0.730, 0.520] : 96.6875 +[0.730, 0.530] : 97.1250 +[0.730, 0.540] : 99.2500 +[0.730, 0.550] : 97.1875 +[0.730, 0.560] : 97.1250 +[0.730, 0.570] : 97.1250 +[0.730, 0.580] : 97.6875 + +[0.740, 0.360] : 98.3750 +[0.740, 0.370] : 98.9375 +[0.740, 0.380] : 98.2500 +[0.740, 0.390] : 98.2500 +[0.740, 0.400] : 98.5625 +[0.740, 0.410] : 98.5625 +[0.740, 0.420] : 98.3750 +[0.740, 0.430] : 97.6875 +[0.740, 0.440] : 98.5000 +[0.740, 0.450] : 98.1875 +[0.740, 0.460] : 97.5625 +[0.740, 0.470] : 97.7500 +[0.740, 0.480] : 97.8125 +[0.740, 0.490] : 97.6875 +[0.740, 0.500] : 96.7500 +[0.740, 0.510] : 97.3125 +[0.740, 0.520] : 97.9375 +[0.740, 0.530] : 97.1250 +[0.740, 0.540] : 97.6875 +[0.740, 0.550] : 96.3750 +[0.740, 0.560] : 97.2500 +[0.740, 0.570] : 97.0625 +[0.740, 0.580] : 96.6250 + +[0.750, 0.360] : 98.2500 +[0.750, 0.370] : 98.1250 +[0.750, 0.380] : 98.5000 +[0.750, 0.390] : 98.0000 +[0.750, 0.400] : 98.4375 +[0.750, 0.410] : 98.8125 +[0.750, 0.420] : 99.9375 +[0.750, 0.430] : 97.3125 +[0.750, 0.440] : 97.8750 +[0.750, 0.450] : 98.3750 +[0.750, 0.460] : 98.3750 +[0.750, 0.470] : 98.5000 +[0.750, 0.480] : 97.5625 +[0.750, 0.490] : 97.0625 +[0.750, 0.500] : 97.5625 +[0.750, 0.510] : 97.1250 +[0.750, 0.520] : 97.9375 +[0.750, 0.530] : 97.5000 +[0.750, 0.540] : 99.7500 +[0.750, 0.550] : 97.0625 +[0.750, 0.560] : 97.0625 +[0.750, 0.570] : 96.3750 +[0.750, 0.580] : 97.8125 + +[0.760, 0.360] : 98.6875 +[0.760, 0.370] : 99.0625 +[0.760, 0.380] : 99.0000 +[0.760, 0.390] : 98.6875 +[0.760, 0.400] : 99.3125 +[0.760, 0.410] : 98.4375 +[0.760, 0.420] : 98.5625 +[0.760, 0.430] : 97.8125 +[0.760, 0.440] : 98.1875 +[0.760, 0.450] : 98.6875 +[0.760, 0.460] : 98.5000 +[0.760, 0.470] : 97.7500 +[0.760, 0.480] : 98.5000 +[0.760, 0.490] : 97.6250 +[0.760, 0.500] : 97.9375 +[0.760, 0.510] : 98.6875 +[0.760, 0.520] : 97.5625 +[0.760, 0.530] : 97.8125 +[0.760, 0.540] : 98.7500 +[0.760, 0.550] : 97.3750 +[0.760, 0.560] : 97.8750 +[0.760, 0.570] : 98.4375 +[0.760, 0.580] : 97.9375 + +[0.770, 0.360] : 98.8125 +[0.770, 0.370] : 99.0000 +[0.770, 0.380] : 98.6250 +[0.770, 0.390] : 98.8750 +[0.770, 0.400] : 98.5625 +[0.770, 0.410] : 98.3750 +[0.770, 0.420] : 98.6250 +[0.770, 0.430] : 98.5625 +[0.770, 0.440] : 98.5000 +[0.770, 0.450] : 98.2500 +[0.770, 0.460] : 98.3125 +[0.770, 0.470] : 99.6875 +[0.770, 0.480] : 98.3750 +[0.770, 0.490] : 98.7500 +[0.770, 0.500] : 97.4375 +[0.770, 0.510] : 98.1875 +[0.770, 0.520] : 97.5000 +[0.770, 0.530] : 97.8125 +[0.770, 0.540] : 97.5000 +[0.770, 0.550] : 98.3125 +[0.770, 0.560] : 97.0000 +[0.770, 0.570] : 97.5625 +[0.770, 0.580] : 97.8125 + +[0.780, 0.360] : 98.6875 +[0.780, 0.370] : 99.3125 +[0.780, 0.380] : 99.1250 +[0.780, 0.390] : 98.8125 +[0.780, 0.400] : 98.9375 +[0.780, 0.410] : 98.6875 +[0.780, 0.420] : 98.5625 +[0.780, 0.430] : 98.1875 +[0.780, 0.440] : 98.6250 +[0.780, 0.450] : 98.5625 +[0.780, 0.460] : 97.6250 +[0.780, 0.470] : 98.1875 +[0.780, 0.480] : 98.3125 +[0.780, 0.490] : 98.5625 +[0.780, 0.500] : 98.3125 +[0.780, 0.510] : 98.7500 +[0.780, 0.520] : 97.5000 +[0.780, 0.530] : 98.0625 +[0.780, 0.540] : 97.8750 +[0.780, 0.550] : 97.0000 +[0.780, 0.560] : 97.4375 +[0.780, 0.570] : 97.1875 +[0.780, 0.580] : 98.0625 + +[0.790, 0.360] : 98.8750 +[0.790, 0.370] : 99.1250 +[0.790, 0.380] : 99.1875 +[0.790, 0.390] : 99.5625 +[0.790, 0.400] : 98.8750 +[0.790, 0.410] : 98.7500 +[0.790, 0.420] : 98.6875 +[0.790, 0.430] : 98.3125 +[0.790, 0.440] : 98.6875 +[0.790, 0.450] : 98.3750 +[0.790, 0.460] : 98.4375 +[0.790, 0.470] : 98.1875 +[0.790, 0.480] : 98.2500 +[0.790, 0.490] : 98.3750 +[0.790, 0.500] : 98.5000 +[0.790, 0.510] : 98.0625 +[0.790, 0.520] : 98.3125 +[0.790, 0.530] : 98.5000 +[0.790, 0.540] : 98.0625 +[0.790, 0.550] : 97.5000 +[0.790, 0.560] : 97.7500 +[0.790, 0.570] : 97.7500 +[0.790, 0.580] : 97.3125 + +[0.800, 0.360] : 98.8125 +[0.800, 0.370] : 99.3750 +[0.800, 0.380] : 99.0625 +[0.800, 0.390] : 99.2500 +[0.800, 0.400] : 98.9375 +[0.800, 0.410] : 98.6250 +[0.800, 0.420] : 98.5625 +[0.800, 0.430] : 99.1250 +[0.800, 0.440] : 98.6250 +[0.800, 0.450] : 98.1875 +[0.800, 0.470] : 98.8125 +[0.800, 0.480] : 98.3750 +[0.800, 0.490] : 98.5000 +[0.800, 0.500] : 98.5625 +[0.800, 0.510] : 98.6250 +[0.800, 0.520] : 98.4375 +[0.800, 0.530] : 97.8750 +[0.800, 0.540] : 98.1875 +[0.800, 0.550] : 99.0625 +[0.800, 0.560] : 98.2500 +[0.800, 0.570] : 97.8750 +[0.800, 0.580] : 97.2500 + +[0.810, 0.360] : 98.6875 +[0.810, 0.370] : 99.2500 +[0.810, 0.380] : 99.0625 +[0.810, 0.390] : 99.1250 +[0.810, 0.400] : 99.1250 +[0.810, 0.410] : 99.1875 +[0.810, 0.420] : 99.2500 +[0.810, 0.430] : 98.1250 +[0.810, 0.440] : 98.5000 +[0.810, 0.450] : 98.5000 +[0.810, 0.460] : 98.3125 +[0.810, 0.470] : 98.3125 +[0.810, 0.480] : 98.1875 +[0.810, 0.490] : 98.6250 +[0.810, 0.500] : 99.2500 +[0.810, 0.510] : 98.1875 +[0.810, 0.520] : 98.4375 +[0.810, 0.530] : 98.2500 +[0.810, 0.540] : 97.8750 +[0.810, 0.550] : 98.2500 +[0.810, 0.560] : 97.7500 +[0.810, 0.570] : 97.8750 +[0.810, 0.580] : 97.6875 + +[0.820, 0.360] : 99.0625 +[0.820, 0.370] : 98.8125 +[0.820, 0.380] : 98.8750 +[0.820, 0.390] : 98.5625 +[0.820, 0.400] : 98.5000 +[0.820, 0.410] : 98.8750 +[0.820, 0.420] : 98.4375 +[0.820, 0.430] : 98.6875 +[0.820, 0.440] : 97.9375 +[0.820, 0.450] : 98.4375 +[0.820, 0.460] : 98.7500 +[0.820, 0.470] : 98.6250 +[0.820, 0.480] : 98.6250 +[0.820, 0.490] : 98.1875 +[0.820, 0.500] : 98.3125 +[0.820, 0.510] : 98.3125 +[0.820, 0.520] : 98.5000 +[0.820, 0.530] : 98.4375 +[0.820, 0.540] : 98.1875 +[0.820, 0.550] : 98.5000 +[0.820, 0.560] : 97.9375 +[0.820, 0.570] : 97.8750 +[0.820, 0.580] : 97.9375 + +[0.830, 0.360] : 99.0000 +[0.830, 0.370] : 99.6250 +[0.830, 0.380] : 99.4375 +[0.830, 0.390] : 99.6250 +[0.830, 0.400] : 98.9375 +[0.830, 0.410] : 98.9375 +[0.830, 0.420] : 99.5625 +[0.830, 0.430] : 98.5625 +[0.830, 0.440] : 98.5625 +[0.830, 0.450] : 98.9375 +[0.830, 0.460] : 98.7500 +[0.830, 0.470] : 98.8750 +[0.830, 0.480] : 98.3125 +[0.830, 0.490] : 99.3750 +[0.830, 0.500] : 98.0625 +[0.830, 0.510] : 98.5000 +[0.830, 0.520] : 98.1250 +[0.830, 0.530] : 98.0625 +[0.830, 0.540] : 99.2500 +[0.830, 0.550] : 97.5000 +[0.830, 0.560] : 97.5000 +[0.830, 0.570] : 96.8750 +[0.830, 0.580] : 97.9375 + +[0.840, 0.360] : 98.9375 +[0.840, 0.370] : 98.7500 +[0.840, 0.380] : 99.0625 +[0.840, 0.390] : 98.5625 +[0.840, 0.400] : 99.5000 +[0.840, 0.410] : 98.9375 +[0.840, 0.420] : 98.8125 +[0.840, 0.430] : 98.8750 +[0.840, 0.440] : 98.5625 +[0.840, 0.450] : 98.3125 +[0.840, 0.460] : 98.2500 +[0.840, 0.470] : 99.7500 +[0.840, 0.480] : 97.9375 +[0.840, 0.490] : 98.1875 +[0.840, 0.500] : 98.1875 +[0.840, 0.510] : 98.0000 +[0.840, 0.520] : 98.8750 +[0.840, 0.530] : 99.0000 +[0.840, 0.540] : 98.9375 +[0.840, 0.550] : 98.1875 +[0.840, 0.560] : 98.3125 +[0.840, 0.570] : 97.6875 +[0.840, 0.580] : 98.4375 + +[0.850, 0.360] : 99.0625 +[0.850, 0.370] : 98.6250 +[0.850, 0.380] : 98.8750 +[0.850, 0.390] : 99.3125 +[0.850, 0.400] : 98.8125 +[0.850, 0.410] : 98.6875 +[0.850, 0.420] : 98.6250 +[0.850, 0.430] : 98.3750 +[0.850, 0.440] : 98.3750 +[0.850, 0.450] : 99.1250 +[0.850, 0.460] : 98.7500 +[0.850, 0.470] : 99.1250 +[0.850, 0.480] : 98.8125 +[0.850, 0.490] : 98.3125 +[0.850, 0.500] : 98.5000 +[0.850, 0.510] : 99.0625 +[0.850, 0.520] : 98.1250 +[0.850, 0.530] : 98.3125 +[0.850, 0.540] : 98.0625 +[0.850, 0.550] : 97.5000 +[0.850, 0.560] : 98.8750 +[0.850, 0.570] : 98.3750 +[0.850, 0.580] : 97.7500 + +[0.860, 0.360] : 98.8125 +[0.860, 0.370] : 99.0000 +[0.860, 0.380] : 98.8125 +[0.860, 0.390] : 99.1250 +[0.860, 0.400] : 98.9375 +[0.860, 0.410] : 98.6875 +[0.860, 0.420] : 98.5625 +[0.860, 0.430] : 98.5625 +[0.860, 0.440] : 98.8125 +[0.860, 0.450] : 98.0000 +[0.860, 0.460] : 98.6875 +[0.860, 0.470] : 99.0625 +[0.860, 0.480] : 98.6250 +[0.860, 0.490] : 98.0000 +[0.860, 0.500] : 97.8125 +[0.860, 0.510] : 98.3125 +[0.860, 0.520] : 98.0625 +[0.860, 0.530] : 98.0625 +[0.860, 0.530] : 98.0625 +[0.860, 0.540] : 97.9375 +[0.860, 0.550] : 97.6875 +[0.860, 0.560] : 97.9375 +[0.860, 0.570] : 98.2500 +[0.860, 0.580] : 96.5000 + +[0.870, 0.360] : 99.1250 +[0.870, 0.370] : 99.0625 +[0.870, 0.380] : 99.0000 +[0.870, 0.390] : 98.9375 +[0.870, 0.400] : 98.3125 +[0.870, 0.410] : 97.6250 +[0.870, 0.420] : 98.5000 +[0.870, 0.430] : 98.8750 +[0.870, 0.440] : 98.5000 +[0.870, 0.450] : 98.6875 +[0.870, 0.460] : 98.8125 +[0.870, 0.470] : 98.6250 +[0.870, 0.480] : 98.8125 +[0.870, 0.490] : 98.3750 +[0.870, 0.500] : 97.8125 +[0.870, 0.510] : 97.9375 +[0.870, 0.520] : 97.7500 +[0.870, 0.530] : 98.7500 +[0.870, 0.540] : 97.6875 +[0.870, 0.550] : 97.5000 +[0.870, 0.560] : 98.4375 +[0.870, 0.570] : 98.0625 +[0.870, 0.580] : 97.7500 + +[0.880, 0.360] : 98.5625 +[0.880, 0.370] : 98.8750 +[0.880, 0.380] : 99.0000 +[0.880, 0.390] : 98.2500 +[0.880, 0.400] : 99.2500 +[0.880, 0.410] : 98.8750 +[0.880, 0.420] : 98.8125 +[0.880, 0.430] : 99.0625 +[0.880, 0.440] : 98.1875 +[0.880, 0.450] : 98.9375 +[0.880, 0.460] : 97.9375 +[0.880, 0.470] : 99.1875 +[0.880, 0.480] : 98.2500 +[0.880, 0.490] : 98.3125 +[0.880, 0.500] : 98.0000 +[0.880, 0.510] : 98.1250 +[0.880, 0.520] : 99.1250 +[0.880, 0.530] : 98.8125 +[0.880, 0.540] : 97.6875 +[0.880, 0.550] : 97.9375 +[0.880, 0.560] : 98.0625 +[0.880, 0.570] : 98.0625 +[0.880, 0.580] : 97.8750 + +[0.890, 0.360] : 98.5625 +[0.890, 0.370] : 99.2500 +[0.890, 0.380] : 99.1250 +[0.890, 0.390] : 98.1875 +[0.890, 0.400] : 99.1875 +[0.890, 0.410] : 99.0625 +[0.890, 0.420] : 99.1250 +[0.890, 0.430] : 99.0000 +[0.890, 0.440] : 98.8750 +[0.890, 0.450] : 98.1875 +[0.890, 0.460] : 98.3750 +[0.890, 0.470] : 98.1875 +[0.890, 0.480] : 98.5000 +[0.890, 0.490] : 99.0000 +[0.890, 0.500] : 98.1875 +[0.890, 0.510] : 98.3125 +[0.890, 0.520] : 98.5000 +[0.890, 0.530] : 98.3125 +[0.890, 0.540] : 98.0000 +[0.890, 0.550] : 98.7500 +[0.890, 0.560] : 97.9375 +[0.890, 0.570] : 97.5625 +[0.890, 0.580] : 97.6250 + +[0.900, 0.360] : 99.0000 +[0.900, 0.370] : 99.0000 +[0.900, 0.380] : 98.8125 +[0.900, 0.390] : 99.4375 +[0.900, 0.400] : 99.1875 +[0.900, 0.410] : 98.1875 +[0.900, 0.420] : 99.4375 +[0.900, 0.430] : 99.1250 +[0.900, 0.440] : 99.4375 +[0.900, 0.450] : 98.6875 +[0.900, 0.460] : 97.8750 +[0.900, 0.470] : 98.7500 +[0.900, 0.480] : 98.5625 +[0.900, 0.490] : 98.1250 +[0.900, 0.500] : 98.3125 +[0.900, 0.510] : 98.3750 +[0.900, 0.520] : 98.1875 +[0.900, 0.530] : 97.9375 +[0.900, 0.540] : 98.8125 +[0.900, 0.550] : 98.5625 +[0.900, 0.560] : 99.3750 +[0.900, 0.570] : 98.3125 +[0.900, 0.580] : 98.0000 + +[0.910, 0.360] : 99.1875 +[0.910, 0.370] : 99.0625 +[0.910, 0.380] : 99.1875 +[0.910, 0.390] : 98.6250 +[0.910, 0.400] : 98.8125 +[0.910, 0.410] : 98.6250 +[0.910, 0.420] : 99.1250 +[0.910, 0.430] : 98.6875 +[0.910, 0.440] : 98.7500 +[0.910, 0.450] : 99.3125 +[0.910, 0.460] : 98.8125 +[0.910, 0.470] : 98.4375 +[0.910, 0.480] : 98.4375 +[0.910, 0.490] : 98.7500 +[0.910, 0.500] : 98.7500 +[0.910, 0.510] : 98.3750 +[0.910, 0.520] : 98.8750 +[0.910, 0.530] : 98.1250 +[0.910, 0.540] : 98.3750 +[0.910, 0.550] : 98.2500 +[0.910, 0.560] : 98.3125 +[0.910, 0.570] : 97.5625 +[0.910, 0.580] : 97.4375 + +[0.920, 0.360] : 98.8750 +[0.920, 0.370] : 98.7500 +[0.920, 0.380] : 99.4375 +[0.920, 0.390] : 99.2500 +[0.920, 0.400] : 99.7500 +[0.920, 0.410] : 99.1250 +[0.920, 0.420] : 99.1875 +[0.920, 0.430] : 99.3125 +[0.920, 0.440] : 98.8125 +[0.920, 0.450] : 98.6250 +[0.920, 0.460] : 98.8750 +[0.920, 0.470] : 99.0000 +[0.920, 0.480] : 98.8750 +[0.920, 0.490] : 98.5625 +[0.920, 0.500] : 99.2500 +[0.920, 0.510] : 98.6250 +[0.920, 0.520] : 98.8125 +[0.920, 0.530] : 99.3750 +[0.920, 0.540] : 98.8750 +[0.920, 0.550] : 97.7500 +[0.920, 0.560] : 97.3750 +[0.920, 0.570] : 98.3125 +[0.920, 0.580] : 97.8125 + +[0.930, 0.360] : 98.2500 +[0.930, 0.370] : 98.9375 +[0.930, 0.380] : 99.1875 +[0.930, 0.390] : 98.8125 +[0.930, 0.400] : 98.7500 +[0.930, 0.410] : 99.5000 +[0.930, 0.420] : 98.7500 +[0.930, 0.430] : 98.6875 +[0.930, 0.440] : 98.7500 +[0.930, 0.450] : 98.5625 +[0.930, 0.460] : 99.3125 +[0.930, 0.470] : 98.7500 +[0.930, 0.480] : 98.4375 +[0.930, 0.490] : 98.2500 +[0.930, 0.500] : 98.3125 +[0.930, 0.510] : 98.1250 +[0.930, 0.520] : 98.1875 +[0.930, 0.530] : 98.1250 +[0.930, 0.540] : 98.0000 +[0.930, 0.550] : 98.3750 +[0.930, 0.560] : 97.9375 +[0.930, 0.570] : 97.5000 +[0.930, 0.580] : 97.4375 + +[0.940, 0.360] : 98.5625 +[0.940, 0.370] : 99.0000 +[0.940, 0.380] : 99.6875 +[0.940, 0.390] : 99.5625 +[0.940, 0.400] : 99.3125 +[0.940, 0.410] : 99.5000 +[0.940, 0.420] : 99.5000 +[0.940, 0.430] : 99.1875 +[0.940, 0.440] : 98.6250 +[0.940, 0.450] : 99.1250 +[0.940, 0.460] : 99.0625 +[0.940, 0.470] : 98.7500 +[0.940, 0.480] : 98.5000 +[0.940, 0.490] : 98.6875 +[0.940, 0.500] : 98.3125 +[0.940, 0.510] : 98.8750 +[0.940, 0.520] : 99.0625 +[0.940, 0.530] : 98.5000 +[0.940, 0.540] : 98.5000 +[0.940, 0.550] : 98.0000 +[0.940, 0.560] : 97.8125 +[0.940, 0.570] : 97.8125 +[0.940, 0.580] : 97.7500 + +[0.950, 0.360] : 98.6875 +[0.950, 0.370] : 98.7500 +[0.950, 0.380] : 98.9375 +[0.950, 0.390] : 98.7500 +[0.950, 0.400] : 99.3750 +[0.950, 0.410] : 99.1250 +[0.950, 0.420] : 99.1250 +[0.950, 0.430] : 99.1875 +[0.950, 0.440] : 98.7500 +[0.950, 0.450] : 98.6250 +[0.950, 0.460] : 99.6250 +[0.950, 0.470] : 99.8125 +[0.950, 0.480] : 98.8125 +[0.950, 0.490] : 99.6875 +[0.950, 0.500] : 97.8750 +[0.950, 0.510] : 98.2500 +[0.950, 0.520] : 98.5000 +[0.950, 0.530] : 98.2500 +[0.950, 0.540] : 98.0000 +[0.950, 0.550] : 98.1875 +[0.950, 0.560] : 98.7500 +[0.950, 0.570] : 98.0000 +[0.950, 0.580] : 97.2500 + +[0.960, 0.360] : 98.4375 +[0.960, 0.370] : 98.8125 +[0.960, 0.380] : 99.1250 +[0.960, 0.390] : 99.0000 +[0.960, 0.400] : 98.7500 +[0.960, 0.410] : 98.6875 +[0.960, 0.420] : 98.9375 +[0.960, 0.430] : 98.7500 +[0.960, 0.440] : 99.1250 +[0.960, 0.450] : 98.5625 +[0.960, 0.460] : 98.4375 +[0.960, 0.470] : 98.5000 +[0.960, 0.480] : 98.5000 +[0.960, 0.490] : 98.4375 +[0.960, 0.500] : 98.4375 +[0.960, 0.510] : 98.5625 +[0.960, 0.520] : 98.3750 +[0.960, 0.530] : 98.0000 +[0.960, 0.540] : 98.6250 +[0.960, 0.550] : 98.3750 +[0.960, 0.560] : 97.8125 +[0.960, 0.570] : 97.3750 +[0.960, 0.580] : 97.7500 + +[0.832, 0.422] : 98.4375 \ No newline at end of file diff --git a/aurmr_tasks/scripts/successScores23_720-970-10_360-580-10.txt b/aurmr_tasks/scripts/successScores23_720-970-10_360-580-10.txt new file mode 100644 index 0000000..b751892 --- /dev/null +++ b/aurmr_tasks/scripts/successScores23_720-970-10_360-580-10.txt @@ -0,0 +1,622 @@ +[0.720, 0.360] : 89.6296 +[0.720, 0.370] : 90.6667 +[0.720, 0.380] : 90.2222 +[0.720, 0.390] : 90.4444 +[0.720, 0.400] : 90.0000 +[0.720, 0.410] : 88.7407 +[0.720, 0.420] : 89.4074 +[0.720, 0.430] : 89.4815 +[0.720, 0.440] : 89.3333 +[0.720, 0.450] : 89.1111 +[0.720, 0.460] : 89.4815 +[0.720, 0.470] : 89.5556 +[0.720, 0.480] : 88.7407 +[0.720, 0.490] : 89.4074 +[0.720, 0.500] : 88.4444 +[0.720, 0.510] : 90.2963 +[0.720, 0.520] : 89.4815 +[0.720, 0.530] : 88.8148 +[0.720, 0.540] : 88.4444 +[0.720, 0.550] : 89.9259 +[0.720, 0.560] : 90.1481 +[0.720, 0.570] : 90.5926 +[0.720, 0.580] : 89.2593 + +[0.730, 0.360] : 90.6667 +[0.730, 0.370] : 89.8519 +[0.730, 0.380] : 89.9259 +[0.730, 0.390] : 88.9630 +[0.730, 0.400] : 89.8519 +[0.730, 0.410] : 90.6667 +[0.730, 0.420] : 90.6667 +[0.730, 0.430] : 90.2963 +[0.730, 0.440] : 89.4815 +[0.730, 0.450] : 90.6667 +[0.730, 0.460] : 91.0370 +[0.730, 0.470] : 90.1481 +[0.730, 0.480] : 89.7037 +[0.730, 0.490] : 90.7407 +[0.730, 0.500] : 89.7778 +[0.730, 0.510] : 90.3704 +[0.730, 0.520] : 89.0370 +[0.730, 0.530] : 89.6296 +[0.730, 0.540] : 89.2593 +[0.730, 0.550] : 90.5185 +[0.730, 0.560] : 91.7778 +[0.730, 0.570] : 91.0370 +[0.730, 0.580] : 89.7778 + +[0.740, 0.360] : 91.8519 +[0.740, 0.370] : 92.2222 +[0.740, 0.380] : 91.1111 +[0.740, 0.390] : 92.2963 +[0.740, 0.400] : 91.7037 +[0.740, 0.410] : 92.0741 +[0.740, 0.420] : 91.5556 +[0.740, 0.430] : 91.7778 +[0.740, 0.440] : 91.7778 +[0.740, 0.450] : 91.5556 +[0.740, 0.460] : 91.1852 +[0.740, 0.470] : 92.3704 +[0.740, 0.480] : 91.7778 +[0.740, 0.490] : 91.3333 +[0.740, 0.500] : 91.6296 +[0.740, 0.510] : 92.4444 +[0.740, 0.520] : 90.8148 +[0.740, 0.530] : 90.8148 +[0.740, 0.540] : 91.3333 +[0.740, 0.550] : 90.5926 +[0.740, 0.560] : 91.8519 +[0.740, 0.570] : 92.2963 +[0.740, 0.580] : 92.4444 + +[0.750, 0.360] : 93.9259 +[0.750, 0.370] : 92.9630 +[0.750, 0.380] : 93.4815 +[0.750, 0.390] : 93.1111 +[0.750, 0.400] : 93.7037 +[0.750, 0.410] : 93.1852 +[0.750, 0.420] : 93.4815 +[0.750, 0.430] : 93.5556 +[0.750, 0.440] : 93.7778 +[0.750, 0.450] : 92.2963 +[0.750, 0.460] : 93.7037 +[0.750, 0.470] : 93.5556 +[0.750, 0.480] : 93.1852 +[0.750, 0.490] : 94.0000 +[0.750, 0.500] : 92.8148 +[0.750, 0.510] : 92.6667 +[0.750, 0.520] : 93.3333 +[0.750, 0.530] : 92.2963 +[0.750, 0.540] : 92.3704 +[0.750, 0.550] : 92.2222 +[0.750, 0.560] : 92.7407 +[0.750, 0.570] : 93.7778 +[0.750, 0.580] : 93.4815 + +[0.760, 0.360] : 96.6667 +[0.760, 0.370] : 95.1852 +[0.760, 0.380] : 95.0370 +[0.760, 0.390] : 95.4074 +[0.760, 0.400] : 95.5556 +[0.760, 0.410] : 95.4074 +[0.760, 0.420] : 95.3333 +[0.760, 0.430] : 95.7037 +[0.760, 0.440] : 96.2222 +[0.760, 0.450] : 95.7778 +[0.760, 0.460] : 95.3333 +[0.760, 0.470] : 95.8519 +[0.760, 0.480] : 94.2222 +[0.760, 0.490] : 95.8519 +[0.760, 0.500] : 95.6296 +[0.760, 0.510] : 93.4815 +[0.760, 0.520] : 95.2593 +[0.760, 0.530] : 95.6296 +[0.760, 0.540] : 94.4444 +[0.760, 0.550] : 94.5185 +[0.760, 0.560] : 95.2593 +[0.760, 0.570] : 96.4444 +[0.760, 0.580] : 94.5926 + +[0.770, 0.360] : 97.1852 +[0.770, 0.370] : 98.1481 +[0.770, 0.380] : 98.0000 +[0.770, 0.390] : 97.4815 +[0.770, 0.400] : 97.7037 +[0.770, 0.410] : 96.7407 +[0.770, 0.420] : 96.9630 +[0.770, 0.430] : 96.2963 +[0.770, 0.440] : 97.0370 +[0.770, 0.450] : 96.9630 +[0.770, 0.460] : 95.9259 +[0.770, 0.470] : 96.7407 +[0.770, 0.480] : 96.2963 +[0.770, 0.490] : 96.9630 +[0.770, 0.500] : 96.4444 +[0.770, 0.510] : 96.4444 +[0.770, 0.520] : 95.6296 +[0.770, 0.530] : 96.9630 +[0.770, 0.540] : 96.2222 +[0.770, 0.550] : 96.4444 +[0.770, 0.560] : 95.7037 +[0.770, 0.570] : 94.8148 +[0.770, 0.580] : 96.0000 + +[0.780, 0.360] : 98.2222 +[0.780, 0.370] : 98.2222 +[0.780, 0.380] : 97.5556 +[0.780, 0.390] : 97.7037 +[0.780, 0.400] : 97.5556 +[0.780, 0.410] : 98.1481 +[0.780, 0.420] : 98.5926 +[0.780, 0.430] : 97.9259 +[0.780, 0.440] : 98.1481 +[0.780, 0.450] : 97.7778 +[0.780, 0.460] : 97.9259 +[0.780, 0.470] : 97.6296 +[0.780, 0.480] : 97.1852 +[0.780, 0.490] : 98.0000 +[0.780, 0.500] : 97.6296 +[0.780, 0.510] : 97.9259 +[0.780, 0.520] : 96.5185 +[0.780, 0.530] : 98.0000 +[0.780, 0.540] : 97.1852 +[0.780, 0.550] : 96.9630 +[0.780, 0.560] : 96.3704 +[0.780, 0.570] : 96.9630 +[0.780, 0.580] : 96.2222 + +[0.790, 0.360] : 98.5926 +[0.790, 0.370] : 99.1852 +[0.790, 0.380] : 98.8148 +[0.790, 0.390] : 98.8148 +[0.790, 0.400] : 98.8889 +[0.790, 0.410] : 98.7407 +[0.790, 0.420] : 98.7407 +[0.790, 0.430] : 98.3704 +[0.790, 0.440] : 98.8148 +[0.790, 0.450] : 98.0741 +[0.790, 0.460] : 98.6667 +[0.790, 0.470] : 98.2963 +[0.790, 0.480] : 97.9259 +[0.790, 0.490] : 98.7407 +[0.790, 0.500] : 97.3333 +[0.790, 0.510] : 98.1481 +[0.790, 0.520] : 97.7037 +[0.790, 0.530] : 97.9259 +[0.790, 0.540] : 97.7778 +[0.790, 0.550] : 97.6296 +[0.790, 0.560] : 97.7778 +[0.790, 0.570] : 97.7037 +[0.790, 0.580] : 97.2593 + +[0.800, 0.360] : 99.4815 +[0.800, 0.370] : 98.9630 +[0.800, 0.380] : 99.1111 +[0.800, 0.390] : 99.3333 +[0.800, 0.400] : 99.4815 +[0.800, 0.410] : 99.3333 +[0.800, 0.420] : 99.1852 +[0.800, 0.430] : 99.1111 +[0.800, 0.440] : 99.1852 +[0.800, 0.450] : 98.6667 +[0.800, 0.460] : 98.1481 +[0.800, 0.470] : 99.3333 +[0.800, 0.480] : 98.6667 +[0.800, 0.490] : 99.4074 +[0.800, 0.500] : 98.1481 +[0.800, 0.510] : 98.5926 +[0.800, 0.520] : 98.0741 +[0.800, 0.530] : 98.0741 +[0.800, 0.540] : 98.1481 +[0.800, 0.550] : 98.5926 +[0.800, 0.560] : 98.4444 +[0.800, 0.570] : 98.1481 +[0.800, 0.580] : 97.8519 + +[0.810, 0.360] : 99.0370 +[0.810, 0.370] : 98.7407 +[0.810, 0.380] : 98.9630 +[0.810, 0.390] : 99.0370 +[0.810, 0.400] : 99.1111 +[0.810, 0.410] : 99.5556 +[0.810, 0.420] : 98.7407 +[0.810, 0.430] : 99.1111 +[0.810, 0.440] : 99.3333 +[0.810, 0.450] : 99.1111 +[0.810, 0.460] : 98.7407 +[0.810, 0.470] : 99.2593 +[0.810, 0.480] : 98.5185 +[0.810, 0.490] : 98.8889 +[0.810, 0.500] : 98.2222 +[0.810, 0.510] : 98.2222 +[0.810, 0.520] : 98.8148 +[0.810, 0.530] : 98.5185 +[0.810, 0.540] : 97.7037 +[0.810, 0.550] : 98.2222 +[0.810, 0.560] : 97.4074 +[0.810, 0.570] : 97.1852 +[0.810, 0.580] : 97.9259 + +[0.820, 0.360] : 99.4074 +[0.820, 0.370] : 99.2593 +[0.820, 0.380] : 99.5556 +[0.820, 0.390] : 98.5185 +[0.820, 0.400] : 98.7407 +[0.820, 0.410] : 99.1111 +[0.820, 0.420] : 99.3333 +[0.820, 0.430] : 99.4074 +[0.820, 0.440] : 99.0370 +[0.820, 0.450] : 98.9630 +[0.820, 0.460] : 98.9630 +[0.820, 0.470] : 99.0370 +[0.820, 0.480] : 98.2222 +[0.820, 0.490] : 98.5185 +[0.820, 0.500] : 98.0000 +[0.820, 0.510] : 97.8519 +[0.820, 0.520] : 97.8519 +[0.820, 0.530] : 98.2963 +[0.820, 0.540] : 97.4815 +[0.820, 0.550] : 97.7778 +[0.820, 0.560] : 97.5556 +[0.820, 0.570] : 97.6296 +[0.820, 0.580] : 98.5185 + +[0.830, 0.360] : 99.0370 +[0.830, 0.370] : 98.9630 +[0.830, 0.380] : 99.4815 +[0.830, 0.390] : 99.1111 +[0.830, 0.400] : 99.4815 +[0.830, 0.410] : 99.3333 +[0.830, 0.420] : 99.2593 +[0.830, 0.430] : 98.5185 +[0.830, 0.440] : 98.1481 +[0.830, 0.450] : 98.8148 +[0.830, 0.460] : 98.8148 +[0.830, 0.470] : 98.0741 +[0.830, 0.480] : 98.7407 +[0.830, 0.490] : 98.1481 +[0.830, 0.500] : 97.7037 +[0.830, 0.510] : 98.4444 +[0.830, 0.520] : 98.0741 +[0.830, 0.530] : 97.9259 +[0.830, 0.540] : 98.8148 +[0.830, 0.550] : 97.3333 +[0.830, 0.560] : 97.9259 +[0.830, 0.570] : 97.7778 +[0.830, 0.580] : 98.6667 + +[0.840, 0.360] : 99.4815 +[0.840, 0.370] : 99.1852 +[0.840, 0.380] : 99.2593 +[0.840, 0.390] : 98.8889 +[0.840, 0.400] : 99.4815 +[0.840, 0.410] : 98.9630 +[0.840, 0.420] : 99.3333 +[0.840, 0.430] : 99.1111 +[0.840, 0.440] : 98.5926 +[0.840, 0.450] : 98.9630 +[0.840, 0.460] : 99.4815 +[0.840, 0.470] : 98.8148 +[0.840, 0.480] : 98.0000 +[0.840, 0.490] : 97.8519 +[0.840, 0.500] : 98.0000 +[0.840, 0.510] : 98.2963 +[0.840, 0.520] : 97.9259 +[0.840, 0.530] : 97.1111 +[0.840, 0.540] : 98.1481 +[0.840, 0.550] : 97.6296 +[0.840, 0.560] : 97.1852 +[0.840, 0.570] : 96.6667 +[0.840, 0.580] : 97.7037 + +[0.850, 0.360] : 99.5556 +[0.850, 0.370] : 99.2593 +[0.850, 0.380] : 99.7778 +[0.850, 0.390] : 98.8148 +[0.850, 0.400] : 98.2222 +[0.850, 0.410] : 97.9259 +[0.850, 0.420] : 97.9259 +[0.850, 0.430] : 98.7407 +[0.850, 0.440] : 98.2963 +[0.850, 0.450] : 98.2222 +[0.850, 0.460] : 99.2593 +[0.850, 0.470] : 98.0000 +[0.850, 0.480] : 99.1111 +[0.850, 0.490] : 97.5556 +[0.850, 0.500] : 98.3704 +[0.850, 0.510] : 98.2963 +[0.850, 0.520] : 99.0370 +[0.850, 0.530] : 96.5185 +[0.850, 0.540] : 96.5926 +[0.850, 0.550] : 96.3704 +[0.850, 0.560] : 97.3333 +[0.850, 0.570] : 98.1481 +[0.850, 0.580] : 96.1481 + +[0.860, 0.360] : 98.8148 +[0.860, 0.370] : 98.8148 +[0.860, 0.380] : 98.8889 +[0.860, 0.390] : 98.8889 +[0.860, 0.400] : 98.3704 +[0.860, 0.410] : 99.0370 +[0.860, 0.420] : 98.3704 +[0.860, 0.430] : 98.6667 +[0.860, 0.440] : 98.4444 +[0.860, 0.450] : 98.3704 +[0.860, 0.460] : 98.2222 +[0.860, 0.470] : 97.7037 +[0.860, 0.480] : 97.7778 +[0.860, 0.490] : 98.1481 +[0.860, 0.500] : 97.7037 +[0.860, 0.510] : 97.8519 +[0.860, 0.520] : 97.2593 +[0.860, 0.530] : 96.8148 +[0.860, 0.540] : 96.8148 +[0.860, 0.550] : 96.0000 +[0.860, 0.560] : 96.1481 +[0.860, 0.570] : 96.4444 +[0.860, 0.580] : 96.0000 + +[0.870, 0.360] : 99.0370 +[0.870, 0.370] : 98.8889 +[0.870, 0.380] : 99.1852 +[0.870, 0.390] : 99.0370 +[0.870, 0.400] : 99.1111 +[0.870, 0.410] : 98.1481 +[0.870, 0.420] : 98.5185 +[0.870, 0.430] : 98.5185 +[0.870, 0.440] : 98.2222 +[0.870, 0.450] : 98.5185 +[0.870, 0.460] : 98.1481 +[0.870, 0.470] : 98.0741 +[0.870, 0.480] : 97.4074 +[0.870, 0.490] : 97.8519 +[0.870, 0.500] : 97.8519 +[0.870, 0.510] : 97.4815 +[0.870, 0.520] : 97.4815 +[0.870, 0.530] : 96.5926 +[0.870, 0.540] : 97.0370 +[0.870, 0.550] : 96.5185 +[0.870, 0.560] : 97.7037 +[0.870, 0.570] : 96.6667 +[0.870, 0.580] : 96.1481 + +[0.880, 0.360] : 98.9630 +[0.880, 0.370] : 98.6667 +[0.880, 0.380] : 98.6667 +[0.880, 0.390] : 98.6667 +[0.880, 0.400] : 99.1111 +[0.880, 0.410] : 98.2222 +[0.880, 0.420] : 98.7407 +[0.880, 0.430] : 98.3704 +[0.880, 0.440] : 98.4444 +[0.880, 0.450] : 98.0000 +[0.880, 0.460] : 98.0000 +[0.880, 0.470] : 97.3333 +[0.880, 0.480] : 96.9630 +[0.880, 0.490] : 97.6296 +[0.880, 0.500] : 97.7778 +[0.880, 0.510] : 96.7407 +[0.880, 0.520] : 96.8148 +[0.880, 0.530] : 97.5556 +[0.880, 0.540] : 96.0741 +[0.880, 0.550] : 96.5185 +[0.880, 0.560] : 97.1852 +[0.880, 0.570] : 97.1852 +[0.880, 0.580] : 96.0000 + +[0.890, 0.360] : 98.8889 +[0.890, 0.370] : 99.1111 +[0.890, 0.380] : 98.4444 +[0.890, 0.390] : 98.7407 +[0.890, 0.400] : 99.3333 +[0.890, 0.410] : 98.8148 +[0.890, 0.420] : 98.8148 +[0.890, 0.430] : 98.5926 +[0.890, 0.440] : 98.0741 +[0.890, 0.450] : 97.7778 +[0.890, 0.460] : 97.6296 +[0.890, 0.470] : 97.2593 +[0.890, 0.480] : 97.2593 +[0.890, 0.490] : 98.6667 +[0.890, 0.500] : 96.8889 +[0.890, 0.510] : 97.4074 +[0.890, 0.520] : 96.5926 +[0.890, 0.530] : 97.1852 +[0.890, 0.540] : 95.7037 +[0.890, 0.550] : 96.6667 +[0.890, 0.560] : 96.2963 +[0.890, 0.570] : 96.2222 +[0.890, 0.580] : 95.4074 + +[0.900, 0.360] : 99.3333 +[0.900, 0.370] : 98.5185 +[0.900, 0.380] : 98.8889 +[0.900, 0.390] : 98.5185 +[0.900, 0.400] : 98.3704 +[0.900, 0.410] : 99.0370 +[0.900, 0.420] : 99.1111 +[0.900, 0.430] : 99.1111 +[0.900, 0.440] : 98.5926 +[0.900, 0.450] : 98.2963 +[0.900, 0.460] : 97.4815 +[0.900, 0.470] : 97.6296 +[0.900, 0.480] : 97.4815 +[0.900, 0.490] : 98.3704 +[0.900, 0.500] : 97.3333 +[0.900, 0.510] : 97.0370 +[0.900, 0.520] : 97.1852 +[0.900, 0.530] : 97.1852 +[0.900, 0.540] : 97.0370 +[0.900, 0.550] : 96.1481 +[0.900, 0.560] : 95.4074 +[0.900, 0.570] : 95.6296 +[0.900, 0.580] : 96.8889 + +[0.910, 0.360] : 98.9630 +[0.910, 0.370] : 98.9630 +[0.910, 0.380] : 98.5185 +[0.910, 0.390] : 98.4444 +[0.910, 0.400] : 99.3333 +[0.910, 0.410] : 99.6296 +[0.910, 0.420] : 99.7778 +[0.910, 0.430] : 99.0370 +[0.910, 0.440] : 98.8148 +[0.910, 0.450] : 98.0000 +[0.910, 0.460] : 97.7037 +[0.910, 0.470] : 97.7037 +[0.910, 0.480] : 97.7778 +[0.910, 0.490] : 98.3704 +[0.910, 0.500] : 97.3333 +[0.910, 0.510] : 97.4074 +[0.910, 0.520] : 97.8519 +[0.910, 0.530] : 98.3704 +[0.910, 0.540] : 96.5926 +[0.910, 0.550] : 96.6667 +[0.910, 0.560] : 96.8148 +[0.910, 0.570] : 95.9259 +[0.910, 0.580] : 98.4444 + +[0.920, 0.360] : 99.2593 +[0.920, 0.370] : 99.7778 +[0.920, 0.380] : 98.8889 +[0.920, 0.390] : 99.2593 +[0.920, 0.400] : 99.1852 +[0.920, 0.410] : 99.1111 +[0.920, 0.420] : 99.1111 +[0.920, 0.430] : 98.7407 +[0.920, 0.440] : 98.2222 +[0.920, 0.450] : 98.2222 +[0.920, 0.460] : 98.4444 +[0.920, 0.470] : 98.0000 +[0.920, 0.480] : 97.6296 +[0.920, 0.490] : 98.1481 +[0.920, 0.500] : 96.8889 +[0.920, 0.510] : 97.7778 +[0.920, 0.520] : 97.4074 +[0.920, 0.530] : 98.2963 +[0.920, 0.540] : 97.7037 +[0.920, 0.550] : 96.8148 +[0.920, 0.560] : 97.4815 +[0.920, 0.570] : 96.1481 +[0.920, 0.580] : 97.4074 + +[0.930, 0.360] : 99.4815 +[0.930, 0.370] : 99.6296 +[0.930, 0.380] : 98.6667 +[0.930, 0.390] : 98.9630 +[0.930, 0.400] : 99.3333 +[0.930, 0.410] : 98.3704 +[0.930, 0.420] : 98.6667 +[0.930, 0.430] : 98.0741 +[0.930, 0.440] : 98.5926 +[0.930, 0.450] : 97.6296 +[0.930, 0.460] : 98.2963 +[0.930, 0.470] : 98.2222 +[0.930, 0.480] : 97.8519 +[0.930, 0.490] : 98.0741 +[0.930, 0.500] : 97.0370 +[0.930, 0.510] : 97.3333 +[0.930, 0.520] : 98.7407 +[0.930, 0.530] : 97.7778 +[0.930, 0.540] : 96.1481 +[0.930, 0.550] : 95.7778 +[0.930, 0.560] : 96.2963 +[0.930, 0.570] : 96.4444 + +[0.940, 0.360] : 99.4074 +[0.940, 0.370] : 99.4815 +[0.940, 0.380] : 98.6667 +[0.940, 0.390] : 98.4444 +[0.940, 0.400] : 98.9630 +[0.940, 0.410] : 98.8148 +[0.940, 0.420] : 98.2222 +[0.940, 0.430] : 98.8889 +[0.940, 0.440] : 98.3704 +[0.940, 0.450] : 98.5926 +[0.940, 0.460] : 98.2222 +[0.940, 0.470] : 97.1852 +[0.940, 0.480] : 97.3333 +[0.940, 0.490] : 97.0370 +[0.940, 0.500] : 96.2222 +[0.940, 0.510] : 97.1852 +[0.940, 0.520] : 96.5185 +[0.940, 0.530] : 96.7407 +[0.940, 0.540] : 96.6667 +[0.940, 0.550] : 95.8519 +[0.940, 0.560] : 95.4815 +[0.940, 0.570] : 95.1111 +[0.940, 0.580] : 95.8519 + +[0.950, 0.360] : 99.4074 +[0.950, 0.370] : 99.1852 +[0.950, 0.380] : 99.0370 +[0.950, 0.390] : 98.7407 +[0.950, 0.400] : 98.5926 +[0.950, 0.410] : 98.8148 +[0.950, 0.420] : 98.9630 +[0.950, 0.430] : 98.5185 +[0.950, 0.440] : 98.7407 +[0.950, 0.450] : 98.2963 +[0.950, 0.460] : 97.4815 +[0.950, 0.470] : 97.4074 +[0.950, 0.480] : 97.7037 +[0.950, 0.490] : 97.8519 +[0.950, 0.500] : 96.6667 +[0.950, 0.510] : 97.4815 +[0.950, 0.520] : 96.8148 +[0.950, 0.530] : 96.8889 +[0.950, 0.540] : 96.8148 +[0.950, 0.550] : 96.5926 +[0.950, 0.560] : 96.6667 +[0.950, 0.570] : 96.5926 +[0.950, 0.580] : 96.1481 + +[0.960, 0.360] : 99.4074 +[0.960, 0.370] : 99.1111 +[0.960, 0.380] : 98.7407 +[0.960, 0.390] : 98.8148 +[0.960, 0.400] : 98.7407 +[0.960, 0.410] : 99.1111 +[0.960, 0.420] : 98.0741 +[0.960, 0.430] : 98.4444 +[0.960, 0.440] : 98.0741 +[0.960, 0.450] : 98.0741 +[0.960, 0.460] : 97.7778 +[0.960, 0.470] : 98.6667 +[0.960, 0.480] : 97.0370 +[0.960, 0.490] : 96.8148 +[0.960, 0.500] : 97.8519 +[0.960, 0.510] : 98.2963 +[0.960, 0.520] : 96.8889 +[0.960, 0.530] : 97.1111 +[0.960, 0.540] : 96.7407 +[0.960, 0.550] : 96.2963 +[0.960, 0.560] : 95.5556 +[0.960, 0.570] : 95.7778 +[0.960, 0.580] : 96.3704 + +[0.970, 0.360] : 99.5556 +[0.970, 0.370] : 99.0370 +[0.970, 0.380] : 99.0370 +[0.970, 0.390] : 99.7037 +[0.970, 0.400] : 98.6667 +[0.970, 0.410] : 99.4074 +[0.970, 0.420] : 99.2593 +[0.970, 0.430] : 98.0000 +[0.970, 0.440] : 98.2222 +[0.970, 0.450] : 98.2963 +[0.970, 0.460] : 98.0000 +[0.970, 0.470] : 99.4074 +[0.970, 0.480] : 97.4815 +[0.970, 0.490] : 97.1111 +[0.970, 0.500] : 97.4815 +[0.970, 0.510] : 96.9630 +[0.970, 0.520] : 97.1111 +[0.970, 0.530] : 96.9630 +[0.970, 0.540] : 97.0370 +[0.970, 0.550] : 96.9630 +[0.970, 0.560] : 96.3704 +[0.970, 0.570] : 94.5185 +[0.970, 0.580] : 95.4074 diff --git a/aurmr_tasks/src/aurmr_tasks/common/tahoma.py b/aurmr_tasks/src/aurmr_tasks/common/tahoma.py index ed31395..b239597 100644 --- a/aurmr_tasks/src/aurmr_tasks/common/tahoma.py +++ b/aurmr_tasks/src/aurmr_tasks/common/tahoma.py @@ -1,27 +1,38 @@ import copy +import math import sys +import numpy as np from functools import wraps +from turtle import pos +import std_msgs from actionlib import SimpleActionClient -from control_msgs.msg import FollowJointTrajectoryAction, GripperCommandAction, GripperCommandGoal -from control_msgs.msg import FollowJointTrajectoryGoal -from trajectory_msgs.msg import JointTrajectoryPoint +from actionlib_msgs.msg import GoalStatusArray, GoalStatus +from control_msgs.msg import FollowJointTrajectoryAction, GripperCommandAction, GripperCommandGoal, FollowJointTrajectoryGoal +from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory import tf2_ros -import tf2_geometry_msgs + +#import tf2_geometry_msgs import actionlib import control_msgs.msg import trajectory_msgs.msg import rospy +from tf2_geometry_msgs import from_msg_msg +from geometry_msgs.msg import PoseStamped, WrenchStamped, Quaternion, Pose, Point +# from robotiq_2f_gripper_control.msg import vacuum_gripper_input arStatus from aurmr_tasks.util import all_close, pose_dist -from moveit_msgs.msg import MoveItErrorCodes, MoveGroupAction, DisplayTrajectory +from moveit_msgs.msg import MoveItErrorCodes, MoveGroupAction, DisplayTrajectory, RobotState from moveit_msgs.srv import GetPositionIK, GetPositionIKRequest +from sensor_msgs.msg import JointState import moveit_commander from controller_manager_msgs.srv import ListControllers, SwitchController from tahoma_moveit_config.msg import ServoToPoseAction, ServoToPoseGoal + + ARM_GROUP_NAME = 'manipulator' JOINT_ACTION_SERVER = '/pos_joint_traj_controller/follow_joint_trajectory' JOINT_GROUP_CONTROLLER = 'joint_group_pos_controller' @@ -63,14 +74,11 @@ def requires_controller(named): """ - Args: named: - Returns: a decorator factory that replaces an invoked function with a wrapped function call, ensuring that the correct controller is activated before the call. - """ def decorator(function): # Use this functools helper to make docstrings of the passed in function get bubbled up @@ -87,10 +95,8 @@ def wrapper(self, *args, **kwargs): def moveit_error_string(val): """Returns a string associated with a MoveItErrorCode. - Args: val: The val field from moveit_msgs/MoveItErrorCodes.msg - Returns: The string associated with the error value, 'UNKNOWN_ERROR_CODE' if the value is invalid. """ @@ -99,6 +105,10 @@ def moveit_error_string(val): class Tahoma: def __init__(self, in_sim=False): + import yaml + import os + import rospkg + self.in_sim = in_sim self.joint_state = None self.point_cloud = None @@ -114,7 +124,8 @@ def __init__(self, in_sim=False): self._gripper_client = actionlib.SimpleActionClient(GRIPPER_ACTION_SERVER, GripperCommandAction) server_reached = self._joint_traj_client.wait_for_server(rospy.Duration(10)) if not server_reached: - rospy.signal_shutdown('Unable to connect to arm action server. Timeout exceeded.') + print('[TAHOMA] Unable to connect to arm action server. Timeout exceeded. Exiting...') + rospy.signal_shutdown('[TAHOMA] Unable to connect to arm action server. Timeout exceeded.') sys.exit() self._move_group_client = actionlib.SimpleActionClient( MOVE_GROUP_ACTION_SERVER, MoveGroupAction) @@ -124,7 +135,10 @@ def __init__(self, in_sim=False): self.commander = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface(synchronous=True) self.move_group = moveit_commander.MoveGroupCommander(ARM_GROUP_NAME) - self.move_group.set_max_velocity_scaling_factor(.15) + self.MAX_VEL_FACTOR = .6 + self.MAX_ACC_FACTOR = .6 + self.move_group.set_max_velocity_scaling_factor(self.MAX_VEL_FACTOR) + self.move_group.set_max_acceleration_scaling_factor(self.MAX_ACC_FACTOR) self.display_trajectory_publisher = rospy.Publisher( "/move_group/display_planned_path", DisplayTrajectory, @@ -135,18 +149,79 @@ def __init__(self, in_sim=False): self._controller_switcher = rospy.ServiceProxy("/controller_manager/switch_controller", SwitchController) self.servo_to_pose_client = SimpleActionClient("/servo_server/servo_to_pose", ServoToPoseAction) + self.grasp_pose_pub = rospy.Publisher("~grasp_pose", PoseStamped, queue_size=1) + planning_frame = self.move_group.get_planning_frame() + # print(f"\nplanning_frame: {planning_frame}") + eef_link = self.move_group.get_end_effector_link() # "arm_tool0" by default, i.e. robot flange + # print(f"eef_link: {eef_link}") + self.move_group.set_end_effector_link("epick_end_effector") eef_link = self.move_group.get_end_effector_link() group_names = self.commander.get_group_names() + self.wrench_listener = rospy.Subscriber("/wrench", WrenchStamped, self.wrench_cb) + # self.gripper_status_listener = rospy.Subscriber("/gripper_control/status", VacuumGripperStatus, self.gripper_status_cb) + self.traj_status_listener = rospy.Subscriber("/scaled_pos_joint_traj_controller/follow_joint_trajectory/status", GoalStatusArray, self.goal_status_cb) + self.force_mag = 0 + self.torque_mag = 0 + self.object_detected = False + self.goal_finished = True + self.goal_stamp = 0 # Misc variables self.planning_frame = planning_frame self.eef_link = eef_link - self.group_names = group_names + self.group_names = group_names self.active_controllers = None self.update_running_controllers() + rospack = rospkg.RosPack() + yaml_path = os.path.join(rospack.get_path('tahoma_description'), 'config/pod_sizes.yaml') + with open(yaml_path, 'r') as pod_sizes_file: + self.pod_sizes = yaml.safe_load(pod_sizes_file) + + yaml_path = os.path.join(rospack.get_path('tahoma_description'), 'config/end_effector_sizes.yaml') + with open(yaml_path, 'r') as end_effector_sizes_file: + self.end_effector = yaml.safe_load(end_effector_sizes_file) + + # self.move_to_joint_angles(joints='tote_approach', startpoint='current') + + # result is the same as for self.commander.get_current_state(), which is more correct because + # self.commader has get_current_state() function, self.move_group not + # still, the same results - strange... + # self.home_state = self.move_group.get_current_state() + + self.home_state = RobotState() # 'tote_approach' - object dropping position + self.home_state.joint_state.header.stamp = rospy.Time.now() + self.home_state.joint_state.header.frame_id = 'base_link' + # ['arm_shoulder_pan_joint', 'arm_shoulder_lift_joint', 'arm_elbow_joint', 'arm_wrist_1_joint', 'arm_wrist_2_joint', 'arm_wrist_3_joint'] + self.home_state.joint_state.name = self.move_group.get_active_joints() + self.home_state.joint_state.position = [2.86, -1.47, 1.58, 4.75, 1.59, 4.83] # the order is the same as joint_state.name + # self.move_to_joint_angles(joints=self.home_state.joint_state, startpoint='current') + + + def wrench_cb(self, msg: WrenchStamped): + self.force_mag = math.sqrt(msg.wrench.force.x**2 + msg.wrench.force.y**2+ msg.wrench.force.z**2) + self.torque_mag = math.sqrt(msg.wrench.torque.x**2 + msg.wrench.torque.y**2+ msg.wrench.torque.z**2) + + + # def gripper_status_cb(self, msg: VacuumGripperStatus): + # self.object_detected = (msg.gPO < 95) + + + def goal_status_cb(self, msg: GoalStatusArray): + latest_time = 0 + latest_status = GoalStatus.SUCCEEDED + for g in msg.status_list: + new_stamp = g.goal_id.stamp.secs + g.goal_id.stamp.nsecs*10**(-9) + if new_stamp > latest_time: + latest_time = new_stamp + latest_status = g.status + self.goal_finished = latest_status != GoalStatus.PENDING and latest_status != GoalStatus.ACTIVE + self.goal_stamp = latest_time + # home_state_joint_positions = [-1.01, -0.46, -1.62, -1.05, -5.26, 3.14] + + def update_running_controllers(self): controllers_status = self._controller_lister().controller self.active_controllers = [] @@ -154,6 +229,7 @@ def update_running_controllers(self): if controller.state == "running": self.active_controllers.append(controller.name) + def wait_for_controllers(self, timeout): rate = rospy.Rate(1) try_until = rospy.Time.now() + rospy.Duration(timeout) @@ -170,6 +246,7 @@ def wait_for_controllers(self, timeout): rate.sleep() return False + def activate_controller(self, named): # Hacked to work for what we need right now to_enable = named @@ -194,6 +271,7 @@ def activate_controller(self, named): else: return True + def is_controller_active(self, named): self.update_running_controllers() to_check = named @@ -201,18 +279,31 @@ def is_controller_active(self, named): to_check = JOINT_TRAJ_CONTROLLER_SIM return to_check in self.active_controllers + def open_gripper(self, return_before_done=False): - goal = GripperCommandGoal(position=0, max_effort=1) + goal = GripperCommandGoal() + goal.command.position = 0 + goal.command.max_effort = 1 self._gripper_client.send_goal(goal) if not return_before_done: self._gripper_client.wait_for_result() + + def check_gripper_item(self): + return self.object_detected + + def close_gripper(self, return_before_done=False): - goal = GripperCommandGoal(position=0.83, max_effort=1) + goal = GripperCommandGoal() + goal.command.position = 0.83 + goal.command.max_effort = 1 self._gripper_client.send_goal(goal) + rospy.loginfo("Waiting for gripper" + str(return_before_done)) if not return_before_done: + rospy.loginfo("Waiting for gripper. \n") self._gripper_client.wait_for_result() + @requires_controller(JOINT_TRAJ_CONTROLLER) def move_to_pose_unsafe(self, pose, return_before_done=False): joint_names = [key for key in pose] @@ -234,6 +325,7 @@ def move_to_pose_unsafe(self, pose, return_before_done=False): # print('Received the following result:') # print(self.trajectory_client.get_result()) + @requires_controller(JOINT_TRAJ_CONTROLLER) def move_to_joint_angles_unsafe(self, joint_state): """ @@ -250,124 +342,182 @@ def move_to_joint_angles_unsafe(self, joint_state): self._joint_traj_client.send_goal(goal) self._joint_traj_client.wait_for_result(rospy.Duration(10)) + def get_joint_values_for_name(self, name): self.move_group.set_named_target(name) values = self.move_group.get_joint_value_target() return values + @requires_controller(JOINT_TRAJ_CONTROLLER) def move_to_joint_angles(self, joints, allowed_planning_time=10.0, - execution_timeout=15.0, - num_planning_attempts=8, + # home_state_joint_positions = [-1.01, -0.46, -1.62, -1.05, -5.26, 3.14] + execution_timeout=15.0, + num_planning_attempts=20, plan_only=False, replan=False, replan_attempts=5, - tolerance=0.01): - """Moves the end-effector to a pose, using motion planning. - + joint_tolerance=0.01, + startpoint = 'home'): + """ Moves the end-effector to a pose, using motion planning. + Args: - joints: A list of (name, value) for the arm joints. Alternatively a string name of a stored configuration - allowed_planning_time: float. The maximum duration to wait for a - planning result. - execution_timeout: float. The maximum duration to wait for an arm - motion to execute (or for planning to fail completely), in - seconds. + joints: dict("joint_name":joint_value) + list(joint_values) + JointState. + string "name" of a stored configuration [self.move_group.remember_joint_values("name", values)] + values = self.move_group.get_current_joint_values() (= list of joint values) if no values provided + Target joint angles + allowed_planning_time: float. + The maximum duration to wait for a planning result. + execution_timeout: float. + The maximum duration to wait for an arm motion to execute + (or for planning to fail completely), in seconds. group_name: string. - num_planning_attempts: int. The number of times to compute the same - plan. The shortest path is ultimately used. For random - planners, this can help get shorter, less weird paths. - plan_only: bool. If True, then this method does not execute the - plan on the robot. Useful for determining whether this is - likely to succeed. - replan: bool. If True, then if an execution fails (while the arm is - moving), then come up with a new plan and execute it. - replan_attempts: int. How many times to replan if the execution - fails. - tolerance: float. The goal tolerance, in meters. - + num_planning_attempts: int. + The number of times to compute the same plan. + The shortest path is ultimately used. + For random planners, this can help get shorter, less weird paths. + plan_only: bool. + If True, then this method does not execute the plan on the robot. + Useful for determining whether this is likely to succeed. + replan: bool. + If True, then if an execution fails (while the arm is moving), then come up with a new plan and execute it. + replan_attempts: int. + How many times to replan if the execution fails. + joint_tolerance: float. + The goal joint tolerance, in radians. + Returns: - string describing the error if an error occurred, else None. - """ + string describing the error if an error occurred, else None. """ + self.move_group.stop() self.move_group.set_num_planning_attempts(num_planning_attempts) self.move_group.allow_replanning(replan) - self.move_group.set_goal_joint_tolerance(tolerance) + self.move_group.set_goal_joint_tolerance(joint_tolerance) self.move_group.set_planning_time(allowed_planning_time) + if startpoint == 'home': + self.move_group.set_start_state(self.home_state) + else: + self.move_group.set_start_state_to_current_state() - joint_values = joints if isinstance(joints, str): joint_values = self.get_joint_values_for_name(joints) self.move_group.set_named_target(joints) - else: + elif isinstance(joints, JointState): + joint_values = list(joints.position) + # joint_names = list(joints.name) + # joint_states = dict(zip(joint_names, joint_values)) + # print(f"joint_states:\n{joint_states}") self.move_group.set_joint_value_target(joints) - self.move_group.go(wait=True) + elif isinstance(joints, list): + self.move_group.set_joint_value_target(joints) + joint_values = joints + + # self.move_group.go(wait=True) + success, plan, planning_time, error_code = self.move_group.plan() + if not success: + return False + + # # A `DisplayTrajectory`_ msg has two primary fields, trajectory_start and trajectory. + # # We populate the trajectory_start with our current robot state to copy over + # # any AttachedCollisionObjects and add our plan to the trajectory. + # display_trajectory = DisplayTrajectory() + # display_trajectory.trajectory_start = self.home_state if startpoint == 'home' else self.commander.get_current_state() + # display_trajectory.trajectory.append(plan) + # # Publish + # self.display_trajectory_publisher.publish(display_trajectory) - # Calling ``stop()`` ensures that there is no residual movement + # Now, we call the planner to compute the plan and execute it. + ret = self.move_group.execute(plan, wait=True) + + # Calling `stop()` ensures that there is no residual movement self.move_group.stop() + # print(f"\ngoal_joint_values: {joint_values}") current_joints = self.move_group.get_current_joint_values() - return all_close(joint_values, current_joints, tolerance) + # print(f"current_joint_values: {current_joints}") + return all_close(joint_values, current_joints, joint_tolerance=joint_tolerance) + @requires_controller(JOINT_TRAJ_CONTROLLER) def move_to_pose(self, pose_stamped, allowed_planning_time=10.0, execution_timeout=15.0, - num_planning_attempts=8, + num_planning_attempts=20, orientation_constraint=None, replan=True, replan_attempts=5, - tolerance=0.01): - """Moves the end-effector to a pose, using motion planning. - + position_tolerance=0.001, + orientation_tolerance=0.01, + startpoint = 'home'): + """ Moves the end-effector to a pose, using motion planning. + Args: - pose: geometry_msgs/PoseStamped. The goal pose for the gripper. - allowed_planning_time: float. The maximum duration to wait for a - planning result. - execution_timeout: float. The maximum duration to wait for an arm - motion to execute (or for planning to fail completely), in - seconds. - num_planning_attempts: int. The number of times to compute the same - plan. The shortest path is ultimately used. For random - planners, this can help get shorter, less weird paths. - orientation_constraint: moveit_msgs/OrientationConstraint. An - orientation constraint for the entire path. - replan: bool. If True, then if an execution fails (while the arm is - moving), then come up with a new plan and execute it. - replan_attempts: int. How many times to replan if the execution - fails. - tolerance: float. The goal tolerance, in meters. - + pose: geometry_msgs/PoseStamped. + The goal pose for the gripper. + allowed_planning_time: float. + The maximum duration to wait for a planning result. + execution_timeout: float. + The maximum duration to wait for an arm motion to execute + (or for planning to fail completely), in seconds. + num_planning_attempts: int. + The number of times to compute the same plan. + The shortest path is ultimately used. + For random planners, this can help get shorter, less weird paths. + orientation_constraint: moveit_msgs/OrientationConstraint. + An orientation constraint for the entire path. + replan: bool. + If True, then if an execution fails (while the arm is moving), + then come up with a new plan and execute it. + replan_attempts: int. + How many times to replan if the execution fails. + position_tolerance: float. + The goal position tolerance, in meters. + orientation_tolerance: float. + The goal orientation tolerance, in radians. 1 degree = 0.017 rad + startpoint: string. + The startpoint for motion planning. If any other than 'home', current robot state will be startpoint + Returns: - string describing the error if an error occurred, else None. - """ + string describing the error if an error occurred, else None. """ - goal_in_planning_frame = self.tf2_buffer.transform(pose_stamped, self.move_group.get_planning_frame(), + pose_stamped = from_msg_msg(pose_stamped) + # print(f"\npose_stamped:\n{pose_stamped.pose}") + goal_in_planning_frame = self.tf2_buffer.transform(pose_stamped, self.planning_frame, rospy.Duration(1)) + # print(f"\ngoal_in_planning_frame:\n{goal_in_planning_frame.pose}") - self.move_group.set_end_effector_link("gripper_equilibrium_grasp") + self.move_group.set_end_effector_link("epick_end_effector") self.move_group.set_pose_target(pose_stamped) self.move_group.set_planning_time(allowed_planning_time) self.move_group.set_num_planning_attempts(num_planning_attempts) self.move_group.allow_replanning(replan) - self.move_group.set_goal_position_tolerance(tolerance) + self.move_group.set_goal_position_tolerance(position_tolerance) + self.move_group.set_goal_orientation_tolerance(orientation_tolerance) + if startpoint == 'home': + self.move_group.set_start_state(self.home_state) + else: + self.move_group.set_start_state_to_current_state() + success, plan, planning_time, error_code = self.move_group.plan() if not success: return False - # A `DisplayTrajectory`_ msg has two primary fields, trajectory_start and trajectory. - # We populate the trajectory_start with our current robot state to copy over - # any AttachedCollisionObjects and add our plan to the trajectory. - display_trajectory = DisplayTrajectory() - display_trajectory.trajectory_start = self.commander.get_current_state() - display_trajectory.trajectory.append(plan) - # Publish - self.display_trajectory_publisher.publish(display_trajectory) + # # A `DisplayTrajectory`_ msg has two primary fields, trajectory_start and trajectory. + # # We populate the trajectory_start with our current robot state to copy over + # # any AttachedCollisionObjects and add our plan to the trajectory. + # display_trajectory = DisplayTrajectory() + # display_trajectory.trajectory_start = self.home_state if startpoint == 'home' else self.commander.get_current_state() + # display_trajectory.trajectory.append(plan) + # # Publish + # self.display_trajectory_publisher.publish(display_trajectory) # Now, we call the planner to compute the plan and execute it. - self.move_group.execute(plan, wait=True) + ret = self.move_group.execute(plan, wait=True) # Calling `stop()` ensures that there is no residual movement self.move_group.stop() # It is always good to clear your targets after planning with poses. @@ -375,18 +525,27 @@ def move_to_pose(self, self.move_group.clear_pose_targets() current_pose = self.move_group.get_current_pose() - rospy.loginfo(f"Pose dist: {pose_dist(goal_in_planning_frame, current_pose)}") - return all_close(goal_in_planning_frame, current_pose, tolerance) + # print(f"\ncurrent_pose:\n{current_pose.pose}") + current_joint_values = self.move_group.get_current_joint_values() + # print(f"\ncurrent_joint_values: {current_joint_values}") + # print(f"pose_dist: {pose_dist(goal_in_planning_frame, current_pose)}") + return all_close(goal_in_planning_frame, current_pose, + position_tolerance=position_tolerance, + orientation_tolerance=orientation_tolerance) + @requires_controller(JOINT_TRAJ_CONTROLLER) def straight_move_to_pose(self, pose_stamped, tolerance=0.01, - ee_step=0.0025, - jump_threshold=2.0, - avoid_collisions=True): - """Moves the end-effector to a pose in a straight line. - + ee_step=0.001, + jump_threshold=6.0, + avoid_collisions=True, + use_force=False, + use_gripper=False): + """ + Moves the end-effector to a pose in a straight line. + Args: pose_stamped: geometry_msgs/PoseStamped. The goal pose for the gripper. @@ -395,26 +554,81 @@ def straight_move_to_pose(self, configuration space allowed between two poses in the path. Used to prevent "jumps" in the IK solution. avoid_collisions: bool. Whether to check for obstacles or not. - + Returns: string describing the error if an error occurred, else None. """ - self.move_group.set_end_effector_link("arm_tool0") + + self.move_group.set_end_effector_link("epick_end_effector") goal_in_planning_frame = self.tf2_buffer.transform(pose_stamped, self.planning_frame, rospy.Duration(1)) - waypoints = [goal_in_planning_frame.pose] + self.grasp_pose_pub.publish(pose_stamped) + waypoints = [goal_in_planning_frame.pose] (plan, fraction) = self.move_group.compute_cartesian_path( waypoints, ee_step, jump_threshold, avoid_collisions ) + plan = self.move_group.retime_trajectory(self.move_group.get_current_state(), plan, velocity_scaling_factor=.05, acceleration_scaling_factor=.05) if fraction < .9: rospy.logwarn(f"Not moving in cartesian path. Only {fraction} waypoints reached") - return False - self.move_group.execute(plan, wait=True) - + # return False + + wait = not (use_force or use_gripper) + rospy.loginfo("Waiting?: " + str(wait)) + old_goal_stamp = self.goal_stamp + + ret = self.move_group.execute(plan, wait=wait) + if use_gripper: + self.close_gripper(return_before_done=True) + + if not wait: # # A `DisplayTrajectory`_ msg has two primary fields, trajectory_start and trajectory. + # # We populate the trajectory_start with our current robot state to copy over + # # any AttachedCollisionObjects and add our plan to the trajectory. + # display_trajectory = DisplayTrajectory() + # display_trajectory.trajectory_start = self.home_state if startpoint == 'home' else self.commander.get_current_state() + # display_trajectory.trajectory.append(plan) + # # Publish + # self.display_trajectory_publisher.publish(display_trajectory) + timeout = 1 + steps = 0 + while steps < timeout and (self.goal_stamp == old_goal_stamp): + rospy.loginfo("Waiting for moveit goal to update:" + str(old_goal_stamp) + " " + str(self.goal_stamp)) + rospy.sleep(.01) + steps = steps + .01 + + + force_limit = 50 + early_stop = False + rospy.loginfo("Waiting?: " + str(wait) + " " + str(self.goal_finished)) + timeout = 5 + steps = 0 + while steps < timeout and not self.goal_finished: + # rospy.loginfo("Waiting for feedback or goal finishing") + if use_force and self.force_mag > force_limit: + self.move_group.stop() + rospy.loginfo("Stopping movement due to force feedback") + early_stop = True + break + elif use_gripper and self.object_detected: + self.move_group.stop() + rospy.loginfo("Stopping movement due to object detection") + early_stop = True + break + rospy.sleep(.01) + steps = steps + .01 # # A `DisplayTrajectory`_ msg has two primary fields, trajectory_start and trajectory. + # # We populate the trajectory_start with our current robot state to copy over + # # any AttachedCollisionObjects and add our plan to the trajectory. + # display_trajectory = DisplayTrajectory() + # display_trajectory.trajectory_start = self.home_state if startpoint == 'home' else self.commander.get_current_state() + # display_trajectory.trajectory.append(plan) + # # Publish + # self.display_trajectory_publisher.publish(display_trajectory) + + current_pose = self.move_group.get_current_pose() rospy.loginfo(f"Pose dist: {pose_dist(goal_in_planning_frame, current_pose)}") - return all_close(goal_in_planning_frame, current_pose, tolerance) + return early_stop or all_close(goal_in_planning_frame, current_pose, tolerance) + @requires_controller(JOINT_GROUP_CONTROLLER) def servo_to_pose(self, @@ -441,10 +655,11 @@ def servo_to_pose(self, return False return result.error_code == 0 + def check_pose(self, pose_stamped, allowed_planning_time=10.0, - group_name='manipulator', + group_name=ARM_GROUP_NAME, tolerance=0.01): return self.move_to_pose( pose_stamped, @@ -453,36 +668,354 @@ def check_pose(self, tolerance=tolerance, plan_only=True) - def compute_ik(self, pose_stamped, timeout=rospy.Duration(5)): - """Computes inverse kinematics for the given pose. + def compute_ik(self, pose_stamped, timeout=rospy.Duration(5)): + """ Computes inverse kinematics (joint values) for the given pose. + Note: if you are interested in returning the IK solutions, we have shown how to access them. - + Args: pose_stamped: geometry_msgs/PoseStamped. - timeout: rospy.Duration. How long to wait before giving up on the - IK solution. - + timeout: rospy.Duration. How long to wait before giving up on the IK solution. + Returns: A list of (name, value) for the arm joints if the IK solution - was found, False otherwise. - """ + was found, False otherwise. """ + # compute the ik solution for the robot to move to the pre-grasp pose request = GetPositionIKRequest() + request.ik_request.group_name = ARM_GROUP_NAME + # set robot state to home state each time method is run + request.ik_request.robot_state = self.home_state + request.ik_request.avoid_collisions = True + request.ik_request.ik_link_name = "epick_end_effector" request.ik_request.pose_stamped = pose_stamped - request.ik_request.group_name = 'manipulator' request.ik_request.timeout = timeout + response = self._compute_ik(request) error_str = moveit_error_string(response.error_code.val) success = error_str == 'SUCCESS' if not success: - return False + return None + + # response.solution = moveit_msgs/RobotState + # moveit_msgs/RobotState: + # sensor_msgs/JointState joint_state: + # string[] name + # float64[] position + # float64[] velocity + # float64[] effort + # sensor_msgs/MultiDOFJointState multi_dof_joint_state [eg. planar joint has 3DOF = (x,y,yaw)] NA because urdf file defines joints as 1DOF joints + # ... joint_state = response.solution.joint_state - joints = [] - for name, position in zip(joint_state.name, joint_state.position): - if name in self.commander.get_active_joint_names(): - joints.append((name, position)) - return joints + # print(f"\njoint values before:\t{joint_state.position}") + + # wrap angles into range [-2pi, 2pi) for all joints except elbow_joint which should be within [-pi, pi) + # joints = dict() + # for joint_name, joint_value in zip(joint_state.name, joint_state.position): + # joints[joint_name] = self.wrap_angle(joint_value, math.pi) if joint_name == 'arm_elbow_joint' else self.wrap_angle(joint_value, 2*math.pi) + joint_state.position = tuple([self.wrap_angle(joint_value, math.pi) if joint_name == 'arm_elbow_joint' \ + else self.wrap_angle(joint_value, 2*math.pi) \ + for joint_name, joint_value in zip(joint_state.name, joint_state.position)]) + # print(f"joint values after: \t{joint_state.position}") + + # joints = [] + # for name, position in zip(joint_state.name, joint_state.position): + # # There is no public function get_active_joint_names() in self.commander, still it's callable + # # On the other hand, there is self.move_group.get_active_joints() which gives the same result + # if name in self.commander.get_active_joint_names(): + # joints.append((name, position)) + return joint_state + + + def compute_path(self,goal_config, + allowed_planning_time=10.0, + num_planning_attempts=20, + orientation_constraint=None, + replan=True, + replan_attempts=1, + joint_tolerance=0.01, + position_tolerance=0.001, + orientation_tolerance=0.01, + startpoint = 'home', + goal_type='pose', + collision_free=False): + """ Computes a path for the end-effector to a pose from startpoint, using motion planning. + + Args: + goal_config: geometry_msgs/PoseStamped, list of joint values, sensor_msgs/JointState + The goal configuration for the end-effector. + allowed_planning_time: float. + The maximum duration to wait for a planning result. + num_planning_attempts: int. + The number of times to compute the same plan. + The shortest path is ultimately used. + For random planners, this can help get shorter, less weird paths. + orientation_constraint: moveit_msgs/OrientationConstraint. + An orientation constraint for the entire path. + replan: bool. + If True, then if an execution fails (while the arm is moving), + then come up with a new plan and execute it. + replan_attempts: int. + How many times to replan if the execution fails. + joint_tolerance: float. + The goal joint tolerance, in radians. + position_tolerance: float. + The goal position tolerance, in meters. + orientation_tolerance: float. + The goal orientation tolerance, in radians. 1 degree = 0.017 rad + startpoint: string. + The startpoint for motion planning. If any other than 'home', current robot state will be startpoint + + Returns: + moveit_msgs/RobotTrajectory.msg if path exists, else None. """ + + goal_config = from_msg_msg(goal_config) + + self.move_group.set_end_effector_link("epick_end_effector") + self.move_group.set_planning_time(allowed_planning_time) + self.move_group.set_num_planning_attempts(num_planning_attempts) + self.move_group.allow_replanning(replan) + self.move_group.set_goal_joint_tolerance(joint_tolerance) + self.move_group.set_goal_position_tolerance(position_tolerance) + self.move_group.set_goal_orientation_tolerance(orientation_tolerance) + if startpoint == 'home': + self.move_group.set_start_state(self.home_state) + else: + self.move_group.set_start_state_to_current_state() + success = False + if goal_type == 'joint': + self.move_group.set_joint_value_target(goal_config) + success, plan, planning_time, error_code = self.move_group.plan() + elif goal_type == 'pose': + if collision_free: + for attempt_id in range(replan_attempts): + goal_joint_state = self.compute_ik(goal_config, timeout=rospy.Duration(5)) + if goal_joint_state is not None: + # print(f"goal_joint_values:\n{goal_joint_state.position}") + self.move_group.set_joint_value_target(goal_joint_state) + success, plan, planning_time, error_code = self.move_group.plan() + if success: + break + else: + self.move_group.set_pose_target(goal_config) + success, plan, planning_time, error_code = self.move_group.plan() + + # It is always good to clear your targets after planning with poses. + # Note: there is no equivalent function for clear_joint_value_targets() + self.move_group.clear_pose_targets() + + if not success: + return None + return tuple([plan, planning_time]) + + + def execute_path(self, path, goal_pose=None, full_cycle=False): + """ Executes the path generated by compute_path() method + + Args: + path: moveit_msgs/RobotTrajectory.msg. + The path + goal_pose: geometry_msgs/PoseStamped. + The goal pose for the end-effector. + full_cycle: bool. + If True, executes the full cycle, i.e. returns the robot along the reversed path """ + + ret = self.move_group.execute(path, wait=True) # Call the planner to compute the reversed path and execute it. + self.move_group.stop() # Calling 'stop()' ensures that there is no residual movement + + # print(f"\ngoal_pose_in_bin_frame:\n{goal_pose.pose}") + if goal_pose is not None: + # Transform pose_stamped in bin's referrence frame to self.planning_frame (= world) + goal_in_planning_frame = self.tf2_buffer.transform(goal_pose, self.planning_frame, rospy.Duration(1)) + # print(f"\ngoal_pose_in_planning_frame:\n{goal_in_planning_frame.pose}") + current_pose = self.move_group.get_current_pose() + # print(f"\ncurrent_pose:\n{current_pose.pose}") + pose_diff = pose_dist(goal_in_planning_frame, current_pose) + print(f"\nposes_difference:\n\tdistance: {pose_diff[0]:2.4f} meters\n\t angle: {pose_diff[1]:2.4f} degrees\n") + + if full_cycle: + # Return the robot to original pose, i.e. reverse the path + path.joint_trajectory.points.reverse() + total_duration = path.joint_trajectory.points[0].time_from_start + for point in path.joint_trajectory.points: + point.time_from_start = total_duration - point.time_from_start # point.time_from_start is reversed, decreasing over path, while it has to increase + point.velocities = (np.array(point.velocities) * (-1)).tolist() # point.velocities/accelerations should be inverted to get smooth reversed path + point.accelerations = (np.array(point.accelerations) * (-1)).tolist() + ret = self.move_group.execute(path, wait=True) # Call the planner to compute the reversed path and execute it. + self.move_group.stop() # Calling `stop()` ensures that there is no residual movement + + + def wrap_angle(self, angle:float, limit:float): + # wrap angle into range [-limit, limit) + return (angle - limit) % (2*limit) - limit + def cancel_all_goals(self): self._move_group_client.cancel_all_goals() self._joint_traj_client.cancel_all_goals() + + + def remove_pod_collision_geometry(self): + # self.scene.remove_world_object() # if no name is provided, then all objects are removed from the scene + self.scene.clear() # There is no such function as moveit_commander.planning_scene_interface.PlanningSceneInterface.clear() aka scene.clear(), but this function works! + + + def add_pod_collision_geometry(self, pod_id: str='1A', include_backwall: bool=False, include_flaps: bool=True): + ''' + Adds collision boxes for each bin into MoveIt planning scene depending on pod's id + + pod_id = 1A: pod of 13x4 small bins, the current + pod_id = 1B: pod of 8x2 large bins, to the right of the current + pod_id = 1C: pod of 12x4 small bins, opposite to the current + pod_id = 1D: pod of 7x2 large bins, to the left of the current + + pod_id = 2A: pod of 8x3 large bins, used by Henry + pod_id = 2B: pod of 8x1 large bins, used by Varad and Michael + pod_id = 2C: pod of 8x3 large bins, opposite to the one used by Henry + pod_id = 2B: pod of 7x1 large bins, opposite to the one used by Varad and Michael + ''' + POD_BRACE_WIDTH = self.pod_sizes["general"]["brace_width"] + POD_BRACE_FRAME_WIDTH = self.pod_sizes["general"]["brace_frame_width"] + POD_BRACE_FRAME_THICKNESS = self.pod_sizes["general"]["brace_frame_thickness"] + POD_BIN_FLAP_HEIGHT = self.pod_sizes["general"]["bin_flap_height"] + POD_BIN_WALL_THICKNESS = self.pod_sizes["general"]["bin_wall_thickness"] + + POD_BIN_HEIGHTS = self.pod_sizes[pod_id]["bin_heights"] + POD_BIN_WIDTH = self.pod_sizes[pod_id]["bin_width"] + POD_BOTTOM_BIN_WIDTH = self.pod_sizes[pod_id]["bottom_bin_width"] + POD_BIN_DEPTH = self.pod_sizes[pod_id]["bin_depth"] + POD_TOP_BIN_DEPTH = self.pod_sizes[pod_id]["top_bin_depth"] + POD_BIN_BOTTOM_THICKNESS = self.pod_sizes[pod_id]["bin_bottom_thickness"] + + NUM_ROWS = len(POD_BIN_HEIGHTS) + if pod_id[1] == 'B' or pod_id[1] == 'D': + if pod_id[0] == '1': + X_OFFSET = POD_BRACE_WIDTH/2 - POD_BIN_WIDTH + elif pod_id[0] == '2': + X_OFFSET = (POD_BRACE_WIDTH - POD_BIN_WIDTH)/2 + NUM_COLUMNS = 1 + elif pod_id[1] == 'A' or pod_id[1] == 'C': + X_OFFSET = 0 + if pod_id[0] == '1': + NUM_COLUMNS = 4 + elif pod_id[0] == '2': + NUM_COLUMNS = 3 + + X_AXIS_QUAT = Quaternion(x=0, y=0, z=0, w=1) + + # Clear the scene from collision boxes + self.remove_pod_collision_geometry() + + # SETUP POD BIN'S COLLISION BOXES + Z_OFFSET = 0 + for row_id in range(NUM_ROWS): + if pod_id[1] == 'B' or pod_id[1] == 'D': + BIN_DEPTH = POD_TOP_BIN_DEPTH if row_id == (NUM_ROWS - 1) else POD_BIN_DEPTH + if pod_id[0] == '1' and row_id == 0: + BIN_WIDTH = POD_BOTTOM_BIN_WIDTH + NUM_COLUMNS = 1 + else: + BIN_WIDTH = POD_BIN_WIDTH + if pod_id[0] == '1': + NUM_COLUMNS = 2 + elif pod_id[0] == '2': + NUM_COLUMNS = 1 + elif pod_id[1] == 'A' or pod_id[1] == 'C': + BIN_WIDTH = POD_BIN_WIDTH + BIN_DEPTH = POD_BIN_DEPTH + if pod_id[0] == '1': + NUM_COLUMNS = 4 + elif pod_id[0] == '2': + NUM_COLUMNS = 3 + + # LEFT & RIGHT MARGINS (Applicable to B and C faces of pods) + if X_OFFSET != 0: + self.scene.add_box(name=f"left_margin_row{row_id+1:02d}", + pose=PoseStamped(header=std_msgs.msg.Header(frame_id="pod_fabric_base"), + pose=Pose(position=Point(x=X_OFFSET/2, + y=POD_BIN_WALL_THICKNESS/2, + z=Z_OFFSET + POD_BIN_HEIGHTS[row_id]/2), orientation=X_AXIS_QUAT) + ), + size=(X_OFFSET, POD_BIN_WALL_THICKNESS, POD_BIN_HEIGHTS[row_id])) + self.scene.add_box(name=f"right_margin_row{row_id+1:02d}", + pose=PoseStamped(header=std_msgs.msg.Header(frame_id="pod_fabric_base"), + pose=Pose(position=Point(x=POD_BRACE_WIDTH - X_OFFSET/2, + y=POD_BIN_WALL_THICKNESS/2, + z=Z_OFFSET + POD_BIN_HEIGHTS[row_id]/2), orientation=X_AXIS_QUAT) + ), + size=(X_OFFSET, POD_BIN_WALL_THICKNESS, POD_BIN_HEIGHTS[row_id])) + + for col_id in range(NUM_COLUMNS): + # LEFT WALL + self.scene.add_box(name=f"left_wall_row{row_id+1:02d}_col{col_id+1:1d}", + pose=PoseStamped(header=std_msgs.msg.Header(frame_id="pod_fabric_base"), + pose=Pose(position=Point(x=X_OFFSET + col_id * BIN_WIDTH + POD_BIN_WALL_THICKNESS/2, + y=BIN_DEPTH/2, + z=Z_OFFSET + POD_BIN_HEIGHTS[row_id]/2), orientation=X_AXIS_QUAT) + ), + size=(POD_BIN_WALL_THICKNESS, BIN_DEPTH, POD_BIN_HEIGHTS[row_id])) + # RIGHT WALL + if col_id == NUM_COLUMNS - 1: + self.scene.add_box(name=f"right_wall_row{row_id+1:02d}_col{col_id+1:1d}", + pose=PoseStamped(header=std_msgs.msg.Header(frame_id="pod_fabric_base"), + pose=Pose(position=Point(x=X_OFFSET + (col_id + 1) * BIN_WIDTH - POD_BIN_WALL_THICKNESS/2, + y=BIN_DEPTH/2, + z=Z_OFFSET + POD_BIN_HEIGHTS[row_id]/2), orientation=X_AXIS_QUAT) + ), + size=(POD_BIN_WALL_THICKNESS, BIN_DEPTH, POD_BIN_HEIGHTS[row_id])) + # BOTTOM + self.scene.add_box(name=f"bottom_row{row_id+1:02d}_col{col_id+1:1d}", + pose=PoseStamped(header=std_msgs.msg.Header(frame_id="pod_fabric_base"), + pose=Pose(position=Point(x=X_OFFSET + (2*col_id + 1) * BIN_WIDTH/2, + y=BIN_DEPTH/2, + z=Z_OFFSET + POD_BIN_BOTTOM_THICKNESS/2), orientation=X_AXIS_QUAT) + ), + size=(BIN_WIDTH, BIN_DEPTH, POD_BIN_BOTTOM_THICKNESS)) + # TOP + self.scene.add_box(name=f"top_row{row_id+1:02d}_col{col_id+1:1d}", + pose=PoseStamped(header=std_msgs.msg.Header(frame_id="pod_fabric_base"), + pose=Pose(position=Point(x=X_OFFSET + (2*col_id + 1) * BIN_WIDTH/2, + y=BIN_DEPTH/2, + z=Z_OFFSET + POD_BIN_HEIGHTS[row_id] - POD_BIN_WALL_THICKNESS/2), orientation=X_AXIS_QUAT) + ), + size=(BIN_WIDTH, BIN_DEPTH, POD_BIN_WALL_THICKNESS)) + # BACK WALL + if include_backwall: + self.scene.add_box(name=f"back_wall_row{row_id+1:02d}_col{col_id+1:1d}", + pose=PoseStamped(header=std_msgs.msg.Header(frame_id="pod_fabric_base"), + pose=Pose(position=Point(x=X_OFFSET + (2*col_id + 1) * BIN_WIDTH/2, + y=BIN_DEPTH - POD_BIN_WALL_THICKNESS/2, + z=Z_OFFSET + POD_BIN_HEIGHTS[row_id]/2), orientation=X_AXIS_QUAT) + ), + size=(BIN_WIDTH, POD_BIN_WALL_THICKNESS, POD_BIN_HEIGHTS[row_id])) + # FLAPS + if include_flaps: + self.scene.add_box(name=f"flap_row{row_id+1:02d}_col{col_id+1:1d}", + pose=PoseStamped(header=std_msgs.msg.Header(frame_id="pod_fabric_base"), + pose=Pose(position=Point(x=X_OFFSET + (2*col_id + 1) * BIN_WIDTH/2, + y=POD_BIN_WALL_THICKNESS/2, + z=Z_OFFSET + POD_BIN_FLAP_HEIGHT/2), orientation=X_AXIS_QUAT) + ), + size=(BIN_WIDTH, POD_BIN_WALL_THICKNESS, POD_BIN_FLAP_HEIGHT)) + + Z_OFFSET += POD_BIN_HEIGHTS[row_id] + + # LEFTSIDE BRACE FRAME + self.scene.add_box(name=f"brace_leftside_frame", + pose=PoseStamped(header=std_msgs.msg.Header(frame_id="pod_fabric_base"), + pose=Pose(position=Point(x=1/2 * POD_BRACE_FRAME_WIDTH, + y=1/2 * POD_BRACE_FRAME_THICKNESS, + z=1/2 * sum(POD_BIN_HEIGHTS)), orientation=X_AXIS_QUAT) + ), + size=(POD_BRACE_FRAME_WIDTH, POD_BRACE_FRAME_THICKNESS, sum(POD_BIN_HEIGHTS))) + # RIGHTSIDE BRACE FRAME + self.scene.add_box(name=f"brace_side_frame", + pose=PoseStamped(header=std_msgs.msg.Header(frame_id="pod_fabric_base"), + pose=Pose(position=Point(x=POD_BRACE_WIDTH - 1/2 * POD_BRACE_FRAME_WIDTH, + y=1/2 * POD_BRACE_FRAME_THICKNESS, + z=1/2 * sum(POD_BIN_HEIGHTS)), orientation=X_AXIS_QUAT) + ), + size=(POD_BRACE_FRAME_WIDTH, POD_BRACE_FRAME_THICKNESS, sum(POD_BIN_HEIGHTS))) + + + diff --git a/aurmr_tasks/src/aurmr_tasks/pod.py b/aurmr_tasks/src/aurmr_tasks/pod.py index 89d577e..8f5445f 100644 --- a/aurmr_tasks/src/aurmr_tasks/pod.py +++ b/aurmr_tasks/src/aurmr_tasks/pod.py @@ -9,11 +9,16 @@ import aurmr_tasks.common.control_flow as cf from aurmr_tasks.util import formulate_ud_str_auto -BIN_IDS = [] +BIN_IDS_1 = [] +BIN_IDS_2 = [] for letter in "abcdefghijklm": for i in range(4): - BIN_IDS.append(f"{i + 1}{letter}") + BIN_IDS_1.append(f"{i + 1}{letter}") + +for letter in "abcdefgh": + for i in range(3): + BIN_IDS_2.append(f"{i + 1}{letter}") def load_sm(stows): diff --git a/aurmr_tasks/src/aurmr_tasks/util.py b/aurmr_tasks/src/aurmr_tasks/util.py index d8a8f2d..a3fb2cf 100644 --- a/aurmr_tasks/src/aurmr_tasks/util.py +++ b/aurmr_tasks/src/aurmr_tasks/util.py @@ -1,7 +1,7 @@ from copy import deepcopy import geometry_msgs.msg -from math import pi, tau, dist, fabs, cos +from math import pi, tau, dist, fabs, acos import rospy from geometry_msgs.msg import PoseStamped @@ -48,33 +48,38 @@ def formulate_ud_str_auto(name, template, input_keys, output_key, transitions=No StateMachine.add_auto(name, Formulator(template, input_keys, output_key), ["succeeded"],transitions=transitions) -def all_close(goal, actual, tolerance): +def all_close(goal, actual, joint_tolerance=0.01, position_tolerance=0.001, orientation_tolerance=1.0): """ Convenience method for testing if the values in two lists are within a tolerance of each other. For Pose and PoseStamped inputs, the angle between the two quaternions is compared (the angle between the identical orientations q and -q is calculated correctly). - @param: goal A list of floats, a Pose or a PoseStamped - @param: actual A list of floats, a Pose or a PoseStamped - @param: tolerance A float + @param: goal: A list of floats, a Pose or a PoseStamped + @param: actual: A list of floats, a Pose or a PoseStamped + @param: joint_tolerance: A float. radians + @param: position_tolerance: A float. meters + @param: orientation_tolerance: A float. radians @returns: bool """ if type(goal) is list: for index in range(len(goal)): - if abs(actual[index] - goal[index]) > tolerance: + if abs(actual[index] - goal[index]) > joint_tolerance: return False elif type(goal) is geometry_msgs.msg.PoseStamped: assert (goal.header.frame_id == actual.header.frame_id) - return all_close(goal.pose, actual.pose, tolerance) + return all_close(goal.pose, actual.pose, + position_tolerance=position_tolerance, + orientation_tolerance=orientation_tolerance) elif type(goal) is geometry_msgs.msg.Pose: x0, y0, z0, qx0, qy0, qz0, qw0 = pose_to_list(actual) x1, y1, z1, qx1, qy1, qz1, qw1 = pose_to_list(goal) # Euclidean distance d = dist((x1, y1, z1), (x0, y0, z0)) - # phi = angle between orientations - cos_phi_half = fabs(qx0 * qx1 + qy0 * qy1 + qz0 * qz1 + qw0 * qw1) - return d <= tolerance and cos_phi_half >= cos(tolerance / 2.0) + # phi = angle between orientations in degrees + # cos_phi_half = fabs(qx0 * qx1 + qy0 * qy1 + qz0 * qz1 + qw0 * qw1) + phi = acos(fabs(qx0 * qx1 + qy0 * qy1 + qz0 * qz1 + qw0 * qw1)) + return (d <= position_tolerance) and (phi <= orientation_tolerance) return True @@ -89,7 +94,7 @@ def pose_dist(goal, actual): # Euclidean distance d = dist((x1, y1, z1), (x0, y0, z0)) # phi = angle between orientations - cos_phi_half = fabs(qx0 * qx1 + qy0 * qy1 + qz0 * qz1 + qw0 * qw1) - return (d, cos_phi_half) + phi = acos(fabs(qx0 * qx1 + qy0 * qy1 + qz0 * qz1 + qw0 * qw1)) * 180 / pi + return (d, phi) else: return None diff --git a/robotiq b/robotiq index 083815b..8178af5 160000 --- a/robotiq +++ b/robotiq @@ -1 +1 @@ -Subproject commit 083815b2c51ed2dc7423bcdc09174ffb020ab737 +Subproject commit 8178af5728fa024344436f86b97bed1525f3e446 diff --git a/tahoma_description/config/end_effector_sizes.yaml b/tahoma_description/config/end_effector_sizes.yaml new file mode 100644 index 0000000..7087315 --- /dev/null +++ b/tahoma_description/config/end_effector_sizes.yaml @@ -0,0 +1,15 @@ +{ + robotiq_epick_dimX: 0.0750, + robotiq_epick_dimY: 0.0830, + robotiq_epick_dimZ: 0.1047, + robotiq_epick_finH: 0.0024, + robotiq_epick_earH: 0.0125, + robotiq_epick_earR: 0.0059, + epick_cylinder_H: 0.0900, + epick_cylinder_R: 0.0059, + epick_end_effect_H: 0.0100, + epick_end_effect_R: 0.0225, + coupling_D: 0.0700, + coupling_H: 0.0200, + coupling_h: 0.0030, +} \ No newline at end of file diff --git a/tahoma_description/config/pod1_recipe_790-00265-021_2.txt b/tahoma_description/config/pod1_recipe_790-00265-021_2.txt new file mode 100644 index 0000000..5c0afe0 --- /dev/null +++ b/tahoma_description/config/pod1_recipe_790-00265-021_2.txt @@ -0,0 +1,2433 @@ +{ + "id": { + "partNumber": "790-00265-021", + "recipeVersion": 2 + }, + "partNumber": "790-00265-021", + "barcode": "RCP79000265021", + "podTypeId": 400, + "podProfileId": 2, + "emptyMassKg": 68, + "emptyCGHeightMm": 1041, + "heightWhenLifted": 291, + "bins": [ + { + "binDimensionIndex": 0, + "binDimensionName": "A1610", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 0, + "inset": 0, + "primaryFace": "A", + "shelf": 1, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 0, + "binDimensionName": "A1610", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 0, + "inset": 0, + "primaryFace": "A", + "shelf": 1, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 0, + "binDimensionName": "A1610", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 0, + "inset": 0, + "primaryFace": "A", + "shelf": 1, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 0, + "binDimensionName": "A1610", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 0, + "inset": 0, + "primaryFace": "A", + "shelf": 1, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A24812", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 229, + "inset": 0, + "primaryFace": "A", + "shelf": 2, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A24812", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 229, + "inset": 0, + "primaryFace": "A", + "shelf": 2, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A24812", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 229, + "inset": 0, + "primaryFace": "A", + "shelf": 2, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A24812", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 229, + "inset": 0, + "primaryFace": "A", + "shelf": 2, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 2, + "binDimensionName": "A3", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 381, + "inset": 0, + "primaryFace": "A", + "shelf": 4, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 2, + "binDimensionName": "A3", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 381, + "inset": 0, + "primaryFace": "A", + "shelf": 4, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 2, + "binDimensionName": "A3", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 381, + "inset": 0, + "primaryFace": "A", + "shelf": 4, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 2, + "binDimensionName": "A3", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 381, + "inset": 0, + "primaryFace": "A", + "shelf": 4, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A24812", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 584, + "inset": 0, + "primaryFace": "A", + "shelf": 5, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A24812", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 584, + "inset": 0, + "primaryFace": "A", + "shelf": 5, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A24812", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 584, + "inset": 0, + "primaryFace": "A", + "shelf": 5, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A24812", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 584, + "inset": 0, + "primaryFace": "A", + "shelf": 5, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A57911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 737, + "inset": 0, + "primaryFace": "A", + "shelf": 8, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A57911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 737, + "inset": 0, + "primaryFace": "A", + "shelf": 8, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A57911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 737, + "inset": 0, + "primaryFace": "A", + "shelf": 8, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A57911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 737, + "inset": 0, + "primaryFace": "A", + "shelf": 8, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 0, + "binDimensionName": "A1610", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 864, + "inset": 0, + "primaryFace": "A", + "shelf": 11, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 0, + "binDimensionName": "A1610", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 864, + "inset": 0, + "primaryFace": "A", + "shelf": 11, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 0, + "binDimensionName": "A1610", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 864, + "inset": 0, + "primaryFace": "A", + "shelf": 11, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 0, + "binDimensionName": "A1610", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 864, + "inset": 0, + "primaryFace": "A", + "shelf": 11, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A57911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 1092, + "inset": 0, + "primaryFace": "A", + "shelf": 14, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A57911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 1092, + "inset": 0, + "primaryFace": "A", + "shelf": 14, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A57911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 1092, + "inset": 0, + "primaryFace": "A", + "shelf": 14, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A57911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 1092, + "inset": 0, + "primaryFace": "A", + "shelf": 14, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A24812", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 1219, + "inset": 0, + "primaryFace": "A", + "shelf": 17, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "H", + "colorLevel": 8 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A24812", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 1219, + "inset": 0, + "primaryFace": "A", + "shelf": 17, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "H", + "colorLevel": 8 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A24812", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 1219, + "inset": 0, + "primaryFace": "A", + "shelf": 17, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "H", + "colorLevel": 8 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A24812", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 1219, + "inset": 0, + "primaryFace": "A", + "shelf": 17, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "H", + "colorLevel": 8 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A57911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 1372, + "inset": 0, + "primaryFace": "A", + "shelf": 20, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "I", + "colorLevel": 9 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A57911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 1372, + "inset": 0, + "primaryFace": "A", + "shelf": 20, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "I", + "colorLevel": 9 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A57911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 1372, + "inset": 0, + "primaryFace": "A", + "shelf": 20, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "I", + "colorLevel": 9 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A57911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 1372, + "inset": 0, + "primaryFace": "A", + "shelf": 20, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "I", + "colorLevel": 9 + }, + { + "binDimensionIndex": 0, + "binDimensionName": "A1610", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 1499, + "inset": 0, + "primaryFace": "A", + "shelf": 22, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "J", + "colorLevel": 10 + }, + { + "binDimensionIndex": 0, + "binDimensionName": "A1610", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 1499, + "inset": 0, + "primaryFace": "A", + "shelf": 22, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "J", + "colorLevel": 10 + }, + { + "binDimensionIndex": 0, + "binDimensionName": "A1610", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 1499, + "inset": 0, + "primaryFace": "A", + "shelf": 22, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "J", + "colorLevel": 10 + }, + { + "binDimensionIndex": 0, + "binDimensionName": "A1610", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 1499, + "inset": 0, + "primaryFace": "A", + "shelf": 22, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "J", + "colorLevel": 10 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A57911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 1727, + "inset": 0, + "primaryFace": "A", + "shelf": 25, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "K", + "colorLevel": 11 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A57911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 1727, + "inset": 0, + "primaryFace": "A", + "shelf": 25, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "K", + "colorLevel": 11 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A57911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 1727, + "inset": 0, + "primaryFace": "A", + "shelf": 25, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "K", + "colorLevel": 11 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A57911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 1727, + "inset": 0, + "primaryFace": "A", + "shelf": 25, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "K", + "colorLevel": 11 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A24812", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 1854, + "inset": 0, + "primaryFace": "A", + "shelf": 26, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "L", + "colorLevel": 12 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A24812", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 1854, + "inset": 0, + "primaryFace": "A", + "shelf": 26, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "L", + "colorLevel": 12 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A24812", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 1854, + "inset": 0, + "primaryFace": "A", + "shelf": 26, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "L", + "colorLevel": 12 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A24812", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 1854, + "inset": 0, + "primaryFace": "A", + "shelf": 26, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "L", + "colorLevel": 12 + }, + { + "binDimensionIndex": 4, + "binDimensionName": "A13", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 2007, + "inset": 0, + "primaryFace": "A", + "shelf": 28, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "M", + "colorLevel": 13 + }, + { + "binDimensionIndex": 4, + "binDimensionName": "A13", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 2007, + "inset": 0, + "primaryFace": "A", + "shelf": 28, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "M", + "colorLevel": 13 + }, + { + "binDimensionIndex": 4, + "binDimensionName": "A13", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 2007, + "inset": 0, + "primaryFace": "A", + "shelf": 28, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "M", + "colorLevel": 13 + }, + { + "binDimensionIndex": 4, + "binDimensionName": "A13", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 2007, + "inset": 0, + "primaryFace": "A", + "shelf": 28, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "M", + "colorLevel": 13 + }, + { + "binDimensionIndex": 5, + "binDimensionName": "B1", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 154, + "y": 0, + "inset": 0, + "primaryFace": "B", + "shelf": 1, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 6, + "binDimensionName": "B27", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 154, + "y": 356, + "inset": 0, + "primaryFace": "B", + "shelf": 3, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 6, + "binDimensionName": "B27", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 471, + "y": 356, + "inset": 0, + "primaryFace": "B", + "shelf": 3, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 7, + "binDimensionName": "B35", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 154, + "y": 610, + "inset": 0, + "primaryFace": "B", + "shelf": 6, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 7, + "binDimensionName": "B35", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 471, + "y": 610, + "inset": 0, + "primaryFace": "B", + "shelf": 6, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 8, + "binDimensionName": "B4", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 154, + "y": 838, + "inset": 0, + "primaryFace": "B", + "shelf": 10, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 8, + "binDimensionName": "B4", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 471, + "y": 838, + "inset": 0, + "primaryFace": "B", + "shelf": 10, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 7, + "binDimensionName": "B35", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 154, + "y": 1143, + "inset": 0, + "primaryFace": "B", + "shelf": 16, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 7, + "binDimensionName": "B35", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 471, + "y": 1143, + "inset": 0, + "primaryFace": "B", + "shelf": 16, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 9, + "binDimensionName": "B6", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 154, + "y": 1372, + "inset": 0, + "primaryFace": "B", + "shelf": 20, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 9, + "binDimensionName": "B6", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 471, + "y": 1372, + "inset": 0, + "primaryFace": "B", + "shelf": 20, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 6, + "binDimensionName": "B27", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 154, + "y": 1702, + "inset": 0, + "primaryFace": "B", + "shelf": 24, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 6, + "binDimensionName": "B27", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 471, + "y": 1702, + "inset": 0, + "primaryFace": "B", + "shelf": 24, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 10, + "binDimensionName": "B8", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 154, + "y": 1956, + "inset": 0, + "primaryFace": "B", + "shelf": 27, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "H", + "colorLevel": 8 + }, + { + "binDimensionIndex": 10, + "binDimensionName": "B8", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 471, + "y": 1956, + "inset": 0, + "primaryFace": "B", + "shelf": 27, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "H", + "colorLevel": 8 + }, + { + "binDimensionIndex": 11, + "binDimensionName": "C13810", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 0, + "inset": 0, + "primaryFace": "C", + "shelf": 1, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 11, + "binDimensionName": "C13810", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 0, + "inset": 0, + "primaryFace": "C", + "shelf": 1, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 11, + "binDimensionName": "C13810", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 0, + "inset": 0, + "primaryFace": "C", + "shelf": 1, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 11, + "binDimensionName": "C13810", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 0, + "inset": 0, + "primaryFace": "C", + "shelf": 1, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 229, + "inset": 0, + "primaryFace": "C", + "shelf": 2, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 229, + "inset": 0, + "primaryFace": "C", + "shelf": 2, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 229, + "inset": 0, + "primaryFace": "C", + "shelf": 2, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 229, + "inset": 0, + "primaryFace": "C", + "shelf": 2, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 11, + "binDimensionName": "C13810", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 381, + "inset": 0, + "primaryFace": "C", + "shelf": 4, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 11, + "binDimensionName": "C13810", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 381, + "inset": 0, + "primaryFace": "C", + "shelf": 4, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 11, + "binDimensionName": "C13810", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 381, + "inset": 0, + "primaryFace": "C", + "shelf": 4, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 11, + "binDimensionName": "C13810", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 381, + "inset": 0, + "primaryFace": "C", + "shelf": 4, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 610, + "inset": 0, + "primaryFace": "C", + "shelf": 6, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 610, + "inset": 0, + "primaryFace": "C", + "shelf": 6, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 610, + "inset": 0, + "primaryFace": "C", + "shelf": 6, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 610, + "inset": 0, + "primaryFace": "C", + "shelf": 6, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 13, + "binDimensionName": "C5", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 762, + "inset": 0, + "primaryFace": "C", + "shelf": 9, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 13, + "binDimensionName": "C5", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 762, + "inset": 0, + "primaryFace": "C", + "shelf": 9, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 13, + "binDimensionName": "C5", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 762, + "inset": 0, + "primaryFace": "C", + "shelf": 9, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 13, + "binDimensionName": "C5", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 762, + "inset": 0, + "primaryFace": "C", + "shelf": 9, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 965, + "inset": 0, + "primaryFace": "C", + "shelf": 12, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 965, + "inset": 0, + "primaryFace": "C", + "shelf": 12, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 965, + "inset": 0, + "primaryFace": "C", + "shelf": 12, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 965, + "inset": 0, + "primaryFace": "C", + "shelf": 12, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 14, + "binDimensionName": "C7", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 1118, + "inset": 0, + "primaryFace": "C", + "shelf": 15, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 14, + "binDimensionName": "C7", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 1118, + "inset": 0, + "primaryFace": "C", + "shelf": 15, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 14, + "binDimensionName": "C7", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 1118, + "inset": 0, + "primaryFace": "C", + "shelf": 15, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 14, + "binDimensionName": "C7", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 1118, + "inset": 0, + "primaryFace": "C", + "shelf": 15, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 11, + "binDimensionName": "C13810", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 1245, + "inset": 0, + "primaryFace": "C", + "shelf": 18, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "H", + "colorLevel": 8 + }, + { + "binDimensionIndex": 11, + "binDimensionName": "C13810", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 1245, + "inset": 0, + "primaryFace": "C", + "shelf": 18, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "H", + "colorLevel": 8 + }, + { + "binDimensionIndex": 11, + "binDimensionName": "C13810", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 1245, + "inset": 0, + "primaryFace": "C", + "shelf": 18, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "H", + "colorLevel": 8 + }, + { + "binDimensionIndex": 11, + "binDimensionName": "C13810", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 1245, + "inset": 0, + "primaryFace": "C", + "shelf": 18, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "H", + "colorLevel": 8 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 1473, + "inset": 0, + "primaryFace": "C", + "shelf": 21, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "I", + "colorLevel": 9 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 1473, + "inset": 0, + "primaryFace": "C", + "shelf": 21, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "I", + "colorLevel": 9 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 1473, + "inset": 0, + "primaryFace": "C", + "shelf": 21, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "I", + "colorLevel": 9 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 1473, + "inset": 0, + "primaryFace": "C", + "shelf": 21, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "I", + "colorLevel": 9 + }, + { + "binDimensionIndex": 11, + "binDimensionName": "C13810", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 1626, + "inset": 0, + "primaryFace": "C", + "shelf": 23, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "J", + "colorLevel": 10 + }, + { + "binDimensionIndex": 11, + "binDimensionName": "C13810", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 1626, + "inset": 0, + "primaryFace": "C", + "shelf": 23, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "J", + "colorLevel": 10 + }, + { + "binDimensionIndex": 11, + "binDimensionName": "C13810", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 1626, + "inset": 0, + "primaryFace": "C", + "shelf": 23, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "J", + "colorLevel": 10 + }, + { + "binDimensionIndex": 11, + "binDimensionName": "C13810", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 1626, + "inset": 0, + "primaryFace": "C", + "shelf": 23, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "J", + "colorLevel": 10 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 1854, + "inset": 0, + "primaryFace": "C", + "shelf": 26, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "K", + "colorLevel": 11 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 1854, + "inset": 0, + "primaryFace": "C", + "shelf": 26, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "K", + "colorLevel": 11 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 1854, + "inset": 0, + "primaryFace": "C", + "shelf": 26, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "K", + "colorLevel": 11 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "C246911", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 1854, + "inset": 0, + "primaryFace": "C", + "shelf": 26, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "K", + "colorLevel": 11 + }, + { + "binDimensionIndex": 15, + "binDimensionName": "C12", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 2007, + "inset": 0, + "primaryFace": "C", + "shelf": 28, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "L", + "colorLevel": 12 + }, + { + "binDimensionIndex": 15, + "binDimensionName": "C12", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 234, + "y": 2007, + "inset": 0, + "primaryFace": "C", + "shelf": 28, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "L", + "colorLevel": 12 + }, + { + "binDimensionIndex": 15, + "binDimensionName": "C12", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 468, + "y": 2007, + "inset": 0, + "primaryFace": "C", + "shelf": 28, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "L", + "colorLevel": 12 + }, + { + "binDimensionIndex": 15, + "binDimensionName": "C12", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 702, + "y": 2007, + "inset": 0, + "primaryFace": "C", + "shelf": 28, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 4, + "vendingMachineLetter": "L", + "colorLevel": 12 + }, + { + "binDimensionIndex": 16, + "binDimensionName": "D1", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 154, + "y": 0, + "inset": 0, + "primaryFace": "D", + "shelf": 1, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 17, + "binDimensionName": "D2356", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 154, + "y": 356, + "inset": 0, + "primaryFace": "D", + "shelf": 3, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 17, + "binDimensionName": "D2356", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 471, + "y": 356, + "inset": 0, + "primaryFace": "D", + "shelf": 3, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 17, + "binDimensionName": "D2356", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 154, + "y": 686, + "inset": 0, + "primaryFace": "D", + "shelf": 7, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 17, + "binDimensionName": "D2356", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 471, + "y": 686, + "inset": 0, + "primaryFace": "D", + "shelf": 7, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 18, + "binDimensionName": "D4", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 154, + "y": 1016, + "inset": 0, + "primaryFace": "D", + "shelf": 13, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 18, + "binDimensionName": "D4", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 471, + "y": 1016, + "inset": 0, + "primaryFace": "D", + "shelf": 13, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 17, + "binDimensionName": "D2356", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 154, + "y": 1295, + "inset": 0, + "primaryFace": "D", + "shelf": 19, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 17, + "binDimensionName": "D2356", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 471, + "y": 1295, + "inset": 0, + "primaryFace": "D", + "shelf": 19, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 17, + "binDimensionName": "D2356", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 154, + "y": 1626, + "inset": 0, + "primaryFace": "D", + "shelf": 23, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 17, + "binDimensionName": "D2356", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 471, + "y": 1626, + "inset": 0, + "primaryFace": "D", + "shelf": 23, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 19, + "binDimensionName": "D7", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 154, + "y": 1956, + "inset": 0, + "primaryFace": "D", + "shelf": 27, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 19, + "binDimensionName": "D7", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 471, + "y": 1956, + "inset": 0, + "primaryFace": "D", + "shelf": 27, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "G", + "colorLevel": 7 + } + ], + "binDimensions": [ + { + "name": "A1610", + "width": 234, + "height": 223, + "depth": 152, + "angle": 0, + "lipHeight": 26, + "drawer": false, + "minProductDepth": 0, + "maxMass": 8 + }, + { + "name": "A24812", + "width": 234, + "height": 146, + "depth": 152, + "angle": 0, + "lipHeight": 26, + "drawer": false, + "minProductDepth": 0, + "maxMass": 4 + }, + { + "name": "A3", + "width": 234, + "height": 197, + "depth": 152, + "angle": 0, + "lipHeight": 26, + "drawer": false, + "minProductDepth": 0, + "maxMass": 4 + }, + { + "name": "A57911", + "width": 234, + "height": 121, + "depth": 152, + "angle": 0, + "lipHeight": 26, + "drawer": false, + "minProductDepth": 0, + "maxMass": 4 + }, + { + "name": "A13", + "width": 234, + "height": 261, + "depth": 152, + "angle": 0, + "lipHeight": 26, + "drawer": false, + "minProductDepth": 0, + "maxMass": 4 + }, + { + "name": "B1", + "width": 633, + "height": 346, + "depth": 470, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 65 + }, + { + "name": "B27", + "width": 316, + "height": 244, + "depth": 470, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 15 + }, + { + "name": "B35", + "width": 316, + "height": 219, + "depth": 470, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 15 + }, + { + "name": "B4", + "width": 316, + "height": 295, + "depth": 470, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 15 + }, + { + "name": "B6", + "width": 316, + "height": 320, + "depth": 470, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 15 + }, + { + "name": "B8", + "width": 316, + "height": 308, + "depth": 368, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 15 + }, + { + "name": "C13810", + "width": 234, + "height": 223, + "depth": 152, + "angle": 0, + "lipHeight": 26, + "drawer": false, + "minProductDepth": 0, + "maxMass": 8 + }, + { + "name": "C246911", + "width": 234, + "height": 146, + "depth": 152, + "angle": 0, + "lipHeight": 26, + "drawer": false, + "minProductDepth": 0, + "maxMass": 4 + }, + { + "name": "C5", + "width": 234, + "height": 197, + "depth": 152, + "angle": 0, + "lipHeight": 26, + "drawer": false, + "minProductDepth": 0, + "maxMass": 4 + }, + { + "name": "C7", + "width": 234, + "height": 121, + "depth": 152, + "angle": 0, + "lipHeight": 26, + "drawer": false, + "minProductDepth": 0, + "maxMass": 4 + }, + { + "name": "C12", + "width": 234, + "height": 261, + "depth": 152, + "angle": 0, + "lipHeight": 26, + "drawer": false, + "minProductDepth": 0, + "maxMass": 4 + }, + { + "name": "D1", + "width": 633, + "height": 346, + "depth": 470, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 65 + }, + { + "name": "D2356", + "width": 316, + "height": 320, + "depth": 470, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 15 + }, + { + "name": "D4", + "width": 316, + "height": 269, + "depth": 470, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 15 + }, + { + "name": "D7", + "width": 316, + "height": 308, + "depth": 368, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 15 + } + ], + "trays": [ + { + "name": null, + "trayTypeId": 167, + "leg": 2286, + "lip": 0, + "rise": 0, + "thicknesses": [ + 0, + 0, + 0, + 0 + ] + } + ], + "fbaFamily": "H11", + "faceBarcodeLocations": [], + "recipeTag": null, + "cgheightLimitAtZeroRadiusMm": 1295, + "cgheightLimitAtMaxRadiusMm": 1029 +} \ No newline at end of file diff --git a/tahoma_description/config/pod2_recipe_790-00265-022_2.txt b/tahoma_description/config/pod2_recipe_790-00265-022_2.txt new file mode 100644 index 0000000..02ce452 --- /dev/null +++ b/tahoma_description/config/pod2_recipe_790-00265-022_2.txt @@ -0,0 +1,1366 @@ +{ + "id": { + "partNumber": "790-00265-022", + "recipeVersion": 2 + }, + "partNumber": "790-00265-022", + "barcode": "RCP79000265022", + "podTypeId": 400, + "podProfileId": 2, + "emptyMassKg": 80, + "emptyCGHeightMm": 1041, + "heightWhenLifted": 291, + "bins": [ + { + "binDimensionIndex": 0, + "binDimensionName": "A1", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 0, + "inset": 0, + "primaryFace": "A", + "shelf": 1, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 0, + "binDimensionName": "A1", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 313, + "y": 0, + "inset": 0, + "primaryFace": "A", + "shelf": 1, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 0, + "binDimensionName": "A1", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 626, + "y": 0, + "inset": 0, + "primaryFace": "A", + "shelf": 1, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A2", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 356, + "inset": 0, + "primaryFace": "A", + "shelf": 3, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A2", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 313, + "y": 356, + "inset": 0, + "primaryFace": "A", + "shelf": 3, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 1, + "binDimensionName": "A2", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 626, + "y": 356, + "inset": 0, + "primaryFace": "A", + "shelf": 3, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 2, + "binDimensionName": "A35", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 660, + "inset": 0, + "primaryFace": "A", + "shelf": 7, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 2, + "binDimensionName": "A35", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 313, + "y": 660, + "inset": 0, + "primaryFace": "A", + "shelf": 7, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 2, + "binDimensionName": "A35", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 626, + "y": 660, + "inset": 0, + "primaryFace": "A", + "shelf": 7, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A4", + Current pod "x": 0, + "y": 914, + "inset": 0, + "primaryFace": "A", + "shelf": 11, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A4", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 313, + "y": 914, + "inset": 0, + "primaryFace": "A", + "shelf": 11, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 3, + "binDimensionName": "A4", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 626, + "y": 914, + "inset": 0, + Current pod "primaryFace": "A", + "shelf": 11, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 2, + "binDimensionName": "A35", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 1143, + "inset": 0, + "primaryFace": "A", + "shelf": 14, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 2, + "binDimensionName": "A35", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 313, + "y": 1143, + "inset": 0, + "primaryFace": "A", + "shelf": 14, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 2, + "binDimensionName": "A35", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 626, + "y": 1143, + "inset": 0, + "primaryFace": "A", + "shelf": 14, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 4, + "binDimensionName": "A6", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 1397, + "inset": 0, + "primaryFace": "A", + "shelf": 17, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 4, + "binDimensionName": "A6", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 313, + "y": 1397, + "inset": 0, + "primaryFace": "A", + "shelf": 17, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 4, + "binDimensionName": "A6", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 626, + "y": 1397, + "inset": 0, + "primaryFace": "A", + "shelf": 17, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 5, + "binDimensionName": "A7", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 1676, + "inset": 0, + "primaryFace": "A", + "shelf": 20, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 5, + "binDimensionName": "A7", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 313, + "y": 1676, + "inset": 0, + "primaryFace": "A", + "shelf": 20, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 5, + "binDimensionName": "A7", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 626, + "y": 1676, + "inset": 0, + "primaryFace": "A", + "shelf": 20, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 6, + "binDimensionName": "A8", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 2007, + "inset": 0, + "primaryFace": "A", + "shelf": 23, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "H", + "colorLevel": 8 + }, + { + "binDimensionIndex": 6, + "binDimensionName": "A8", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 313, + "y": 2007, + "inset": 0, + "primaryFace": "A", + "shelf": 23, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "H", + "colorLevel": 8 + }, + { + "binDimensionIndex": 6, + "binDimensionName": "A8", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 626, + "y": 2007, + "inset": 0, + "primaryFace": "A", + "shelf": 23, + "accessibleFaces": [ + "A" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "H", + "colorLevel": 8 + }, + { + "binDimensionIndex": 7, + "binDimensionName": "B1", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 356, + "y": 0, + "inset": 0, + "primaryFace": "B", + "shelf": 1, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 8, + "binDimensionName": "B27", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 356, + "y": 356, + "inset": 0, + "primaryFace": "B", + "shelf": 3, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 9, + "binDimensionName": "B35", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 356, + "y": 610, + "inset": 0, + "primaryFace": "B", + "shelf": 5, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 10, + "binDimensionName": "B4", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 356, + "y": 838, + "inset": 0, + "primaryFace": "B", + "shelf": 9, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 9, + "binDimensionName": "B35", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 356, + "y": 1143, + "inset": 0, + "primaryFace": "B", + "shelf": 14, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 11, + "binDimensionName": "B6", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 356, + "y": 1372, + "inset": 0, + "primaryFace": "B", + "shelf": 16, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 8, + "binDimensionName": "B27", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 356, + "y": 1702, + "inset": 0, + "primaryFace": "B", + "shelf": 21, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 12, + "binDimensionName": "B8", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 356, + "y": 1956, + "inset": 0, + "primaryFace": "B", + "shelf": 22, + "accessibleFaces": [ + "B" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "H", + "colorLevel": 8 + }, + { + "binDimensionIndex": 13, + "binDimensionName": "C1", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 0, + "inset": 0, + "primaryFace": "C", + "shelf": 1, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 13, + "binDimensionName": "C1", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 313, + "y": 0, + "inset": 0, + "primaryFace": "C", + "shelf": 1, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 13, + "binDimensionName": "C1", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 626, + "y": 0, + "inset": 0, + "primaryFace": "C", + "shelf": 1, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 14, + "binDimensionName": "C257", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 330, + "inset": 0, + "primaryFace": "C", + "shelf": 2, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 14, + "binDimensionName": "C257", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 313, + "y": 330, + "inset": 0, + "primaryFace": "C", + "shelf": 2, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 14, + "binDimensionName": "C257", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 626, + "y": 330, + "inset": 0, + "primaryFace": "C", + "shelf": 2, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 15, + "binDimensionName": "C3", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 635, + "inset": 0, + "primaryFace": "C", + "shelf": 6, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 15, + "binDimensionName": "C3", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 313, + "y": 635, + "inset": 0, + "primaryFace": "C", + "shelf": 6, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 15, + "binDimensionName": "C3", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 626, + "y": 635, + "inset": 0, + "primaryFace": "C", + "shelf": 6, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 16, + "binDimensionName": "C4", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 889, + "inset": 0, + "primaryFace": "C", + "shelf": 10, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 16, + "binDimensionName": "C4", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 313, + "y": 889, + "inset": 0, + "primaryFace": "C", + "shelf": 10, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 16, + "binDimensionName": "C4", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 626, + "y": 889, + "inset": 0, + "primaryFace": "C", + "shelf": 10, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 14, + "binDimensionName": "C257", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 1118, + "inset": 0, + "primaryFace": "C", + "shelf": 13, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 14, + "binDimensionName": "C257", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 313, + "y": 1118, + "inset": 0, + "primaryFace": "C", + "shelf": 13, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 14, + "binDimensionName": "C257", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 626, + "y": 1118, + "inset": 0, + "primaryFace": "C", + "shelf": 13, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 17, + "binDimensionName": "C6", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 1422, + "inset": 0, + "primaryFace": "C", + "shelf": 18, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 17, + "binDimensionName": "C6", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 313, + "y": 1422, + "inset": 0, + "primaryFace": "C", + "shelf": 18, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 17, + "binDimensionName": "C6", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 626, + "y": 1422, + "inset": 0, + "primaryFace": "C", + "shelf": 18, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 14, + "binDimensionName": "C257", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 1702, + "inset": 0, + "primaryFace": "C", + "shelf": 21, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 14, + "binDimensionName": "C257", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 313, + "y": 1702, + "inset": 0, + "primaryFace": "C", + "shelf": 21, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 14, + "binDimensionName": "C257", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 626, + "y": 1702, + "inset": 0, + "primaryFace": "C", + "shelf": 21, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "G", + "colorLevel": 7 + }, + { + "binDimensionIndex": 18, + "binDimensionName": "C8", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 0, + "y": 2007, + "inset": 0, + "primaryFace": "C", + "shelf": 23, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "H", + "colorLevel": 8 + }, + { + "binDimensionIndex": 18, + "binDimensionName": "C8", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 313, + "y": 2007, + "inset": 0, + "primaryFace": "C", + "shelf": 23, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 2, + "vendingMachineLetter": "H", + "colorLevel": 8 + }, + { + "binDimensionIndex": 18, + "binDimensionName": "C8", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 626, + "y": 2007, + "inset": 0, + "primaryFace": "C", + "shelf": 23, + "accessibleFaces": [ + "C" + ], + "vendingMachineNumber": 3, + "vendingMachineLetter": "H", + "colorLevel": 8 + }, + { + "binDimensionIndex": 19, + "binDimensionName": "D1", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 356, + "y": 0, + "inset": 0, + "primaryFace": "D", + "shelf": 1, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "A", + "colorLevel": 1 + }, + { + "binDimensionIndex": 20, + "binDimensionName": "D2356", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 356, + "y": 357, + "inset": 0, + "primaryFace": "D", + "shelf": 4, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "B", + "colorLevel": 2 + }, + { + "binDimensionIndex": 20, + "binDimensionName": "D2356", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 356, + "y": 686, + "inset": 0, + "primaryFace": "D", + "shelf": 8, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "C", + "colorLevel": 3 + }, + { + "binDimensionIndex": 21, + "binDimensionName": "D4", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 356, + "y": 1016, + "inset": 0, + "primaryFace": "D", + "shelf": 12, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "D", + "colorLevel": 4 + }, + { + "binDimensionIndex": 20, + "binDimensionName": "D2356", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 356, + "y": 1295, + "inset": 0, + "primaryFace": "D", + "shelf": 15, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "E", + "colorLevel": 5 + }, + { + "binDimensionIndex": 20, + "binDimensionName": "D2356", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 356, + "y": 1626, + "inset": 0, + "primaryFace": "D", + "shelf": 19, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "F", + "colorLevel": 6 + }, + { + "binDimensionIndex": 22, + "binDimensionName": "D7", + "trayRecipeIndex": 0, + "trayRecipeName": null, + "x": 356, + "y": 1956, + "inset": 0, + "primaryFace": "D", + "shelf": 22, + "accessibleFaces": [ + "D" + ], + "vendingMachineNumber": 1, + "vendingMachineLetter": "G", + "colorLevel": 7 + } + ], + "binDimensions": [ + { + "name": "A1", + "width": 313, + "height": 346, + "depth": 356, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 22 + }, + { + "name": "A2", + "width": 313, + "height": 295, + "depth": 356, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "A35", + "width": 313, + "height": 244, + "depth": 356, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "A4", + "width": 313, + "height": 219, + "depth": 356, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "A6", + "width": 313, + "height": 269, + "depth": 356, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "A7", + "width": 313, + "height": 320, + "depth": 356, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "A8", + "width": 313, + "height": 257, + "depth": 356, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "B1", + "width": 229, + "height": 346, + "depth": 470, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 22 + }, + { + "name": "B27", + "width": 229, + "height": 244, + "depth": 470, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "B35", + "width": 229, + "height": 219, + "depth": 470, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "B4", + "width": 229, + "height": 295, + "depth": 470, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "B6", + "width": 229, + "height": 320, + "depth": 470, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "B8", + "width": 229, + "height": 308, + "depth": 368, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "C1", + "width": 313, + "height": 320, + "depth": 356, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 22 + }, + { + "name": "C257", + "width": 313, + "height": 295, + "depth": 356, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "C3", + "width": 313, + "height": 244, + "depth": 356, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "C4", + "width": 313, + "height": 219, + "depth": 356, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "C6", + "width": 313, + "height": 269, + "depth": 356, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "C8", + "width": 313, + "height": 257, + "depth": 356, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "D1", + "width": 229, + "height": 346, + "depth": 470, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 22 + }, + { + "name": "D2356", + "width": 229, + "height": 320, + "depth": 470, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "D4", + "width": 229, + "height": 269, + "depth": 470, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + }, + { + "name": "D7", + "width": 229, + "height": 308, + "depth": 368, + "angle": 0, + "lipHeight": 22, + "drawer": false, + "minProductDepth": 0, + "maxMass": 11 + } + ], + "trays": [ + { + "name": null, + "trayTypeId": 167, + "leg": 2286, + "lip": 0, + "rise": 0, + "thicknesses": [ + 0, + 0, + 0, + 0 + ] + } + ], + "fbaFamily": "H12", + "faceBarcodeLocations": [ + { + "face": "A", + "x": 0, + "y": 2136, + "barcode": "A Side" + } + ], + "recipeTag": null, + "cgheightLimitAtZeroRadiusMm": 1457, + "cgheightLimitAtMaxRadiusMm": 1028 +} \ No newline at end of file diff --git a/tahoma_description/config/pod_sizes.yaml b/tahoma_description/config/pod_sizes.yaml new file mode 100644 index 0000000..f5d35ca --- /dev/null +++ b/tahoma_description/config/pod_sizes.yaml @@ -0,0 +1,88 @@ +{ + general: { # Universal pod sizes, in reality + brace_frame_thickness: 0.005, + brace_frame_width: 0.033, + base_frame_width: 0.041, + base_width: 0.9400, # In .stl file: 0.9398 + brace_width: 0.9390, # In .stl file: 0.9188 + base_height: 0.265, # In .stl file: 0.295 + brace_height: 2.308, # In .stl file: 2.311 + brace_to_fabric_XY_offset: 0.004, # In .stl file: 0.010 + bin_flap_height: 0.032, # In recipe: 0.026 or 0.022 + bin_wall_thickness: 0.002, + }, + + 1A: { # (13x4) Ref. -> pod1_recipe_790-00265-021.txt "bindDimensions" : A1-A13. corresponds to reality. Current face + bin_heights: [0.223, 0.146, 0.197, 0.146, 0.121, 0.223, 0.121, 0.146, 0.121, 0.223, 0.121, 0.146, 0.261], + bin_bottom_thickness: 0.005, + bin_width: 0.234, + bottom_bin_width: 0.000, + bin_depth: 0.152, + top_bin_depth: 0.000, + brace_to_fabric_z_offset: 0.085, + }, + 1B: { # (8x2) Ref. -> pod1_recipe_790-00265-021.txt "bindDimensions" : B1-B8. corresponds to reality + bin_heights: [0.346, 0.244, 0.219, 0.295, 0.219, 0.320, 0.244, 0.308], + bin_bottom_thickness: 0.007, + bin_width: 0.316, + bottom_bin_width: 0.633, + bin_depth: 0.470, + top_bin_depth: 0.368, + brace_to_fabric_z_offset: 0.085, + }, + 1C: { # (12x4) Ref. -> pod1_recipe_790-00265-021.txt "bindDimensions" : C1-C12. haven't been measured in reality + bin_heights: [0.223, 0.146, 0.223, 0.146, 0.197, 0.146, 0.121, 0.223, 0.146, 0.223, 0.146, 0.261], + bin_bottom_thickness: 0.005, + bin_width: 0.234, + bottom_bin_width: 0.000, + bin_depth: 0.152, + top_bin_depth: 0.000, + brace_to_fabric_z_offset: 0.085, + }, + 1D: { # (7x2) Ref. -> pod1_recipe_790-00265-021.txt "bindDimensions" : D1-D7. corresponds to reality + bin_heights: [0.346, 0.320, 0.320, 0.269, 0.320, 0.320, 0.308], + bin_bottom_thickness: 0.007, + bin_width: 0.316, + bottom_bin_width: 0.633, + bin_depth: 0.470, + top_bin_depth: 0.368, + brace_to_fabric_z_offset: 0.085, + }, + + 2A: { # (8x3) Ref. -> pod2_recipe_790-00265-022.txt "bindDimensions" : A1-A8 with minor changes corresponding to reality. Henry using face + bin_heights: [0.346, 0.295, 0.250, 0.225, 0.250, 0.275, 0.325, 0.275], # In recipe: [0.346, 0.295, 0.244, 0.219, 0.244, 0.269, 0.320, 0.257] + bin_bottom_thickness: 0.007, + bin_width: 0.313, + bottom_bin_width: 0.000, + bin_depth: 0.356, + top_bin_depth: 0.000, + brace_to_fabric_z_offset: 0.035, + }, + 2B: { # (8x1) Ref. -> pod2_recipe_790-00265-022.txt "bindDimensions" : B1-B8. haven't been measured in reality + bin_heights: [0.346, 0.244, 0.219, 0.295, 0.219, 0.320, 0.244, 0.308], + bin_bottom_thickness: 0.007, + bin_width: 0.229, + bottom_bin_width: 0.000, + bin_depth: 0.470, + top_bin_depth: 0.368, + brace_to_fabric_z_offset: 0.035, + }, + 2C: { # (8x3) Ref. -> pod2_recipe_790-00265-022.txt "bindDimensions" : C1-C8. haven't been measured in reality + bin_heights: [0.320, 0.295, 0.244, 0.219, 0.295, 0.269, 0.295, 0.257], + bin_bottom_thickness: 0.007, + bin_width: 0.313, + bottom_bin_width: 0.000, + bin_depth: 0.356, + top_bin_depth: 0.000, + brace_to_fabric_z_offset: 0.035, + }, + 2D: { # (7x1) Ref. -> pod2_recipe_790-00265-022.txt "bindDimensions" : D1-D7. haven't been measured in reality + bin_heights: [0.346, 0.320, 0.320, 0.269, 0.320, 0.320, 0.308], + bin_bottom_thickness: 0.007, + bin_width: 0.229, + bottom_bin_width: 0.000, + bin_depth: 0.470, + top_bin_depth: 0.368, + brace_to_fabric_z_offset: 0.035, + }, +} \ No newline at end of file diff --git a/tahoma_description/launch/load_pod.launch b/tahoma_description/launch/load_pod.launch index 9f7ac93..18d78a9 100644 --- a/tahoma_description/launch/load_pod.launch +++ b/tahoma_description/launch/load_pod.launch @@ -1,8 +1,9 @@ - + + - + diff --git a/tahoma_description/launch/view_models.launch b/tahoma_description/launch/view_models.launch index 10bd46d..78aeb9d 100644 --- a/tahoma_description/launch/view_models.launch +++ b/tahoma_description/launch/view_models.launch @@ -1,6 +1,6 @@ - + @@ -12,8 +12,10 @@ - - + + + + diff --git a/tahoma_description/robots/pod.xacro b/tahoma_description/robots/pod.xacro new file mode 100644 index 0000000..73e8684 --- /dev/null +++ b/tahoma_description/robots/pod.xacro @@ -0,0 +1,118 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tahoma_description/robots/pod1.xacro b/tahoma_description/robots/pod1.xacro index fb06a4b..659ff52 100644 --- a/tahoma_description/robots/pod1.xacro +++ b/tahoma_description/robots/pod1.xacro @@ -1,5 +1,7 @@ - - + + + diff --git a/tahoma_description/robots/pod2.xacro b/tahoma_description/robots/pod2.xacro new file mode 100644 index 0000000..64708f1 --- /dev/null +++ b/tahoma_description/robots/pod2.xacro @@ -0,0 +1,7 @@ + + + + + + diff --git a/tahoma_description/robots/tahoma.xacro b/tahoma_description/robots/tahoma.xacro index 8be8225..39f0a50 100644 --- a/tahoma_description/robots/tahoma.xacro +++ b/tahoma_description/robots/tahoma.xacro @@ -1,11 +1,10 @@ - - + - + diff --git a/tahoma_description/urdf/azure_kinect.urdf.xacro b/tahoma_description/urdf/azure_kinect.urdf.xacro new file mode 100644 index 0000000..e05520c --- /dev/null +++ b/tahoma_description/urdf/azure_kinect.urdf.xacro @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tahoma_description/urdf/pod.xacro b/tahoma_description/urdf/pod.xacro index a211ca3..e0694ca 100644 --- a/tahoma_description/urdf/pod.xacro +++ b/tahoma_description/urdf/pod.xacro @@ -1,12 +1,67 @@ - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -21,149 +76,208 @@ - + - + + - - + + + - - + + - + + - + - + - + - + - - - + + - - - - - - - + + - - + + + - + - - + + + - + - - - - - + + + + + + + + + + - + - - - - - + + + + + + + + + - - - - - - + + + + + + + + + + + + + + + - - - - + + + + - - + + + + + + + + + + + + - + + + + + + + + + + - - + + + - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - + + + - - - + + - \ No newline at end of file diff --git a/tahoma_description/urdf/pod1.xacro b/tahoma_description/urdf/pod1.xacro new file mode 100644 index 0000000..2e45fd4 --- /dev/null +++ b/tahoma_description/urdf/pod1.xacro @@ -0,0 +1,180 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/tahoma_description/urdf/pod2.xacro b/tahoma_description/urdf/pod2.xacro new file mode 100644 index 0000000..0bab424 --- /dev/null +++ b/tahoma_description/urdf/pod2.xacro @@ -0,0 +1,180 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/tahoma_description/urdf/pod_frame.xacro b/tahoma_description/urdf/pod_frame.xacro new file mode 100644 index 0000000..d1fb129 --- /dev/null +++ b/tahoma_description/urdf/pod_frame.xacro @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/tahoma_description/urdf/shelf_stack.xacro b/tahoma_description/urdf/shelf_stack.xacro new file mode 100644 index 0000000..5cf4440 --- /dev/null +++ b/tahoma_description/urdf/shelf_stack.xacro @@ -0,0 +1,148 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/tahoma_description/urdf/tahoma.xacro b/tahoma_description/urdf/tahoma.xacro index b0bbc6b..afe9cb6 100644 --- a/tahoma_description/urdf/tahoma.xacro +++ b/tahoma_description/urdf/tahoma.xacro @@ -1,257 +1,119 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - + + + + + - - - + - + - + - + - - + - + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + - - - - - - - - - - - - - - - - - + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + - diff --git a/tahoma_moveit_config/CMakeLists.txt b/tahoma_moveit_config/CMakeLists.txt index b90b6c9..c9e953c 100644 --- a/tahoma_moveit_config/CMakeLists.txt +++ b/tahoma_moveit_config/CMakeLists.txt @@ -7,7 +7,7 @@ find_package(catkin REQUIRED COMPONENTS actionlib_msgs geometry_msgs message_generation - moveit_servo + #moveit_servo std_srvs ) @@ -25,7 +25,7 @@ catkin_package(INCLUDE_DIRS include actionlib_msgs geometry_msgs message_runtime -moveit_servo + #moveit_servo std_srvs) include_directories(include) @@ -34,14 +34,14 @@ include_directories(SYSTEM ) -add_executable(servo_server - src/servo_server.cpp - src/pose_tracking.cpp -) -add_dependencies(servo_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(servo_server - ${catkin_LIBRARIES} -) +#add_executable(servo_server +# src/servo_server.cpp +# src/pose_tracking.cpp +#) +#add_dependencies(servo_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +#target_link_libraries(servo_server +# ${catkin_LIBRARIES} +#) if (CATKIN_ENABLE_TESTING) find_package(roslaunch REQUIRED) diff --git a/tahoma_moveit_config/config/joint_limits.yaml b/tahoma_moveit_config/config/joint_limits.yaml index c96840a..4e50088 100644 --- a/tahoma_moveit_config/config/joint_limits.yaml +++ b/tahoma_moveit_config/config/joint_limits.yaml @@ -3,32 +3,44 @@ # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: arm_elbow_joint: + max_position: 3.1415926535897931 + min_position: -3.1415926535897931 has_velocity_limits: true - max_velocity: 3.1415926535897931 + max_velocity: 3.1415926535897931 has_acceleration_limits: false max_acceleration: 0 arm_shoulder_lift_joint: + max_position: 6.283185307 + min_position: -6.283185307 has_velocity_limits: true - max_velocity: 2.0943951023931953 + max_velocity: 2.094395102 has_acceleration_limits: false max_acceleration: 0 arm_shoulder_pan_joint: + max_position: 6.283185307 + min_position: -6.283185307 has_velocity_limits: true - max_velocity: 2.0943951023931953 + max_velocity: 2.094395102 has_acceleration_limits: false max_acceleration: 0 arm_wrist_1_joint: + max_position: 6.283185307 + min_position: -6.283185307 has_velocity_limits: true - max_velocity: 3.1415926535897931 + max_velocity: 3.1415926535897931 has_acceleration_limits: false max_acceleration: 0 arm_wrist_2_joint: + max_position: 6.283185307 + min_position: -6.283185307 has_velocity_limits: true - max_velocity: 3.1415926535897931 + max_velocity: 3.1415926535897931 has_acceleration_limits: false max_acceleration: 0 arm_wrist_3_joint: + max_position: 6.283185307 + min_position: -6.283185307 has_velocity_limits: true - max_velocity: 3.1415926535897931 + max_velocity: 3.1415926535897931 has_acceleration_limits: false max_acceleration: 0 \ No newline at end of file diff --git a/tahoma_moveit_config/config/kinematics.yaml b/tahoma_moveit_config/config/kinematics.yaml index 51e29f5..81dd109 100644 --- a/tahoma_moveit_config/config/kinematics.yaml +++ b/tahoma_moveit_config/config/kinematics.yaml @@ -5,6 +5,6 @@ # kinematics_solver_attempts: 3 manipulator: kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin - solve_type: Distance + solve_type: Manipulation1 kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 + kinematics_solver_timeout: 0.005 \ No newline at end of file diff --git a/tahoma_moveit_config/config/ompl_planning.yaml b/tahoma_moveit_config/config/ompl_planning.yaml index 39ba9f1..84ffa20 100644 --- a/tahoma_moveit_config/config/ompl_planning.yaml +++ b/tahoma_moveit_config/config/ompl_planning.yaml @@ -121,7 +121,7 @@ planner_configs: dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 manipulator: - longest_valid_segment_fraction: 0.01 + longest_valid_segment_fraction: 0.001 default_planner_config: RRTConnect planner_configs: - SBL diff --git a/tahoma_moveit_config/config/ros_controllers.yaml b/tahoma_moveit_config/config/ros_controllers.yaml index 9af8187..f06c421 100644 --- a/tahoma_moveit_config/config/ros_controllers.yaml +++ b/tahoma_moveit_config/config/ros_controllers.yaml @@ -10,9 +10,3 @@ controller_list: - arm_wrist_2_joint - arm_wrist_3_joint -- name: "gripper_controller" - action_ns: gripper_cmd - type: GripperCommand - default: true - joints: - - gripper_finger_joint \ No newline at end of file diff --git a/tahoma_moveit_config/config/servo.yaml b/tahoma_moveit_config/config/servo.yaml index 7c16534..2b8ba74 100644 --- a/tahoma_moveit_config/config/servo.yaml +++ b/tahoma_moveit_config/config/servo.yaml @@ -29,11 +29,11 @@ publish_joint_accelerations: false ## MoveIt properties move_group_name: manipulator -planning_frame: arm_base_link # The MoveIt planning frame. Often 'base_link' or 'world' +planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world'. The previous value: arm_base_link ## Other frames -ee_frame_name: arm_tool0 # The name of the end effector link, used to return the EE pose -robot_link_command_frame: arm_tool0 # commands must be given in the frame of a robot link. Usually either the base or end effector +ee_frame_name: epick_end_effector # The name of the end effector link, used to return the EE pose. The previous value: arm_tool0 +robot_link_command_frame: epick_end_effector # commands must be given in the frame of a robot link. Usually either the base or end effector. The previous value: arm_tool0 ## Stopping behaviour incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command @@ -55,7 +55,9 @@ command_out_topic: /joint_group_pos_controller/command # Publish outgoing comman ## Collision checking for the entire robot body check_collisions: false -collision_check_rate: 50 # [Hz] Collision-checking can easily bog down a CPU if done too often. +# If robot poses are very close to scene objects/self-collision, the rate should be high as it determines how often to check for collision +# It relates to cases when path is collision free, but control isn't good enough to follow the path +collision_check_rate: 200 # [Hz] Collision-checking can easily bog down a CPU if done too often. # Two collision check algorithms are available: # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. # "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits @@ -84,6 +86,6 @@ x_derivative_gain: 1 y_derivative_gain: 1 z_derivative_gain: 1 -angular_proportional_gain: 1 -angular_integral_gain: 0.2 -angular_derivative_gain: 0.5 \ No newline at end of file +angular_proportional_gain: 1 # The previous value: 1 +angular_integral_gain: 0.2 # The previous value: 0.2 +angular_derivative_gain: 0.5 # The previous value: 0.5 \ No newline at end of file diff --git a/tahoma_moveit_config/config/tahoma.srdf b/tahoma_moveit_config/config/tahoma.srdf index b51074a..b353713 100644 --- a/tahoma_moveit_config/config/tahoma.srdf +++ b/tahoma_moveit_config/config/tahoma.srdf @@ -14,8 +14,7 @@ - - + @@ -25,6 +24,7 @@ + @@ -36,10 +36,18 @@ - - - - + + + + + + + + + + + + @@ -65,27 +73,103 @@ - - - + - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - + @@ -99,25 +183,14 @@ - - - - - - - - - + - - - - - + @@ -131,14 +204,12 @@ + - - - - - + @@ -149,84 +220,55 @@ + + + + + + + + + + + - - + - - - - - - - - - + - - + - - - - - - - - - + - - + + - - - - - - - - - + - - - - - - + - - - - - - - - - - @@ -235,28 +277,15 @@ + - - - - - - + - - - - - - - - - - @@ -265,113 +294,21 @@ - - - - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + @@ -384,6 +321,7 @@ + @@ -395,6 +333,7 @@ + @@ -405,29 +344,8 @@ - - - - - - - - - - - - - + - - - - - - - - - @@ -436,15 +354,7 @@ - - - - - - - - - + @@ -453,59 +363,7 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + @@ -517,6 +375,7 @@ + @@ -525,10 +384,14 @@ + + + + diff --git a/tahoma_moveit_config/launch/move_group.launch b/tahoma_moveit_config/launch/move_group.launch index ae38de5..9a4569c 100644 --- a/tahoma_moveit_config/launch/move_group.launch +++ b/tahoma_moveit_config/launch/move_group.launch @@ -3,8 +3,7 @@ - + diff --git a/tahoma_moveit_config/src/servo_server.cpp b/tahoma_moveit_config/src/servo_server.cpp old mode 100644 new mode 100755