ps.createLockedJoint('romeo/' + j, robot.robotNames[0] + '/' + j, val) lockrhand = list() for j, v in robot.rightHandOpen.items(): lockrhand.append('romeo/' + j) if type(v) is float or type(v) is int: val = [ v, ] else: val = v ps.createLockedJoint('romeo/' + j, robot.robotNames[0] + '/' + j, val) lockHands = lockrhand + locklhand ## Create static stability constraint robot.leftAnkle = robot.robotNames[0] + '/' + robot.leftAnkle robot.rightAnkle = robot.robotNames[0] + '/' + robot.rightAnkle ps.addPartialCom('romeo', ['romeo/root_joint']) q = robot.getInitialConfig() r = placard.rank q[r:r + 3] = [.4, 0, 1.2] ps.addPartialCom("romeo", ["romeo/root_joint"]) robot.createStaticStabilityConstraint('balance/', 'romeo', robot.leftAnkle, robot.rightAnkle, q) balanceConstraints = [ 'balance/pose-left-foot', 'balance/pose-right-foot', 'balance/relative-com', ] commonConstraints = Constraints (numConstraints = balanceConstraints + \ lockHands)
ps.createLockedJoint('romeo/' + j, robot.displayName + '/' + j, val) lockrhand = list() for j, v in robot.rightHandOpen.iteritems(): lockrhand.append('romeo/' + j) if type(v) is float or type(v) is int: val = [ v, ] else: val = v ps.createLockedJoint('romeo/' + j, robot.displayName + '/' + j, val) lockHands = lockrhand + locklhand ## Create static stability constraint robot.leftAnkle = robot.displayName + '/' + robot.leftAnkle robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle ps.addPartialCom('romeo', ['romeo/root_joint']) q = robot.getInitialConfig() r = placard.rank q[r:r + 3] = [.4, 0, 1.2] ps.createStaticStabilityConstraints('balance-romeo', q, 'romeo', type=ProblemSolver.FIXED_ON_THE_GROUND) commonConstraints = Constraints(numConstraints=ps.balanceConstraints(), lockedJoints=lockHands) # build graph rules = [ Rule([
val = v; ps.createLockedJoint ('romeo/' + j, robot.displayName + '/' + j, val) lockrhand = list() for j,v in robot.rightHandOpen.iteritems(): lockrhand.append ('romeo/' + j) if type(v) is float or type(v) is int: val = [v,] else: val = v; ps.createLockedJoint ('romeo/' + j, robot.displayName + '/' + j, val) lockHands = lockrhand + locklhand ## Create static stability constraint robot.leftAnkle = robot.displayName + '/' + robot.leftAnkle robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle ps.addPartialCom ('romeo', ['romeo/root_joint']) q = robot.getInitialConfig () r = placard.rank q [r:r+3] = [.4, 0, 1.2] ps.createStaticStabilityConstraints ('balance-romeo', q, 'romeo', type = ProblemSolver.FIXED_ON_THE_GROUND) commonConstraints = Constraints (numConstraints = ps.balanceConstraints (), lockedJoints = lockHands) # build graph rules = [Rule (["romeo/l_hand","romeo/r_hand",], ["placard/low", ""], True), Rule (["romeo/l_hand","romeo/r_hand",], ["", "placard/high"], True), Rule (["romeo/l_hand","romeo/r_hand",], ["placard/low", "placard/high"], True), ]