from arboris.core import NamedObjectsList 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()
joints['l_elbow_pitch'].gpos[:] = 1.27058836 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(
# H_parentCoM_bCoM = H_parentCoM_parent * H_parent_b * H_b_bCoM
# <=> (H_0_parent * H_parent_parentCoM)^(-1) * (H_0_b * H_b_bCoM)
for b in w.getbodies()[1:]:
parent = b.parentjoint.frame0.body
H_parentCoM_bCoM[b.name] = dot( Hinv( dot(parent.pose, H_body_com[parent.name]) ), dot(b.pose, H_body_com[b.name]) )
for b in w.getbodies()[1:]:
j = b.parentjoint
list_of_children[b.name] = [childj.frame1.body.name for childj in b.childrenjoints]
if j.name in icub.get_joints_data():
parent = j.frame0.body
child = j.frame1.body
# We look for the vector and direction between body center of Mass and the hinge center and axis of rotation
# H_parentCoM_hinge = (H_parent_parentCoM)^(-1) * H_parent_hinge
# V_parentCoM_hinge = H_parentCoM_hinge[0:3,3]
H_parent_hinge = j.frame0.bpose
H_parentCoM_hinge = dot( Hinv(H_body_com[parent.name]), H_parent_hinge)
V_parentCoM_hinge = tuple(H_parentCoM_hinge[0:3,3])
hinge_axis_in_parentCoM = tuple(H_parentCoM_hinge[0:3,2])
list_of_dof[b.name] = (V_parentCoM_hinge, hinge_axis_in_parentCoM, jLim[j.name][0], jLim[j.name][1])