def setup_ik_from_stance():
"""
Setup inverse kinematics from the simpler stance interface.
Notes
-----
This function is equivalent to :func:`setup_ik_from_tasks` below.
"""
robot.set_z(0.8) # hack to start with the robot above contacts
com_target = robot.get_com_point_mass()
lf_target = robot.left_foot.get_contact(pos=[0, 0.3, 0])
rf_target = robot.right_foot.get_contact(pos=[0, -0.3, 0])
stance = Stance(com=com_target, left_foot=lf_target, right_foot=rf_target)
stance.dof_tasks[robot.R_SHOULDER_R] = -0.5
stance.dof_tasks[robot.L_SHOULDER_R] = +0.5
stance.bind(robot)
[+0.88804317, 0.30865173, -0.34075422, 0.77617097],
[+0.14893562, -0.89431632, -0.42192002, 1.63681912],
[+0., 0., 0., 1.]])
contact = pymanoid.Contact(
shape=(0.12, 0.06),
pos=[0.15, -0.15, 0.],
# rpy=[-0.19798375, 0.13503151, 0],
rpy=[-0.35, -0.35, 0.05],
friction=0.7)
mass = 38. # [kg]
z_target = 0.8 # [m]
init_pos = array([0., 0., z_target])
init_vel = 4. * (contact.p - init_pos) * array([1., 1., 0.])
pendulum = InvertedPendulum(mass, init_pos, init_vel, contact, z_target)
stance = Stance(com=pendulum.com, right_foot=contact)
stance.bind(robot)
robot.ik.solve()
g = -sim.gravity[2]
lambda_min = 0.1 * g
lambda_max = 2.0 * g
stabilizer_2d = Stabilizer2D(pendulum, lambda_min, lambda_max, nb_steps=10)
stabilizer_3d = Stabilizer3D(pendulum,
lambda_min,
lambda_max,
nb_steps=10,
cop_gain=2.)
sim.schedule(stabilizer_2d)
sim.schedule(stabilizer_3d)
sim.schedule(pendulum)