const = w.getconstraints() 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']]
def __init__(self, *args, **kargs): Task.__init__(self, *args, **kargs) self._subtask = NamedObjectsList([]) self._n_problem = 0
def __init__(self, gforcemax, dgforcemax=None, qlim=None, vlim=None, \ tasks=None, events=None, \ data=None, options=None, solver_options=None, name=None): """ Initialize the instance of the LQP controller. :param gforcemax: max gforce (N or Nm) {'joint_name': max_gforce} :type gforcemax: dict :param dgforcemax: max dgforce (N/s or Nm/s) {'joint_name': max_dgforce} :type dgforcemax: dict :param qlim: position limits (rad) {'joint_name': (qmin,qmax)} :type qlim: dict :param vlim: velocity limits (rad/s) {'joint_name': vmax} :type vlim: dict :param events: a list of events (from LQPctrl.events) :type events: list :param tasks: a list of tasks (from LQPctrl.tasks) :type tasks: list :param data: a dictionnary of data interesting to all component of LQP :type data: dict :param options: a dictionnary of options for the LQPctrl :type options: dict :param solver_options: a dictionnary of options for the solver :type options: dict :param name: the name of the LQP controller """ Controller.__init__(self, name=name) self.gforcemax = dict(gforcemax) self.dgforcemax = dgforcemax or {} self.qlim = qlim or {} self.vlim = vlim or {} self.tasks = NamedObjectsList(tasks) if tasks is not None \ else NamedObjectsList([]) self.events = NamedObjectsList(events) if events is not None \ else NamedObjectsList([]) self.data = lqpc_data(data) self.options = lqpc_options(options) self.solver_options = lqpc_solver_options(solver_options) self.world = None self.bodies = [] self.WeightController = None self._qlim = [] self._vlim = [] self.ndof = 0 self.S = zeros((0, 0)) self.constraints = [] self.is_enabled = {} self._mus = [] self.n_fc = 0 self.Jc = zeros((0, 0)) self.dJc = zeros((0, 0)) self._gforcemax = [] self._dgforcemax = [] self._prec_gforce = None self.n_chi = 0 self.n_gforce = 0 self.n_problem = 0 self.collision_shapes = [] self.X_solution = zeros(0) self._gforce = zeros(0) self.E_base = zeros((0, 0)) self.f_base = zeros(0) self.cost = "" self.norm = "" self.formalism = "" self._performance = {}
const = w.getconstraints() 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)))
def add_human36(world, height=1.741, mass=73, anat_lengths=None, name=''): """Add an anthropometric humanoid model to the world. :param height: the human height in meters. Ignored if ``anat_lengths`` is provided. :type height: float :param mass: the human mass in kilograms. :type mass: float :param anat_lengths: the human anatomical lengths. Computed from ``height`` if not provided :type anat_lengths: dict :param name: name of the human, used to prefix every object name. :type name: string :param return_lists: if True, returns of a tuple of the added objects :return: None **Exemples** >>> w = World() >>> # add a normal human >>> add_human36(w, height=1.8, name="Bob's ") >>> # add a human with a shorter left arm >>> L = anat_lengths_from_height(1.8) >>> L['yhumerusL'] *= .7 >>> L['yforearmL'] *= .7 >>> L['yhandL'] *= .7 >>> add_human36(w, anat_lengths=L, name="Casimodo's ") >>> frames = w.getframes() >> frames["Bob's Left stylion"].bpose[0:3, 3] >> frames["Bob's Left stylion"].pose[0:3, 3] >> frames["Casimodo's Left stylion"].bpose[0:3, 3] """ assert isinstance(world, World) w = world if anat_lengths is None: L = anat_lengths_from_height(height) else: L = anat_lengths h = height_from_anat_lengths(L) prefix = name # create the bodies bodies = NamedObjectsList() def add_body(name, mass, com_position, gyration_radius): #mass matrix at com mass_g = mass * diag(hstack((gyration_radius**2, (1, 1, 1)))) H_fg = eye(4) H_fg[0:3, 3] = com_position H_gf = Hg.inv(H_fg) #mass matrix at body's frame origin: mass_o = dot(adjoint(H_gf).T, dot(mass_g, adjoint(H_gf))) if name: name = prefix + name bodies.append(Body(name=name, mass=mass_o)) # Lower Part of Trunk add_body("LPT", 0.275 * mass, [0, 0.5108 * L['yvT10'], 0], array([0.2722, 0.2628, 0.226]) * L['yvT10']) add_body("ThighR", 0.1416 * mass, [0, -0.4095 * L['yfemurR'], 0], array([0.329, 0.149, 0.329]) * L['yfemurR']) add_body("ShankR", 0.0433 * mass, [0, -0.4459 * L['ytibiaR'], 0], array([0.255, 0.103, 0.249]) * L['ytibiaR']) add_body("FootR", 0.0137 * mass, [0.4415 * L['xfootR'] - L['xheelR'], -L['yfootR'] / 2., 0.], array([0.124, 0.257, 0.245]) * L['xfootR']) add_body("ThighL", 0.1416 * mass, [0, -0.4095 * L['yfemurL'], 0], array([0.329, 0.149, 0.329]) * L['yfemurL']) add_body("ShankL", 0.0433 * mass, [0, -0.4459 * L['ytibiaL'], 0], array([0.255, 0.103, 0.249]) * L['ytibiaL']) add_body("FootL", 0.0137 * mass, [0.4415 * L['xfootL'] - L['xheelL'], -L["yfootL"] / 2, 0.], array([0.124, 0.257, 0.245]) * L['xfootL']) # Upper Part of Trunk add_body("UPT", 0.1596 * mass, [(L['xsternoclavR'] + L['xsternoclavL']) / 4., 0.7001 * (L['ysternoclavR'] + L['ysternoclavL']) / 2., 0.], array([0.716, 0.659, 0.454]) * L['ysternoclavR']) # right shoulder add_body("ScapulaR", 0., [0., 0., 0.], array([0., 0., 0.])) add_body("ArmR", 0.0271 * mass, [0., -0.5772 * L['yhumerusR'], 0.], array([0.285, 0.158, 0.269]) * L['yhumerusR']) add_body("ForearmR", 0.0162 * mass, [0., -0.4574 * L['yforearmR'], 0.], array([0.276, 0.121, 0.265]) * L['yforearmR']) add_body("HandR", 0.0061 * mass, [0, -0.3691 * L['yhandR'], 0], array([0.235, 0.184, 0.288]) * L['yhandR']) # left shoulder add_body("ScapulaL", 0., [0., 0., 0.], array([0., 0., 0.])) add_body("ArmL", 0.0271 * mass, [0, -0.5772 * L['yhumerusL'], 0.], array([0.285, 0.158, 0.269]) * L['yhumerusL']) add_body("ForearmL", 0.0162 * mass, [0, -0.4574 * L['yforearmL'], 0], array([0.276, 0.121, 0.265]) * L['yforearmL']) add_body("HandL", 0.0061 * mass, [0, -0.3691 * L['yhandL'], 0], array([0.288, 0.184, 0.235]) * L['yhandL']) add_body("Head", 0.0694 * mass, [0, 0.4998 * L['yhead'], 0], array([0.303, 0.261, 0.315]) * L['yhead']) # create the joints and add the links (i.e. joints+bodies) def add_link(body0, transl, joint, body1): if not isinstance(body0, Body): body0 = bodies[prefix + body0] frame0 = SubFrame(body0, Hg.transl(*transl)) w.add_link(frame0, joint, bodies[prefix + body1]) add_link(w.ground, (0, L['yfootL'] + L['ytibiaL'] + L['yfemurL'], 0), FreeJoint(), 'LPT') add_link('LPT', (0, 0, L['zhip'] / 2.), RzRyRxJoint(), 'ThighR') add_link('ThighR', (0, -L['yfemurR'], 0), RzJoint(), 'ShankR') add_link('ShankR', (0, -L['ytibiaR'], 0), RzRxJoint(), 'FootR') add_link('LPT', (0, 0, -L['zhip'] / 2.), RzRyRxJoint(), 'ThighL') add_link('ThighL', (0, -L['yfemurL'], 0), RzJoint(), 'ShankL') add_link('ShankL', (0, -L['ytibiaL'], 0), RzRxJoint(), 'FootL') add_link('LPT', (-L['xvT10'], L['yvT10'], 0), RzRyRxJoint(), 'UPT') add_link('UPT', (L['xsternoclavR'], L['ysternoclavR'], L['zsternoclavR']), RyRxJoint(), 'ScapulaR') add_link('ScapulaR', (-L['xshoulderR'], L['yshoulderR'], L['zshoulderR']), RzRyRxJoint(), 'ArmR') add_link('ArmR', (0, -L['yhumerusR'], 0), RzRyJoint(), 'ForearmR') add_link('ForearmR', (0, -L['yforearmR'], 0), RzRxJoint(), 'HandR') add_link('UPT', (L['xsternoclavL'], L['ysternoclavL'], -L['zsternoclavL']), RyRxJoint(), 'ScapulaL') add_link('ScapulaL', (-L['xshoulderL'], L['yshoulderL'], -L['zshoulderL']), RzRyRxJoint(), 'ArmL') add_link('ArmL', (0, -L['yhumerusL'], 0), RzRyJoint(), 'ForearmL') add_link('ForearmL', (0, -L['yforearmL'], 0), RzRxJoint(), 'HandL') add_link('UPT', (L['xvT10'], L['yvC7'], 0), RzRyRxJoint(), 'Head') # add the tags tags = NamedObjectsList() def add_tag(name, body, position): """Returns data about anatomical landmarks as defined in HuMAnS. :param name: name of the tag (copied from HuMAnS) :param body: name of the body in whose frame the point is defined :position: tag coordinates in meters """ if name: name = prefix + name tag = SubFrame(bodies[prefix + body], Hg.transl(*position), name) tags.append(tag) w.register(tag) # we add 1e-4*h to keep the compatibility with HuMAnS: add_tag('Right foot toe tip', 'FootR', [L['xfootR'] - L['xheelR'] + 1e-4 * h, -L['yfootR'], 0.]) add_tag('Right foot heel', 'FootR', [-L['xheelR'], -L['yfootR'], 0.]) add_tag('Right foot phalange 5', 'FootR', [0.0662 * h, -L['yfootR'], 0.0305 * h]) add_tag('Right foot Phalange 1', 'FootR', [0.0662 * h, -L['yfootR'], -0.0305 * h]) add_tag('Right foot lateral malleolus', "ShankR", [0., -L['ytibiaR'], 0.0249 * h]) add_tag('Femoral lateral epicondyle', "ThighR", [0., -L['yfemurR'], 0.0290 * h]) add_tag('Right great trochanter', "ThighR", [0., 0., 0.0941 * h - L['zhip'] / 2.]) add_tag('Right iliac crest', "LPT", [0.0271 * h, 0.0366 * h, 0.0697 * h]) # we add 1e-4*h to keep the compatibility with HuMAnS: add_tag('Left foot toe tip', "FootL", [L['xfootL'] - L['xheelL'] + 1e-4 * h, -L['yfootL'], 0.]) add_tag('Left foot heel', "FootL", [-L['xheelL'], -L['yfootL'], 0.]) add_tag('Left foot phalange 5', "FootL", [0.0662 * h, -L['yfootL'], -0.0305 * h]) add_tag('Left foot phalange 1', "FootL", [0.0662 * h, -L['yfootL'], 0.0305 * h]) add_tag('Left foot lateral malleolus', "ShankL", [0, -L['ytibiaL'], -0.0249 * h]) add_tag('Left femoral lateral epicondyle', "ThighL", [0, -L['yfemurL'], -0.0290 * h]) add_tag('Left great trochanter', "ThighL", [0, 0, -0.0941 * h + L['zhip'] / 2.]) add_tag('Left iliac crest', "LPT", [0.0271 * h, 0.0366 * h, -0.0697 * h]) add_tag('Substernale (Xyphoid)', "UPT", [0.1219 * h, 0, 0]) add_tag('Suprasternale', "UPT", [(L['xsternoclavL'] + L['xsternoclavL']) / 2., (L['ysternoclavL'] + L['ysternoclavL']) / 2., 0]) add_tag('Right acromion', "ScapulaR", [-L['xshoulderR'], 0.0198 * h + L['yshoulderR'], L['zshoulderR']]) add_tag('Right humeral lateral epicondyle (radiale)', "ArmR", [0., -L['yhumerusR'], 0.0211 * h]) add_tag('Right stylion', "ForearmR", [0., -0.1533 * h, 0.0331 * h]) add_tag('Right 3rd dactylion', "HandR", [0., -L['yhandR'], 0.]) add_tag('Left acromion', "ScapulaL", [-L['xshoulderL'], 0.0198 * h + L['yshoulderL'], -L['zshoulderL']]) add_tag('Left humeral lateral epicondyle (radiale)', "ArmL", [0., -L['yhumerusL'], -0.0211 * h]) add_tag('Left stylion', "ForearmL", [0., -0.1533 * h, -0.0331 * h]) add_tag('Left 3rd dactylion', "HandL", [0., -L['yhandL'], 0.]) add_tag('Cervicale', "UPT", [-0.0392 * 0. + L['xvT10'], L['yvC7'], 0.]) add_tag('Vertex', "Head", [0., L["yhead"], 0.]) # Add point shapes to the feet for k in ('Right foot toe tip', 'Right foot heel', 'Right foot phalange 5', 'Right foot Phalange 1', 'Left foot toe tip', 'Left foot heel', 'Left foot phalange 5', 'Left foot phalange 1'): name = prefix + k shape = Point(tags[name], name=name) w.register(shape) w.init() return None
const = w.getconstraints() 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)