robot = Robot('hrp2_14') robot.setJointBounds("root_joint", [-3, 3, -3, 3, 0, 1, -1, 1, -1, 1, -1, 1, -1, 1]) cl = robot.client q0 = robot.getInitialConfig() # Add constraints ps = ProblemSolver(robot) ps.addPartialCom('hrp2_14', ['root_joint']) robot.createSlidingStabilityConstraint("balance/", 'hrp2_14', robot.leftAnkle, robot.rightAnkle, q0) ps.addNumericalConstraints("balance", [ "balance/relative-com", "balance/relative-pose", "balance/pose-left-foot", ]) # lock hands in closed position lockedjointDict = robot.leftHandClosed() lockedJoints = list() for name, value in lockedjointDict.items(): ljName = "locked_" + name ps.createLockedJoint(ljName, name, value) lockedJoints.append(ljName) lockedjointDict = robot.rightHandClosed() for name, value in lockedjointDict.items(): ljName = "locked_" + name ps.createLockedJoint(ljName, name, value)
robot.setJointBounds("root_joint", [-3, 3, -3, 3, 0, 1, -1, 1, -1, 1, -1, 1, -1, 1]) cl = robot.client q0 = robot.getInitialConfig() # Add constraints wcl = WsClient() wcl.problem.addStaticStabilityConstraints("balance", q0, robot.leftAnkle, robot.rightAnkle, "", Problem.SLIDING) ps = ProblemSolver(robot) ps.addNumericalConstraints("balance", [ "balance/relative-com", "balance/relative-orientation", "balance/relative-position", "balance/orientation-left-foot", "balance/position-left-foot" ]) # lock hands in closed position lockedjointDict = robot.leftHandClosed() lockedJoints = list() for name, value in lockedjointDict.iteritems(): ljName = "locked_" + name ps.createLockedJoint(ljName, name, value) lockedJoints.append(ljName) lockedjointDict = robot.rightHandClosed() for name, value in lockedjointDict.iteritems(): ljName = "locked_" + name ps.createLockedJoint(ljName, name, value)