Object Manipulation Robot Simulator

Sebastián Bolaños (B21031).

Juan Sánchez Brenes (B16068).

Antes de comenzar es necesario que realizen este proyecto para continuar con esta página: Object Manipulation Robot Simulator

Una vez ya instalado la primera parte de este proyecto podemos proceder a realizar otras simulaciones muy utiles y dinámicas. En esta segunda parte del proyecto se simula el robot entero realizando movimientos para mover objetos comunes de concina hacia un eje de coordenadas justo al frente del robot. Además, vamos a implementar un algoritmo que determina si el al aplicar una fuerza sobre el objeto este va a deslizarse.

• Completar la simulación del deslice con el robot completo en que se muestra como un objeto se coloca en una posición determinada al frente del robot.
• Comprobar que el algoritmo de fuerzas y torque funcione adecuadamente.
• Incorporar el algoritmo a los programas existentes del simulador para realizar un simulación que tome en cuenta la posición en donde se aplica la fuerza del robot sobre el objeto.
• Crear una simulación simple para que involucre el nuevo algoritmo en su ejecución.
• Publicar los resultados en la página del wiki para facilitar su acceso a otras personas interesadas en realizarla.

Primeramente va ser necesario ingresar al usuario del chroot el cual fue instalado en la primera parte.

Antes de ejecutar cualquiera de estos programas es necesario tener yarpserver3 abierto.

• En una consola ejecute:

cd ~/local/src
pyrovito -r lwr -a robot_descriptions/tum-rosie/kinematics/lwr/ -d robot_descriptions/tum-rosie/kinematics/sahand/

• En otra consola ejecute:

planar_sliding_simple

• En otra consola ejecute:

xfinger_feeder

Ahora tenes a disposicón un dedo que se puede utilizar para mover la caja. SI intentas controlar el dedo con las flechas del teclado vas a ver que es muy difícil, por eso es más sencillo dejar que el robot se encargue de eso. Cancele planar_slider_simple y xfinger_feeder. Ahora ejecute en dos consolas diferentes:

planar_sliding_simple

slider_control_simple

Cancele todos los programas ejecutados. Ahora ejecute los siguientes:

vfclik -i lwr -i right -d robot_descriptions/tum-rosie/kinematics/lwr/ -s
vfclik -i lwr -i left -d robot_descriptions/tum-rosie/kinematics/lwr/ -s
sahand_yarp_sim -s -d -n -f robot_descriptions/tum-rosie/kinematics/sahand/hands_kin.py

Espere a que se completen los anteriores y ejecute:

pyrovito -r lwr --arm_right --arm_left --hand_right --hand_left -a robot_descriptions/tum-rosie/kinematics/lwr/ -d robot_descriptions/tum-rosie/kinematics/sahand/

Espere a que se completen los anteriores y ejecute:

torque_sim -s -a robot_descriptions/tum-rosie/kinematics/lwr/ -c robot_descriptions/tum-rosie/kinematics/sahand/calibration_data/finger_calibration_data.py -f robot_descriptions/tum-rosie/kinematics/sahand/hands_kin.py

Espere a que se completen los anteriores y ejecute:

planar_sliding -o cmoc/objects/sliding/objects/ice_tea_params.py

Espere a que se completen los anteriores y ejecute:

slider_control  -s -f robot_descriptions/tum-rosie/kinematics/sahand/hands_kin.py -o cmoc/objects/sliding/objects/ice_tea_params.py

Contiene la clase control loop que se encarga de verificar las posiciones de los objetos (dedo, caja y mesa) respecto al punto de referencia (base del robot) en cada ciclo de control . El ciclo de control posee una frecuancia predeterminada de 15 Hz.

La función set_params inicializa los puertos de Yarp para torque_sim, planar_control, ffinger, además de recibir y definir parámetros de peso, posición y dimensiones de la caja. También contiene la función process ejecuta la simulación.

En la siguiente parte, s muy colocar el codigo que llama la función box_forward_3d del archivo box3dmodel.py adentro del if touch donde touch es un booleano que determina si el dedo esta tocando la caja.

Este programa, una vez que se haya inicializado el planar sliding, se encarga de mover el dedo junto con la caja hasta las coordenadas objetivo. Realiza movimientos y cambios costantes en la apliación de la fuerza sobre el objeto.

Sin incluír el box3dmodel en planar_sliding, este programa lleva la caja hasta las coordenadas de origen.

Este algoritmo esta encargado de recibir parametros desde el planar_sliding y los va utilizar para determinar si el objeto se va caer o deslizar en el momento que el robot comienza a empujar dicho objeto. Los parametros necesarios y sus tipos son los siguientes:

• Magnitud de fuerza: array
• Posición de la fuerza: array
• Coeficiente de fricción estático: float
• Coeficiente de fricción cinética: float
• Peso: float
• Vertices de la caja: dict con seis arrays adentro
• Normal de la mesa: array
• Posición de la normal: array

No fue posible integrar completamente el codigo de box3Dmodel pero si se pudo lograr un pequeño avance en su funcionamiento. Primero se tiene que colocar el archivo que contiene el codigo del box3Dmodel en la carpeta src y además al inicio del archivo de planar sliding colocar lo siguiente:

from box3dmodel import box_forward_3d

Seguidamente, colocamos en la línea 271 de planar_sliding las siguientes variables que se encuentran presentes en planar_sliding:

box_forward_3d(fc_local ,self.finger_pos_local, self.obj_par.friction_coef_object_table_static, self.obj_par.friction_coef_object_table, self.obj_par.weight_force, self.obj_par.box_vertices, table_object_normal,self.obj_par.box_planes[self.table_object_face][1])

Donde podemos ver que cada variable fc_local, self.finger_pos_local, self.obj_par.friction_coef_object_table_static y los demas, son variables que son propias del objeto que vamos a usar que se importa escogiendo el archivo que se quiere al ejecutar los comandos de planar sliding y slider control.

Aunque no fue posible hacer un buen funcionamiento del modelo 3D, se implemetaron algunas modificaciones al codigo del modelo para que se pudiera avanzar más en su ejecución.

En la función vertices2plane_distances esta el siguiente codigo:

return([vertex2plane_distance(vertex,plane_normal,plane_point) for vertex in vertices])

Donde al realizar el for vertex in vertices vertixes es un entero que se le va a mandar a la función vertex2plane_distance. Vertex2plane_distance tiene una parte en donde realiza una resta entre vertex y plane_point, el cual es el punto que represanta el plano. Python da un error al realizar una operación de resta entre un entero y un arreglo. Se puede ver que las dos variables en su nombre probablemente sean arreglos entonces se intento de proponer una solución que entregue a vertex2plane_distance una variable vertex tipo arreglo. Se implemento el siguiente codigo en la función vertices2plane_distances:

for x in xrange(6):
      for y in xrange(3):
               return([vertex2plane_distance(vertices[x][y],plane_normal,plane_point)]) 

Este codigo entrega a vertex2plane_distance en su primer parametro un arreglo que se supone es la intención de vertexes. Es importante notar que vertices son los vertices de la caja que se le entrega en parametro a las funciones mencionadas, pero por alguna razón el for en el vertices2plane_distances hace que vertex sea un enterro, que por su nombre se puede suponer no es así.

El segundo error despues de arreglar el código de arriba es el siguiente:

if ((vertex!=pivot).any()):
AttributeError: 'bool' object has no attribute 'any'

Que ocurre en la línea 204. Se puede ver en el codigo fuente de box3Dmodel que en líneas superiores a la 204 a esta se aplica un for a in b que por alguna razón hace que el valor a sea un entero, en este error las variables vertex y pivot son el a. Este error ocurre por que los tipos de variable cuando se evalua el if de vertex y pivot son enteros, pero se sabe por su nombre que son arreglos.

se cambio el codigo:

pivots=[]
  for pivot in vertices:
      print "Pivot:", pivot
      pivot_torques=[]
      for vertex in vertices:
          if ((vertex!=pivot).any()):
              pivot_torques.append(cross((vertex-pivot),table_normal))
      print "Table torques:", pivot_torques
      

Por:

  pivots=[]
  for x in xrange(6):
      for y in xrange(3):               
              pivot_torques=[]
              for h in xrange(6):
                      for j in xrange(3):
                                 if ((vertices[h][j]!=vertices[x][y])).any():
                                      pivot_torques.append(cross((vertex-pivot),table_normal))
              print "Table torques:", pivot_torques
              

Hay que asegurar que las indentaciones de las lineas por debajo a este codigo esten al mismo nivel que el for h.

Por último, al realizar el cambio de arriba nos sale el error en donde no pudimos avanzar más:

if ((vertices[h][j]!=vertices[x][y])).any():
TypeError: 'int' object has no attribute '__getitem__'

Logramos el objetivo de poner a correr la simulación de slider_control que simula el movimiento de empuje del dedo, logramos cambiar la altura de la mesa y además logramos incorporar el box3dmodel.py de manera que planar_sliding corra sin problemas hasta que se hace el primer contacto con el objeto.