Visibility constraint #151

matteo-pacher · 2020-04-29T16:33:15Z

Visibility constraint for MoveIt backend:

added :class:compas_fab.robots.constraints.VisibilityConstraint
added :class:compas_fab.backends.ros.messages.VisibilityConstraint

I am testing it as a way of getting IK solutions that don't obstruct iGPS visibility.

Initial tests seem promising :)

What type of change is this?

New feature in a backwards-compatible manner.

Checklist

I added a line to the CHANGELOG.rst file in the Unreleased section under the most fitting heading (e.g. Added, Changed, Removed).
I ran all tests on my computer and it's all green (i.e. invoke test).
I ran lint on my computer and there are no errors (i.e. invoke lint) .
I added new functions/classes and made them available on a second-level import, e.g. compas_fab.robots.Configuration.
I have added necessary documentation (to be completed and corrected)

SENSOR_X, SENSOR_Y, SENSOR_Z in the VisibilityConstraint moveit messsage definition are just constants, not arguments to pass

…ler/compas_fab into VisibilityConstraint

romanarust

Hi Matteo! Great! I am curious to see how and if this constraint works. As discussed, would be cool if you can add an example (in the docs).

romanarust · 2020-05-14T08:49:34Z

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

+                 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


Suggested change

self.cone_sides = cone_sides if cone_sides >= 3 else 3

self.cone_sides = max(cone_sides, 3)

romanarust · 2020-05-14T08:49:53Z

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

-    def __init__(self):
-        raise NotImplementedError
+
+    def __init__(self, target_radius=None, target_pose=None, cone_sides=None,


Suggested change

def __init__(self, target_radius=None, target_pose=None, cone_sides=None,

def __init__(self, target_radius=None, target_pose=None, cone_sides=3,

romanarust · 2020-05-14T08:51:50Z

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

+        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


Suggested change

kwargs['cone_sides'] = visibility_constraint.cone_sides if visibility_constraint.cone_sides >= 3 else 3

kwargs['cone_sides'] = visibility_constraint.cone_sides

romanarust · 2020-05-14T08:53:17Z

src/compas_fab/backends/ros/planner_backend_moveit.py

@@ -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))


See comment further below: the reference frame in this case should already be defined in c.

Suggested change

VisibilityConstraint.from_visibility_constraint(c, header.frame_id))

VisibilityConstraint.from_visibility_constraint(c)

romanarust · 2020-05-14T09:30:36Z

src/compas_fab/robots/constraints.py

+
+    Examples
+    --------
+    TODO


Would be good to add an example, or maybe even better: an example for our docs with a nice graphic on how the visibility constraint works :)

romanarust · 2020-05-14T09:36:39Z

src/compas_fab/robots/constraints.py

+    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


not sure if this is appropriate here, as this refers to moveit's docs

romanarust · 2020-05-14T12:50:16Z

src/compas_fab/robots/constraints.py

+
+    Attributes
+    ----------
+    target_frame: :class:`compas.geometry.Frame`


As discussed just before in a meeting, to reduce the number of parameters to pass to the constraint, maybe it would be good add another class: A frame relative to a robots link (like the PoseStamped in ROS). Then you don't have to pass target_frame+target_frame_reference_link and sensor_frame+sensor_frame_reference_link individually, but only target_reference_frame and sensor_reference_frame. (if nameing of ReferenceFrame would be approved ;))

romanarust · 2020-05-14T14:20:14Z

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

+
+    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,


delete this line and make them as attributes of the VisibilityConstraint

romanarust · 2020-05-14T14:21:13Z

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

@@ -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
+


Suggested change

SENSOR_Z = 0

SENSOR_Y = 1

SENSOR_X = 2

romanarust · 2020-05-14T14:27:33Z

src/compas_fab/robots/constraints.py

+        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


probably needs to be defined and checked.
see possible_types of BoundingVolume check

matteo-pacher added 3 commits April 29, 2020 15:04

VisibilityConstraint class and ROSmsg

8d78226

VisibilityConstraint(ROSmsg) bug fix

0ec102a

SENSOR_X, SENSOR_Y, SENSOR_Z in the VisibilityConstraint moveit messsage definition are just constants, not arguments to pass

Update CHANGELOG.rst

ea439c7

matteo-pacher requested review from gonzalocasas and romanarust April 29, 2020 16:33

matteo-pacher added 2 commits April 29, 2020 18:47

doc

4b88aa8

Merge branch 'VisibilityConstraint' of https://github.com/gramaziokoh…

b854186

…ler/compas_fab into VisibilityConstraint

romanarust requested changes May 14, 2020

View reviewed changes

gonzalocasas force-pushed the master branch from 5c5e7b2 to 9b00a2e Compare November 3, 2020 00:35

Base automatically changed from master to main February 23, 2021 13:56

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Visibility constraint #151

Visibility constraint #151

matteo-pacher commented Apr 29, 2020

romanarust left a comment

romanarust May 14, 2020

romanarust May 14, 2020

romanarust May 14, 2020

romanarust May 14, 2020

romanarust May 14, 2020

romanarust May 14, 2020

romanarust May 14, 2020

romanarust May 14, 2020

romanarust May 14, 2020

romanarust May 14, 2020

	self.cone_sides = cone_sides if cone_sides >= 3 else 3
	self.cone_sides = max(cone_sides, 3)

	def __init__(self, target_radius=None, target_pose=None, cone_sides=None,
	def __init__(self, target_radius=None, target_pose=None, cone_sides=3,

	kwargs['cone_sides'] = visibility_constraint.cone_sides if visibility_constraint.cone_sides >= 3 else 3
	kwargs['cone_sides'] = visibility_constraint.cone_sides

	VisibilityConstraint.from_visibility_constraint(c, header.frame_id))
	VisibilityConstraint.from_visibility_constraint(c)

+    SENSOR_Z = 0
+    SENSOR_Y = 1
+    SENSOR_X = 2

Visibility constraint #151

Are you sure you want to change the base?

Visibility constraint #151

Conversation

matteo-pacher commented Apr 29, 2020

What type of change is this?

Checklist

romanarust left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment