Add support for WorldFixedOrientConstraint

RobotLocomotion · Mar 14, 2017 · 3b72344 · 3b72344
1 parent 064927f
commit 3b72344
Showing 1 changed file with 9 additions and 0 deletions.
diff --git a/src/python/director/pydrakeik.py b/src/python/director/pydrakeik.py
@@ -328,6 +328,14 @@ def handleQuatConstraint(self, c, fields):
         qc = pydrakeik.WorldQuatConstraint(self.rigidBodyTree, bodyId, quat, tolerance, tspan)
         return qc
 
+    def handleWorldFixedOrientConstraint(self, c, fields):
+
+        bodyId = self.bodyNameToId[c.linkName]
+        tspan = np.asarray(c.tspan, dtype=float)
+
+        wc = pydrakeik.WorldFixedOrientConstraint(self.rigidBodyTree, bodyId, tspan)
+        return wc
+
     def handleWorldGazeDirConstraint(self, c, fields):
 
         bodyId = self.bodyNameToId[c.linkName]
@@ -408,6 +416,7 @@ def makeConstraints(self, fields):
             ikconstraints.PositionConstraint : self.handlePositionConstraint,
             ikconstraints.FixedLinkFromRobotPoseConstraint : self.handleFixedLinkFromRobotPoseConstraint,
             ikconstraints.QuatConstraint : self.handleQuatConstraint,
+            ikconstraints.WorldFixedOrientConstraint : self.handleWorldFixedOrientConstraint,
             ikconstraints.WorldGazeDirConstraint : self.handleWorldGazeDirConstraint,
             ikconstraints.WorldGazeOrientConstraint : self.handleWorldGazeOrientConstraint,
             ikconstraints.PostureConstraint : self.handlePostureConstraint,