From 8d7822636124eb1411199b2942c64a0962333dc5 Mon Sep 17 00:00:00 2001 From: matteo-pacher <32835868+matteo-pacher@users.noreply.github.com> Date: Wed, 29 Apr 2020 15:04:41 +0200 Subject: [PATCH 1/4] VisibilityConstraint class and ROSmsg --- .../backends/ros/messages/moveit_msgs.py | 56 +++++++- .../backends/ros/planner_backend_moveit.py | 5 + src/compas_fab/robots/constraints.py | 131 +++++++++++++++++- 3 files changed, 185 insertions(+), 7 deletions(-) diff --git a/src/compas_fab/backends/ros/messages/moveit_msgs.py b/src/compas_fab/backends/ros/messages/moveit_msgs.py index c2a73b63e..3aa0a771d 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=None, 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_X = SENSOR_X # int() ? + self.SENSOR_Y = SENSOR_Y # int() ? + self.SENSOR_Z = SENSOR_Z # int() ? + self.sensor_view_direction = sensor_view_direction # int() ? + self.weight = float(weight) or 1. + + @classmethod + def from_visibility_constraint(cls, visibility_constraint): + """Creates a `VisibilityConstraint` from a :class:`compas_fab.robots.VisibilityConstraint`. + """ + target_pose_header = Header(frame_id=visibility_constraint.target_frame_reference_link) + target_pose = Pose.from_frame(visibility_constraint.target_frame) + target_pose_stamped = PoseStamped(target_pose_header, target_pose) + + sensor_pose_header = Header(frame_id=visibility_constraint.sensor_frame_reference_link) + 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_X'] = 0 # visibility_constraint.sensor_x + kwargs['SENSOR_Y'] = 0 # visibility_constraint.sensor_y + kwargs['SENSOR_Z'] = 0 # visibility_constraint.sensor_z + kwargs['sensor_view_direction'] = 0 # visibility_constraint.sensor_view_direction + kwargs['weight'] = visibility_constraint.weight + + print("from_visibility_constraint kwargs\n", kwargs) + + 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..a1b7f45ce 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)) else: raise NotImplementedError @@ -157,6 +161,7 @@ def inverse_kinematics_async(self, callback, errback, robot, frame, group, start_state.attached_collision_objects.append(aco) constraints = self._convert_constraints_to_rosmsg(constraints, header) + print("inverse_kinematics_async constraints\n", constraints) ik_request = PositionIKRequest(group_name=group, robot_state=start_state, diff --git a/src/compas_fab/robots/constraints.py b/src/compas_fab/robots/constraints.py index 286c1b1bb..61c63b7cb 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,121 @@ 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: 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_frame_reference_link: string + The name of the link that the target_frame is associated with. + + target_radius: float + The radius of the disc that should be maintained visible + + sensor_frame: 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. + + sensor_frame_reference_link: string + The name of the link that the sensor_frame is associated with. + + 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 + + uint8 SENSOR_X=2 + uint8 SENSOR_Y=1 + uint8 SENSOR_Z=0 + uint8 sensor_view_direction + # uint8[0, 255] + ### FRAME?? + # The axis that is assumed to indicate the direction of view for the sensor + # X = 2, Y = 1, Z = 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 + -------- + """ + + 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, 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.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})".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.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.weight) From 0ec102ab87e4751095973c32e1438d6e4b334fd0 Mon Sep 17 00:00:00 2001 From: matteo-pacher <32835868+matteo-pacher@users.noreply.github.com> Date: Wed, 29 Apr 2020 18:17:48 +0200 Subject: [PATCH 2/4] VisibilityConstraint(ROSmsg) bug fix SENSOR_X, SENSOR_Y, SENSOR_Z in the VisibilityConstraint moveit messsage definition are just constants, not arguments to pass --- .../backends/ros/messages/moveit_msgs.py | 28 ++++++------ .../backends/ros/planner_backend_moveit.py | 3 +- src/compas_fab/robots/constraints.py | 43 ++++++++----------- 3 files changed, 34 insertions(+), 40 deletions(-) diff --git a/src/compas_fab/backends/ros/messages/moveit_msgs.py b/src/compas_fab/backends/ros/messages/moveit_msgs.py index 3aa0a771d..4c7082623 100644 --- a/src/compas_fab/backends/ros/messages/moveit_msgs.py +++ b/src/compas_fab/backends/ros/messages/moveit_msgs.py @@ -283,28 +283,33 @@ class VisibilityConstraint(ROSmsg): 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=None, weight=1.): + 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_X = SENSOR_X # int() ? - self.SENSOR_Y = SENSOR_Y # int() ? - self.SENSOR_Z = SENSOR_Z # int() ? - self.sensor_view_direction = sensor_view_direction # int() ? + self.sensor_view_direction = int(sensor_view_direction) self.weight = float(weight) or 1. @classmethod - def from_visibility_constraint(cls, visibility_constraint): + def from_visibility_constraint(cls, visibility_constraint, base_link): """Creates a `VisibilityConstraint` from a :class:`compas_fab.robots.VisibilityConstraint`. """ - target_pose_header = Header(frame_id=visibility_constraint.target_frame_reference_link) + 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) - sensor_pose_header = Header(frame_id=visibility_constraint.sensor_frame_reference_link) + 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) @@ -315,14 +320,9 @@ def from_visibility_constraint(cls, visibility_constraint): 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_X'] = 0 # visibility_constraint.sensor_x - kwargs['SENSOR_Y'] = 0 # visibility_constraint.sensor_y - kwargs['SENSOR_Z'] = 0 # visibility_constraint.sensor_z - kwargs['sensor_view_direction'] = 0 # visibility_constraint.sensor_view_direction + kwargs['sensor_view_direction'] = visibility_constraint.sensor_view_direction kwargs['weight'] = visibility_constraint.weight - print("from_visibility_constraint kwargs\n", kwargs) - return cls(**kwargs) diff --git a/src/compas_fab/backends/ros/planner_backend_moveit.py b/src/compas_fab/backends/ros/planner_backend_moveit.py index a1b7f45ce..1e89b1839 100644 --- a/src/compas_fab/backends/ros/planner_backend_moveit.py +++ b/src/compas_fab/backends/ros/planner_backend_moveit.py @@ -133,7 +133,7 @@ def _convert_constraints_to_rosmsg(self, constraints, header): OrientationConstraint.from_orientation_constraint(header, c)) elif c.type == c.VISIBILITY: ros_constraints.visibility_constraints.append( - VisibilityConstraint.from_visibility_constraint(c)) + VisibilityConstraint.from_visibility_constraint(c, header.frame_id)) else: raise NotImplementedError @@ -161,7 +161,6 @@ def inverse_kinematics_async(self, callback, errback, robot, frame, group, start_state.attached_collision_objects.append(aco) constraints = self._convert_constraints_to_rosmsg(constraints, header) - print("inverse_kinematics_async constraints\n", constraints) ik_request = PositionIKRequest(group_name=group, robot_state=start_state, diff --git a/src/compas_fab/robots/constraints.py b/src/compas_fab/robots/constraints.py index 61c63b7cb..71fd92015 100644 --- a/src/compas_fab/robots/constraints.py +++ b/src/compas_fab/robots/constraints.py @@ -360,30 +360,26 @@ class VisibilityConstraint(Constraint): target_frame: 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_frame_reference_link: string + 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 target_radius: float The radius of the disc that should be maintained visible - sensor_frame: 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. - - sensor_frame_reference_link: string + 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 @@ -396,34 +392,29 @@ class VisibilityConstraint(Constraint): 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 - - uint8 SENSOR_X=2 - uint8 SENSOR_Y=1 - uint8 SENSOR_Z=0 - uint8 sensor_view_direction - # uint8[0, 255] - ### FRAME?? - # The axis that is assumed to indicate the direction of view for the sensor - # X = 2, Y = 1, Z = 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, weight=1.0): + 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 @@ -433,6 +424,7 @@ def __init__(self, target_frame, target_radius, sensor_frame, 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): @@ -441,12 +433,15 @@ def scale(self, 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})".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.weight) + 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.weight) + self.cone_sides, self.max_view_angle, self.max_range_angle, + self.sensor_view_direction, self.weight) From ea439c75810be182894ea1cadf18b71c8a7a5b9f Mon Sep 17 00:00:00 2001 From: Matteo Pacher <32835868+matteo-pacher@users.noreply.github.com> Date: Wed, 29 Apr 2020 18:31:02 +0200 Subject: [PATCH 3/4] Update CHANGELOG.rst --- CHANGELOG.rst | 2 ++ 1 file changed, 2 insertions(+) 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** From 4b88aa8651ab2c3c4159a3a1ab7f218b1463f606 Mon Sep 17 00:00:00 2001 From: matteo-pacher <32835868+matteo-pacher@users.noreply.github.com> Date: Wed, 29 Apr 2020 18:47:09 +0200 Subject: [PATCH 4/4] doc --- src/compas_fab/robots/constraints.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/compas_fab/robots/constraints.py b/src/compas_fab/robots/constraints.py index 71fd92015..0583cebc1 100644 --- a/src/compas_fab/robots/constraints.py +++ b/src/compas_fab/robots/constraints.py @@ -357,20 +357,20 @@ class VisibilityConstraint(Constraint): Attributes ---------- - target_frame: Frame + 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_frame_reference_link: string, optional - The name of the link that the target_frame is associated with. - Defaults to base link, thus world coordinates target_radius: float The radius of the disc that should be maintained visible - sensor_frame: Frame + 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