In [1]:

    

from heol_humanoid import HeolHumanoid


    

In [2]:

    

heol = HeolHumanoid(simulator='vrep')


    

    




<DxlMotor name=l_hip_motor_z id=42 pos=0>
<DxlMotor name=l_thigh_x id=43 pos=0>
<DxlMotor name=l_ankle_x id=45 pos=0>
<DxlMotor name=r_hip_y id=51 pos=0>
<DxlMotor name=r_hip_motor_z id=52 pos=0>
<DxlMotor name=r_thigh_x id=53 pos=0>
<DxlMotor name=r_ankle_x id=55 pos=0>
<DxlMotor name=spine_z id=11 pos=0>
<DxlMotor name=chest_x id=12 pos=0>
<DxlMotor name=l_shoulder_x id=31 pos=0>
<DxlMotor name=l_upper_arm_z id=33 pos=0>
<DxlMotor name=r_shoulder_x id=21 pos=0>
<DxlMotor name=r_upper_arm_z id=23 pos=0>
<DxlMotor name=head_z id=13 pos=0>
<DxlMotor name=r_shoulder_motor_y id=22 pos=0>
<DxlMotor name=l_shoulder_motor_y id=32 pos=0>

In [6]:

    

import numpy as np
from math import *


    

In [3]:

    

def rotate_matrix(a,b,g):
    Rx = np.mat([[1,0,0], [0,cos(a),-sin(a)], [0,sin(a),cos(a)]]) #make a rotation of a radians around x axis
    Ry = np.mat([[cos(b),0,sin(b)], [0,1,0], [-sin(b),0,cos(b)]]) #make a rotation of b radians around y axis
    Rz = np.mat([[cos(g),-sin(g),0], [sin(g),cos(g),0], [0,0,1]]) #make a rotation of g radians around z axis
    Rot = Rz*Ry*Rx #make the three rotations
    return Rot


    

In [7]:

    

# return the three angles to go from world reference to chest reference
orient_chest = heol.get_object_orientation('chest_respondable') 

# rot_chest is the rotation matrice to transform any vector you have in the world reference to a vector in the chest reference
# vector in chest reference = rot_chest * vector in world reference
rot_chest=rotate_matrix(-orient_chest[0],-orient_chest[1],-orient_chest[2])


    

In [10]:

    

# coordinate of z vector in world reference
z = np.mat([[0],[0],[1]])


    

In [12]:

    

#z_chest is the verticale reference of the robot
z_chest = rot_chest * z


    

In [13]:

    

z_chest


    

    
Out[13]:





matrix([[  2.86430268e-02],
        [ -9.99589632e-01],
        [  3.80011053e-04]])

In [14]:

    

heol.r_hip_y.goto_position(0,1)


    

In [15]:

    

orient_chest = heol.get_object_orientation('chest_respondable')
rot_chest=rotate_matrix(-orient_chest[0],-orient_chest[1],-orient_chest[2])
z_chest = rot_chest * z


    

In [16]:

    

z_chest


    

    
Out[16]:





matrix([[ 0.02717777],
        [-0.99565085],
        [-0.08911087]])

In [21]:

    

import time
time.sleep(1)
for m in heol.motors:
    m.goal_position=0


    

In [38]:

    

heol.l_hip_y.goto_position(0,1)
heol.r_hip_y.goto_position(0,1)


    

In [40]:

    

heol.r_knee_x.goto_position(0,1)


    

In [ ]: