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))
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()
fsm = WalkingFSM(staircase, robot, swing_height=0.15, cycle=True) mpc = COMTubePredictiveControl( com_target, fsm, preview_buffer, nb_mpc_steps=20, tube_radius=0.01) 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()
callback=lambda u, dT: com_target.integrate_acceleration(u, dT)) fsm = WalkingFSM(staircase, robot, swing_height=0.15, cycle=True) mpc = COMTubePredictiveControl(com_target, fsm, preview_buffer, 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
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 sim.schedule(robot.ik) sim.start() # Don't forget to give the user a prompt if IPython.get_ipython() is None: IPython.embed()
def add_shoulder_neutral_pitch_task(self, weight): self.ik.add_task(DOFTask(self, self.L_SHOULDER_P, 0., weight)) self.ik.add_task(DOFTask(self, self.R_SHOULDER_P, 0., weight))
def add_shoulder_flexion_task(self, weight): self.ik.add_task(DOFTask(self, self.L_SHOULDER_P, -0.5, weight)) self.ik.add_task(DOFTask(self, self.R_SHOULDER_P, -0.5, weight))
def add_shoulder_extension_task(self, weight): self.ik.add_task(DOFTask(self, self.L_SHOULDER_P, +0.5, weight)) self.ik.add_task(DOFTask(self, self.R_SHOULDER_P, +0.5, weight))
def add_shoulder_abduction_task(self, weight): self.ik.add_task(DOFTask(self, self.R_SHOULDER_R, -0.4, weight)) self.ik.add_task(DOFTask(self, self.L_SHOULDER_R, +0.4, weight))
def add_upright_chest_task(self, weight): self.ik.add_task(DOFTask(self, self.ROT_P, 0., weight)) self.ik.add_task(DOFTask(self, self.CHEST_P, 0.2, weight)) self.ik.add_task(DOFTask(self, self.CHEST_Y, 0., weight))
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 robot.ik.verbosity = 2 robot.ik.solve(max_it=100, impr_stop=1e-4) robot.ik.verbosity = 0 # 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 sim.schedule(robot.ik) sim.start()
'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)) robot.ik.add_task( DOFTask(robot, robot.ROT_P, 0., gain=0.9, weight=0.5)) sim = Simulation(dt=dt)