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]])

Faire une primitive qui renvoit le décalage latérale et le décalage avant arrière heol.orientation(left_right) heol.orientation(front_rear)


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 [ ]: