def update_robot_ik():
"""Update IK targets once the WPG has been constructed."""
robot.ik.tasks['COM'].update_target(pendulum.com_state)
robot.ik.add_task(MinCAMTask(robot))
# prevent robot from leaning backwards:
robot.ik.add_task(DOFTask(robot, robot.ROT_P, 0., weight=1e-4))
robot.ik.add_task(DOFTask(robot, robot.CHEST_P, 0.2, weight=1e-4))
robot.ik.add_task(DOFTask(robot, robot.CHEST_Y, 0., weight=1e-4))
# prevent lateral arm collisions with the chest:
robot.ik.add_task(DOFTask(robot, robot.R_SHOULDER_R, -0.4))
robot.ik.add_task(DOFTask(robot, robot.L_SHOULDER_R, +0.4))
# prevent arms from leaning backward:
robot.ik.add_task(DOFTask(robot, robot.L_SHOULDER_P, 0.))
robot.ik.add_task(DOFTask(robot, robot.R_SHOULDER_P, 0.))
robot.ik.tasks[robot.left_foot.name].weight = 1.
robot.ik.tasks[robot.right_foot.name].weight = 1.
robot.ik.tasks['COM'].weight = 1e-2
robot.ik.tasks['MIN_CAM'].weight = 1e-4
robot.ik.tasks['ROT_P'].weight = 1e-4
robot.ik.tasks[robot.chest_p_name].weight = 1e-4
robot.ik.tasks[robot.chest_y_name].weight = 1e-4
if '--shoulders' in sys.argv:
robot.ik.tasks['L_SHOULDER_P'].weight = 1e-4
robot.ik.tasks['L_SHOULDER_R'].weight = 1e-4
robot.ik.tasks['R_SHOULDER_P'].weight = 1e-4
robot.ik.tasks['R_SHOULDER_R'].weight = 1e-4
else: # don't fix shoulders by default
robot.ik.tasks['L_SHOULDER_P'].weight = 1e-5
robot.ik.tasks['L_SHOULDER_R'].weight = 1e-5
robot.ik.tasks['R_SHOULDER_P'].weight = 1e-5
robot.ik.tasks['R_SHOULDER_R'].weight = 1e-5
robot.ik.tasks['POSTURE'].weight = 1e-5
def initialize_robot():
robot.set_dof_values(robot.q_halfsit)
active_dofs = robot.chest + robot.free
active_dofs += robot.left_leg + robot.right_leg
robot.set_active_dofs(active_dofs)
robot.init_ik(gains={
'com': 1.,
'contact': 1.,
'link_pose': 1.,
},
weights={
'com': 10.,
'contact': 1000.,
'link_pose': 100.,
})
robot.set_dof_values([1.], [robot.TRANS_Z]) # warm-start PG
stance = fsm.cur_stance
com_task = COMTask(robot, stance.com)
robot.ik.add_task(com_task)
robot.ik.add_task(ContactTask(robot, robot.left_foot, stance.left_foot))
robot.ik.add_task(ContactTask(robot, robot.right_foot, stance.right_foot))
robot.ik.add_task(MinVelocityTask(robot, weight=1.))
robot.solve_ik(max_it=50)
# active_dofs += [robot.L_ELBOW_P, robot.R_ELBOW_P]
active_dofs += [robot.L_SHOULDER_R, robot.R_SHOULDER_R]
robot.set_dof_limits([+0.15], [+1.], [robot.L_SHOULDER_R])
robot.set_dof_limits([-1.], [-0.15], [robot.R_SHOULDER_R])
robot.set_active_dofs(active_dofs)
com_task.update_target(preview_buffer.com)
robot.ik.add_task(MinCAMTask(robot, weight=1.))
robot.ik.add_task(DOFTask(robot, robot.CHEST_P, 0.2, gain=0.9,
weight=0.05))
robot.ik.add_task(DOFTask(robot, robot.CHEST_Y, 0., gain=0.9, weight=0.05))
robot.ik.add_task(DOFTask(robot, robot.ROT_P, 0., gain=0.9, weight=0.05))
robot.solve_ik()
robot.ik.DEFAULT_WEIGHTS['POSTURE'] = 1e-5
robot.set_pos([0, 0, 2]) # robot initially above contacts
fsm.cur_stance.bind(robot)
robot.ik.solve(max_it=24)
com_target.set_pos(robot.com)
robot.ik.tasks['COM'].update_target(com_target)
robot.ik.solve(max_it=42)
robot.ik.add(DOFTask(robot, robot.WAIST_P, 0.2, weight=1e-3))
robot.ik.add(DOFTask(robot, robot.WAIST_Y, 0., weight=1e-3))
robot.ik.add(DOFTask(robot, robot.WAIST_R, 0., weight=1e-3))
robot.ik.add(DOFTask(robot, robot.ROT_P, 0., weight=1e-3))
robot.ik.add(DOFTask(robot, robot.R_SHOULDER_R, -0.5, weight=1e-3))
robot.ik.add(DOFTask(robot, robot.L_SHOULDER_R, 0.5, weight=1e-3))
robot.ik.add(MinCAMTask(robot, weight=1e-4))
sim.schedule(fsm)
sim.schedule(mpc)
sim.schedule(preview_buffer)
sim.schedule(robot.ik, log_comp_times=True)
com_traj_drawer = TrajectoryDrawer(com_target, 'b-')
lf_traj_drawer = TrajectoryDrawer(robot.left_foot, 'g-')
preview_drawer = PreviewDrawer()
rf_traj_drawer = TrajectoryDrawer(robot.right_foot, 'r-')
tube_drawer = TubeDrawer()
update_com_target = UpdateCOMTargetAccel(com_target, preview_buffer)
wrench_drawer = PointMassWrenchDrawer(com_target, lambda: fsm.cur_stance)
sim.schedule_extra(com_traj_drawer)
nb_mpc_steps=20,
tube_radius=0.01)
robot.init_ik(robot.whole_body)
robot.set_pos([0, 0, 2]) # start IK with the robot above contacts
robot.generate_posture(fsm.cur_stance, max_it=50)
com_target.set_pos(robot.com)
robot.ik.tasks['COM'].update_target(com_target)
robot.ik.add_task(DOFTask(robot, robot.WAIST_P, 0.2))
robot.ik.add_task(DOFTask(robot, robot.WAIST_Y, 0.))
robot.ik.add_task(DOFTask(robot, robot.WAIST_R, 0.))
robot.ik.add_task(DOFTask(robot, robot.ROT_P, 0.))
robot.ik.add_task(DOFTask(robot, robot.R_SHOULDER_R, -0.5))
robot.ik.add_task(DOFTask(robot, robot.L_SHOULDER_R, 0.5))
robot.ik.add_task(MinCAMTask(robot))
robot.ik.tasks['WAIST_P'].weight = 1e-3
robot.ik.tasks['WAIST_Y'].weight = 1e-3
robot.ik.tasks['WAIST_R'].weight = 1e-3
robot.ik.tasks['ROT_P'].weight = 1e-3
robot.ik.tasks['R_SHOULDER_R'].weight = 1e-3
robot.ik.tasks['L_SHOULDER_R'].weight = 1e-3
robot.ik.tasks['MIN_CAM'].weight = 1e-4
robot.ik.tasks['POSTURE'].weight = 1e-5
sim.schedule(fsm)
sim.schedule(mpc)
sim.schedule(preview_buffer)
sim.schedule(robot.ik)
gains={
'com': 1.,
'contact': 1.,
'link_pose': 1.,
'posture': 1.,
},
weights={
'com': 10.,
'contact': 10000.,
'link_pose': 100.,
'posture': 1.,
})
robot.set_dof_values([2.], [robot.TRANS_Z]) # start PG from above
robot.generate_posture(fsm.cur_stance, max_it=200)
robot.ik.tasks['com'].update_target(preview_buffer.com)
robot.ik.add_task(MinCAMTask(robot, weight=0.1))
try: # HRP-4
robot.ik.add_task(
DOFTask(robot, robot.CHEST_P, 0.2, gain=0.9, weight=0.05))
robot.ik.add_task(
DOFTask(robot, robot.CHEST_Y, 0., gain=0.9, weight=0.05))
robot.ik.add_task(
DOFTask(robot, robot.ROT_P, 0., gain=0.9, weight=0.05))
except AttributeError: # JVRC-1
robot.ik.add_task(
DOFTask(robot, robot.WAIST_P, 0.2, gain=0.9, weight=0.5))
robot.ik.add_task(
DOFTask(robot, robot.WAIST_Y, 0., gain=0.9, weight=0.5))
robot.ik.add_task(
DOFTask(robot, robot.WAIST_R, 0., gain=0.9, weight=0.5))