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)