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),
]