def setup_ik_from_tasks(): """ Setup the inverse kinematics task by task. Note ----- This function is equivalent to :func:`setup_ik_from_stance` above. Beginners should take a look at that one first. Notes ----- See this `tutorial on inverse kinematics <https://scaron.info/teaching/inverse-kinematics.html>`_ for details. """ from pymanoid.tasks import COMTask, ContactTask, DOFTask, PostureTask # Prepare targets lf_target = Contact(robot.sole_shape, pos=[0, 0.3, 0]) rf_target = Contact(robot.sole_shape, pos=[0, -0.3, 0]) # Initial robot pose robot.set_dof_values([0.8], dof_indices=[robot.TRANS_Z]) com = PointMass(pos=robot.com, mass=robot.mass) # Prepare tasks left_foot_task = ContactTask(robot, robot.left_foot, lf_target, weight=1., gain=0.85) right_foot_task = ContactTask(robot, robot.right_foot, rf_target, weight=1., gain=0.85) com_task = COMTask(robot, com, weight=1e-2, gain=0.85) posture_task = PostureTask(robot, robot.q_halfsit, weight=1e-6, gain=0.85) # Add tasks to the IK solver robot.ik.add(left_foot_task) robot.ik.add(right_foot_task) robot.ik.add(com_task) robot.ik.add(posture_task) # Add shoulder DOF tasks for a nicer posture robot.ik.add( DOFTask(robot, robot.R_SHOULDER_R, -0.5, gain=0.5, weight=1e-5)) robot.ik.add( DOFTask(robot, robot.L_SHOULDER_R, +0.5, gain=0.5, weight=1e-5))
q_init[dof_id] = dof_ref robot.set_dof_values([-1], [robot.R_SHOULDER_P]) robot.set_dof_values([-1], [robot.L_SHOULDER_P]) robot.set_dof_values([0.8], dof_indices=[robot.TRANS_Z]) init_com = robot.com.copy() # IK targets com = Cube(0.05, pos=robot.com, color='g') lf_target = Contact(robot.sole_shape, pos=[0, 0.3, 0], visible=True) rf_target = Contact(robot.sole_shape, pos=[0, -0.3, 0], visible=True) # IK tasks lf_task = ContactTask(robot, robot.left_foot, lf_target, weight=1000) rf_task = ContactTask(robot, robot.right_foot, rf_target, weight=1000) com_task = COMTask(robot, com, weight=10) reg_task = PostureTask(robot, robot.q, weight=0.1) # regularization task # IK setup robot.init_ik(active_dofs=robot.whole_body) robot.ik.add_tasks([lf_task, rf_task, com_task, reg_task]) for (dof_id, dof_ref) in dof_targets: robot.ik.add_task( DOFTask(robot, dof_id, dof_ref, gain=0.5, weight=0.1)) # First, generate an initial posture robot.solve_ik(max_it=100, conv_tol=1e-4, debug=True) # Next, we move the COM back and forth for 10 seconds move_com_back_and_forth(10) # Finally, we start the simulation with the IK on
robot.set_dof_values([0.8], dof_indices=[robot.TRANS_Z]) com = PointMass(pos=robot.com, mass=robot.mass) # IK tasks lf_task = ContactTask(robot, robot.left_foot, lf_target, weight=1., gain=0.85) rf_task = ContactTask(robot, robot.right_foot, rf_target, weight=1., gain=0.85) com_task = COMTask(robot, com, weight=1e-2, gain=0.85) reg_task = PostureTask(robot, robot.q, weight=1e-6, gain=0.85) # IK setup robot.ik.clear_tasks() robot.ik.add_task(lf_task) robot.ik.add_task(rf_task) robot.ik.add_task(com_task) robot.ik.add_task(reg_task) # Add some DOFTasks for a nicer posture robot.ik.add_task( DOFTask(robot, robot.R_SHOULDER_R, -0.5, gain=0.5, weight=1e-5)) robot.ik.add_task( DOFTask(robot, robot.L_SHOULDER_R, +0.5, gain=0.5, weight=1e-5)) # First, generate an initial posture