Enhance motion planning

.gitmodules

@@ -43,3 +43,12 @@
 [submodule "third-party/usb_cam"]
 	path = third-party/usb_cam
 	url = [email protected]:ros-drivers/usb_cam.git
+[submodule "third-party/stomp_ros"]
+	path = third-party/stomp_ros
+	url = https://github.com/ros-industrial/stomp_ros.git
+[submodule "third-party/stomp"]
+	path = third-party/stomp
+	url = https://github.com/ros-industrial/stomp.git
+[submodule "third-party/ros_industrial_cmake_boilerplate"]
+	path = third-party/ros_industrial_cmake_boilerplate
+	url = https://github.com/ros-industrial/ros_industrial_cmake_boilerplate.git

aurmr_tasks/scripts/aurmr_demo

@@ -23,10 +23,28 @@ from aurmr_perception.util import vec_to_quat_msg, I_QUAT
 from aurmr_tasks.common.states import Wait
 from aurmr_tasks.common.hri import UserPromptForRetry
 
+from aurmr_tasks.common import compliance_controller
 
 from queue import Empty, Queue
-
-
+class PublishStatus(State):
+    def __init__(self, publisher, wait_time=0):
+        State.__init__(self, outcomes=['succeeded'], input_keys=["status", "start_time", "target_object_id"])
+        self.publisher=publisher
+        self.wait_time = wait_time
+    def execute(self, userdata):
+        now = rospy.get_rostime()
+        now = now.secs + now.nsecs*10**(-9)
+        time_delta = now - userdata["start_time"]
+
+
+        msg = PickStatus()
+        msg.status = userdata["status"]
+        msg.object_id = userdata["target_object_id"]
+        msg.time = time_delta
+        # self.publisher.publish("test")
+        self.publisher.publish(msg)
+        rospy.sleep(self.wait_time)
+        return "succeeded"
 
 class PickStowQueue:
     def __init__(self, pick_sm) -> None:
@@ -135,19 +153,33 @@ def main():
             # These are pre-terminating states that note what went wrong in the userdata. This avoids
             # encoding the same information in an explosion of named state transitions
             cf.splat_auto("SPLAT_PICK", "request", ["target_bin_id", "target_object_id", "target_object_asin"])
-            # StateMachine.add_auto("ADD_IN_HAND_COLLISION_BOX_PRE_GRASP", planning_scene.AddInHandCollisionGeometry(robot), ["succeeded", "aborted"])
-            StateMachine.add("GET_GRASP_POSE", perception.GetGraspPose(robot.tf2_buffer, pre_grasp_offset=.015),
-                            {"succeeded": "REMAP_PRE_GRASP_POSE", "aborted":"NOTE_PASS", "preempted":"NOTE_PASS"})
+            # StateMachine.add_auto("ADD_IN_HAND_COLLISION_BOX_PRE_GRASP", motion.AddInHandCollisionGeometry(robot), ["succeeded", "aborted"])
+
+
+            StateMachine.add_auto("CAPTURE_POINTS", perception.GetPoints(), ["succeeded", "aborted"])
+
+            StateMachine.add("GET_GRASP_POSE", perception.GetGraspPose(robot.tf2_buffer, pre_grasp_offset=.065),
+                            {"succeeded": "MOVE_TO_BIN", "aborted":"NOTE_PASS", "preempted":"NOTE_PASS"})
+
+            #StateMachine.add("PASS", PublishStatus(status_pub, wait_time=3), {"succeeded": "done"})
+
+            #StateMachine.add("GET_GRASP_POSE", perception.GetGraspPose(robot.tf2_buffer, pre_grasp_offset=.035),
+            #                {"succeeded": "REMAP_PRE_GRASP_POSE", "aborted":"NOTE_PASS", "preempted":"NOTE_PASS"})
             cf.inject_userdata("NOTE_PASS", "done", "status", PickRequestResponse.OUTCOME_PERCEPTION_FAILURE)
 
+            StateMachine.add_auto("MOVE_TO_BIN", motion.MoveToBin(robot), ["succeeded", "aborted"])
+
+            #StateMachine.add_auto("PUBLISH_CURRENTLY_PICKING", PublishStatus(status_pub), ["succeeded"])
+            # StateMachine.add_auto("ADD_IN_HAND_COLLISION_BOX_PRE_GRASP", planning_scene.AddInHandCollisionGeometry(robot), ["succeeded", "aborted"])
+
             StateMachine.add_auto("REMAP_PRE_GRASP_POSE", cf.input_to_output("pre_grasp_pose", "pose"), ["succeeded"])
             StateMachine.add_auto("CLEAR_SCENE_PREMOVE", planning_scene.ClearCollisionGeometry(robot), ["succeeded", "aborted"])
             StateMachine.add_auto("SETUP_COLLISION_SCENE_PREMOVE", planning_scene.AddFullPodCollisionGeometry(robot), ["succeeded", "aborted"])
 
-            StateMachine.add("MOVE_TO_PRE_GRASP", motion.MoveEndEffectorToPoseManipulable(robot), {"succeeded": "CLEAR_SCENE_PREGRASP", "preempted": "NOTE_PREEMPTION", "aborted": "NOTE_PLANNING_FAILURE"})
-            cf.inject_userdata("NOTE_PLANNING_FAILURE", "done", "status", PickRequestResponse.OUTCOME_PLANNING_FAILURE)
-            StateMachine.add_auto("CLEAR_SCENE_PREGRASP", planning_scene.ClearCollisionGeometry(robot), ["succeeded", "aborted"])
-            StateMachine.add_auto("SETUP_COLLISION_SCENE_PREGRASP", planning_scene.AddPartialPodCollisionGeometry(robot), ["succeeded", "aborted"])
+            # FIXME: if `MOVE_TO_PREBIN`` returns early, IMPLEMENT a method to check that move group has reached goal
+            StateMachine.add_auto("MOVE_TO_PRE_GRASP", motion.MoveEndEffectorToPose(robot), ["succeeded", "aborted", "preempted"])
+            StateMachine.add_auto("CLEAR_SCENE_PREGRASP", motion.ClearCollisionGeometry(robot), ["succeeded", "aborted"])
+            StateMachine.add_auto("SETUP_COLLISION_SCENE_PREGRASP", motion.AddPartialPodCollisionGeometry(robot), ["succeeded", "aborted"])
             StateMachine.add_auto("MOVE_TO_GRASP", motion.grasp_move_to_offset(robot, (0, 0, 0.5)), ["succeeded", "aborted", "preempted"])
             StateMachine.add_auto("LIFT_OBJ", motion.robust_move_to_offset(robot, (0, 0, 0.035), 'base_link'), ["succeeded", "aborted", "preempted"])
             StateMachine.add_auto("ADJUST_RIGHT_IF_COLUMN_1", motion.AdjustRightIfColumn1(robot, (0, 0.05, 0)), ["succeeded", "aborted", "pass"])
@@ -176,7 +208,7 @@ def main():
 
             with pick_retry_sm:
                 StateMachine.add_auto("BLOW_OFF_GRIPPER", motion.BlowOffGripper(robot, return_before_done=True), ["succeeded", "aborted", "preempted"])
-                StateMachine.add_auto("UNBLOCK_VIEW", motion.MoveToJointAngles(robot, "hide"), ["succeeded", "aborted"])
+                StateMachine.add_auto("UNBLOCK_VIEW", motion.MoveToJointAngles(robot, "drop_hide"), ["succeeded", "aborted"])
                 StateMachine.add_auto("PICTURE_WAIT", Wait(2), ["succeeded"])
                 StateMachine.add_auto("UPDATE_BIN_MASKS", perception.UpdateBin(), ["succeeded", "preempted", "aborted"])
                 StateMachine.add('USER_PROMPT_FOR_RETRY', UserPromptForRetry(rospy.get_param("bin_bounds"), robot.tf2_buffer, timeout_connection_secs=1.0), {"retry": "REMAP_GRASP", "continue": "continue"})

aurmr_tasks/src/aurmr_tasks/common/compliance_controller.py

@@ -4,11 +4,14 @@
 from geometry_msgs.msg import PoseStamped
 import time
 
+import numpy as np
+from aurmr_tasks.util import pose_dist
+
 
 from aurmr_tasks.common.visualization_utils import EEFVisualization
 class MoveToOffset(State):
-    def __init__(self, robot, offset, frame_id, detect_object,idle_timeout=3):
-        super().__init__(outcomes=['succeeded', 'aborted'],  output_keys=['starting_grasp_pose_out'], input_keys=['target_pose_in'])
+    def __init__(self, robot, offset, frame_id, detect_object, idle_timeout=3):
+        super().__init__(outcomes=['succeeded', 'aborted'],  input_keys=['offset_moved_in'], output_keys=['offset_moved_out'])
         self.robot = robot
         self.offset = offset
         self.frame_id = frame_id
@@ -28,30 +31,36 @@ def stop(self):
 
     def execute(self, userdata):
         self.close_robot_gripper()
-        start_pose = self.robot.move_group.get_current_pose()
-        start_pose.header.stamp = rospy.Time(0)
-        start_pose = self.robot.tf2_buffer.transform(start_pose, self.target_frame, rospy.Duration(1))
-
-        userdata.starting_grasp_pose_out = start_pose
-        rospy.logdebug("setting starting pose to ", start_pose)
-        self.vis_out.visualize_eef(start_pose)
+        start_pose = self.current_pose_in_target_frame()
 
-        if userdata.target_pose_in:
-            target_pose = userdata.target_pose_in
+        if self.offset is None:
+            offset = userdata.offset_moved_in
         else:
-            target_pose = self.calculate_target_pose(start_pose)
-            self.robot.force_values_init = True
+            offset = self.offset
+
+        target_pose = self.calculate_target_pose(start_pose, offset)
+        self.robot.force_values_init = True
 
         self.vis.visualize_eef(target_pose)
         self.publish_target_pose(target_pose)
 
-        return self.wait_until_stopped(0.001, self.idle_timeout)
+        result = self.wait_until_stopped(0.001, self.idle_timeout)
+
+        current_pose = self.current_pose_in_target_frame()
+        current_pose = current_pose.pose
+        start_pose = start_pose.pose
+        offset_moved = np.array([start_pose.position.x, start_pose.position.y, start_pose.position.z])
+        offset_moved = offset_moved - np.array([current_pose.position.x, current_pose.position.y, current_pose.position.z])
+
+        userdata.offset_moved_out = offset_moved
+
+        return result
 
     def close_robot_gripper(self):
         self.robot.close_gripper(return_before_done=True)
 
-    def calculate_target_pose(self, start_pose):
-        target_pose = apply_offset_to_pose(start_pose, self.offset, self.frame_id, self.robot.tf2_buffer)
+    def calculate_target_pose(self, start_pose, offset):
+        target_pose = apply_offset_to_pose(start_pose, offset, self.frame_id, self.robot.tf2_buffer)
         return self.robot.tf2_buffer.transform(target_pose, self.target_frame, rospy.Duration(1))
 
     def publish_target_pose(self, target_pose):
@@ -99,7 +108,6 @@ def current_pose_in_target_frame(self):
         return current_pose
 
 
-
     def check_object_condition(self):
         return (self.detect_object and self.robot.check_gripper_item())
 

aurmr_tasks/src/aurmr_tasks/common/motion.py

@@ -85,13 +85,13 @@ def execute(self, userdata):
             pose = userdata["pose"]
         success = self.robot.move_to_pose(
                           pose,
-                          allowed_planning_time=10.0,
+                          allowed_planning_time=20.0,
                           execution_timeout=15.0,
-                          num_planning_attempts=12,
+                          num_planning_attempts=40,
                           orientation_constraint=None,
                           replan=True,
-                          replan_attempts=8,
-                          tolerance=0.01)
+                          replan_attempts=20,
+                          tolerance=0.02)
         if success:
             return "succeeded"
         else:
@@ -299,6 +299,24 @@ def execute(self, userdata):
         else:
             return "succeeded"
 
+class MoveToBin(State):
+    def __init__(self, robot):
+        State.__init__(self, input_keys=["target_bin_id"], outcomes=['succeeded', 'aborted'])
+        self.robot = robot
+
+    def execute(self, ud):
+        target = ud["target_bin_id"]
+        if isinstance(target, JointState):
+            to_log = target.position
+        else:
+            target = "pre_bin_" + target.lower()
+        to_log = target
+        rospy.loginfo(f"Moving to {to_log}")
+        success = self.robot.move_to_joint_angles(target, return_before_done=False)
+        if success:
+            return "succeeded"
+        else:
+            return "aborted"
 
 class CloseGripper(State):
     def __init__(self, robot, return_before_done=False):
@@ -350,4 +368,3 @@ def execute(self, ud):
         if not self.robot.check_gripper_item():
             self.robot.open_gripper(return_before_done=True)
         return 'succeeded'
-

aurmr_tasks/src/aurmr_tasks/common/perception.py

@@ -150,18 +150,47 @@ def execute(self, userdata):
         return "aborted"
 
 
+class GetPoints(State):
+    def __init__(self, frame_id='base_link'):
+        State.__init__(
+            self,
+            input_keys=['target_bin_id', 'target_object_id'],
+            output_keys=['points_response'],
+            outcomes=['succeeded', 'aborted']
+        )
+        self.get_points = rospy.ServiceProxy('/aurmr_perception/get_object_points', GetObjectPoints)
+        self.get_points.wait_for_service(timeout=5)
+        self.frame_id = frame_id
+
+    def execute(self, userdata):
+        try:
+            get_points_req = GetObjectPointsRequest(
+                bin_id=userdata['target_bin_id'],
+                object_id=userdata['target_object_id'],
+                frame_id=self.frame_id
+            )
+            print(get_points_req, get_points_req)
+            points_response = self.get_points(get_points_req)
+
+            if not points_response.success:
+                userdata["status"] = "pass"
+                return "aborted"
+
+            userdata['points_response'] = points_response
+            return "succeeded"
+        except:
+            return "aborted"
+
 class GetGraspPose(State):
     def __init__(self, tf_buffer, frame_id='base_link', pre_grasp_offset=.12):
         State.__init__(
             self,
-            input_keys=['target_bin_id', 'target_object_id'],
+            input_keys=['target_bin_id', 'target_object_id', 'points_response'],
             output_keys=['grasp_pose', 'pre_grasp_pose'],
             outcomes=['succeeded', 'preempted', 'aborted']
         )
-        self.get_points = rospy.ServiceProxy('/aurmr_perception/get_object_points', GetObjectPoints)
         self.get_grasp = rospy.ServiceProxy('/grasp_detection/detect_grasps', DetectGraspPoses)
         # Crash during initialization if these aren't running so see the problem early
-        self.get_points.wait_for_service(timeout=5)
         self.get_grasp.wait_for_service(timeout=5)
         self.frame_id = frame_id
         self.pre_grasp_offset = pre_grasp_offset
@@ -180,34 +209,41 @@ def execute(self, userdata):
             bin_id=userdata['target_bin_id'],
             object_id=userdata['target_object_id'],
             frame_id=self.frame_id
+
         )
-        points_response = self.get_points(get_points_req)
 
-        if not points_response.success:
-            return "aborted"
+        try:
 
-        grasp_response = self.get_grasp(points=points_response.points,
-                                        mask=points_response.mask,
-                                        dist_threshold=self.pre_grasp_offset, bin_id=userdata['target_bin_id'])
+            points_response = userdata["points_response"]
 
-        if not grasp_response.success:
-            return "aborted"
+            if not points_response.success:
+                return "aborted"
+
+
+            grasp_response = self.get_grasp(points=points_response.points,
+                                            mask=points_response.mask,
+                                            dist_threshold=self.pre_grasp_offset, bin_id=userdata['target_bin_id'])
 
-        # NOTE: No extra filtering or ranking on our part. Just take the first one
-        # As the arm_tool0 is 26cm in length w.r.t tip of suction cup thus adding 0.26m offset
-        # TODO(henrifung): Pull this number from xacro, perhaps from userdata
-        grasp_pose = grasp_response.poses[0]
+            if not grasp_response.success:
+                return "aborted"
 
-        grasp_pose = add_offset(-0.26, grasp_pose)
+            # NOTE: No extra filtering or ranking on our part. Just take the first one
+            # As the arm_tool0 is 26cm in length w.r.t tip of suction cup thus adding 0.26m offset
+            # TODO(henrifung): Pull this number from xacro, perhaps from userdata
+            grasp_pose = grasp_response.poses[0]
 
-        userdata['grasp_pose'] = grasp_pose
+            grasp_pose = add_offset(-0.26, grasp_pose)
 
-        # adding 0.12m offset for pre grasp pose to prepare it for grasp pose which is use to pick the object
-        pregrasp_pose = add_offset(-self.pre_grasp_offset, grasp_pose)
+            userdata['grasp_pose'] = grasp_pose
 
-        userdata['pre_grasp_pose'] = pregrasp_pose
+            # adding 0.12m offset for pre grasp pose to prepare it for grasp pose which is use to pick the object
+            pregrasp_pose = add_offset(-self.pre_grasp_offset, grasp_pose)
 
-        self.pose_viz.publish(grasp_pose)
-        self.pre_grasp_viz.publish(pregrasp_pose)
+            userdata['pre_grasp_pose'] = pregrasp_pose
 
-        return "succeeded"
+            self.pose_viz.publish(grasp_pose)
+            self.pre_grasp_viz.publish(pregrasp_pose)
+
+            return "succeeded"
+        except:
+            return "aborted"

aurmr_tasks/src/aurmr_tasks/common/tahoma.py

@@ -361,8 +361,9 @@ def move_to_joint_angles(self,
                            plan_only=False,
                            replan=False,
                            replan_attempts=5,
-                           tolerance=0.01,
-                           path_constraints=None):
+                           tolerance=0.005,
+                           path_constraints=None,
+                           return_before_done=False):
         """Moves the end-effector to a pose, using motion planning.
 
         Args:
@@ -407,7 +408,7 @@ def move_to_joint_angles(self,
             self.move_group.set_named_target(joints)
         else:
             self.move_group.set_joint_value_target(joints)
-        self.move_group.go(wait=True)
+        self.move_group.go(wait=not return_before_done)
 
         # Calling ``stop()`` ensures that there is no residual movement
         self.move_group.stop()
@@ -941,3 +942,8 @@ def compute_ik(self, pose_stamped, timeout=rospy.Duration(5)):
     def cancel_all_goals(self):
         self._move_group_client.cancel_all_goals()
         self._joint_traj_client.cancel_all_goals()
+
+    def reached_goal(self):
+        while self._move_group_client.get_state() == GoalStatus.ACTIVE:
+            continue
+        return True

tahoma_bringup/config/controllers.yaml

@@ -221,11 +221,11 @@ ur_cartesian_compliance_controller:
         rot_x: 5.0
         rot_y: 5.0
         rot_z: 5.0
-        max_translation_error_norm: 25
-        max_rotation_error_norm: 5.14
+        max_translation_error_norm: 0.2
+        max_rotation_error_norm: 0.1
 
     solver:
-        error_scale: 0.35
+        error_scale: 0.5
         iterations: 1
         publish_state_feedback: True