Télécommande corporelle

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

[spoiler title=’Programme principal’ style=’default’ collapse_link=’true’]
#!/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

[/spoiler] [spoiler title=’Module kinect.py’ style=’default’ collapse_link=’true’]

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

[/spoiler] [spoiler title=’Module calcul.py’ style=’default’ collapse_link=’true’]
#!/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

[/spoiler]

 

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

 

Laisser un commentaire

Votre adresse e-mail ne sera pas publiée. Les champs obligatoires sont indiqués avec *