Add support for WorldGazeOrientConstraint

RobotLocomotion · Mar 8, 2017 · d853847 · d853847
1 parent 588f3c5
commit d853847
Showing 1 changed file with 19 additions and 0 deletions.
diff --git a/src/python/director/pydrakeik.py b/src/python/director/pydrakeik.py
@@ -341,6 +341,24 @@ def handleWorldGazeDirConstraint(self, c, fields):
         wc = pydrakeik.WorldGazeDirConstraint(self.rigidBodyTree, bodyId, bodyAxis, worldAxis, coneThreshold, tspan)
         return wc
 
+    def handleWorldGazeOrientConstraint(self, c, fields):
+
+        bodyId = self.bodyNameToId[c.linkName]
+        tspan = np.asarray(c.tspan, dtype=float)
+        axis = np.asarray(c.axis, dtype=float)
+        coneThreshold = c.coneThreshold
+        threshold = c.threshold
+
+        if isinstance(c.quaternion, vtk.vtkTransform):
+            quat = transformUtils.getNumpyFromTransform(c.quaternion)
+        else:
+            quat = np.asarray(c.quaternion, dtype=float)
+        if quat.shape == (4,4):
+            quat = transformUtils.transformations.quaternion_from_matrix(quat)
+
+        wc = pydrakeik.WorldGazeOrientConstraint(self.rigidBodyTree, bodyId, axis, quat, coneThreshold, threshold, tspan)
+        return wc
+
     def handlePostureConstraint(self, c, fields):
 
         tspan = np.asarray(c.tspan, dtype=float)
@@ -391,6 +409,7 @@ def makeConstraints(self, fields):
             ikconstraints.FixedLinkFromRobotPoseConstraint : self.handleFixedLinkFromRobotPoseConstraint,
             ikconstraints.QuatConstraint : self.handleQuatConstraint,
             ikconstraints.WorldGazeDirConstraint : self.handleWorldGazeDirConstraint,
+            ikconstraints.WorldGazeOrientConstraint : self.handleWorldGazeOrientConstraint,
             ikconstraints.PostureConstraint : self.handlePostureConstraint,
             ikconstraints.QuasiStaticConstraint : self.handleQuasiStaticConstraint,
             }