Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Visibility constraint #151

Open
wants to merge 5 commits into
base: main
Choose a base branch
from
Open

Visibility constraint #151

wants to merge 5 commits into from

Conversation

matteo-pacher
Copy link
Contributor

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.
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
Copy link
Member

@romanarust romanarust left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

def __init__(self):
raise NotImplementedError

def __init__(self, target_radius=None, target_pose=None, cone_sides=None,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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,

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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@@ -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))
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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)


Examples
--------
TODO
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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


Attributes
----------
target_frame: :class:`compas.geometry.Frame`
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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


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,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

delete this line and make them as attributes of the VisibilityConstraint

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

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
SENSOR_Z = 0
SENSOR_Y = 1
SENSOR_X = 2

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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Base automatically changed from master to main February 23, 2021 13:56
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants