Merge pull request #130 from compas-dev/JointConstraint

Joint constraint

Author: romanarust

CHANGELOG.rst

@@ -19,6 +19,8 @@ Unreleased
 
 * 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
 
 **Removed**
 

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

@@ -154,35 +154,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):