Proyecto Raspberry

PROYECTO FINAL DE CARRERA
Guiado gestual de un robot humanoide

mediante un sensor Kinet!
Estudios: Ingeniera tcnica en informtica de sistemas.
Autor: Sammy Pfeiffer
Director: Joan Aranda Lpe
A!o: "#$$
%uiado gestua& de un ro'ot (umanoide mediante un sensor )inect.
" * +#
ndice de contenido
$ Introduccin......................................................................................................................................,
$.$ )inect........................................................................................................................................,
$." -.mo funciona )inect/..........................................................................................................0
$.1 2ti&idad directa de& sensor: S3e&eton trac3ing..........................................................................4
$.5 6o'ot (umanoide: 7io&oid........................................................................................................4
$.5.$ Ser8omecanismos: Dynami9e&s......................................................................................$#
" :';eti8os.........................................................................................................................................$$
1 <ra'a;os pre8ios..............................................................................................................................$1
1.$ :S.e&eton= s3e&eton trac3ing.................................................................................................$1
1.$.$ Protoco&o :S.................................................................................................................$1
1.$." :pen>I frame?or3.........................................................................................................$1
1." .aractersticas )inect.............................................................................................................$5
5 Dise!o.............................................................................................................................................$,
5.$ P&anificacin tempora&............................................................................................................$,
5." @irm?are de 7io&oid...............................................................................................................$,
5.1 Adaptacin de datos de& esAue&eto..........................................................................................$,
5.5 Simu&acin de& ro'ot y cinemtica in8ersa.............................................................................$+
, Imp&ementacin..............................................................................................................................$0
,.$ Programacin de& firm?are de& 7io&oid para e& acceso a &os Dynami9e&...............................$0
,." Programacin de& simu&ador con cinemtica in8ersa de& 7io&oid...........................................$4
,.".$ .inemtica in8ersa..........................................................................................................$B
,.1 Programacin de& simu&ador de coordenadas de )inect C1DD con e& cam'io sistema de
coordenadas..................................................................................................................................."$
,.5 Programacin de& esca&ado de &as coordenadas conseguidas tras e& cam'io de sistema de
referencia.......................................................................................................................................""
,., .om'inacin de am'os simu&adores para &a ap&icacin fina&................................................."5
+ 6esu&tados.......................................................................................................................................",
0 An&isis econmico........................................................................................................................."B
0.$ Argumentos econmicos de& proyecto:..................................................................................."B
4 .onc&usiones...................................................................................................................................1$
B Lineas futuras de tra'a;o................................................................................................................11
$# 7i'&iografa...................................................................................................................................1,
$$ Agradecimientos...........................................................................................................................10
$" Ane9os:.........................................................................................................................................1B
$".$ Documentacin de &a ap&icacin= gua insta&acin= gua de uso............................................1B
$".$.$ 7io&oid...........................................................................................................................1B
$".$." :rdenador......................................................................................................................5#
$".$.".$ Insta&acin entorno Pyt(on Eindo?s:...................................................................5#
$".$."." Insta&acin entorno pyt(on Linu9:........................................................................5#
$".$.".1 Insta&acin :S.e&eton...........................................................................................5$
$".$.".5 2so de &a ap&icacin..............................................................................................5"
$"." Ane9o cdigo fuente firm?are 7io&oid................................................................................51
$".1 .digo fuente ap&icacin pyt(on: ........................................................................................5+
$".5 Ane9o cdigo dec&aracin estructura simu&acin 7io&oid....................................................,0
$"., Ane9o cdigo dec&aracin de& %2I.......................................................................................,4
1 * +#
5 * +#
1 Introduccin
1.1 Kinect
6ecientemente (a sa&ido a& mercado un interesante sensor= comercia&mente &&amado
)inect.Parafraseando a &a Ei3ipedia encontramos Aue )inect se puede definir como: Fun
contro&ador de ;uego &i're y entretenimiento desarro&&ado por Gicrosoft para &a 8ideoconso&a H'o9
1+#. )inect permite a &os usuarios contro&ar e interactuar con &a conso&a sin necesidad de tener
contacto fsico con un contro&ador de 8ideo;uegos tradiciona&= mediante una interfa natura& de
usuario Aue reconoce gestos= comandos de 8o= y o';etos e imgenes F.
Gicrosoft 6esearc( in8irti 8einte a!os de desarro&&o en &a tecno&oga de )inect. @ue anunciado por
primera 8e e& $ de ;unio de "##B en &a E&ectronic Entertainment E9po "##B como IPro;ect >ata&I.
E& sensor de )inect es una 'arra (orionta& de apro9imadamente "1 cm conectada a una peAue!a
'ase circu&ar con un e;e de articu&acin de rtu&a= y est dise!ado para ser co&ocado
&ongitudina&mente por encima o por de'a;o de& te&e8isor.
)inect se compone principa&mente de:
J2na cmara tradiciona& C6eso&ucin +5#954# 6%7 1#fps K%AD.
J2n emisor de infrarro;os.
J2na cmara de infrarro;os.
J5 Gicrfonos C$+'it samp&ing rate: $+LD.
J2n motor.
, * +#
Ilustracin 1: Sensor Kinect
+ * +#
Ilustracin 2: Kinect desmontado
Ilustracin 3: Imagen de profundidad en escala de grises, imagen webcam, profundidad en escala
de colores
1.2 Cmo funciona Kinect?
Para conocer &a distancia a &a Aue se encuentra cada p9e& de &a imagen de profundidad se emite un a
conste&acin de puntos con e& emisor infrarro;o:
0 * +#
Ilustracin 4: Esquema funcionamiento
Ilustracin : !onstelacin infrarro"a Kinect
Entonces &a cmara infrarro;a detecta esta conste&acin y )inect ca&cu&a &a disparidad para cada
p9e& C&a diferencia entre donde esta'a e& punto a& proyectar&o a donde est en &a proyeccinD. A esto
se &e &&ama cmara de &u estructurada.
1.3 Utilidad directa del sensor: Skeleton tracking.
S3e&eton <rac3ing= o &o Aue es &o mismo= (acer seguimiento de& cuerpo CesAue&etoD de un usuario en
tiempo rea&.
La situacin actua& es Aue Gicrosoft (a anunciado &a sa&ida de un SD) propio para usar e& )inect
inc&uyendo S3e&eton <rac3ing. .on &o cua& (a surgido un mo8imiento para crear dri8ers y
ap&icaciones para )inect mayormente opensource.
A partir de &o cua& se (a creado :pen>I= una organiacin para promo8er &a compati'i&idad e
interopera'i&idad de dispositi8os= ap&icaciones y midd&e?are de interaccin natura& C>atura&
InteractionD.
En :pen>I (an creado una &i'rera para rea&iar S3e&eton <rac3ing inc&uida en un paAuete &&amado
>I<E. Esta &i'rera nos 'rinda seguimiento de& cuerpo de& usuario aportando &as coordenadas en e&
espacio de cada uno de &os puntos de inters Ce9tremidades y articu&acionesD.
1.4 Root !umanoide: "ioloid
2n ro'ot (umanoide es ro'ot antropomorfo Aue= adems de imitar &a apariencia (umana= imita
a&gunos aspectos de su conducta.
4 * +#
Ilustracin #: E"emplo de S$eleton %rac$ing
Para este proyecto se (a usado e& 3it comercia& 7io&oid Premium )it= de 6o'otis. Este ro'ot est
dise!ado para e& aprendia;e y (o''y de &a ro'tica. Est formado por 8arios sensores= una 'atera=
e& contro&ador principa& &&amado .GJ,$#= &os componentes para unir &as diferentes pieas y
ser8omecanismos modu&ares &&amados Dynami9e&s. Estos M&timos pueden usarse conectados en
DaisyJc(ain para construir ro'ots de diferentes formas Ccon ruedas= piernas= (umanoides...D.
Este 3it se programa en un &engua;e . propio= como es costum're en otros paAuetes simi&ares
destinados a &a ro'tica como (o''y.
E9isten otros 3its como por e;emp&o:
JLego Gindstorms.
Ji6o'ot .reate.
JKe9 6o'otics Design System.
J)L6J$
En este proyecto se (a usado e& 7io&oid Premium )it dado Aue satisface nuestras necesidades y
adems e9ista uno a nuestra disponi'i&idad.
B * +#
Ilustracin &: 'ioloid en forma (umanoide
1.4.1 Servomecanismos: Dynamixels
Los Dynami9e& tienen una simi&itud a &os tradiciona&es ser8omotores= am'os son motores 'asados
en posicin= se &e puede indicar un ngu&o a& Aue girar y se posicionar en ese &ugar. Pero no aca'a
a(= se puede ;ugar con unas ,# 8aria'&es distintas en re&acin a su mo8imiento: m9imo ngu&o de
mo8imiento= 8e&ocidad de mo8imiento= fuera= carga actua&= temperatura= etc.
Estos motores pueden ser manipu&ados directamente gracias a& adaptador suministrado con e& )it
estando conectados por DaisyJc(ain unos con otros= con &o cua& se forma una peAue!a red por &a Aue
se pueden mandar mensa;es para e;ecutar mo8imientos= &eer 8aria'&es de &os distintos motores= etc.
<am'in se puede conectar por serie a& contro&ador .GJ,$# y este contro&ador se ocupar de
mandar estos mensa;es.
$# * +#
Ilustracin ): *iferentes configuraciones del $it 'ioloid
Ilustracin +: *,nami-el
Ilustracin 1.: /daptador 0S' 1
*,nami-el 2 Serie
2 Objetivos
E& o';eti8o principa& consiste en contro&ar &os 'raos de un ro'ot (umanoide mediante &os datos
proporcionados por un sensor )inect. Este o';eti8o se compone de &os siguientes su'o';eti8os o
tareas:
JEstudio de &as caractersticas de &os actuadores: ser necesario sa'er como y Aue son
capaces de (acer.
JImp&ementacin de un firm?are de contro& de &os actuadores de& ro'ot.
JImp&ementacin de &a cinemtica in8ersa de &os 'raos: se tendr Aue poder dar un punto en
e& espacio donde se Auiera posicionar &a mano de& ro'ot y Aue este posicione &as
articu&aciones donde sea necesario.
JEstudio de &a reso&ucin y precisin de &os datos ofrecidos por )inect.
JIncorporacin de &a &i'rera de S3e&eton <rac3ing: ser necesario conocer como funciona y
Aue datos ofrece so're e& esAue&eto para tra'a;ar con &.
JSimu&ador para e8a&uacin de &os datos de& esAue&eto: ser necesario representar &os datos
de& esAue&eto para poder contro&ar su precisin.
JSimu&ador para &a simu&acin de &os mo8imientos de& ro'ot: ser necesario simu&ar primero
&os mo8imientos para no poner en pe&igro e& ro'ot y compro'ar Aue &os datos de &os
mo8imientos son correctos.
J2nin de &os simu&adores en una ap&icacin fina& Aue cump&a e& o';eti8o principa&.
JPro'ar y e8a&uar &a ap&icacin fina& de integracin de &os simu&adores. Para e&&o se dise!arn
&as prue'as necesarias una 8e &ogrado e& anterior su'o';eti8o.
$$ * +#
$" * +#
3 rabajos !revios
3.1 #SCeleton$ skeleton tracking.
:S.e&eton es un peAue!o programa Aue (ace uso de &os datos de esAue&eto proporcionados por e&
:pen>I frame?or3 para en8iar por protoco&o :S. estas coordenadas.
3.1.1 "rotocolo OS#
E& protoco&o :S. C:pen Sound .ontro&D es un protoco&o pensado para comunicar instrumentos
musica&es= dispositi8os mu&timedia y ordenadores en tiempo rea&. .omMnmente es transportado por
2DP.
Las caractersticas de este protoco&o son:
JAmp&ia'&e= dinmico. EsAuema de nom'res sim'&icos tipo 26L.
JDatos numricos sim'&icos y de a&ta reso&ucin
JLengua;e de coincidencia de patrones Cpattern matc(ingD para especificar mM&tip&es
receptores de un Mnico mensa;e.
JGarcas de tiempo Ctime tagsD de a&ta reso&ucin.
JGensa;es FempaAuetadosN para aAue&&os e8entos Aue de'en ocurrir simu&tneamente
JSistema de interrogacin para encontrar dinmicamente &as capacidades de un ser8idor
:S. y o'tener documentacin.
2n e;emp&o de un paAuete :S. de :S.e&eton podra ser:
Address pattern: I*;ointI
<ype tag: IsifffI
s: Joint name= c(ec3 out t(e fu&& &ist of ;oints 'e&o?.
i: <(e ID of t(e user.
f: H coordinate of ;oint in inter8a& O#.#= $.#P
f: Q coordinate of ;oint in inter8a& O#.#= $.#P
f: R coordinate of ;oint in inter8a& O#.#= 0.#P
3.1.2 O!en$I %rame&or'
:pen>I C:pen >atura& InteractionD es un frame?or3 mu&ti&engua;e= mu&tip&ataforma= Aue define
APIs para escri'ir ap&icaciones Aue uti&icen Finteraccin natura&N.
La interaccin natura& se refiere a& concepto donde &a interaccin entre (umano y dispositi8o esta
'asada en &os sentidos (umanos= norma&mente odo y 8ista. .on esta premisa e& frame?or3 inc&uye:
J6econocimiento de 8o y comandos.
J%estos con &as manos= gestos predeterminados para rea&iar acciones configura'&es.
J7ody Gotion <rac3ing= donde todo e& cuerpo es ana&iado e interpretado para conseguir un
trac3ing.
$1 * +#
3.2 Caracter%sticas Kinect
Las prue'as rea&iadas dieron &ugar a esta grfica donde podemos 8er &a precisin de &os datos
otorgados por e& )inect. La informacin de un p9e& concreto ser cada 8e ms &e;ana a &a rea&idad
segMn nos a&e;emos de& sensor. >os encontramos Aue e& )inect opera correctamente entre &os +#cm
y &os , metros.
En e& caso de& S3e&eton <rac3ing CsegMn &os desarro&&adores de :pen>I Aue coincide con &os
conse;os de Gicrosoft so're e& uso de )inectD &a distancia idea& se sitMa entre &os " y "=, metros.
$5 * +#
Ilustracin 11: Error en cent3metros de un p3-el en funcin de la distancia de este
4 Dise(o
4.1 &lanificacin tem'oral
Lay tareas Aue se encadenan unas con otras ya Aue resu&ta necesario tra'a;ar con;untamente.
4.2 (irm)are de "ioloid
Ser necesario desarro&&ar un firm?are para comunicarse con &os ser8omotores de& 7io&oid de &a
manera ms simp&e y eficiente posi'&e.
4.3 *da'tacin de datos del es+ueleto
Ser necesario con8ertir &os datos de &as coordenadas de& esAue&eto de ta& manera Aue consigamos
&as coordenadas de cada una de &as manos en referencia a& sistema de coordenadas de nuestro
7io&oid.
Las coordenadas de &os puntos de& esAue&eto nos 8ienen dados en una referencia a& punto de 8ista
de& )inect:
JE& e;e H y e& e;e Q en un rango de O#..$P.
JE& e;e R en un rango de O#..0P.
$, * +#
>ecesitamos tras&adar &as coordenadas de &as manos de& sistema de coordenadas proporcionado por
:S.e&eton a& sistema de coordenadas en referencia a& usuario. E& p&ano formado por e& pec(o de&
usuario de'e tener e& origen de coordenadas.
4.4 Simulacin del root , cinem-tica in.ersa
La cinemtica in8ersa consiste en (a&&ar &os ngu&os de cada articu&acin dada &a configuracin
deseada para &a figura= en nuestro caso= &a posicin de &a mano.
La decisin de mo8er &os 'raos por cinemtica in8ersa (ace Aue crear un simu&ador sea muy
necesario C(acer &as prue'as directamente en e& ro'ot entra!a cierto pe&igroD.
La simu&acin (a de ser8ir para poder pro'ar &a ap&icacin sin tener Aue tener conectado e& ro'ot.
La simu&acin tendr Aue tener cierto contro& so're:
J>o tratar de introducir dentro de& cuerpo &a mano.
J>o tratar de posicionar &a mano en posiciones no a&cana'&es.
$+ * +#
Ilustracin 12: 4osicin relati5a al usuario de sus manos
) Im!lementacin
La imp&ementacin est compuesta por &os siguientes pasos ordenados crono&gicamente segMn (an
sido imp&ementados.
/.1 &rogramacin del firm)are del "ioloid 'ara el acceso a los
0,nami1el
E& cdigo para e& acceso (a de ser &o ms senci&&o y rpido posi'&e.
La comunicacin entre e& ordenador y e& 7io&oid se rea&ia por serie Cusando un adaptador 2S7J
serie en este casoD. .on &o cua& cuanto ms cortos y senci&&os sean &os mensa;es me;or reaccionarn
&as articu&aciones.
La estructura de &os mensa;es imp&ementada es &a siguiente:
HH QQQQ
Donde HH es e& nMmero identificador de& Dynami9e& Aue de'e reci'ir &a orden e QQQQ es &a
Fcantidad de mo8imientoN o &o Aue es &o mismo= e& ngu&o a& Aue Aueremos Aue 8aya e& motor.
Esta cantidad de mo8imiento est codificada en una esca&a de # a $#"1 representando &os 1+#
grados.
E& diagrama de f&u;o es e& siguiente:
E& cdigo fuente consta en e& ane9o.
$0 * +#
Ilustracin 13: *iagrama de flu"o firmware 'ioloid
/.2 &rogramacin del simulador con cinem-tica in.ersa del "ioloid
Se midi e& ro'ot y se consiguieron &as siguientes medidas apro9imadas:
JDistancia (om'ro a (om'ro Canc(oD $#cm.
JDistancia (om'ro a codo Bcm.
JDistancia codo a mano 5=1cm.
JDistancia (om'ro a FcaderaN "#cm.
JLargo ro'ot $#cm.
E& simu&ador se imp&ement a 'ase de formas simp&es conectando un cu'o centra&= representando e&
torso= a &os 'raos. Estos estn representados por unos ci&indros Cuno para e& 'rao y otro para e&
ante'raoD con unas esferas Aue marcan &as articu&aciones y &as manos.
%racias a Kpyt(on= crear esta simu&acin es muy senci&&o. 7asta con dec&arar e& tipo de estructura
C'o9= cy&inder= sp(ereD sus coordenadas= su tama!o y su 8ector direcciona& y ya est representado.
E& cdigo de &a dec&aracin se encuentra en e& ane9o.
$4 * +#
Ilustracin 14: Estructura tronco superior
bioloid en simulacin
).2.1 #inem*tica inversa
Rotai"n del hombro#
<enemos e& 8ector K Aue es e& Aue 8a desde e& e;e C(om'roD a& punto de &a mano.
Para sacar e& ngu&o de rotacin operaremos:
Q esto &o negaremos dado Aue nos da e& ngu&o en negati8o.
$ngulo del odo#
$B * +#
Dado Aue no podemos depender de en Aue e;es estn situadas &as partes de& 'rao &a forma ms
senci&&a de proceder es tratar directamente con &as distancias en un p&ano imaginario.
Primero sacamos &a distancia de un cateto de nuestro tringu&o imaginario:
.ateto adyacente:
CKm S Kector mano= a S ante'rao= ' S 'raoD
Luego sacamos e& otro cateto= Aue estar en re&acin a este:
Q sacamos e& ngu&o ca&cu&ando &a arcotangente como en &a articu&acin anterior:
$ngulo del hombro#
"# * +#
So&o podemos depender de& e;e H dado Aue es so're e& cua& e& ngu&o se mo8er Cimaginariamente
partiendo siempre de a(D= con &o cua& tendremos Aue tra'a;ar como en e& caso anterior con &as
distancias para conseguir e& cateto adyacente pero teniendo en cuenta e& ngu&o de& codo.
<odo esto (a'r Aue mu&tip&icar&o por su factor de esca&a para pasar&e &a informacin en e& formato
apropiado a &os Dynami9e&.
Adems &os ngu&os imaginarios de &os Aue partimos no coinciden con &os de &os Dynami9e&= as
Aue tendremos Aue (acer una peAue!a ca&i'racin.
Esta ca&i'racin consiste en sumar $5+T de offset a todos &os ngu&os.
/.3 &rogramacin del simulador de coordenadas de Kinect 2303 con el
camio sistema de coordenadas
Para dar uso a nuestra cinemtica in8ersa necesitamos &as coordenadas de &as manos en e& mismo
sistema de coordenadas en e& Aue se encuentra nuestra simu&acin. Para e&&o e& primer paso es
conseguir &as coordenadas de &as manos en referencia a& pec(o de& usuario de& )inect. En este caso
(e tomado e& origen de coordenadas en e& (om'ro iAuierdo por comodidad. Luego so&o (ace fa&ta
(acer una tras&acin de coordenadas para situar&o en e& origen de& simu&ador Ce& centro de& cu'o Aue
representa e& torsoD.
Procedimiento:
Sean C9=y=D &as coordenadas de& punto respecto a &os e;es de coordenadas HJQJR.Sean C9#=y#=#D &as
coordenadas de& origen de coordenadas de &os e;es HJQJR respecto a& nue8o sistema de coordenadas
HUJQUJRU.
Sean a='=c e& ngu&o Aue se gira e& e;e HU respecto a &os e;es HJQJR= sean d=e=f e& ngu&o Aue se gira
e& e;e QU respecto a &os e;es HJQJR= y sean g=(=i e& ngu&o Aue se gira e& e;e RU respecto a &os e;es HJ
QJR.

"$ * +#
Ser e& sistema de ecuaciones Aue nos dar &as coordenadas de nuestro punto en referencia a& nue8o
sistema de coordenadas.
/.4 &rogramacin del escalado de las coordenadas conseguidas tras el
camio de sistema de referencia
Dado Aue e& cuerpo (umano es distinto a& cuerpo de& 7io&oid Cprincipa&mente nuestro ante'rao
sumado a nuestra mano es mas &argo Aue nuestro 'rao= mientras en e& 7io&oid pasa a& contrarioD
necesitamos (acer un esca&ado de &as coordenadas de& punto teniendo esto en cuenta.
Para e& 'rao iAuierdo Ce& proceso es muy simi&ar para e& derec(oD:
"" * +#
Lom'ro iAuierdo= codo iAuierdo= mano iAuierda se refiere a &as coordenadas de estos puntos.
dV'rao S distanciaC(om'ro iAuierdo= codo iAuierdoD
dVante'rao S distanciaCcodo iAuierdo= mano iAuierdaD
dV'io&oidV'rao S #.#B
dV'io&oidVante'rao S #.#51
dVtodoV3inect S dV'rao W dVante'rao
dVtodoV'io&oid S dV'io&oidV'rao W dV'io&oidVante'rao
8V(iVaVci S Ccodo iAuierdo X (om'ro iAuierdoD
8V(iVaVci S Ccodo iAuierdo * modu&oC codo iAuierdo D D Y dV'io&oidV'rao
8VciVaVmi S Cmano iAuierda X codo iAuierdoD
8VciVaVmi S C8VciVaVmi *modu&oC 8VciVaVmi D D Y dV'io&oidVante'rao
punto mano iAuierda S 8V(iVaVci W 8VciVaVmi W C#.#,= #.#,= #.#",D
Donde C#.#,= #.#,= #.#",D es &a tras&acin a& origen de coordenadas.
Lo Aue (acemos es usar &os 8ectores proporciona&es a& tama!o de& 'rao y de& ante'rao para situar
correctamente y en esca&a &a posicin de &a mano.
"1 * +#
/./ Cominacin de amos simuladores 'ara la a'licacin final
Inicia&mente se inicia e& %2I C%rap(ica& 2ser InterfaceD en e& cua& se presenta &a simu&acin de&
7io&oid. A su 8e se inicia e& (i&o de e;ecucin Aue se encargar de mantener una estructura de
8aria'&es con &as coordenadas de &os puntos Aue nos interesan a tiempo rea&.
Gientras e& programa siga corriendo se actua&iar &a %2I continuamente y en caso de estar
conectado e& ro'ot se &e en8iarn &as ordenes correspondientes a cada Dynami9e&.
"5 * +#
Ilustracin 1: *iagrama de flu"o de la aplicacin
+ ,esultados
Se (a demostrado &a 8ia'i&idad de& uso de un sensor )inect como interficie Mti& y en tiempo rea&
para e& contro& gestua& de &os 'raos de un ro'ot (umanoide.
Kdeo en youtu'e: (ttp:**???.youtu'e.com*?atc(/8S?i1&4EJ..LG
En cuanto a &a %iabilidad de los mo&imientos durante e& proyecto se (an rea&iado prue'as de
e8a&uacin de &os resu&tados en &os pasos intermedios ta& como podemos encontrar en e& diagrama
de %antt. .oncretamente se (an rea&iado &as siguientes prue'as para cada tarea:
JImp&ementacin de un firm?are para e& contro& de &os actuadores:
Para cada mo8imiento de cada Dynami9e& se &ea e& 8a&or de &a posicin o';eti8o= se &e
manda'a como orden y &uego se &ea &a posicin actua&.
JImp&ementacin de &a cinemtica in8ersa de &os 'raos:
Se pro'aron casos simp&es Cen &os e;es H=Q=RD y casos no tri8ia&es de posiciones de &a mano.
En estos casos se compro'a'a= primero 8isua&mente en e& simu&ador Ca grandes rasgos si
tena sentidoD= &uego ana&iando &os resu&tados de &as coordenadas y ngu&os o'tenidos con
un mode&o (ec(o a mano= si estos datos eran correctos.
", * +#
Ilustracin 1#: !apturas de un 5ideo de uso de la aplicacin
JIncorporacin de &a &i'rera de S3e&eton <rac3ing:
Se compro' Aue &os datos so're e& esAue&eto fueran un f&u;o constante contando e& nMmero
de actua&iaciones de cada articu&acin por segundo. <am'in se compro' Aue &as
coordenadas no sufrieran sa&tos 'ruscos de posicin ana&iando &as coordenadas de 8arias
escenas manua&mente.
JSimu&ador para e8a&uacin de &os datos de& esAue&eto:
Se compro' Aue e& cam'io de sistema de coordenadas para &os puntos de& esAue&eto eran
correctos= primero 8isua&mente en e& simu&ador Ca grandes rasgos para sa'er si tena sentidoD=
&uego ana&iando &os resu&tados de &as coordenadas (aciendo &os c&cu&os manua&mente.
JSimu&ador para simu&acin de &os mo8imientos de& ro'ot:
La compro'acin se rea&i a& imp&ementar &a cinemtica in8ersa de &os 'raos.
En cuanto a& tiem'o de res'uesta se (an ana&iado 8arios 8ideos so're e& uso de &a ap&icacin
donde se (a o'ser8ado Co'ser8ando cuantos frames pasan desde Aue comiena un mo8imiento (asta
Aue e& 'rao de& ro'ot empiea a mo8erse imitndo&oD Aue e& retraso es menor a $,ms en
condiciones de reposo a mo8imiento.
En caso de Aue e& ro'ot este mo8iendo e& 'rao y se &e fuerce a cam'iar de direccin= por &a
dinmica de &os Dynami9e& este tiempo aumenta.
Se (a rea&iado una peAue!a encuesta entre "# personas Aue (an pro'ado e& simu&ador. La prue'a (a
consistido en ;ugar durante unos minutos con e& sistema e;ecutando mo8imientos senci&&os a&
principio= para (a'ituarse= y &uego mo8erse con &i'ertad.
En esta encuesta se &e peda a& usuario Aue puntuara de& $ a& , &as siguientes preguntas:
$. A& mo8er so&o un 'rao= -en Aue grado te (a parecido Aue e& ro'ot responda 'ien/
". A& mo8er &os dos 'raos= -en Aue grado te (a parecido Aue e& ro'ot responda 'ien/
1. En genera&= -piensas Aue &a posicin de &as manos de& ro'ot son fie&es a &as de& usuario/
5. En genera&= -piensas Aue e& ro'ot responde rpidamente a &os mo8imientos de& usuario/
De donde sacamos &as siguientes respuestas:
La cua& nos da una media de 5=$.
"+ * +#
La cua& nos da una media de 1=5,.
"0 * +#
"4 * +#
- .n*lisis econmico
Cone'to (oras Preio hora Total
Dise)o 5, 0#=## Z 1.$,#=## Z
*e+nia $, ",=## Z 10,=## Z
Programai"n "5# ",=## Z +.###=## Z
Total 1## B.,",=## Z
*aterial Preio Preio Final
,ioloid Premium Kit $."##=##[Y 4,#=## Z
Kinet $,#=## Z $,#=## Z
Estai"n de traba-o $.###=##ZYY $+0=## Z
Total $.$+0=## Z
Y .am'io actua& $Z S $=5$[
YY Amortiacin a "5 meses= uso para e& proyecto: 5 meses.
Coste total del 'ro.eto# /0!1234005
4.1 *rgumentos econmicos del 'ro,ecto:
-\ue aporta a& sensor )inect este proyecto/
Da promocin dado Aue se &e da uso para un producto Aue puede &&amar &a atencin.
-\ue aporta a& ro'ot este proyecto/
Principa&mente es un proyecto reproduci'&e Aue a!ade inters didctico. A partir de & se pueden
montar otras configuraciones de& 7io&oid y mane;ar&as a tra8s de& )inect.
Se podra crear a&gMn tipo de ;uego de recreati8a donde &os 'raos podran coger una pe&ota y tirar&a
a a&gMn o';eti8o.
Podra construirse un peAue!o s(o? de promocin= a& pasar por de&ante de &a configuracin de&
proyecto uno podra mo8er e& ro'ot. A& atraer &a atencin de& posi'&e consumidor*c&iente se
afianara e& producto en promocin.
"B * +#
1# * +#
/ #onclusiones
Para conseguir rea&iar este proyecto se (an tenido Aue integrar muc(os e&ementos. A& ser punta de
&ana de este tema no (a'a demasiado materia& de& Aue partir con &o cua& nos (emos 8isto o'&igados
a pro'ar diferentes opciones en cuanto a enfoAues de& pro'&ema y (erramientas uti&iadas.
Dado e& carcter de in8estigacin y desarro&&o de& proyecto se encuentra fuera de nuestro a&cance &a
e8a&uacin precisa de& sistema de guiado ya Aue esto sera origen de otro proyecto.
E& o';eti8o de este proyecto (a sido posi'&e con re&ati8o poco esfuero gracias a Aue &a tecno&oga
de )inect nos permite conseguir un esAue&eto de un usuario fci& y fie&mente. AMn as e& tratamiento
de &os datos para Aue fuera Mti&es reAuiri un esfuero considera'&e por no tener tra'a;os anteriores
en &os cua&es 'asarse.
La resu&tado ser un e;ercicio muy interesante y una gran oportunidad para aprender conceptos
importantes como cinemtica in8ersa= programacin con grficos en 1D y repasar conceptos
ap&icando geometra y trigonometra.
Pyt(on (a resu&tado ser un &engua;e muy senci&&o de aprender teniendo una 'ase de programacin y
Aue permite o'tener grandes resu&tados en poco tiempo.
E& 7io&oid Premium )it es un paAuete incre'&e para adentrarse en e& mundo de &a ro'tica. Lay
'astantes recursos en &a ?e' para conseguir comenar cua&Auier proyecto Aue uno se p&antee.
1$ * +#
1" * +#
0 1ineas %uturas de trabajo
Sera muy posi'&e= y pro'a'&emente se rea&iar= &a o'tencin de& mo8imiento de& resto de& cuerpo
Cespecia&mente &as piernasD con un mtodo simi&ar. En principio &o ms senci&&o sera simp&emente
mo8er &as piernas= sin caminar. Esto es as ya Aue caminar imp&ica ser consciente de &a dinmica de&
cuerpo para Aue este no caiga= para e&&o necesita de sensores para poder faci&itar esta tarea y un
sistema Aue &os monitorice para actuar como sea de'ido en cada caso.
11 * +#
15 * +#
12 3iblio4ra%5a
Especificaciones Dynami9e&s:
(ttp:**???.ro'osa88y.com*6o'oSa88yPages*Support*7io&oid*AHJ$"]"4eng&is(]"B.pdf
Documentacin :pen>I
(ttp:**???.openni.org*documentation
:S.e&eton= de Sense7&oom
(ttps:**git(u'.com*Sense'&oom*:S.e&eton
<rigonometra= Ei3ipedia:
(ttp:**en.?i3ipedia.org*?i3i*In8erseVtrigonometricVfunctions
Kectores en 61 CGoises Ki&&enaD
(ttp:**es.scri'd.com*doc*4+4B5B+*KectoresJenJ
61^adVunitSDocVSide'oardVGedium6ectang&eV7<@V1##9",#_ur&S(ttp
]1A**es.scri'd.com*doc*4+4B5B+*KectoresJenJ61_attri'utesS\uantSegs]1DD]"+@our%en
]1D<rue]"+IA7Parent]1D<ec(no&ogy]","#]","+]","#.omputing]"+E9tension]1Dpdf
]"+%ender]1Dm]"+2ser]1D"+5,,$$+]"+AdLayout]1DJ$50"51+"$"]"+Doc2ser
]1D"1B++$$]"+2serState]1DIn]"+IA7.(i&d]1DKisua&]","#7asic]","..*.
]"+Document]1D4+4B5B+_s3ipS#
Sistemas de coordenadas= ?i3ipedia:
(ttp:**es.?i3ipedia.org*?i3i*SistemaVdeVcoordenadas
.am'io sistema coordenadas
(ttp:**???.te&efonica.net*?e'"*&asmatematicasdemario*%eometria*Ana&itica*.oordenadas*.oor6ec
t.(tm
6epaso geometra y trigonometra
(ttp:**t(a&es.cica.es*rd*6ecursos*rdBB*edBBJ#,51J#5*Distancia.(tm&
@unciones de numpy para Pyt(on:
(ttp:**mat(esaurus.sourceforge.net*mat&a'Jnumpy.(tm&
Documentacin oficia& Pyt(on:
(ttp:**???.pyt(on.org*doc*
Documentacin oficia& Kpyt(on:
(ttp:**???.8pyt(on.org*contents*doc.(tm&
Documentacin pyseria&:
(ttp:**pyseria&.sourceforge.net*
Documentacin simp&e:S. Cpara pyt(onD:
(ttp:**???.i9iJsoft?are.net*content*'odyV'ac3yardVosc.(tm&
Lin3s so're cinemtica in8ersa:
1, * +#
(ttp:**support.ro'otis.com*en*soft?are*ro'op&us*ro'op&usVmotion*motionedit*poseedit*poseuti&ity*ro
'op&usVmotionVi3.(tm
(ttp:**apo&&o.upc.es*(umanoide*trac*?i3i*7io&oid)inematics
(ttp:**???.fcet.staffs.ac.u3*so?$*ro'otmateria&*manipu&atorV3inematics.(tm
(ttp:**eris.&ira&a'.it*?i3i*)DLJsimp&e
(ttp:**es.?i3ipedia.org*?i3i*.inem].1]A$ticaVin8ersa
(ttps:**sites.goog&e.com*site*proyectosro'oticos*cinematicaJin8ersaJi
1+ * +#
11 .4radecimientos
AESS:
Ja8ier @rnande= Jonat(an %ona&e= GiAue& @arr= Pau 7e&tran= Li&ario <ome= L&uis 6o8ira=
Andrs Pardo= etc. todos &os miem'ros Aue (an estado a( ec(ando una mano siempre Aue me (a
(ec(o fa&ta y me (an ayudado a mantener e& nimo.
ESAII:
Joan Aranda= director de este P@. gracias a& cua& (e podido aprender muc(os conceptos Aue seguro
me sern Mti&es en e& futuro.
Josep 6uafa= Aue me ayud a encaminarme (acia un P@..
:tros:
Jordi <ur Escande&&= amigo y consu&tor en cua&Auier duda Aue tu8iera matemtica= gracias por tu
esfuero.
Q en genera& a toda esa gente Aue estu8o ayudndome ya fuera con conocimientos o con su actitud.
10 * +#
14 * +#
12 .nexos:
12.1 0ocumentacin de la a'licacin$ gu%a instalacin$ gu%a de uso
12.1.1 3ioloid
Eindo?s:
>ecesitaremos insta&ar e& firm?are en e& 7io&oid. E& firm?are est compuesto por un arc(i8o .(e9.
Para insta&ar&o podemos insta&ar 6o'oP&us de 6:7:<IS. CLin3 y gua de insta&acin:
(ttp:**support.ro'otis.com*en*soft?are*ro'op&usVmain.(tm D.
Este soft?are contiene un termina& &&amado 6o'oP&us <ermina&.
.onectamos a& puerto asignado en nuestro ordenador como puerto .:G Ca ,0+## 'audiosD.
2na 8e conectado a& 7io&oid de'eremos mantener pu&sado ^ CA&t%rW1D (asta Aue nos sa&ga una
conso&a. 7orramos &os caracteres F^N de ms Aue nos (an sa&ido y:
JE;ecutamos e& comando FLDN Csin &as comi&&asD.
JKamos a @i&es ` <ransmit @i&e y se&eccionamos e& .(e9 de nuestro firm?are.
Para correr este nue8o firm?are podemos reiniciar e& 7io&oid o escri'ir F%:N.
Linu9:
>os (ar fa&ta a&gMn tipo de conso&a para interactuar con serie y repetir &as instrucciones anteriores.
1B * +#
12.1.2 Ordenador
12.1.2.1 5nstalacin entorno &,t!on 6indo)s:
>ecesitaremos insta&ar Pyt(on= en este proyecto se (a usado Pyt(on ".+:
(ttp:**sourceforge.net*pro;ects*py?in1"*fi&es*py?in1"*7ui&d"$+*py?in1"J"$+.?in1"J
py".+.e9e*do?n&oad
Insta&amos PySeria& para e& soporte para serie:
(ttp:**pyseria&.sourceforge.net*
Descarga: (ttp:**pypi.pyt(on.org*pypi*pyseria&
%ua de insta&acin: (ttp:**pyseria&.sourceforge.net*pyseria&.(tm&^insta&&ation
Insta&amos Simp&e:S.:
(ttp:**???.i9iJaudio.net*content*'odyV'ac3yardVpyt(on.(tm&
Para e&&o insta&amos primero py:S.:
(ttps:**trac.8".n&*?i3i*py:S.
Insta&amos >umPy:
(ttp:**numpy.scipy.org*
Descarga: (ttp:**???.scipy.org*Do?n&oad
%ua de insta&acin: (ttp:**???.scipy.org*Insta&&ingVSciPy
Insta&amos Kpyt(on:
(ttp:**???.8pyt(on.org*inde9.(tm&
Descarga y gua: (ttp:**???.8pyt(on.org*contents*do?n&oadV?indo?s.(tm&
Insta&amos ?9Pyt(on:
(ttp:**???.?9pyt(on.org*do?n&oad.p(p
12.1.2.2 5nstalacin entorno ',t!on 7inu1:
Lay Aue insta&ar &as mismas &i'reras Aue en Eindo?s.
5# * +#
12.1.2.3 5nstalacin #SCeleton
:';eti8o usar: (ttps:**git(u'.com*Sense'&oom*:S.e&eton^readme
CE;emp&os a compi&ar (ttps:**git(u'.com*Sense'&oom*:S.e&etonJe9amp&es D
\ue como dice (ay Aue insta&ar &o Aue nos diga: (ttps:**git(u'.com*a8in"*Sensor)inect
Kideo muestra: (ttp:**ur'an(on3ing.com*ideasfordoens*"#$$*#"*$+*s3e&etonJtrac3ingJ?it(J3inectJ
andJprocessing*
Empeamos por Sensor)inect:
Sensor)inect Cde su 6eadmeD:
Insta&& notes:
$D Insta&& unsta'&e :pen>I C(ttp:**???.openni.org*do?n&oadfi&es*openniJ'inaries*"#J&atestJ
unsta'&eD C:pen>IJ7inJEin1"J8$.#.#.",.e9e en &a carpetaD :n Linu9*Gac p&ease do: .*sudo
insta&&.s(
"D Insta&& Sensor Ct(is 8ersion...DCa8in"JSensor)inectJ#$"5'd".ip en &a carpetaD :n Linu9*Gac
p&ease do: .*sudo insta&&.s(
1D Insta&& unsta'&e >I<E C(ttp:**???.openni.org*do?n&oadfi&es*openniJcomp&iantJmidd&e?areJ
'inaries*11J&atestJunsta'&eD C>I<EJ7inJEin1"J8$.1.#.$4.e9e en &a carpetaD :n Linu9*Gac p&ease
do: .*sudo insta&&.s(
2se t(is &icense ?(en as3ed during t(e insta&&ation: #):I3"JeI7Q.&PEKnGo6)n,cdQ5S
1.,D .onectar )inect y aceptar &as insta&aciones de dri8ers de Eindo?s.
5D <est ^$: 6un t(e >iKie?er samp&e to ma3e sure dept( _ image streams are ?or3ing.
,D <est ^": 6un t(e :pen>I*>i2ser<rac3er samp&e p&ay ?it( t(e s3e&eton. DonUt forget to start ?it(
t(e ca&i'ration posea CE9p&ained in t(e PD@sD
,.BD Para Aue funcione e& M&timo test CQ otras cosas mas ade&anteD:
>ote: A&& of t(e >I<E samp&es are using 1"#9"5# reso&ution. Qou need to c(ange it to +5#954# in
t(e HGL fi&es inside t(e Data directory. I (a8e prepared preconfigured HGLs in t(e I>I<EbDataI
dir. Just copy t(em to Ic:bProgram @i&esbPrime Senseb>I<EbDataI.
+D <est ^1: <ry t(e >I<E*Samp&eJPointKie?er samp&e for t(e (and trac3ing demo.
E insta&amos :S.e&eton:
:S.e&etonJ$.#V?in1".ip
E;ecutamos e& e;ecuta'&e.
Este es e& ser8idor :S.e&eton= &os mensa;es sa&en en
(ttps:**git(u'.com*Sense'&oom*:S.e&eton^readme
Podemos pro'ar&o con animata: (ttp:**animata.3i'u.(u*do?n&oads.(tm&animataV?inJ##5.ip
5$ * +#
Para e&&o tenemos Aue e;ecutar :S.e&eton.e9e asi:
:S.e&eton.e9e J3 Jm9 +5# Jmy 54# Jo9 J$+#
Si se Auiere pro'ar a&guna demo:
(ttps:**git(u'.com*Sense'&oom*:S.e&etonJe9amp&es
12.1.2.4 Uso de la a'licacin
Por un &ado de'emos e;ecutar :S.e&eton de &a siguiente manera:
:S.e&eton.e9e J? Jr
Donde J? sir8e para 8er e& 8isor de imagen de profundidad.
Donde Jr sir8e para o'tener &os datos en modo FmirrorN Aue es como funciona e& cdigo en pyt(on.
Para Aue :S.e&eton nos empiece a (acer trac3ing de'emos poner &a FPsiN pose:
Por otro &ado tenemos Aue e;ecutar e& cdigo pyt(on:
Simp&emente podemos e;ecutar 7io&oidArmsGain)inect:S..py.
Para (acer Aue e& 7io&oid e;ecute &os mo8imientos (a'r Aue e&egir e& puerto serie Aue est usando y
dar&e a& 'otn .onnect.
5" * +#
12.2 *ne1o cdigo fuente firm)are "ioloid
// Codigo para coger los mensajes por serie y ejecutar los movimientos
1. #include <avr/io.h>
2. #include <stdio.h>
3. #include <string.h>
4. #include <stdlib.h>
5. #include <avr/interrupt.h>
6. #include "serial.h"
7. #include "dna!i"el.h"
4.
9. /// Control table address
1#.#de$ine %&'()*&%(+,-,(.&* 3#
11.#de$ine %&'()*&%(+,-,(.&/ 31
12.#de$ine %&%01+1.-&%(+,-,(.&* 36
13.#de$ine %&%01+1.-&%(+,-,(.&/ 37
14.#de$ine %&2(3,.' 46
$,.
16.// Default setting
17.#de$ine 415)6*-&7)64.62 1 // 12bps
18.#de$ine 415)6*-&,4 1
$B.
2#.void %rint9o!!+tatus:int 9o!!+tatus;<
21.void %rint1rror9ode:void;<
"".
23.int !ain:void;
24.=
",. int 3alue > #<
"+. unsigned short 'oal%os?2@ > =#A 1#23B<
"0. int inde" > #<
"4. int id > 1< // id de motor
"B. int !ovi!iento > #<
1#. int b2ovingA C%resent%os<
1$. int 9o!!+tatus<
1".
11. serial&initialiDe:576##;< // USA! "nitiali#e
15. d"l&initialiDe: #A 415)6*-&7)64.62 ;< // $ot using device inde%
1,. sei:;<
1+.
10. //printf& '(n(nSerial Comm. moving motor(n(n' )*
14.
1B. Chile :1;
5#. =
5$. char 0eceived4ata?2#@<
5". gets:0eceived4ata;<
51. i$:strlen:0eceived4ata; < 3;
55. print$:"1rror. 5or!ato ordenE ,d2otor 9ant2ov\n";<
5,. else=
51 * +#
5+. //+arsear mensaje
50. char F pch<
54. pch > strtoG :0eceived4ataA" ";<
5B. $or:int i>#< pch H> .6**< iII;
,#. =
,$. i$:i>>#; id>atoi:pch;< //coger id motor
,". i$:i>>1; !ovi!iento>atoi:pch;<// coger cantidad de movimiento
,1. pch > strtoG :.6**A " A.J";<
,5. B
,,.
,+. // ,jecutar orden
,0. d"l&Crite&Cord: idA %&'()*&%(+,-,(.&*A !ovi!iento;<

,4. %rint1rror9ode:;< //+or si -ay algun error
,B. B
+#. B
+$.
+". return 1<
63.B
+5.
+,.
++.
6..// +rint communication result
68.void %rint9o!!+tatus:int 9o!!+tatus;
6K.=
0#. sCitch:9o!!+tatus;
0$. =
0". case 9(22&-L5),*E
01. print$:"9(22&-L5),*E 5ailed trans!it instruction pacGetH\n";<
05. break<
0,.
0+. case 9(22&-L100(0E
00. print$:"9(22&-L100(0E ,ncorrect instruction pacGetH\n";<
04. break<
0B.
4#. case 9(22&0L5),*E
4$. print$:"9(22&0L5),*E 5ailed get status pacGet $ro! deviceH\n";<
4". break<
41.
45. case 9(22&0LM),-,.'E
4,. print$:"9(22&0LM),-,.'E .oC recieving status pacGetH\n";<
4+. break<
40.
44. case 9(22&0L-,21(6-E
4B. print$:"9(22&0L-,21(6-E -here is no status pacGetH\n";<
B#. break<
B$.
55 * +#
B". case 9(22&0L9(006%-E
B1. print$:"9(22&0L9(006%-E ,ncorrect status pacGetH\n";<
B5. break<
B,.
B+. de$aultE
B0. print$:"-his is unGnoCn error codeH\n";<
B4. break<
BB. B
1##.B
$#$.
1/0.// +rint error bit of status pac1et
1#3.void %rint1rror9ode:;
1#4.=
$#,. i$:d"l&get&r"pacGet&error:1007,-&3(*-)'1; >> 1;
$#+. print$:",nput voltage errorH\n";<
$#0.
$#4. i$:d"l&get&r"pacGet&error:1007,-&).'*1; >> 1;
$#B. print$:")ngle li!it errorH\n";<
$$#.
$$$. i$:d"l&get&r"pacGet&error:1007,-&(310/1)-; >> 1;
$$". print$:"(verheat errorH\n";<
$$1.
$$5. i$:d"l&get&r"pacGet&error:1007,-&0).'1; >> 1;
$$,. print$:"(ut o$ range errorH\n";<
$$+.
$$0. i$:d"l&get&r"pacGet&error:1007,-&9/19N+62; >> 1;
$$4. print$:"9hecGsu! errorH\n";<
$$B.
$"#. i$:d"l&get&r"pacGet&error:1007,-&(310*()4; >> 1;
$"$. print$:"(verload errorH\n";<
$"".
$"1. i$:d"l&get&r"pacGet&error:1007,-&,.+-069-,(.; >> 1;
$"5. print$:",nstruction code errorH\n";<
125.B
5, * +#
12.3 Cdigo fuente a'licacin ',t!on:
from ar!graphics import F
1. import C"
2. from )r!'ui2 import F 2"mport t-e 3ui
3. import serial
4. import ti!e
5. import thread
6. from si!ple(+9 import F
7. import nu!p as np
4.
B.
$#.
11.24unciones utiles5
10.26odulo de un vector
13.def !odulus&vector:";E
$5. return np.sOrt::"F";.su!:;;
$,.
16.2Distancia entre dos puntos5
17.def dist:"A;E
$4. return np.sOrt:np.su!::"J;FF2;;
$B.
0/.2Cos&angulo) entre dos vectores
21.def cosangle:"A;E
"". dot > np.dot:"A;
"1. "&!odulus > np.sOrt::"F";.su!:;;
"5. &!odulus > np.sOrt::F;.su!:;;
",. return dot / "&!odulus / &!odulus
06.2 np.rad0deg&angle)
"0.
07.2Angulo entre dos vectores en grados
2K.def angle:"A;E
1#. dot > np.dot:"A;
1$. "&!odulus > np.sOrt::"F";.su!:;;
1". &!odulus > np.sOrt::F;.su!:;;
11. cosang > dot / :"&!odulus F &!odulus;
15. return cosang F 36# / 2 / np.pi
1,.
1+.
8..2Devuelve la solucion en la traslacion
87.2v% vy v# 9 vectors del plano del :inect
89.2point 9 punto a cambiar de un sistema de coordenadas al otro
;/.2s-oulderpoint 9 punto del -ombro
41.def solve&coords:pointA shoulderpointA v"A vA vD;E
5". 2vectores unitarios origen
51. vo" > np.arra::1A#A#;;
55. vo > np.arra::#A1A#;;
5+ * +#
5,. voD > np.arra::#A#A1;;
5+.
50. " > np.arra::cosangle:v"Avo";Acosangle:v"Avo;Acosangle:v"AvoD;;;
54. > np.arra::cosangle:vAvo";Acosangle:vAvo;Acosangle:vAvoD;;;
5B. D > np.arra::cosangle:vDAvo";Acosangle:vDAvo;Acosangle:vDAvoD;;;
,#. ) > np.arra::"AAD;;
,$. return np.linalg.solve:)ApointJshoulderpoint;
,".
,1. 2%9 %/ < %=cosa < y=cosd < #=cosg
,5. 2y9 y/ < %=cosb < y=cose < #=cos-
,,. 2#9 #/ < %=cosc < y=cosf < #=cosi
,+.
,0.
,4.
,B.
6/.2 esolver la cinematica inversa> actuali#ar la animacion y mandar las ordenes
61.def do&,N:handA handcoords;E 2para mano i#? y para mano derec-a> -and 9 / i#?>
-and 9 1 der
+". global ser 2 serie
+1. 4 > #
+5. tryE
+,. target.pos > handcoords?#@?#@A handcoords?#@?1@A handcoords?#@?2@
++. exceptE
+0. print ss.e"c&in$o:;
+4.
+B. if :target." > -orso.siDe."/2. or target." < J-orso.siDe."/2.;P
0#. or :target. > -orso.siDe./2 or target. < J-orso.siDe./2.;P
0$. or :target.D > -orso.siDe.D/2 or target.D < J-orso.siDe.D/2;E
0".
01. if hand >> #E 2 6ano i#?uierda
05. rvector.a"is > target.pos J 0&+houlder&Qoint.pos
0,. rvector.pos > 0&+houlder&Qoint.pos
0+. total&loCer&ar!&length > *oCer&)r!&*ength
00. 2 6irar si el punto esta en el dominio. C es una variable temporal
04. 2 igual a la diferencia enrte la e%tension ma%ima del bra#o y el punto
deseado
0B. 9 > :!ag:rvector.a"is;FF2 J 6pper&)r!&*engthFF2 J
total&loCer&ar!&lengthFF2;/:2F6pper&)r!&*engthFtotal&loCer&ar!&length;
4#. if 9FF2 <> 1E 2Computar cinematica inversa
4$. 4 > sOrt:1J9FF2;
4". elboC&angle > atan2:4A9;
41. if rvector.a"is." > #E
45. shoulder&angle > J
:atan2:rvector.a"is."AsOrt:rvector.a"is.DFF2Irvector.a"is.FF2;;P
4,. Iatan2:total&loCer&ar!&lengthFsin:elboC&angle;A
:6pper&)r!&*engthItotal&loCer&ar!&lengthFcos:elboC&angle;;;;Ipi/2
4+. elseE
50 * +#
40. shoulder&angle > :atan2:J
rvector.a"is."AsOrt:rvector.a"is.DFF2Irvector.a"is.FF2;;P
44. Jatan2:total&loCer&ar!&lengthFsin:elboC&angle;A
4B. 2 @ se vuelve A y B se vuelve @ asi ?ue A se vuelve B
B#. shoulder&pivot&angle > Jatan2:rvector.a"is.Arvector.a"is.D;
B$.
B". shoulder&pivot&angle F> 18# / np.pi 2pasamos a grados
B1. shoulder&angle F> 18# / np.pi
B5. elboC&angle F> 18# / np.pi
B,.
B+. 24ormamos los mensajes para el bioloid> los <1;6 son debidos a la
posicion inicial de los servos
B0. 2para ?ue sean el / ?ue nosotros esperamos tenemos ?ue sumarle esa
cantidad
B4. spa&angle > int:3.5F: :shoulder&pivot&angle I 146; R 36#; ;
BB. co!ando&spa > "Rd Rd\n" R :2A int:spa&angle;; 2 sinta%is comando bioloid
'Cd Cd(n' numDmotor cantidad
$##. sa&angle > int:3.5F: :shoulder&angle I 146; R 36#;;
$#$. co!ando&sa > "Rd Rd\n" R :4A int:sa&angle;;
$#". e&angle > int:3.5F: :elboC&angle I 146; R 36# ; ;
$#1. co!ando&e > "Rd Rd\n" R :6A int:e&angle;;
$#5.
$#,. tryE
$#+. ser.Crite:co!ando&spa; 2 6andar comando por serie
$#0. ser.Crite:co!ando&sa;
$#4. ser.Crite:co!ando&e;
$#B. exceptE
$$#. print :" +erial %ort .ot 9onnected ";
$$$.
$$". 2 Actuali#ar animacion
$$1. $ra!e&1.0ight&)r!&%ivot.+et3alue:shoulder&pivot&angle;
$$5. $ra!e&1.0ight&+houlder.+et3alue:shoulder&angle;
$$,. $ra!e&1.0ight&1lboC.+et3alue:elboC&angle;
$$+.
update&-orso:shoulder&pivot&angleA$ra!e&1.*e$t&)r!&%ivot.'et3alue:;Ashoulder&angle
AP
$$0.
$ra!e&1.*e$t&+houlder.'et3alue:;AelboC&angleA$ra!e&1.*e$t&1lboC.'et3alue:;;
$$4. target.color > color.orange
$$B. elseE
$"#. print "4o!ain 1rror"
$"$. target.color > color.red
$"". 4 > #
$"1. 2,l codigo para el bra#o i#?uierdo es casi lo mismo> cambia algun signo y algun
pi
$"5. elif hand >> 1E
54 * +#
$",. rvector.a"is > target.pos J *&+houlder&Qoint.pos
$"+. rvector.pos > *&+houlder&Qoint.pos
$"0. total&loCer&ar!&length > *oCer&)r!&*ength
$"4.
$"B. 9 > :!ag:rvector.a"is;FF2 J 6pper&)r!&*engthFF2 J
total&loCer&ar!&lengthFF2;/:2F6pper&)r!&*engthFtotal&loCer&ar!&length;
$1#. if 9FF2 <> 1E
$1$. 4 > sOrt:1J9FF2;
$1". elboC&angle > atan2:4A9;
$11. if rvector.a"is." > #E
$15.
shoulder&angle > :atan2:rvector.a"is."AsOrt:rvector.a"is.DFF2Irvector.a"is.FF2;;P
$1,. Jatan2:total&loCer&ar!&lengthFsin:elboC&angle;A
$1+. elseE
$10. shoulder&angle > J:atan2:J
rvector.a"is."AsOrt:rvector.a"is.DFF2Irvector.a"is.FF2;;P
$14. Iatan2:total&loCer&ar!&lengthFsin:elboC&angle;A
$1B. shoulder&pivot&angle > Jatan2:rvector.a"is.Arvector.a"is.D;
$5#.
$5$. shoulder&pivot&angle F> 18# / np.pi 2pasamos a grados
$5". shoulder&angle F> 18# / np.pi
$51. elboC&angle F> 18# / np.pi
$55.
$5,.
$5+. spa&angle > int:3.5F::Jshoulder&pivot&angle I 146 ; R 36#; ;
$50. co!ando&spa > "Rd Rd\n" R :1A int:spa&angle;;
$54. sa&angle > int:3.5F: :Jshoulder&angle I 146; R 36#;;
$5B. co!ando&sa > "Rd Rd\n" R :3A int:sa&angle;;
$,#. e&angle > int:3.5F::JelboC&angle I 146 ; R 36#; ;
$,$. co!ando&e > "Rd Rd\n" R :5A int:e&angle;;
$,".
$,1. tryE
$,5. ser.Crite:co!ando&spa; 2 6andamos comando por serie
$,,. ser.Crite:co!ando&sa;
$,+. ser.Crite:co!ando&e;
$,0. exceptE
$,4. print :" +erial %ort .ot 9onnected ";
$,B.
$+#.
$+$. $ra!e&1.*e$t&)r!&%ivot.+et3alue:shoulder&pivot&angle;
$+". $ra!e&1.*e$t&+houlder.+et3alue:shoulder&angle;
$+1. $ra!e&1.*e$t&1lboC.+et3alue:elboC&angle;
$+5.
update&-orso:$ra!e&1.0ight&)r!&%ivot.'et3alue:;Ashoulder&pivot&angleA$ra!e&1.0ight
&+houlder.'et3alue:;AP
5B * +#
$+,. shoulder&angleA$ra!e&1.0ight&1lboC.'et3alue:;AelboC&angle;
$++. target.color > color.orange
$+0. elseE
$+4. print "4o!ain 1rror"
$+B. target.color > color.red
$0#. 4 > #
$0$. elseE
$0". print "4o!ain 1rror"
$01. target.color > color.red
$05. 4 > #
$0,.
176.def update&-orso:0&shoulder&pivotA*&shoulder&pivotA0&shoulderA*&shoulderA0&elboCA
*&elboC;E 2Actuali#ar animacion
$00. 2Conectamos bra#o a -ombro
$04. 0&6pper&)r!.pos > 0&+houlder&Qoint.pos
$0B. *&6pper&)r!.pos > *&+houlder&Qoint.pos
$4#.
$4$.
0&6pper&)r!.a"is >rotate:vector:#A#A6pper&)r!&*ength;Aangle>:$ra!e&1.0ight&)r!&%iv
ot.'et3alue:;/18#.;FpiAP
$4". a"is > vector:6pper&)r!&*engthA#A#;; 2 otamos bra#o
$41.
*&6pper&)r!.a"is >rotate:vector:#A#A6pper&)r!&*ength;Aangle>:$ra!e&1.*e$t&)r!&%ivo
t.'et3alue:;/18#.;FpiAP
$45. a"is > vector:6pper&)r!&*engthA#A#;; 2 otamos bra#o
$4,.
$4+. 0&6pper&)r!.a"is > rotate:0&6pper&)r!.a"isAangle>J
:$ra!e&1.0ight&+houlder.'et3alue:;/18#.;FpiIpi/2AP
$40. a"is > nor!:cross:0&6pper&)r!.a"isAvector:1A#A#;;;; 2otamos bra#o
$44.
*&6pper&)r!.a"is > rotate:*&6pper&)r!.a"isAangle>:$ra!e&1.*e$t&+houlder.'et3alue:;
/18#.;FpiJpi/2AP
$4B. a"is > nor!:cross:*&6pper&)r!.a"isAvector:1A#A#;;;; 2otatamos bra#o
$B#.
$B$. 0&1lboC.pos > 0&+houlder&Qoint.pos I 0&6pper&)r!.a"is 2Conectamos codo
$B". *&1lboC.pos > *&+houlder&Qoint.pos I *&6pper&)r!.a"is
$B1.
$B5. 0&*oCer&)r!.pos > 0&1lboC.pos 2Conectamos antebra#o
$B,. 0&*oCer&)r!.a"is > nor!:0&6pper&)r!.a"is;F*oCer&)r!&*ength
$B+.
$B0. if 0&shoulder <> # E 2+arc-e para ambiguedad de angulo
$B4. 0te!p > J1
$BB. elseE
"##. 0te!p > 1
"#$. 0&*oCer&)r!.a"is >rotate:0&*oCer&)r!.a"isAangle>I
:$ra!e&1.0ight&1lboC.'et3alue:;/18#.;FpiF0te!pAa"is > J
nor!:cross:0&*oCer&)r!.a"isAvector:1A#A#;;;;
,# * +#
"#". *&*oCer&)r!.pos > *&1lboC.pos
"#1. *&*oCer&)r!.a"is > nor!:*&6pper&)r!.a"is;F*oCer&)r!&*ength
"#5. if *&shoulder <> # E 2+arc-e para ambiguedad de angulo
"#,. *te!p > J1
"#+. elseE
"#0. *te!p > 1
"#4. *&*oCer&)r!.a"is > rotate:*&*oCer&)r!.a"isAangle>I
:$ra!e&1.*e$t&1lboC.'et3alue:;/18#.;FpiF*te!pAa"is> nor!:cross:*&*oCer&)r!.a"isAve
ctor:1A#A#;;;;
"#B. 0&/and.pos > 0&*oCer&)r!.pos I
nor!:0&*oCer&)r!.a"is;F:/and&0adiusI*oCer&)r!&*ength; 2Conectar mano
"$#. *&/and.pos > *&*oCer&)r!.pos I
nor!:*&*oCer&)r!.a"is;F:/and&0adiusI*oCer&)r!&*ength;
"$$.
010.2 Asociamos eventos a funciones
213.def bind9ontrols:;E
"$5. $ra!e&1.9o!!&9onnect.7ind:C".13-&76--(.A serial9onnect;
"$,. $ra!e&1.*e$t&)r!&%ivot.7ind:C".13-&+90(**Aon+croll;
"$+. $ra!e&1.0ight&)r!&%ivot.7ind:C".13-&+90(**Aon+croll;
"$0. $ra!e&1.*e$t&+houlder.7ind:C".13-&+90(**Aon+croll;
"$4. $ra!e&1.0ight&+houlder.7ind:C".13-&+90(**Aon+croll;
"$B. $ra!e&1.*e$t&1lboC.7ind:C".13-&+90(**Aon+croll;
""#. $ra!e&1.0ight&1lboC.7ind:C".13-&+90(**Aon+croll;
""$.
""".
008.2 4uncion para atender eventos
224.def on+croll:event;E
"",. Soint&angle > event.'et%osition:;
""+. Soint > event.'et,d:;
""0.
update&-orso:$ra!e&1.0ight&)r!&%ivot.'et3alue:;A$ra!e&1.*e$t&)r!&%ivot.'et3alue:;A
$ra!e&1.0ight&+houlder.'et3alue:;AP
""4.
$ra!e&1.*e$t&+houlder.'et3alue:;A$ra!e&1.0ight&1lboC.'et3alue:;A$ra!e&1.*e$t&1lboC
.'et3alue:;;
""B.
08/.2 4uncion para conectar puerto serie
231.def serial9onnect:event;E
"1". global ser
"11. tryE
"15. ser > serial.+erial::int:$ra!e&1.9o!!&%ort.3alue;J1;A576##;
"1,. print :" 9onnected to "Istr:ser.portstr;;
"1+. exceptE
"10. print :" ,nvalid 9o!! %ort ";
"14.
"1B.
"5#.
,$ * +#
"5$.
0;0.2Address pattern5 '/joint'
0;8.2!ype tag5 'sifff'
0;;.2s5 Eoint name> c-ec1 out t-e full list of joints beloF.
0;G.2i5 !-e "D of t-e user.
0;6.2f15 B coordinate of joint in interval H/./> 1./I
0;..2f05 A coordinate of joint in interval H/./> 1./I
0;7.2f85 @ coordinate of joint in interval H/./> ../I
"5B.
",#.
0G1.2 define a messageJ-andler function for t-e server to call.
0G0.2 'manejador' de los mensajes KSC ?ue lleguen
253.def data&handler:addrA tagsA dataA source;E
",5. 2 Lariables para las coordenadas de las diferentes articulaciones
",,. global rs 2 ig-t s-oulder> -ombro derec-o
",+. global ls 2 Meft s-oulder> -ombro i#?uierdo
",0. global rh 2 ig-t -and> mano derec-a
",4. global lh 2 Meft -and> mano i#?uierda
",B. global rhi 2 ig-t -ip> cadera derec-a
"+#. global lhi 2 Meft -ip> cadera i#?uierda
"+$. global re 2 ig-t elboF> codo derec-o
"+". global le 2 Meft elboF> codo i#?uierdo
"+1. global rh&pG 2 cuenta de actuali#aciones de coordenadas de r- 'p1 9 pac1ets'
"+5. global lh&pG
"+,. global rs&pG
"++. global ls&pG
"+0. global rhi&pG
"+4. global lhi&pG
"+B. global re&pG
"0#. global le&pG
"0$. 2 3estion de mano derec-a...
"0". if data?#@ >> "r&hand"E
"01. rh&pGI>1
"05. rh > np.arra::data?2@A data?3@A data?4@;;
"0,. 2print 'JJJ'
"0+. 2print 'received neF osc msg from Cs' C getUrlStr&source)
"00. 2print 'Fit- addr 5 Cs' C addr
"04. 2print 'typetags 5 Cs' C tags
"0B. 2print 't-e actual data is 5Cs' C data
"4#. 2print 'JJJ'
"4$. if data?#@ >> "l&hand"E
"4". lh&pGI>1
"41. lh > np.arra::data?2@A data?3@A data?4@;;
"45. elif data?#@ >> "r&shoulder"E
"4,. rs&pGI>1
"4+. rs > np.arra::data?2@A data?3@A data?4@;;
"40. elif data?#@ >> "l&shoulder"E
," * +#
"44. ls&pGI>1
"4B. ls > np.arra::data?2@A data?3@A data?4@;;
"B#. elif data?#@ >> "r&hip"E
"B$. rhi&pGI>1
"B". rhi > np.arra::data?2@A data?3@A data?4@;;
"B1. elif data?#@ >> "l&hip"E
"B5. lhi&pGI>1
"B,. lhi > np.arra::data?2@A data?3@A data?4@;;
"B+. elif data?#@ >> "r&elboC"E
"B0. re&pGI>1
"B4. re > np.arra::data?2@A data?3@A data?4@;;
"BB. elif data?#@ >> "l&elboC"E
1##. le&pGI>1
1#$. le > np.arra::data?2@A data?3@A data?4@;;
1#".
1#1.
8/;.2 !ransformacion de coordenadas de un sistema a otro
3#5.def coords-rans$or!:no!brethreadA du!!var;E
1#+. global rs
1#0. global rh
1#4. global lh
1#B. global ls
1$#. global rhi
1$$. global lhi
1$". global re
1$1. global le
1$5. global rh&pG
1$,. global lh&pG
1$+. global rs&pG
1$0. global ls&pG
1$4. global rhi&pG
1$B. global lhi&pG
1"#. global re&pG
1"$. global le&pG
1"".
1"1. starttracGing > 5alse
1"5. 2 bucle de actuali#acion de coordenadas tratadas
1",. while 1E
1"+.
1"0.
if starttracGing >> 5alse and rh&pG > # and lh&pG > # and rs&pG > # and ls&pG > #
and rhi&pG > #and lhi&pG > # and re&pG > # and le&pG > #E
1"4. starttracGing > -rue
1"B. elif starttracGing >> -rueE
11#. 2 nos copiamos las coordenadas
11$. hi > ls
11". hd > rs
,1 * +#
111. ci > lhi
115. cd > rhi
11,. !i > lh
11+. !d > rh
110.
114. 2 creamos los vectores iniciales del eje de coordenadas
11B. e1 > np.arra::1A#A#;;
15#. e2 > np.arra::#A1A#;;
15$. e3 > np.arra::#A#A1;;
15".
151. 2 sacamos los 8 vectores del nuevo eje de coordenadas
155. 2 el primer vector es directo> eje B
15,. v1 > hiJhd
15+. vte!p > hi J ci
150. v1nor!al > v1 / !odulus&vector:v1;
154. vte!pnor! > vte!p / !odulus&vector:vte!p;
15B. c& > ci I :$loat:np.dot:vte!pAv1;; / $loat:np.dot:v1Av1;;; F v1
1,#. 2 cD es el punto del codo en proyeccion a la paralela del eje B a la misma
distancia
1,$. 2 de el eje B a la ?ue estaba el punto del codo original &para formar el
eje A)
1,". v2 > hi J c&
1,1. 2 el eje @ se saca con la normal
1,5. v3 > np.cross:v1Av2;
1,,.
1,+.
1,0. 2 normali#o los vectores
1,4. v1 > v1 / !odulus&vector:v1;
1,B. v2 > v2 / !odulus&vector:v2;
1+#. v3 > v3 / !odulus&vector:v3;
1+$.
1+". 2 saco los cosenos para la matri# a resolver
1+1. a > cosangle:e1Av1;
1+5. b > cosangle:e2Av1;
1+,. c > cosangle:e3Av1;
1++. d > cosangle:e1Av2;
1+0. e > cosangle:e2Av2;
1+4. $ > cosangle:e3Av2;
1+B. g > cosangle:e1Av3;
10#. h > cosangle:e2Av3;
10$. i > cosangle:e3Av3;
10".
101. 2 creo la matri#
105. " > np.arra::aAdAg;;
10,. > np.arra::bAeAh;;
10+. D > np.arra::cA$Ai;;
100. ) > np.arra::"AAD;;
,5 * +#
104.
10B. 2esuelvo el sistema para los diferentes puntos ?ue pasar al nuevo eje de
coordenadas
14#. tryE
14$. sol!i > np.linalg.solve:)A!iJhi; 2 solucion mano i#?uierda
14". sol!d > np.linalg.solve:)A!dJhi; 2 solucion mano derec-a
141. solcodoiDO > np.linalg.solve:)AleJhi; 2 solucion codo i#?uierdo
145. solcododer > np.linalg.solve:)AreJhi; 2 solucion codo derec-o
14,. solhd > np.linalg.solve:)AhdJhi; 2 solucion -ombro derec-o
14+. except np.linalg.*in)lg1rrorE
140. print "*in)lg1rrorH +ingular !atri"A cant co!pute"
144.
14B.
1B#. 2 !oca escalar los puntos a las coordenadas del sistema de la simulacion
1B$.
1B". 2 Sacamos las distancias del bra#o y antebra#o para ambos sistemas> 1inect
y bioloid
1B1. d&braDo > dist:np.arra::#A#A#;;AsolcodoiDO;
1B5. d&antebraDo > dist:solcodoiDOAsol!i;
1B,. d&bioloid&braDo > #.#K
1B+. d&bioloid&antebraDo > #.#43
1B0. d&todo&Ginect > d&braDo I d&antebraDo
1B4. d&todo&bioloid > d&bioloid&braDo I d&bioloid&antebraDo
1BB.
5##.
5#$. 2 ,scalamos el punto
5#". 2 +artiendo de codo i#?uierdo> con lo cual / / /> faltaria restar para el
otro codo el -ombro
5#1. v&hi&a&ci > :solcodoiDO / !odulus&vector:solcodoiDO; ; F d&bioloid&braDo
5#5. v&ci&a&!i > :sol!i J solcodoiDO;
5#,. v&ci&a&!i > :v&ci&a&!i /!odulus&vector:v&ci&a&!i; ; F d&bioloid&antebraDo
5#+. 2 !rasladamos teniendo en cuenta el centro del sistema de coordenadas &lo
del < &/./...) )
5#0. punto!anoiDO.pos > v&hi&a&ci I v&ci&a&!i I :#.#5A #.#5A #.#25;
5#4.
5#B. 2 Derec-a
5$#. v&hd&a&cd > :solcododer J solhd;
5$$. v&hd&a&cd > :v&hd&a&cd / !odulus&vector:v&hd&a&cd; ; F d&bioloid&braDo
5$". v&cd&a&!d > :sol!d J solcododer;
5$1. v&cd&a&!d > :v&cd&a&!d /!odulus&vector:v&cd&a&!d; ; F d&bioloid&antebraDo
5$5. punto!anoder.pos > v&hd&a&cd I v&cd&a&!d I :J#.#5A #.#5A #.#25;
5$,.
5$+. 2 Calcular cinematicas inversas para cada mano
5$0. do&,N:#A punto!anoiDO.pos;
5$4. do&,N:1A punto!anoder.pos;
5$B.
5"#.
,, * +#
;01.2 6ain de la aplicacion> parcialmente generado por F%+yt-on
;00.222222222222222222222222
;08.2 Start t-e F% application -andler
;0;.222222222222222222222222
425.if &&na!e&& >> "&&!ain&&"E
5"+. 2 "niciar 3U"
5"0. app > C".%+i!ple)pp:#;
5"4. C".,nit)ll,!age/andlers:;
5"B. $ra!e&1 > 25ra!e:.oneA J1A "";
51#. bind9ontrols:;
51$.
51". 2 Creamos variables globales &notadas en dataD-andler)
511. rh&pG > #
515. lh&pG > #
51,. rs&pG > #
51+. ls&pG > #
510. rhi&pG > #
514. lhi&pG > #
51B. re&pG > #
55#. le&pG > #
55$.
55". rs > np.arra::#.#A #.#A #.#;;
551. rh > np.arra::#.#A #.#A #.#;;
555. lh > np.arra::#.#A #.#A #.#;;
55,. ls > np.arra::#.#A #.#A #.#;;
55+. rhi > np.arra::#.#A #.#A #.#;;
550. lhi > np.arra::#.#A #.#A #.#;;
554. re > np.arra::#.#A #.#A #.#;;
55B. le > np.arra::#.#A #.#A #.#;;
5,#.
5,$. 2 %y# vectores
5,". v" > np.arra::#.#A #.#A #.#;;
5,1. v > np.arra::#.#A #.#A #.#;;
5,5. vD > np.arra::#.#A #.#A #.#;;
5,,.
5,+.
5,0. 2 "niciamos el recolector de los mensajes KSC
5,4. init(+9+erver:T127.#.#.1TA 711#;
5,B. set(+9/andler:"/Soint"A data&handler; 2 anyadimos nuestra funcion como data
-andler
5+#. print "(+9 recolector inicialiDado"
5+$. du!!var > #
5+". tryE
5+1. thread.start&neC&thread:coords-rans$or!A :"coords-rans$or!
-hread"A du!!var;;
5+5. exceptE
5+,. print ss.e"c&in$o:;
,+ * +#
5++.
5+0. print "9oords-rans$or! thread iniciado"
5+4. 2 +untos reales pintados en blanco
5+B. punto!anoiDO > points:pos>:#.1A #.1A #.1;A siDe > 4#A color>color.Chite;
50#. punto!anoder > points:pos>:J#.1A #.1A #.1;A siDe > 4#A color>color.Chite;
50$.
ball > sphere:pos>*&+houlder&Qoint.posA radius>6pper&)r!&*engthI*oCer&)r!&*engthA
opacit>#.1;
50".
ball > sphere:pos>0&+houlder&Qoint.posA radius>6pper&)r!&*engthI*oCer&)r!&*engthA
opacit>#.1;
501.
505. 2 Acabar de iniciar 3U"
50,. app.+et-opMindoC:$ra!e&1;
50+. $ra!e&1.+hoC:;
500. app.2ain*oop:;
504.
50B. 2 !erminar bien t-read
54#. tryE
54$. thread.Soin:;
54". exceptE
541. print ss.e"c&in$o:;
12.4 *ne1o cdigo declaracin estructura simulacin "ioloid
from visual import F
$.
0. 2 +arametros !orso
1.
4. 9 > #
5. 4 > #
6. drag > 5alse
7. 6pper&)r!&*ength > .#K
8. *oCer&)r!&*ength > .#43
K. /and&0adius > #.#275
1#.shoulder&pivot&angle > #
11.shoulder&angle > #
12.elboC&angle > #
13.ar!&radius > #.#2
$5.
15.torso&" > .1
16.torso& > .2
17.torso&D > .1
$4.
,0 * +#
19.2 Crear graficos del torso
2#.-orso > bo":siDe > :torso&"Atorso&Atorso&D;Acolor > color.green;
21.0&+houlder&Qoint > sphere:pos > :-orso.siDe."/2A-orso.siDe./4A-orso.siDe.D/4;Arad
ius > #.#3Acolor >color.red;
22.*&+houlder&Qoint > sphere:pos > :J
-orso.siDe."/2A-orso.siDe./4A-orso.siDe.D/4;Aradius > #.#3Acolor >color.red;
23.0&6pper&)r! > clinder:pos > 0&+houlder&Qoint.posAa"is > :6pper&)r!&*engthA#A#;Ara
dius >ar!&radiusAcolor > color.blue;
24.*&6pper&)r! > clinder:pos > *&+houlder&Qoint.posAa"is > :J
6pper&)r!&*engthA#A#;Aradius >ar!&radiusAcolor > color.blue;
25.0&1lboC > sphere:pos > 0&6pper&)r!.pos I
0&6pper&)r!.a"isAradius > ar!&radiusAcolor > color.red;
26.*&1lboC > sphere:pos > *&6pper&)r!.pos I
*&6pper&)r!.a"isAradius > ar!&radiusAcolor > color.red;
27.0&*oCer&)r! > clinder:pos > 0&1lboC.posAa"is > :*oCer&)r!&*engthA#A#;Aradius > ar
!&radiusAcolor >color.blue;
28.*&*oCer&)r! > clinder:pos > *&1lboC.posAa"is > :J*oCer&)r!&*engthA#A#;Aradius > a
r!&radiusAcolor >color.blue;
2K.0&/and > sphere:pos > 0&1lboC.posI0&*oCer&)r!.a"isAradius > /and&0adiusAcolor > co
lor.redA!aGe&trail>-rueA interval>1#A retain>1#;
3#.0&/and.trail&obSect.color > color.orange
31.*&/and > sphere:pos > *&1lboC.posI*&*oCer&)r!.a"isAradius > /and&0adiusAcolor > co
lor.redA!aGe&trail>-rueA interval>1#A retain>1#;
32.*&/and.trail&obSect.color > color.orange
11.
8;.2 Crear esfera y vector para el error
35.rvector > arroC:pos > :#A#A#;Acolor>color.orangeA a"is>:#A#A#.2;;
36.target > sphere:pos > rvector.posAcolor > color.orangeAradius > #.#2;
12./ *ne1o cdigo declaracin del 8U5
2N/usr/bin/env pyt-on
1. 2 JOJ coding5 isoJ77G9J1G JOJ
0. 2 generated by F%3lade /.6.8
1.
4. import C"
G. 2 begin F%3lade5 e%tracode
6. 2 end F%3lade
0.
4.
B.
1#.class 25ra!e:C".5ra!e;E
$$. def &&init&&:sel$A FargsA FFGCds;E
,4 * +#
$". 2 begin F%3lade5 6y4rame.DDinitDD
$1.
$5. GCds?"stle"@ > C".415)6*-&50)21&+-U*1
$,. C".5ra!e.&&init&&:sel$A FargsA FFGCds;
$+. sel$.9o!!&%ort > C".+pin9trl:sel$A J1A "4"A !in>#A !a">1##;
$0. sel$.9o!!&9onnect > C".7utton:sel$A J1A "9onnect";
$4. sel$.*e$t&)r!&%ivot > C".+lider:sel$A 13A #A J
14#A 14#A stle>C".+*&/(0,V(.-)*WC".+*&*)71*+;
$B. sel$.0ight&)r!&%ivot > C".+lider:sel$A 1#A #A J
14#A 14#A stle>C".+*&/(0,V(.-)*WC".+*&*)71*+;
"#. sel$.label&1 > C".+tatic-e"t:sel$A J1A "+liders";
"$. sel$.*e$t&+houlder > C".+lider:sel$A 14A #A J2#A 12#A stle>C".+*&/(0,V(.-)*W
C".+*&*)71*+;
"". sel$.0ight&+houlder > C".+lider:sel$A 11A #A J2#A 12#A stle>C".+*&/(0,V(.-)*W
C".+*&*)71*+;
"1. sel$.label&2 > C".+tatic-e"t:sel$A J1A "+liders";
"5. sel$.*e$t&1lboC > C".+lider:sel$A 15A #A J2#A 1##A stle>C".+*&/(0,V(.-)*W
C".+*&*)71*+;
",. sel$.0ight&1lboC > C".+lider:sel$A 12A#A J2#A 1##A stle>C".+*&/(0,V(.-)*W
C".+*&*)71*+;
"+.
"0. sel$.&&set&properties:;
"4. sel$.&&do&laout:;
"B. 2 end F%3lade
1#.
1$. def &&set&properties:sel$;E
1". 2 begin F%3lade5 6y4rame.DDsetDproperties
11. sel$.+et-itle:"7ioloid )r!s";
15. 2 end F%3lade
1,.
1+. def &&do&laout:sel$;E
10. 2 begin F%3lade5 6y4rame.DDdoDlayout
14. grid&siDer&1 > C".'rid+iDer:3A 3A 1A 3;
1B. siDer&2 > C".7o"+iDer:C"./(0,V(.-)*;
5#. siDer&2.)dd:sel$.9o!!&%ortA #A #A #;
5$. siDer&2.)dd:sel$.9o!!&9onnectA #A #A #;
5". grid&siDer&1.)dd:siDer&2A 1A C".1L%).4A #;
51. grid&siDer&1.)dd:sel$.*e$t&)r!&%ivotA #A C".1L%).4A #;
55. grid&siDer&1.)dd:sel$.0ight&)r!&%ivotA #A C".1L%).4A #;
5,. grid&siDer&1.)dd:sel$.label&1A #A C".)*,'.&91.-10&/(0,V(.-)*A #;
5+. grid&siDer&1.)dd:sel$.*e$t&+houlderA #A C".1L%).4A #;
50. grid&siDer&1.)dd:sel$.0ight&+houlderA #A C".1L%).4A #;
54. grid&siDer&1.)dd:sel$.label&2A #A C".)*,'.&91.-10&/(0,V(.-)*A #;
5B. grid&siDer&1.)dd:sel$.*e$t&1lboCA #A C".1L%).4A #;
,#. grid&siDer&1.)dd:sel$.0ight&1lboCA #A C".1L%).4A #;
,$. sel$.+et+iDer:grid&siDer&1;
,". grid&siDer&1.5it:sel$;
,B * +#
,1. sel$.*aout:;
,5. 2 end F%3lade
,,.
,+.
,0.
G7.2 end of class 6y4rame
+# * +#

Proyecto Raspberry

Cargado por

Información del documento

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Proyecto Raspberry

Cargado por

Copyright:

Formatos disponibles

PROYECTO FINAL DE CARRERA

Guiado gestual de un robot humanoide

También podría gustarte