diff --git a/src/python/director/pydrakeik.py b/src/python/director/pydrakeik.py index 2c26da735..ef1077dc1 100644 --- 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,