diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 099c18e3c..216643af4 100644 --- a/CHANGELOG.rst +++ b/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** diff --git a/src/compas_fab/backends/ros/messages/moveit_msgs.py b/src/compas_fab/backends/ros/messages/moveit_msgs.py index c2a73b63e..4c7082623 100644 --- a/src/compas_fab/backends/ros/messages/moveit_msgs.py +++ b/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,6 +252,7 @@ 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[] @@ -258,6 +260,7 @@ def __init__(self, constraints=None): 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, diff --git a/src/compas_fab/backends/ros/planner_backend_moveit.py b/src/compas_fab/backends/ros/planner_backend_moveit.py index 86305f0b4..1e89b1839 100644 --- a/src/compas_fab/backends/ros/planner_backend_moveit.py +++ b/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 diff --git a/src/compas_fab/robots/constraints.py b/src/compas_fab/robots/constraints.py index 286c1b1bb..0583cebc1 100644 --- a/src/compas_fab/robots/constraints.py +++ b/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)