events.append(Event([IfF("start lifting", False), IfF("in CH", True)], [ChW(tasks['l_gluteal_force'], 1e-5, tt), ChW(tasks['r_gluteal_force'], 1e-5, tt), Prtr("start lifiting"), DelayF("can del const", True, tt), SetF("start lifting", True)])) events.append(Event(IfF("can del const", True), [Prtr("CAN DEL CONST"),CAct(const["l_gluteal"], False, True), CAct(const["r_gluteal"], False, True), ChW(tasks['standing_pose'], 1e-3, tt), ChW(tasks['l_gluteal_acc'], 0, tt), ChW(tasks['r_gluteal_acc'], 0, tt), DelayF("can del sitting", True, tt), SetF("can del const", False)])) events.append(Event(IfF("can del sitting", True), [ChW(tasks['sitting_pose'], 0., tt), SetF("can del sitting", False)])) ## LQP CONTROLLER from LQPctrl.LQPctrl import LQPcontroller gforcemax = icub.get_torque_limits() opt = {'base weights' : (1e-7, 1e-7, 1e-7)} sopt = {'show_progress':False, 'abstol' : 1e-12, 'reltol' : 1e-12, 'feastol' : 1e-12,} lqpc = LQPcontroller(gforcemax, tasks=tasks, events=events, options = opt, solver_options=sopt) w.register(lqpc) ############################ # # # SET OBSERVERS & SIMULATE # # # ############################ obs = get_usual_observers(w)
from numpy import eye root_goal = eye(4) root_goal[0:3, 3] = [0, 0, .59] tasks.append( JointTask(joints['root'], KpCtrl(root_goal, 10), [], 1, 0, True, "root")) spine_joints = [joints['torso_' + n] for n in ['pitch', 'roll', 'yaw']] tasks.append( MultiJointTask(spine_joints, KpCtrl([0, 0, 0], 10), [], 1, 0, True, "spine")) ## EVENTS events = [] ## LQP CONTROLLER from LQPctrl.LQPctrl import LQPcontroller gforcemax = icub.get_torque_limits() data = {'bodies': icub_bodies} data.update({ 'collision shapes': [(shapes['l_hand_palm'], shapes['wall']), (shapes['r_hand_palm'], shapes['wall'])] }) opt = {'avoidance horizon': .1} sopt = {"show_progress": False} lqpc = LQPcontroller(gforcemax, tasks=tasks, data=data, options=opt, solver_options=sopt) w.register(lqpc)
numpy.set_printoptions(suppress=True) from numpy import dot, eye, pi ################################################################################ # # Create world with iCub # ################################################################################ w = World() icub.add(w) w.update_dynamic() jLim = icub.get_joint_limits(in_radian=True) tauLim = icub.get_torque_limits() # interesting values we want to obtain H_body_com = {} body_mass = {} body_moment_of_inertia = {} H_parentCoM_bCoM = {} list_of_dof = {} list_of_children = {} # We compute the displacement from the body frame to the body center of mass, aligned with the principal axis of inertia H_body_com["ground"] = eye(4) for b in w.getbodies()[1:]: H_body_com[b.name] = principalframe(b.mass) Mb_com = transport(b.mass, H_body_com[b.name]) # if all good, Mb_com is diagonal