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
