Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix = '_capsule'
#Robot.urdfSuffix = ''
#Robot.srdfSuffix= ''
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.setNumericalConstraints("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.iteritems():
ljName = "locked_" + name
ps.createLockedJoint(ljName, name, value)
lockedJoints.append(ljName)
