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)
