Télécommande corporelle
par Arnaud BULCKE · Publié · Mis à jour
Groupe 1
Coralie BABIN et Ana SIMOES
Objectifs
La télécommande corporelle doit permettre à une personne de piloter un objet (robot, domotique, …) avec les mouvements de son corps.
Moyens
Le capteur Kinect
Le capteur Kinect permet, en scannant l’espace, de détecter la présence d’une personne et d’acquérir, en 3 dimensions, les positions des points de son squelette :
Le capteur sera programmé en Python.
Le robot Braduino
Réalisation :
1- Création d’un programme permettant d’afficher le squelette capté par la kinect.
2- Construction du braduino.
3- Faire en sorte que le braduino bouge en fonction des mouvements captés par la kinect.
Assemblage du robot
Braduino
Braduino est un projet de bras de robot à très bas cout.
Programmes
Bibliothèques Python utilisées :
Pilotes du capteurs Kinect : Kinect for Windows SDK v1.8
Coté PC
Programme principal
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import sys, os
from math import *
# Import du module kinect
from kinect import *
# Import du module serie (pour communiquer avec l'Arduino)
import serie
# Import du module de calcul
from calcul import *
if __name__ == '__main__':
# Boucle principale ###########################################################################
ser = serie.connexion()
done = False
while not done:
# Attend un événement ....
for e in pygame.event.get(): #get_event():#
if e.type == QUITEVENT: # l'utilisateur ferme la fenêtre graphique
#raise SystemExit, "QUIT"
done = True
break
#######################################################################################
elif e.type == KINECTEVENT: # le capteur kinect dispose de nouvelles données
p0 = get_joint_pos_xyz("ShoulderLeft")
if p0 is not None:
p0 = point(p0.x, p0.y, p0.z)
p3 = get_joint_pos_xyz("WristRight")
if p3 is not None:
p3 = point(p3.x, p3.y, p3.z)
if p0 is not None and p3 is not None:
# plan xy (haut/bas)
p1 = point(p0.x, p0.y+1, p0.z)
p2 = point(p3.x, p3.y, p0.z)
angle_1 = 180 - int(angle(p1, p0, p2, deg = True))
# plan xz (avant/arrière)
p1 = point(p0.x, p0.y, p0.z+1)
p2 = point(p3.x, p0.y, p3.z)
angle_2 = 180 - int(angle(p1, p0, p2, deg = True))
# Envoi des angles à l'Arduino
if ser is not None:
ser.write(str(angle_2).encode()+","+str(angle_1).encode()+"\n")
# Affichage du squelette
if draw_skeleton and not done:
draw_skeletons(e.skeletons)
# Affichage des angles
if p0 is not None:
update_score(str(angle_1).encode()+" "+str(angle_2).encode())
else:
update_score()
update_display()
elif e.type == KEYDOWN: # L'utilisateur a tapé une touche du clavier
if e.key == K_ESCAPE:
done = True
break
# Position de la caméra
elif e.key == K_UP:
Kinect.camera.elevation_angle += 2
elif e.key == K_DOWN:
Kinect.camera.elevation_angle -= 2
elif e.key == K_x:
Kinect.camera.elevation_angle = 2
# Articulation "courante" (affichée à l'écran)
elif e.key == K_RIGHT:
modif_joint(1) # On ajoute 1
elif e.key == K_LEFT:
modif_joint(-1) # On enlève 1
elif e.key == K_RETURN:
print get_joint_pos("")[0] # Exemple d'utilisation de la fonction get_joint_pos()
quit() # Fonction de fermeture de la fenêtre graphique
Module kinect.py
Source d’inspiration : https://github.com/Microsoft/PTVS/blob/master/Python/Product/PyKinect/PyKinect/PyGameDemo.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
##########################################################################################
#
# Module pour l'acquisition des données du capteur Kinect
# l'affichage de la vidéo
# et des "squelettes"
#
# source : https://github.com/sridharavinash/Itches/blob/master/PyGameDemo.py
##########################################################################################
# Modules
##########################################################################################
# Pour le capteur kinect
import pykinect
from pykinect import nui
from pykinect.nui import JointId
# Pour l'interface graphique
import pygame
from pygame.color import THECOLORS
from pygame.locals import *
# Divers
import ctypes
import itertools
import thread
import sys
# Constantes
##########################################################################################
VIDEO_WINSIZE = 640,480
KINECTEVENT = pygame.USEREVENT
# DISPLAYSCOREEVENT = pygame.USEREVENT + 3
SKELETON_JOINTS = {"ShoulderCenter": JointId.ShoulderCenter,
"ShoulderLeft": JointId.ShoulderLeft,
"ElbowLeft": JointId.ElbowLeft,
"WristLeft": JointId.WristLeft,
"HandLeft": JointId.HandLeft,
"ShoulderRight": JointId.ShoulderRight,
"ElbowRight": JointId.ElbowRight,
"WristRight": JointId.WristRight,
"HandRight": JointId.HandRight,
"HipCenter": JointId.HipCenter,
"HipLeft": JointId.HipLeft,
"KneeLeft": JointId.KneeLeft,
"AnkleLeft": JointId.AnkleLeft,
"FootLeft": JointId.FootLeft,
"HipRight": JointId.HipRight,
"KneeRight": JointId.KneeRight,
"AnkleRight": JointId.AnkleRight,
"FootRight": JointId.FootRight,
"Spine": JointId.Spine,
"Head": JointId.Head}
SKELETON_JOINTS_NAMES = ["ShoulderLeft", "ElbowLeft", "WristLeft", "HandLeft",
"ShoulderCenter",
"ShoulderRight", "ElbowRight", "WristRight", "HandRight",
"HipCenter",
"HipLeft", "KneeLeft", "AnkleLeft", "FootLeft",
"HipRight", "KneeRight", "AnkleRight", "FootRight",
"Spine", "Head"]
SKELETON_COLORS = [THECOLORS["red"],
THECOLORS["blue"],
THECOLORS["green"],
THECOLORS["orange"],
THECOLORS["purple"],
THECOLORS["yellow"],
THECOLORS["violet"]]
LEFT_ARM = (JointId.ShoulderCenter,
JointId.ShoulderLeft,
JointId.ElbowLeft,
JointId.WristLeft,
JointId.HandLeft)
RIGHT_ARM = (JointId.ShoulderCenter,
JointId.ShoulderRight,
JointId.ElbowRight,
JointId.WristRight,
JointId.HandRight)
LEFT_LEG = (JointId.HipCenter,
JointId.HipLeft,
JointId.KneeLeft,
JointId.AnkleLeft,
JointId.FootLeft)
RIGHT_LEG = (JointId.HipCenter,
JointId.HipRight,
JointId.KneeRight,
JointId.AnkleRight,
JointId.FootRight)
SPINE = (JointId.HipCenter,
JointId.Spine,
JointId.ShoulderCenter,
JointId.Head)
QUITEVENT = pygame.QUIT
skeleton_to_depth_image = nui.SkeletonEngine.skeleton_to_depth_image
# Variables globales
##########################################################################################
full_screen = False
draw_skeleton = True
video_display = True
CurJoint = 0 # Articulation "courante"
CurScore = "" # Texte de score "courant"
# Fonctions
##########################################################################################
# recipe to get address of surface: http://archives.seul.org/pygame/users/Apr-2008/msg00218.html
if hasattr(ctypes.pythonapi, 'Py_InitModule4'):
Py_ssize_t = ctypes.c_int
elif hasattr(ctypes.pythonapi, 'Py_InitModule4_64'):
Py_ssize_t = ctypes.c_int64
else:
raise TypeError("Cannot determine type of Py_ssize_t")
_PyObject_AsWriteBuffer = ctypes.pythonapi.PyObject_AsWriteBuffer
_PyObject_AsWriteBuffer.restype = ctypes.c_int
_PyObject_AsWriteBuffer.argtypes = [ctypes.py_object,
ctypes.POINTER(ctypes.c_void_p),
ctypes.POINTER(Py_ssize_t)]
##########################################################################################
def surface_to_array(surface):
try:
buffer_interface = surface.get_buffer()
except:
pygame.quit()
sys.exit(0)
address = ctypes.c_void_p()
size = Py_ssize_t()
_PyObject_AsWriteBuffer(buffer_interface,
ctypes.byref(address), ctypes.byref(size))
bytes = (ctypes.c_byte * size.value).from_address(address.value)
bytes.object = buffer_interface
return bytes
##########################################################################################
def draw_skeletons(skeletons = None):
u""" Dessine le squelette (le 1er qui est détecté)
"""
global Skeleton, dispInfo
if skeletons is not None:
Skeleton = get_skeleton(skeletons)
def draw_skeleton_data(pSkelton, positions, width = 4):
start = pSkelton.SkeletonPositions[positions[0]]
for position in itertools.islice(positions, 1, None):
next = pSkelton.SkeletonPositions[position.value]
curstart = skeleton_to_depth_image(start, dispInfo.current_w, dispInfo.current_h)
curend = skeleton_to_depth_image(next, dispInfo.current_w, dispInfo.current_h)
pygame.draw.line(screen, SKELETON_COLORS[0], curstart, curend, width)
start = next
if Skeleton is not None:
HeadPos = skeleton_to_depth_image(Skeleton.SkeletonPositions[JointId.Head],
dispInfo.current_w, dispInfo.current_h)
draw_skeleton_data(Skeleton, SPINE, 10)
pygame.draw.circle(screen, SKELETON_COLORS[0], (int(HeadPos[0]), int(HeadPos[1])), 20, 0)
c = SKELETON_JOINTS[SKELETON_JOINTS_NAMES[CurJoint]]
CurPos = skeleton_to_depth_image(Skeleton.SkeletonPositions[c],
dispInfo.current_w, dispInfo.current_h)
pygame.draw.circle(screen, SKELETON_COLORS[1], (int(CurPos[0]), int(CurPos[1])), 10, 0)
# drawing the limbs
draw_skeleton_data(Skeleton, LEFT_ARM)
draw_skeleton_data(Skeleton, RIGHT_ARM)
draw_skeleton_data(Skeleton, LEFT_LEG)
draw_skeleton_data(Skeleton, RIGHT_LEG)
##########################################################################################
def get_skeleton(skeletons):
u""" Renvoie le premier squelette ()
détecté par le capteur Kinect
Renvoie None si aucun squelette n'est détecté
"""
global dispInfo
if skeletons is not None:
for index, data in enumerate(skeletons):
# position de la tête
HeadPos = skeleton_to_depth_image(data.SkeletonPositions[JointId.Head],
dispInfo.current_w, dispInfo.current_h)
if HeadPos[0] > 0 and HeadPos[1] > 0:
return data
##########################################################################################
def get_joint(n):
u""" Renvoie l'articulation (nui.structs.JointId) du Kinect
n peut être de type :
- int = indice de l'articulation dans la liste SKELETON_JOINTS_NAMES
- str = nom clef de l'articulation dans le dictionnaire SKELETON_JOINTS
- nui.structs.JointId (déja du type à renvoyer)
Renvoie None si aucune articulation ne correspond à n
"""
if type(n) == int:
if 0 <= n < len(SKELETON_JOINTS_NAMES):
return SKELETON_JOINTS[SKELETON_JOINTS_NAMES[n]]
elif type(n) == str:
if n in SKELETON_JOINTS_NAMES:
return SKELETON_JOINTS[n]
elif isinstance(n, nui.structs.JointId):
return n
##########################################################################################
def get_joint_name(n):
u""" Renvoie le nom de l'articulation (nui.structs.JointId) du Kinect
n peut être de type :
- int = indice de l'articulation dans la liste SKELETON_JOINTS_NAMES
- str = nom clef de l'articulation dans le dictionnaire SKELETON_JOINTS
- nui.structs.JointId (déja du type à renvoyer)
Renvoie None si aucune articulation ne correspond à n
"""
if type(n) == int:
if 0 <= n < len(SKELETON_JOINTS_NAMES):
return SKELETON_JOINTS_NAMES[n]
elif type(n) == str:
if n in SKELETON_JOINTS_NAMES:
return n
elif isinstance(n, nui.structs.JointId):
for k, v in SKELETON_JOINTS.items():
if v == n:
return k
##########################################################################################
def get_joint_pos(n):
u""" Renvoie les données de position
de l'articulation (nui.structs.JointId) du Kinect
dans le système de coordonnées de l'écran (2D)
n peut être de type :
- int = indice de l'articulation dans la liste SKELETON_JOINTS_NAMES
- str = nom clef de l'articulation dans le dictionnaire SKELETON_JOINTS
- nui.structs.JointId
Renvoie None si aucune articulation ne correspond à n
"""
j = get_joint(n)
if j is not None and Skeleton is not None:
return skeleton_to_depth_image(Skeleton.SkeletonPositions[j],
dispInfo.current_w, dispInfo.current_h)
##########################################################################################
def get_joint_pos_xyz(n):
u""" Renvoie les données de position
de l'articulation (nui.structs.JointId) du Kinect
dans le système de coordonnées capteur (3D)
n peut être de type :
- int = indice de l'articulation dans la liste SKELETON_JOINTS_NAMES
- str = nom clef de l'articulation dans le dictionnaire SKELETON_JOINTS
- nui.structs.JointId
Renvoie None si aucune articulation ne correspond à n
"""
j = get_joint(n)
if j is not None and Skeleton is not None:
return Skeleton.SkeletonPositions[j]
##########################################################################################
def video_frame_ready(frame):
if not video_display:
return
with screen_lock:
address = surface_to_array(screen)
frame.image.copy_bits(address)
del address
if draw_skeleton:
draw_skeletons()
update_score()
pygame.display.update()
##########################################################################################
def depth_frame_ready(frame):
return
##########################################################################################
def update_score(texte = None):
global CurScore
# Par défaut : les coordonnées du point
if Skeleton is not None and texte is None:
point = get_joint_pos(CurJoint)
texte = get_joint_name(CurJoint)+" : "+ str(int(round(point[0]))) + " , "+ str(int(round(point[1])))
elif texte is not None:
CurScore = texte
font = pygame.font.Font(None, 36)
text = font.render(CurScore, 1, (10,10,100))
try:
screen.blit(text,(50,40))
except:
pass
##########################################################################################
def update_display():
u""" Raffraichi la fenêtre graphique
"""
pygame.display.update()
##########################################################################################
def get_event():
u""" Attend puis renvoie un événement de la part de l'interface graphique
(touche de clavier, souris, evenement du capteur, ...)
"""
global dispInfo
dispInfo = pygame.display.Info()
return pygame.event.wait()
##########################################################################################
def quit():
u""" Quitte la fenêtre graphique
"""
pygame.quit()
##########################################################################################
def modif_joint(i):
global CurJoint
CurJoint = (CurJoint+i) % len(SKELETON_JOINTS_NAMES)
# Initialisations
##########################################################################################
pygame.init()
screen_lock = thread.allocate()
screen = pygame.display.set_mode(VIDEO_WINSIZE,0,32)
pygame.display.set_caption('Kinectino')
Skeleton = None # Le skelette "courant" (si détecté) ... à afficher
screen.fill(THECOLORS["black"])
update_display()
dispInfo = pygame.display.Info()
# get_event()
Kinect = nui.Runtime()
Kinect.skeleton_engine.enabled = True
##########################################################################################
def post_frame(frame):
try:
pygame.event.post(pygame.event.Event(KINECTEVENT, skeletons = frame.SkeletonData))
except:
# event queue full
pass
Kinect.skeleton_frame_ready += post_frame
Kinect.video_frame_ready += video_frame_ready
Kinect.video_stream.open(nui.ImageStreamType.Video, 2,
nui.ImageResolution.Resolution640x480, nui.ImageType.Color)
# Kinect.depth_frame_ready += depth_frame_ready
# Kinect.depth_stream.open(nui.ImageStreamType.Depth, 2,
# nui.ImageResolution.Resolution640x480, nui.ImageType.Depth)
print(u'Commandes : ')
print(u' \u2191 - Lever le capteur')
print(u' \u2193 - Baisser le capteur')
print(u" \u2194 - Changer d'articulation")
Module calcul.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
from math import *
class point():
def __init__(self, x, y, z):
self.x = x
self.y = y
self.z = z
def __repr__(self):
def r(n):
return round(n,3)
return "point("+str(r(self.x))+","+str(r(self.y))+","+str(r(self.z))+")"
class vecteur():
def __init__(self,*args):
if len(args) == 2:
pt1 = args[0]
pt2 = args[1]
self.x = pt2.x - pt1.x
self.y = pt2.y - pt1.y
self.z = pt2.z - pt1.z
elif len(args) == 3:
self.x, self.y, self.z = args
elif len(args) == 1:
self.x, self.y, self.z = args[0].x, args[0].y, args[0].z
self.norm = self.calc_norm()
#self.dir = self.calc_dir()
def calc_norm(self):
return sqrt(self.x**2+self.y**2+self.z**2)
def calc_dir(self):
return vecteur(self) / self.norm
def __mul__(self, x):
if isinstance(x, vecteur):
return vecteur(self.y*x.z-self.z*x.y,
self.z*x.x-self.x*x.z,
self.x*x.z-self.z*x.x)
else:
return vecteur(self.x*x,
self.y*x,
self.z*x)
def __div__(self, x):
if x != 0:
return vecteur(self.x/x,
self.y/x,
self.z/x)
else:
return vecteur(self.x,
self.y,
self.z)
def __rmul__(self, x):
if isinstance(x, vecteur):
return vecteur(self.z*x.y-self.y*x.z,
self.x*x.z-self.z*x.x,
self.z*x.x-self.x*x.z)
else:
return vecteur(self.x*x,
self.y*x,
self.z*x)
def pscal(self, v):
return self.x*v.x + self.y*v.y + self.z*v.z
def angle(pt1, pt2, pt3, deg = False):
v1 = vecteur(pt2, pt1).calc_dir()
v2 = vecteur(pt2, pt3).calc_dir()
if deg:
return r2d(acos(v2.pscal(v1)))
else:
return acos(v2.pscal(v1))
def r2d(a):
return 180/pi*a
def d2r(a):
return pi/180*a
Coté Arduino
#include <Servo.h>;
Servo s1;
Servo s2;
void setup()
{
s1.attach(9); // avant/arrière
s2.attach(10); // haut/bas
Serial.begin(115200);
s1.write(90);
s2.write(90);
}
void loop() {
// si le buffer n'est pas vide ...
if (Serial.available()) {
int a1 = Serial.parseInt();
Serial.read(); // la virgule entre les deux nombres
int a2 = Serial.parseInt();
if (Serial.read() == '\n') {
s1.write(a1);
s2.write(a2);
Serial.flush();
}
}
delay(10);
}
Résultat
Carnet de bord
| Date |
Activité recherche documentaire, formation technique, analyse fonctionnelle, expérimentation simulation |
Réalisation conception fabrication assemblage |
Objectifs pour la prochaine séance |
Remarques |
| Formation Arduino | ||||
| 14/12 | Activité 1 : Piloter un logiciel 3D (Solidworks) par programmation : découverte des macro et des fonctions de visualisation de l’API de Solidworks |
|||
| 11/01 | Activité 2 : Piloter Solidworks à partir d’un programme Python. |
|||
| 18/01 | Préparation du robot Braduino | Assemblage d’une articulation du robot Braduino | ||
| 01/02 | Fixation du bras sur le socle / réglages | |||
| 08/03 | Acquisition des coordonnées des poignets/coudes/épaules droits pour pilotage d’une articulation du robot. | Apprendre à calculer un angle formé par 3 points dans l’espace ! | ||
| 22/3 | ||||
| 17/05 | Journée de valorisation à Ladoux |