Ejemplo n.º 1
0
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()
Ejemplo n.º 2
0
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])