aurmr_tasks/scripts/analyze_workspace contains a program to run reach…

…ability/manipulability tests with different parameters (pod's position, motion planning parameters, rpy perturbations, etc). aurmr_perception/visualization.py is modified to visualize epick-end-effector with suction cup and metal cylinder. aurmr_tasks/src/aurmr_tasks/tahoma.py is modified to created more precise collsion scene for the pod in MoveIt, to account for different pod models, and to compute/execute motion plans for the given target pose/joint_values. robotiq/robotiq_epick_visualization/robotiq_epick_model_macro.xacro is modified to correspond the dimensions (collison objects) of the end-effector from meshes/visual/robotiq_epick_full.stl (which includes a suction cup and a metal cylinder to original epick end-effector). Those dimensions are explicitly defined in xacro files. tahoma_description/robots(or urdf)/pod.xacro is renamed to pod1.xacro (pod with 13x4 small bins). pod2.xacro file is created for the pod with 8x3 large bins. tahoma_description/urdf/tahoma.xacro is modified to explicitly define the dimensions and relative distances of coupling/connector_cover/robotiq_epick_end-effector and to include all of them (visual and collision objects) according to dae/stl files and real measurements. tahoma_moveit_config/config/omply_planning.yaml file has decreased 'longest_valid_segment_fraction' parameter from 0.01 to 0.001 to address thin bin walls (2 mm) in motion planning. tahoma_moveit_config/config/servo.yaml has collsion_check_rate of 200 instead of defalut 50 to avoid collision of the end-effector with bins when pre-grasp poses are very close to the bin's walls or when the robot is too close to the pod. tahoma_moveit_config/config/tahoma.srdf file is refined to exlcude obvious links from collision checking process

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,20 +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 = color[i][0]
+        # 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"
+        # 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]

aurmr_tasks/frames.gv

@@ -0,0 +1,2 @@
+digraph G {
+"no tf data recieved"}

aurmr_tasks/scripts/analyze_workspace_launcher.py

@@ -0,0 +1,16 @@
+#!/usr/bin/env python
+
+import rospy
+import subprocess
+
+def main():
+    pod_x = rospy.get_param('~pod_x', 'default_arg1_value')
+    pod_y = rospy.get_param('~pod_y', 'default_arg2_value')
+
+    cmd = ['python', '/path/to/my_script.py', pod_x, pod_y]
+
+    subprocess.call(cmd)
+
+if __name__ == '__main__':
+    rospy.init_node('my_script_launcher')
+    main()