w.update_dynamic() spine_joints = [joints['torso_'+n] for n in ['pitch', 'roll', 'yaw']] + \ [joints['head_' +n] for n in ['pitch', 'roll', 'yaw']] ######################################### # # # CREATE TASKS, EVENTS & LQP controller # # # ######################################### ## TASKS from LQPctrl.task import MultiJointTask, ForceTask, FrameTask, CoMTask from LQPctrl.task_ctrl import KpCtrl, ValueCtrl, KpTrajCtrl tasks = NamedObjectsList([]) tasks.append(MultiJointTask(icub_joints, KpCtrl(sitting_pose, 10), [], 1e-3 , 0, True, "sitting_pose")) tasks.append(MultiJointTask(icub_joints, KpCtrl(standing_pose, 10), [], 0. , 0, True, "standing_pose")) tasks.append(MultiJointTask(spine_joints, KpCtrl(zeros(len(spine_joints)), 10), [], 1e-2 , 0, True, "spine")) tasks.append(CoMTask(icub_bodies, KpCtrl([0, 0, 0], 10), [0, 1], 0., 0, name='com')) ## EVENTS from LQPctrl.event import Event, AtT, Prtr, SetF, IfF, ChW, InCH, CAct, DelayF events = [] tt=.01 feet_frames = [frames[n] for n in ['lf1', 'lf2', 'lf3', 'lf4', 'rf1', 'rf2', 'rf3', 'rf4']] events.append(Event(AtT(.0), SetF("start lifting", False))) events.append(Event(AtT(.1), SetF("avance com", True))) events.append(Event(InCH(feet_frames, 'CoM', [0,1]), [SetF("in CH", True)] ))
w.update_dynamic() spine_joints = [joints['torso_'+n] for n in ['pitch', 'roll', 'yaw']] + \ [joints['head_' +n] for n in ['pitch', 'roll', 'yaw']] ######################################### # # # CREATE TASKS, EVENTS & LQP controller # # # ######################################### ## TASKS from LQPctrl.task import MultiJointTask, ForceTask, FrameTask, CoMTask from LQPctrl.task_ctrl import KpCtrl, ValueCtrl, KpTrajCtrl tasks = NamedObjectsList([]) tasks.append(MultiJointTask(icub_joints, KpCtrl(sitting_pose, 10), [], 1e-3 , 0, True, "sitting_pose")) tasks.append(MultiJointTask(icub_joints, KpCtrl(standing_pose, 10), [], 0. , 0, True, "standing_pose")) tasks.append(MultiJointTask(spine_joints, KpCtrl(zeros(len(spine_joints)), 10), [], 1e-2 , 0, True, "spine")) tasks.append(ForceTask( const["l_gluteal"], ValueCtrl([0, 0, 0]) , [], 0., 0, True, "l_gluteal_force")) tasks.append(ForceTask( const["r_gluteal"], ValueCtrl([0, 0, 0]) , [], 0., 0, True, "r_gluteal_force")) tasks.append(FrameTask( frames["l_hip_2"], KpTrajCtrl([None,None,zeros((1,6))], 10), [3,4,5], 1., 0, True, "l_gluteal_acc")) tasks.append(FrameTask( frames["r_hip_2"], KpTrajCtrl([None,None,zeros((1,6))], 10), [3,4,5], 1., 0, True, "r_gluteal_acc")) tasks.append(CoMTask(icub_bodies, KpCtrl([0, 0, 0], 10), [0, 1], 0., 0, name='com')) ## EVENTS from LQPctrl.event import Event, AtT, Prtr, SetF, IfF, ChW, InCH, CAct, DelayF events = [] tt=.25 feet_frames = [frames[n] for n in ['lf1', 'lf2', 'lf3', 'lf4', 'rf1', 'rf2', 'rf3', 'rf4']] events.append(Event(AtT(.0), SetF("start lifting", False)))
w.update_dynamic() spine_joints = [joints['torso_'+n] for n in ['pitch', 'roll', 'yaw']] + \ [joints['head_' +n] for n in ['pitch', 'roll', 'yaw']] ######################################### # # # CREATE TASKS, EVENTS & LQP controller # # # ######################################### ## TASKS from LQPctrl.task import MultiJointTask, ForceTask, FrameTask, CoMTask from LQPctrl.task_ctrl import KpCtrl, ValueCtrl, KpTrajCtrl tasks = NamedObjectsList([]) tasks.append(MultiJointTask(icub_joints, KpCtrl(sitting_pose, 10), [], 1e-3 , 0, True, "sitting_pose")) tasks.append(MultiJointTask(spine_joints, KpCtrl(zeros(len(spine_joints)), 10), [], 1e-2 , 0, True, "spine")) ## EVENTS events = [] ## LQP CONTROLLER from LQPctrl.LQPctrl import LQPcontroller gforcemax = icub.get_torque_limits() lqpc = LQPcontroller(gforcemax, tasks=tasks) w.register(lqpc) ############################