joints['l_elbow_yaw'].gpos[:] = 0.85522174
joints['l_wrist_roll'].gpos[:] = 0.15702068
joints['l_wrist_pitch'].gpos[:] = 0.41882797
joints['r_shoulder_pitch'].gpos[:] = -0.65695599
joints['r_shoulder_roll'].gpos[:] = 0.58153898
joints['r_shoulder_yaw'].gpos[:] = 0.68249881
joints['r_elbow_pitch'].gpos[:] = 1.27058836
joints['r_elbow_yaw'].gpos[:] = 0.85522174
joints['r_wrist_roll'].gpos[:] = 0.15702068
joints['r_wrist_pitch'].gpos[:] = 0.41882797
w.update_dynamic()
icub_joints = [joints[n] for n in icub.get_joints_data()]
icub_bodies = [bodies[n] for n in icub.get_bodies_data()]
standing_pose = [j.gpos[0] for j in icub_joints]
#########################################
# #
# CREATE TASKS, EVENTS & LQP controller #
# #
#########################################
## TASKS
from LQPctrl.task import MultiJointTask, FrameTask, JointTask
from LQPctrl.task_ctrl import KpCtrl
from arboris.homogeneousmatrix import rotz
tasks = []
tasks.append(
MultiJointTask(icub_joints, KpCtrl(standing_pose, 10), [], .01, 0, True,
from numpy import pi, array, zeros ################################# # # # CREATE WORLD & INITIALIZATION # # # ################################# w = create_icub_and_init(chair=True, gravity=True) joints = w.getjoints() frames = w.getframes() bodies = w.getbodies() icub_joints = [joints[n] for n in icub.get_joints_data()] icub_bodies = [bodies[n] for n in icub.get_bodies_data()] standing_pose = [j.gpos[0] for j in icub_joints] joints['l_knee'].gpos[:] = -pi/2 joints['r_knee'].gpos[:] = -pi/2 joints['l_ankle_pitch'].gpos[:] = -pi/10 joints['r_ankle_pitch'].gpos[:] = -pi/10 joints['l_hip_pitch'].gpos[:] = pi/2 - pi/10 joints['r_hip_pitch'].gpos[:] = pi/2 - pi/10 joints['root'].gpos[0:3,3] = array([0,0,.597]) - [-0.2, 0, 0.2231] + [-0.01916-0.012, 0, 0.0587] sitting_pose = [j.gpos[0] for j in icub_joints] const = w.getconstraints() w.update_dynamic() spine_joints = [joints['torso_'+n] for n in ['pitch', 'roll', 'yaw']] + \