0,
0, # gripper_right
0,
0, # head
]
q_init = robot.getCurrentConfig()
ps.addPartialCom("talos", ["talos/root_joint"])
ps.createStaticStabilityConstraints("balance", half_sitting, "talos",
ProblemSolver.FIXED_ON_THE_GROUND)
foot_placement = ["balance/pose-left-foot", "balance/pose-right-foot"]
foot_placement_complement = []
robot.setCurrentConfig(half_sitting)
com_wf = np.array(robot.getPartialCom("talos"))
tf_la = Transform(robot.getJointPosition(robot.leftAnkle))
com_la = tf_la.inverse().transform(com_wf)
ps.createRelativeComConstraint("com_talos", "talos", robot.leftAnkle,
com_la.tolist(), (True, True, True))
left_gripper_lock = []
right_gripper_lock = []
head_lock = []
other_lock = ["talos/torso_1_joint"]
for n in robot.jointNames:
s = robot.getJointConfigSize(n)
r = robot.rankInConfiguration[n]
if n.startswith("talos/gripper_right"):
ps.createLockedJoint(n, n, half_sitting[r:r + s])
