From 37b34c89724f1cbc8443fe3f804894a1c4b4f026 Mon Sep 17 00:00:00 2001 From: Sam Creasey Date: Wed, 25 Jan 2017 15:11:09 -0500 Subject: [PATCH] Add support for WorldFixedOrientConstraint --- src/python/director/pydrakeik.py | 9 +++++++++ 1 file changed, 9 insertions(+) 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,