from heol_humanoid import HeolHumanoid
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>
import numpy as np
from math import *
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
# 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])
# coordinate of z vector in world reference
z = np.mat([[0],[0],[1]])
#z_chest is the verticale reference of the robot
z_chest = rot_chest * z
z_chest
matrix([[ 2.86430268e-02], [ -9.99589632e-01], [ 3.80011053e-04]])
heol.r_hip_y.goto_position(0,1)
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
z_chest
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)
import time
time.sleep(1)
for m in heol.motors:
m.goal_position=0
heol.l_hip_y.goto_position(0,1)
heol.r_hip_y.goto_position(0,1)
heol.r_knee_x.goto_position(0,1)