# Add constraints wcl = WsClient() wcl.problem.addStaticStabilityConstraints("balance", q0, robot.leftAnkle, robot.rightAnkle) ps = ProblemSolver(robot) ps.setNumericalConstraints("balance", [ "balance/relative-com", "balance/relative-orientation", "balance/relative-position", "balance/orientation-left-foot", "balance/position-left-foot" ]) # lock hands in closed position lockedDofs = robot.leftHandClosed() for name, value in lockedDofs.iteritems(): ps.lockDof(name, value, 0, 0) lockedDofs = robot.rightHandClosed() for name, value in lockedDofs.iteritems(): ps.lockDof(name, value, 0, 0) q1 = [ 0.0, 0.0, 0.705, 1.0, 0., 0., 0.0, 0.0, 0.0, 0.0, 0.0, -0.4, 0, -1.2, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0 ] res = ps.applyConstraints(q1) if res[0]:
# Add constraints wcl = WsClient () wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle, robot.rightAnkle) ps = ProblemSolver (robot) ps.setNumericalConstraints ("balance", ["balance/relative-com", "balance/relative-orientation", "balance/relative-position", "balance/orientation-left-foot", "balance/position-left-foot"]) # lock hands in closed position lockedDofs = robot.leftHandClosed () for name, value in lockedDofs.iteritems (): ps.lockDof (name, value, 0, 0) lockedDofs = robot.rightHandClosed () for name, value in lockedDofs.iteritems (): ps.lockDof (name, value, 0, 0) q1 = [0.0, 0.0, 0.705, 1.0, 0., 0., 0.0, 0.0, 0.0, 0.0, 0.0, -0.4, 0, -1.2, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0] res = ps.applyConstraints (q1) if res [0]: q1proj = res [1] else: raise RuntimeError ("Failed to apply constraint.")