Merge pull request #124 from cpaxton/feature/smartmove-constraints

adding more smartmove constraints

costar_predicator/predicator_landmark/src/predicator_landmark/landmark.py

@@ -59,7 +59,17 @@ def get_waypoints_srv(self,req):
 
         return resp
 
-    def get_waypoints(self,frame_type,predicates,transforms,names):
+    def get_waypoints(self, frame_type, predicates, transforms, names, constraints):
+        '''
+        Parameters:
+        -----------
+        frame_type: object class to match when generating new frames
+        predicates: other list of predicates to add
+        transforms: used to generate the actual poses
+        names: should be same size as transforms
+        above: prunes list of transforms based on criteria, transforms should
+               be above their parent frame in the world coordinates
+        '''
         self.and_srv.wait_for_service()
 
         if not len(names) == len(transforms):
@@ -89,6 +99,7 @@ def get_waypoints(self,frame_type,predicates,transforms,names):
         poses = []
         for tform in transforms:
             poses.append(pm.fromMsg(tform))
+            break # we only do one
 
         if frame_type not in self.obj_symmetries.keys():
             self.obj_symmetries[frame_type] = ObjectSymmetry()
@@ -108,15 +119,6 @@ def get_waypoints(self,frame_type,predicates,transforms,names):
                     quaternion_list.append(rot_matrix.GetQuaternion())
         quaternion_list = np.array(quaternion_list)
 
-        unique_rot_matrix = list()
-        # quaternion_list = np.around(quaternion_list,decimals=5)
-        # b = np.ascontiguousarray(quaternion_list).view(np.dtype((np.void, quaternion_list.dtype.itemsize * quaternion_list.shape[1])))
-        # _, unique_indices = np.unique(b, return_index=True)
-
-        # for index in unique_indices:
-        #     unique_rot_matrix.append(  pm.Rotation.Quaternion( *quaternion_list[index].tolist() )  )
-
-
         unique_brute_force = list()
         for i in xrange(len(quaternion_list)):
             unique = True
@@ -126,13 +128,11 @@ def get_waypoints(self,frame_type,predicates,transforms,names):
                 else:
                     if abs(quaternion_list[i].dot(quaternion_list[j])) > 0.99:
                         unique = False
-                        # print i, j, ' is not unique', quaternion_list[i], quaternion_list[j]
                         break
             if unique:
                 unique_brute_force.append(i)
-        # print 'unique brute force: ', len(unique_brute_force)
-        # print unique_brute_force
 
+        unique_rot_matrix = list()
         for index in unique_brute_force:
             unique_rot_matrix.append(  pm.Rotation.Quaternion( *quaternion_list[index].tolist() )  )
 
@@ -142,18 +142,33 @@ def get_waypoints(self,frame_type,predicates,transforms,names):
 
         for match in res.matching:
             try:
-                (trans,rot) = self.listener.lookupTransform(self.world,match,rospy.Time(0))
-                # for (pose, name) in zip(poses,names):
+                (trans,rot) = self.listener.lookupTransform(self.world, match, rospy.Time(0))
+                match_tform = pm.fromTf((trans,rot))
 
                 if frame_type in self.obj_symmetries:
                     for rot_matrix in unique_rot_matrix:
                         tform = pm.Frame(rot_matrix)
-                        new_poses.append(pm.toMsg(pm.fromTf((trans,rot)) * tform * poses[0]))
+                        world_tform = match_tform * tform * poses[0]
+                        violated = False
+                        for constraint in constraints:
+                            v1 = match_tform.p[constraint.pose_variable]
+                            v2 = world_tform.p[constraint.pose_variable]
+                            if constraint.greater and not (v2 >= v1 + constraint.threshold):
+                                violated = True
+                                #print "VIOLATED", constraint
+                                break
+                            elif not constraint.greater and not (v2 <= v1 - constraint.threshold):
+                                violated = True
+                                #print "VIOLATED", constraint
+                                break
+                        if violated:
+                            continue
+                        #else:
+                        #    print match_tform.p, world_tform.p
+
+                        new_poses.append(pm.toMsg(world_tform))
                         new_names.append(match + "/" + names[0] + "/x%fy%fz%f"%(rot_matrix.GetRPY()))
 
-                        #print match, str(match + "/" + names[0] + "/x%fy%fz%f"%(rot_matrix.GetRPY())),
-                        # pm.toTf(pm.fromTf((trans,rot)) * tform * poses[0])[0]
-
                         objects.append(match)
 
             except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):

costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py

@@ -826,7 +826,8 @@ def query(self, req, disable_target_object_collision = False):
                 req.obj_class, # object class to move to
                 req.predicates, # predicates to match
                 [req.pose], # offset/transform from each member of the class
-                [req.name] # placeholder name
+                [req.name], # placeholder name
+                req.constraints,
                 )
 
         if res is None:
@@ -918,7 +919,18 @@ def query(self, req, disable_target_object_collision = False):
 
         return possible_goals
 
-    def smartmove_multipurpose_gripper(self, stamp, possible_goals, distance, gripper_function, velocity, acceleration, backup_in_gripper_frame):
+    def smartmove_multipurpose_gripper(self,
+            stamp,
+            possible_goals,
+            distance,
+            gripper_function,
+            velocity,
+            acceleration,
+            backup_in_gripper_frame):
+        '''
+        Basic function that handles collecting and aggregating smartmoves
+        '''
+
         list_of_valid_sequence = list()
         list_of_invalid_sequence = list()
         self.backoff_waypoints = []

costar_robot/costar_robot_msgs/CMakeLists.txt

@@ -36,11 +36,10 @@ find_package(catkin REQUIRED std_msgs geometry_msgs predicator_msgs sensor_msgs
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+add_message_files(
+  FILES
+  Constraint.msg
+)
 
 ## Generate services in the 'srv' folder
 add_service_files(

costar_robot/costar_robot_msgs/msg/Constraint.msg

@@ -0,0 +1,7 @@
+int32 pose_variable
+float32 threshold
+bool greater
+
+int32 POSE_X=0
+int32 POSE_Y=1
+int32 POSE_Z=2

costar_robot/costar_robot_msgs/srv/SmartMove.srv

@@ -5,5 +5,10 @@ string name # name of the requested motion
 float32 accel
 float32 vel
 float32 backoff # distance to move back when doing grasp or release
+
+# Restrictions when generating new poses
+# --------------------------------------
+bool above # new poses should only be created above object goals
+costar_robot_msgs/Constraint[] constraints
 ---
 string ack # what happened