Merge branch 'master' into start_full_configuration

Author: romanarust

CHANGELOG.rst

@@ -29,6 +29,10 @@ Unreleased
 * ``MoveItPlanner``: ``inverse_kinematics`` takes now instance of ``Configuration`` and ``robot``
 * Property :class:`compas_fab.robots.Robot.artist` does not try to scale robot
   geometry if links and/or joints are not defined.
+* In :class:``compas_fab.robots.constraints.JointConstraint``, added ``tolerance_above`` and
+  ``tolerance_below`` for allowing asymmetrical constraints.
+* In :class:``compas_fab.robots.Robot``, changed the ``constraints_from_configuration``
+  function with ``tolerances_above`` and ``tolerances_below``.
 
 **Removed**
 

requirements-dev.txt

@@ -1,3 +1,4 @@
+attrs>=19.3.0
 sphinx_compas_theme>=0.4
 sphinx>=1.6
 invoke>=0.14

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

@@ -270,7 +270,7 @@ def from_joint_constraint(cls, joint_constraint):
         """Creates a `JointConstraint` from a :class:`compas_fab.robots.JointConstraint`.
         """
         c = joint_constraint
-        return cls(c.joint_name, c.value, c.tolerance, c.tolerance, c.weight)
+        return cls(c.joint_name, c.value, c.tolerance_above, c.tolerance_below, c.weight)
 
 
 class VisibilityConstraint(ROSmsg):

src/compas_fab/robots/constraints.py

@@ -159,35 +159,40 @@ class JointConstraint(Constraint):
         The name of the joint this contraint refers to.
     value: float
         The targeted value for that joint.
-    tolerance: float
-        The bound to be achieved is [value - tolerance, value + tolerance]
+    tolerance_above: float
+        Tolerance above the targeted joint value, in radians. Defaults to 0.
+    tolerance_below: float
+        Tolerance below the targeted joint value, in radians. Defaults to 0.
+        The bound to be achieved is [value - tolerance_below, value + tolerance_above].
     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
     --------
     >>> from compas_fab.robots import JointConstraint
-    >>> jc = JointConstraint("joint_0", 1.4, 0.1)
+    >>> jc = JointConstraint("joint_0", 1.4, 0.1, 0.1, 1.0)
 
     """
 
-    def __init__(self, joint_name, value, tolerance=0., weight=1.):
+    def __init__(self, joint_name, value, tolerance_above=0., tolerance_below=0., weight=1.):
         super(JointConstraint, self).__init__(self.JOINT, weight)
         self.joint_name = joint_name
         self.value = value
-        self.tolerance = tolerance
+        self.tolerance_above = abs(tolerance_above)
+        self.tolerance_below = abs(tolerance_below)
 
     def scale(self, scale_factor):
         self.value *= scale_factor
-        self.tolerance *= scale_factor
+        self.tolerance_above *= scale_factor
+        self.tolerance_below *= scale_factor
 
     def __repr__(self):
-        return "JointConstraint('{0}', {1}, {2}, {3})".format(self.joint_name, self.value, self.tolerance, self.weight)
+        return "JointConstraint('{0}', {1}, {2}, {3}, {4})".format(self.joint_name, self.value, self.tolerance_above, self.tolerance_below, self.weight)
 
     def copy(self):
         cls = type(self)
-        return cls(self.joint_name, self.value, self.tolerance, self.weight)
+        return cls(self.joint_name, self.value, self.tolerance_above, self.tolerance_below, self.weight)
 
 
 class OrientationConstraint(Constraint):

src/compas_fab/robots/robot.py

@@ -869,33 +869,40 @@ def constraints_from_frame(self, frame_WCF, tolerance_position, tolerances_axes,
         oc = self.orientation_constraint_from_frame(frame_WCF, tolerances_axes, group)
         return [pc, oc]
 
-    def constraints_from_configuration(self, configuration, tolerances, group=None):
+    def constraints_from_configuration(self, configuration, tolerances_above, tolerances_below, group=None):
         """Returns joint constraints on all joints of the configuration.
 
         Parameters
         ----------
         configuration: :class:`compas_fab.robots.Configuration`
             The target configuration.
-        tolerances: list of float
-            The tolerances (as +/-) on each of the joints defining the bound in radian
-            to be achieved. If only one value is passed it will be used to create
-            bounds for all joint constraints.
+        tolerances_above: list of float
+            The tolerances above the targeted configuration's joint value on each
+            of the joints, defining the upper bound in radian to be achieved.
+            If only one value is passed, it will be used to create upper bounds
+            for all joint constraints.
+        tolerances_below: list of float
+            The tolerances below the targeted configuration's joint value on each
+            of the joints, defining the upper bound in radian to be achieved.
+            If only one value is passed, it will be used to create lower bounds
+            for all joint constraints.
         group: str, optional
             The planning group for which we specify the constraint. Defaults to
             the robot's main planning group.
 
         Examples
         --------
         >>> configuration = Configuration.from_revolute_values([-0.042, 4.295, -4.110, -3.327, 4.755, 0.])
-        >>> tolerances = [math.radians(5)] * 6
+        >>> tolerances_above = [math.radians(1)] * 6
+        >>> tolerances_below = [math.radians(1)] * 6
         >>> group = robot.main_group_name
-        >>> robot.constraints_from_configuration(configuration, tolerances, group)
-        [JointConstraint('shoulder_pan_joint', -0.042, 0.08726646259971647, 1.0), \
-        JointConstraint('shoulder_lift_joint', 4.295, 0.08726646259971647, 1.0), \
-        JointConstraint('elbow_joint', -4.11, 0.08726646259971647, 1.0), \
-        JointConstraint('wrist_1_joint', -3.327, 0.08726646259971647, 1.0), \
-        JointConstraint('wrist_2_joint', 4.755, 0.08726646259971647, 1.0), \
-        JointConstraint('wrist_3_joint', 0.0, 0.08726646259971647, 1.0)]
+        >>> robot.constraints_from_configuration(configuration, tolerances_above, tolerances_below, group)
+        [JointConstraint('shoulder_pan_joint', -0.042, 0.017453292519943295, 0.017453292519943295, 1.0), \
+        JointConstraint('shoulder_lift_joint', 4.295, 0.017453292519943295, 0.017453292519943295, 1.0), \
+        JointConstraint('elbow_joint', -4.11, 0.017453292519943295, 0.017453292519943295, 1.0), \
+        JointConstraint('wrist_1_joint', -3.327, 0.017453292519943295, 0.017453292519943295, 1.0), \
+        JointConstraint('wrist_2_joint', 4.755, 0.017453292519943295, 0.017453292519943295, 1.0), \
+        JointConstraint('wrist_3_joint', 0.0, 0.017453292519943295, 0.017453292519943295, 1.0)]
 
         Raises
         ------
@@ -917,15 +924,20 @@ def constraints_from_configuration(self, configuration, tolerances, group=None):
         if len(joint_names) != len(configuration.values):
             raise ValueError("The passed configuration has %d values, the group %s needs however: %d" % (
                 len(configuration.values), group, len(joint_names)))
-        if len(tolerances) == 1:
-            tolerances = tolerances * len(joint_names)
-        elif len(tolerances) != len(configuration.values):
-            raise ValueError("The passed configuration has %d values, the tolerances however: %d" % (
-                len(configuration.values), len(tolerances)))
+        if len(tolerances_above) == 1:
+            tolerances_above = tolerances_above * len(joint_names)
+        elif len(tolerances_above) != len(configuration.values):
+            raise ValueError("The passed configuration has %d values, the tolerances_above however: %d" % (
+                len(configuration.values), len(tolerances_above)))
+        if len(tolerances_below) == 1:
+            tolerances_below = tolerances_below * len(joint_names)
+        elif len(tolerances_below) != len(configuration.values):
+            raise ValueError("The passed configuration has %d values, the tolerances_below however: %d" % (
+                len(configuration.values), len(tolerances_below)))
 
         constraints = []
-        for name, value, tolerance in zip(joint_names, configuration.values, tolerances):
-            constraints.append(JointConstraint(name, value, tolerance))
+        for name, value, tolerance_above, tolerance_below in zip(joint_names, configuration.values, tolerances_above, tolerances_below):
+            constraints.append(JointConstraint(name, value, tolerance_above, tolerance_below))
         return constraints
 
     # ==========================================================================
@@ -1254,9 +1266,10 @@ def plan_motion(self, goal_constraints, start_configuration=None,
         1.0
         >>> # Example with joint constraints (to the UP configuration)
         >>> configuration = Configuration.from_revolute_values([0.0, -1.5707, 0.0, -1.5707, 0.0, 0.0])
-        >>> tolerances = [math.radians(5)] * 6
+        >>> tolerances_above = [math.radians(5)] * len(configuration.values)
+        >>> tolerances_below = [math.radians(5)] * len(configuration.values)
         >>> group = robot.main_group_name
-        >>> goal_constraints = robot.constraints_from_configuration(configuration, tolerances, group)
+        >>> goal_constraints = robot.constraints_from_configuration(configuration, tolerances_above, tolerances_below, group)
         >>> trajectory = robot.plan_motion(goal_constraints, start_configuration, group, planner_id='RRT')
         >>> trajectory.fraction
         1.0