Visibility constraint

CHANGELOG.rst

@@ -17,6 +17,8 @@ Unreleased
 * Added ``Configuration.scaled``
 * Added ``full_joint_state`` to ``Robot.inverse_kinematics``
 * Added ``Semantics.get_all_configurable_joints``
+* Added :class:``compas_fab.robots.constraints.VisibilityConstraint``
+* Added :class:``compas_fab.backends.ros.messages.VisibilityConstraint``
 
 **Changed**
 

src/compas_fab/backends/ros/messages/moveit_msgs.py

@@ -242,6 +242,7 @@ def __init__(self, keys=None, values=None, descriptions=None):
 class WorkspaceParameters(ROSmsg):
     """http://docs.ros.org/kinetic/api/moveit_msgs/html/msg/WorkspaceParameters.html
     """
+
     def __init__(self, header=None, min_corner=None, max_corner=None):
         self.header = header or Header()
         self.min_corner = min_corner or Vector3(-1000, -1000, -1000)
@@ -251,13 +252,15 @@ def __init__(self, header=None, min_corner=None, max_corner=None):
 class TrajectoryConstraints(ROSmsg):
     """http://docs.ros.org/kinetic/api/moveit_msgs/html/msg/TrajectoryConstraints.html
     """
+
     def __init__(self, constraints=None):
         self.constraints = constraints or []  # Constraints[]
 
 
 class JointConstraint(ROSmsg):
     """http://docs.ros.org/kinetic/api/moveit_msgs/html/msg/JointConstraint.html
     """
+
     def __init__(self, joint_name="", position=0, tolerance_above=0, tolerance_below=0, weight=1.):
         self.joint_name = joint_name
         self.position = float(position)
@@ -276,13 +279,57 @@ def from_joint_constraint(cls, joint_constraint):
 class VisibilityConstraint(ROSmsg):
     """http://docs.ros.org/kinetic/api/moveit_msgs/html/msg/VisibilityConstraint.html
     """
-    def __init__(self):
-        raise NotImplementedError
+
+    def __init__(self, target_radius=None, target_pose=None, cone_sides=None,
+                 sensor_pose=None, max_view_angle=0.0, max_range_angle=0.0,
+                 SENSOR_X=None, SENSOR_Y=None, SENSOR_Z=None,
+                 sensor_view_direction=2, weight=1.):
+        self.target_radius = float(target_radius)
+        self.target_pose = target_pose
+        self.cone_sides = cone_sides if cone_sides >= 3 else 3
+        self.sensor_pose = sensor_pose
+        self.max_view_angle = float(max_view_angle)
+        self.max_range_angle = float(max_range_angle)
+        self.sensor_view_direction = int(sensor_view_direction)
+        self.weight = float(weight) or 1.
+
+    @classmethod
+    def from_visibility_constraint(cls, visibility_constraint, base_link):
+        """Creates a `VisibilityConstraint` from a :class:`compas_fab.robots.VisibilityConstraint`.
+        """
+        if visibility_constraint.target_frame_reference_link is not None:
+            target_pose_frame_id = visibility_constraint.target_frame_reference_link
+        else:
+            target_pose_frame_id = base_link
+        target_pose_header = Header(frame_id=target_pose_frame_id)
+        target_pose = Pose.from_frame(visibility_constraint.target_frame)
+        target_pose_stamped = PoseStamped(target_pose_header, target_pose)
+
+        if visibility_constraint.sensor_frame_reference_link is not None:
+            sensor_pose_frame_id = visibility_constraint.sensor_frame_reference_link
+        else:
+            sensor_pose_frame_id = base_link
+        sensor_pose_header = Header(frame_id=sensor_pose_frame_id)
+        sensor_pose = Pose.from_frame(visibility_constraint.sensor_frame)
+        sensor_pose_stamped = PoseStamped(sensor_pose_header, sensor_pose)
+
+        kwargs = {}
+        kwargs['target_radius'] = visibility_constraint.target_radius
+        kwargs['target_pose'] = target_pose_stamped
+        kwargs['cone_sides'] = visibility_constraint.cone_sides if visibility_constraint.cone_sides >= 3 else 3
+        kwargs['sensor_pose'] = sensor_pose_stamped
+        kwargs['max_view_angle'] = visibility_constraint.max_view_angle
+        kwargs['max_range_angle'] = visibility_constraint.max_range_angle
+        kwargs['sensor_view_direction'] = visibility_constraint.sensor_view_direction
+        kwargs['weight'] = visibility_constraint.weight
+
+        return cls(**kwargs)
 
 
 class BoundingVolume(ROSmsg):
     """http://docs.ros.org/kinetic/api/moveit_msgs/html/msg/BoundingVolume.html
     """
+
     def __init__(self, primitives=None, primitive_poses=None, meshes=None,
                  mesh_poses=None):
         self.primitives = primitives or []            # shape_msgs/SolidPrimitive[]
@@ -347,6 +394,7 @@ def from_bounding_volume(cls, bounding_volume):
 class PositionConstraint(ROSmsg):
     """http://docs.ros.org/kinetic/api/moveit_msgs/html/msg/PositionConstraint.html
     """
+
     def __init__(self, header=None, link_name=None, target_point_offset=None,
                  constraint_region=None, weight=None):
         self.header = header or Header()
@@ -366,6 +414,7 @@ def from_position_constraint(cls, header, position_constraint):
 class OrientationConstraint(ROSmsg):
     """http://docs.ros.org/kinetic/api/moveit_msgs/html/msg/OrientationConstraint.html
     """
+
     def __init__(self, header=None, orientation=None, link_name=None,
                  absolute_x_axis_tolerance=0.0, absolute_y_axis_tolerance=0.0,
                  absolute_z_axis_tolerance=0.0, weight=1):
@@ -437,6 +486,7 @@ def human_readable(self):
 class AllowedCollisionMatrix(ROSmsg):
     """http://docs.ros.org/melodic/api/moveit_msgs/html/msg/AllowedCollisionMatrix.html
     """
+
     def __init__(self, entry_names=None, entry_values=None, default_entry_names=None, default_entry_values=None):
         self.entry_names = entry_names or []  # string[]
         self.entry_values = entry_values or []  # moveit_msgs/AllowedCollisionEntry[]
@@ -447,6 +497,7 @@ def __init__(self, entry_names=None, entry_values=None, default_entry_names=None
 class PlanningSceneWorld(ROSmsg):
     """http://docs.ros.org/melodic/api/moveit_msgs/html/msg/PlanningSceneWorld.html
     """
+
     def __init__(self, collision_objects=None, octomap=None):
         self.collision_objects = collision_objects or []  # collision objects # CollisionObject[]
         self.octomap = octomap or OctomapWithPose()  # octomap_msgs/OctomapWithPose
@@ -462,6 +513,7 @@ def from_msg(cls, msg):
 class PlanningScene(ROSmsg):
     """http://docs.ros.org/melodic/api/moveit_msgs/html/msg/PlanningScene.html
     """
+
     def __init__(self, name='', robot_state=None, robot_model_name='',
                  fixed_frame_transforms=None, allowed_collision_matrix=None,
                  link_padding=None, link_scale=None, object_colors=None, world=None,

src/compas_fab/backends/ros/planner_backend_moveit.py

@@ -34,6 +34,7 @@
 from compas_fab.backends.ros.messages import PositionIKRequest
 from compas_fab.backends.ros.messages import RobotState
 from compas_fab.backends.ros.messages import TrajectoryConstraints
+from compas_fab.backends.ros.messages import VisibilityConstraint
 from compas_fab.backends.ros.planner_backend import PlannerBackend
 from compas_fab.backends.ros.planner_backend import ServiceDescription
 from compas_fab.robots import Configuration
@@ -130,6 +131,9 @@ def _convert_constraints_to_rosmsg(self, constraints, header):
             elif c.type == c.ORIENTATION:
                 ros_constraints.orientation_constraints.append(
                     OrientationConstraint.from_orientation_constraint(header, c))
+            elif c.type == c.VISIBILITY:
+                ros_constraints.visibility_constraints.append(
+                    VisibilityConstraint.from_visibility_constraint(c, header.frame_id))
             else:
                 raise NotImplementedError
 

src/compas_fab/robots/constraints.py

@@ -1,12 +1,14 @@
-from __future__ import print_function
 from __future__ import absolute_import
 from __future__ import division
+from __future__ import print_function
 
-from compas.geometry import Scale
+from compas.geometry import Frame
 from compas.geometry import Rotation
+from compas.geometry import Scale
 
 __all__ = ['BoundingVolume', 'Constraint', 'JointConstraint',
-           'OrientationConstraint', 'PositionConstraint']
+           'OrientationConstraint', 'PositionConstraint',
+           'VisibilityConstraint']
 
 
 class BoundingVolume(object):
@@ -126,11 +128,12 @@ class Constraint(object):
     JOINT = 1
     POSITION = 2
     ORIENTATION = 3
-    possible_types = (JOINT, POSITION, ORIENTATION)
+    VISIBILITY = 4
+    possible_types = (JOINT, POSITION, ORIENTATION, VISIBILITY)
 
     def __init__(self, type, weight=1.):
         if type not in self.possible_types:
-            raise ValueError("Type must be %d, %d or %d" % self.possible_types)
+            raise ValueError("Type must be %d, %d, %d, or %d" % self.possible_types)
         self.type = type
         self.weight = weight
 
@@ -329,3 +332,116 @@ def __repr__(self):
     def copy(self):
         cls = type(self)
         return cls(self.link_name, self.bounding_volume.copy(), self.weight)
+
+
+class VisibilityConstraint(Constraint):
+    """The constraint is useful to maintain visibility to a disc (the target) in a particular frame.
+    This disc forms the base of a visibiliy cone whose tip is at the origin of the sensor.
+    Maintaining visibility is done by ensuring the robot does not obstruct the visibility cone.
+    Note:
+    This constraint does NOT enforce minimum or maximum distances between the sensor
+    and the target, nor does it enforce the target to be in the field of view of
+    the sensor. A PositionConstraint can (and probably should) be used for such purposes.
+
+    It is important to note that the sensor and the target frames must not result in collision -
+    if they are associated with sensor frames or robot links they must be specified to be outside
+    the robot's body or all states will violate the VisibilityConstraint.
+    The visibiliy cone is allowed to collide with the actual sensor associated with the header frame,
+    just not with any other links.
+
+    Detailed explanations of visibility constraint:
+    http://docs.ros.org/kinetic/api/moveit_core/html/classkinematic__constraints_1_1VisibilityConstraint.html
+
+    NOTE: getMarkers() could be used to visualize feedback?
+    http://docs.ros.org/kinetic/api/moveit_core/html/classkinematic__constraints_1_1VisibilityConstraint.html#a28a578edc7a97627aaa78ef061871072
+
+    Attributes
+    ----------
+    target_frame: :class:`compas.geometry.Frame`
+        The frame of the disc; as the robot moves, the frame of the disc may change as well
+        this can be in the frame of a particular robot link, for example
+    target_radius: float
+        The radius of the disc that should be maintained visible
+    sensor_frame: :class:`compas.geometry.Frame`
+        The frame in which visibility is to be maintained.
+        Frame id defaults to robot base link.
+        It is assumed the sensor can look directly at the target, in any direction.
+        This assumption is usually not true, but additional PositionConstraints
+        can resolve this issue.
+    target_frame_reference_link: string, optional
+        The name of the link that the target_frame is associated with.
+        Defaults to base link, thus world coordinates
+    sensor_frame_reference_link: string, optional
+        The name of the link that the sensor_frame is associated with.
+        Defaults to base link, thus world coordinates
+    cone_sides: int, optional
+        From the sensor origin towards the target, the disc forms a visibility cone.
+        This cone is approximated using many sides. For example, when using 4 sides,
+        that in fact makes the visibility region be a pyramid.
+        This value should always be 3 or more.
+        Defaults to 3
+    max_view_angle: float, optional
+        Even though the disc is maintained visible, the visibility cone can be very small
+        because of the orientation of the disc with respect to the sensor. It is possible
+        for example to view the disk almost from a side, in which case the visibility cone
+        can end up having close to 0 volume. The view angle is defined to be the angle between
+        the normal to the visibility disc and the direction vector from the sensor origin.
+        The value below represents the minimum desired view angle. For a perfect view,
+        this value will be 0 (the two vectors are exact opposites). For a completely obstructed view
+        this value will be Pi/2 (the vectors are perpendicular). This value defined below
+        is the maximum view angle to be maintained. This should be a value in the open interval
+        (0, Pi/2). If 0 is set, the view angle is NOT enforced.
+        Defaults to 0.0
+    max_range_angle: float, optional
+        This angle is used similarly to max_view_angle but limits the maximum angle
+        between the sensor origin direction vector and the axis that connects the
+        sensor origin to the target frame origin. The value is again in the range (0, Pi/2)
+        and is NOT enforced if set to 0.
+        Defaults to 0.0
+    sensor_view_direction: int, optional
+        The axis that is assumed to indicate the direction of view for the sensor
+        X axis = 2, Y axis = 1, Z axis = 0
+        Defaults to 0
+    weight: float, optional
+        A weighting factor for this constraint. Denotes relative importance to
+        other constraints. Closer to zero means less important. Defaults to 1.
+
+    Examples
+    --------
+    TODO
+    """
+
+    def __init__(self, target_frame, target_radius, sensor_frame,
+                 target_frame_reference_link=None, sensor_frame_reference_link=None,
+                 cone_sides=3, max_view_angle=0.0, max_range_angle=0.0,
+                 sensor_view_direction=0, weight=1.0):
+        super(VisibilityConstraint, self).__init__(self.VISIBILITY, weight)
+        self.target_frame = target_frame
+        self.target_radius = target_radius
+        self.sensor_frame = sensor_frame
+        self.target_frame_reference_link = target_frame_reference_link
+        self.sensor_frame_reference_link = sensor_frame_reference_link
+        self.cone_sides = cone_sides
+        self.max_view_angle = max_view_angle
+        self.max_range_angle = max_range_angle
+        self.sensor_view_direction = sensor_view_direction
+        self.weight = weight
+
+    def scale(self, scale_factor):
+        self.target_frame = Frame(self.target_frame.point * scale_factor, self.target_frame.xaxis, self.target_frame.yaxis)
+        self.target_radius *= scale_factor
+        self.sensor_frame = Frame(self.sensor_frame.point * scale_factor, self.sensor_frame.xaxis, self.sensor_frame.yaxis)
+
+    def __repr__(self):
+        return "VisibilityConstraint({0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, {9})".format(
+            self.target_frame, self.target_radius, self.sensor_frame,
+            self.target_frame_reference_link, self.sensor_frame_reference_link,
+            self.cone_sides, self.max_view_angle, self.max_range_angle,
+            self.sensor_view_direction, self.weight)
+
+    def copy(self):
+        cls = type(self)
+        return cls(self.target_frame, self.target_radius, self.sensor_frame,
+                   self.target_frame_reference_link, self.sensor_frame_reference_link,
+                   self.cone_sides, self.max_view_angle, self.max_range_angle,
+                   self.sensor_view_direction, self.weight)