bio_ik
1.0.0
Este repositorio proporciona una implementación independiente con licencia BSD de una variedad de métodos de optimización para resolver eficientemente problemas de cinemática inversa generalizada.
Todo el módulo fue implementado por Philipp Ruppel como parte de su tesis de maestría.
Para una reimplementación basada en C++ del algoritmo "BioIK" original, tal como se vendió originalmente en la tienda Unity, puede usar el modo no predeterminado bio1 . El modo predeterminado bio2_memetic no comparte código con esta implementación, se demostró que lo supera en términos de tasa de éxito, precisión y eficiencia, y en realidad se puede utilizar para aplicaciones robóticas precisas [4].
Necesitará la versión ROS Indigo o posterior (wiki.ros.org). El software se desarrolló en Ubuntu Linux 16.04 LTS con ROS Kinetic, pero también se probó en Ubuntu Linux 14.04 LTS con ROS Indigo. Las versiones más nuevas de ROS deberían funcionar, pero es posible que necesiten alguna adaptación. Consulte a continuación las instrucciones específicas de la versión.
Descargue el paquete bio_ik y descomprímalo en su espacio de trabajo catkin.
Ejecute catkin_make para compilar su espacio de trabajo:
roscd
cd src
git clone https://github.com/TAMS-Group/bio_ik.git
roscd
catkin_make
Configure Moveit para usar bio_ik como solucionador de cinemática (consulte la siguiente sección).
Utilice Moveit para planificar y ejecutar movimientos o utilice su propio código junto con el nodo move_group para mover su robot.
Como es habitual, la API pública se especifica en los archivos de encabezado públicos del paquete bio_ik , ubicados en el subdirectorio include/bio_ik ; las fuentes, incluidos algunos archivos de encabezado privados, se encuentran en el subdirectorio src .
Para facilitar su uso y compatibilidad con el código existente, el algoritmo bio_ik está encapsulado como un complemento cinemático de Moveit. Por lo tanto, bio_ik se puede utilizar como reemplazo directo del solucionador IK predeterminado basado en Orocos/KDL. Dado el nombre de un efector final y una pose objetivo de 6 grados de libertad, bio_ik buscará una configuración de articulación de robot válida que alcance el objetivo determinado.
En nuestras pruebas (ver más abajo), tanto en términos de tasa de éxito como de tiempo de solución, bio_ik superó regularmente al solucionador Orocos [1] y es competitivo con trac-ik [2]. El algoritmo bio_ik también se puede utilizar para sistemas de alta DOF, como serpientes robot, y convergerá automáticamente a las mejores soluciones aproximadas para brazos de baja DOF donde algunas posturas objetivo no se pueden alcanzar exactamente.
Si bien puede escribir los archivos de configuración de Moveit a mano, la forma más sencilla es ejecutar el asistente de configuración de Moveit para su robot y luego seleccionar bio_ik como solucionador de IK al configurar los efectores finales. Una vez configurado, se puede llamar al solucionador mediante la API Moveit estándar o utilizarlo de forma interactiva desde rviz mediante el complemento GUI de MotionPlanning.
Asegúrese de tener un modelo URDF (o xacro) para su robot.
Ejecute el asistente de configuración de Moveit para crear los archivos de configuración de Moveit:
rosrun moveit_setup_assistant moveit_setup_assistant
El asistente de configuración busca automáticamente todos los complementos de resolución de IK disponibles en su espacio de trabajo. Por lo tanto, puede seleccionar bio_ik como solucionador de IK de la lista desplegable para cada efector final y luego configurar los parámetros cinemáticos, es decir, la precisión de posición predeterminada (metros) y el tiempo de espera (en segundos). Para brazos típicos de 6 o 7 DOF, una precisión de 0,001 m (o menor) y un tiempo de espera de 1 ms deberían estar bien. Es posible que los robots más complejos necesiten un tiempo de espera más prolongado.
Genere los archivos de configuración de moveit desde el asistente de configuración. Por supuesto, también puedes editar el archivo de configuración config/kinematics.yaml con tu editor de texto favorito. Por ejemplo, una configuración para el robot PR2 podría verse así:
# example kinematics.yaml for the PR2 robot
right_arm:
# kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
# kinematics_solver_attempts: 1
kinematics_solver: bio_ik/BioIKKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 1
left_arm:
kinematics_solver: bio_ik/BioIKKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 1
all:
kinematics_solver: bio_ik/BioIKKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.02
kinematics_solver_attempts: 1
# optional bio_ik configuration parameters
# center_joints_weight: 1
# minimal_displacement_weight: 1
# avoid_joint_limits_weight: 1
Para una primera prueba, ejecute la demostración creada por Moveit. Una vez que rviz se esté ejecutando, habilite el complemento de planificación de movimiento, luego seleccione uno de los efectores finales de su robot. Rviz debería mostrar un marcador interactivo 6-D (posición y orientación) para los efectores finales seleccionados. Mueve el marcador interactivo y observa cómo bio_ik calcula las posturas de tu robot.
Si también instaló la demostración de bio_ik (ver más abajo), debería poder ejecutar una de las demostraciones predefinidas:
roslaunch pr2_bioik_moveit demo.launch
roslaunch pr2_bioik_moveit valve.launch
roslaunch pr2_bioik_moveit dance.launch
Ahora está listo para usar bio_ik desde sus programas C/C++ y Python, usando la API Moveit estándar. Para solicitar explícitamente una solución IK en C++:
robot_model_loader::RobotModelLoader robot_model_loader(robot);
auto robot_model = robot_model_loader.getModel();
auto joint_model_group = robot_model->getJointModelGroup(group);
auto tip_names = joint_model_group->getSolverInstance()->getTipFrames();
kinematics::KinematicsQueryOptions opts;
opts.return_approximate_solution = true; // optional
robot_state::RobotState robot_state_ik(robot_model);
// traditional "basic" bio_ik usage. The end-effector goal poses
// and end-effector link names are passed into the setFromIK()
// call. The KinematicsQueryOptions are empty.
//
bool ok = robot_state_ik.setFromIK(
joint_model_group, // joints to be used for IK
tip_transforms, // multiple end-effector goal poses
tip_names, // names of the end-effector links
attempts, timeout, // solver attempts and timeout
moveit::core::GroupStateValidityCallbackFn(),
opts // mostly empty
);
Para muchas aplicaciones de robots, es esencial especificar más que una simple pose de efector final. Los ejemplos típicos incluyen
En bio_ik, dichas tareas se especifican como una combinación de múltiples objetivos individuales.
Luego, el algoritmo intenta encontrar una configuración de robot que cumpla todos los objetivos dados simultáneamente minimizando una función de error cuadrática construida a partir de los objetivos individuales ponderados. Si bien la API Moveit actual no admite tareas con múltiples objetivos directamente, proporciona la clase KinematicQueryOptions. Por lo tanto, bio_ik simplemente proporciona un conjunto de objetivos de movimiento predefinidos y una combinación de los objetivos especificados por el usuario se pasa a través de Moveit al solucionador de IK. No se requieren cambios de API en Moveit, pero el uso del solucionador IK ahora consiste en pasar los objetivos ponderados a través de KinematicQueryOptions. Los objetivos predefinidos incluyen:
Para resolver un problema de movimiento en su robot, el truco ahora consiste en construir una combinación adecuada de objetivos individuales.
En el siguiente ejemplo, queremos agarrar y luego girar lentamente una rueda de válvula con las pinzas izquierda y derecha del robot PR2:
bio_ik::BioIKKinematicsQueryOptions ik_options;
ik_options.replace = true;
ik_options.return_approximate_solution = true;
auto* ll_goal = new bio_ik::PoseGoal();
auto* lr_goal = new bio_ik::PoseGoal();
auto* rl_goal = new bio_ik::PoseGoal();
auto* rr_goal = new bio_ik::PoseGoal();
ll_goal->setLinkName("l_gripper_l_finger_tip_link");
lr_goal->setLinkName("l_gripper_r_finger_tip_link");
rl_goal->setLinkName("r_gripper_l_finger_tip_link");
rr_goal->setLinkName("r_gripper_r_finger_tip_link");
ik_options.goals.emplace_back(ll_goal);
ik_options.goals.emplace_back(lr_goal);
ik_options.goals.emplace_back(rl_goal);
ik_options.goals.emplace_back(rr_goal);
También nos fijamos un par de objetivos secundarios. Primero queremos que la cabeza del PR2 mire al centro de la válvula. En segundo lugar, queremos evitar, si es posible, límites conjuntos en todas las articulaciones. En tercer lugar, queremos que las soluciones IK sean lo más parecidas posible a la configuración conjunta anterior, es decir, movimientos pequeños y eficientes. Esto se maneja agregando MinimalDisplacementGoal. Cuarto, queremos evitar los movimientos de elevación del torso, que son muy lentos en el PR2. Todo esto se especifica fácilmente:
auto* lookat_goal = new bio_ik::LookAtGoal();
lookat_goal->setLinkName("sensor_mount_link");
ik_options.goals.emplace_back(lookat_goal);
auto* avoid_joint_limits_goal = new bio_ik::AvoidJointLimitsGoal();
ik_options.goals.emplace_back(avoid_joint_limits_goal);
auto* minimal_displacement_goal = new bio_ik::MinimalDisplacementGoal();
ik_options.goals.emplace_back(minimal_displacement_goal);
auto* torso_goal = new bio_ik::PositionGoal();
torso_goal->setLinkName("torso_lift_link");
torso_goal->setWeight(1);
torso_goal->setPosition(tf::Vector3( -0.05, 0, 1.0 ));
ik_options.goals.emplace_back(torso_goal);
Para el movimiento de giro real, calculamos un conjunto de posturas de agarre requeridas en un bucle:
for(int i = 0; ; i++) {
tf::Vector3 center(0.7, 0, 1);
double t = i * 0.1;
double r = 0.1;
double a = sin(t) * 1;
double dx = fmin(0.0, cos(t) * -0.1);
double dy = cos(a) * r;
double dz = sin(a) * r;
tf::Vector3 dl(dx, +dy, +dz);
tf::Vector3 dr(dx, -dy, -dz);
tf::Vector3 dg = tf::Vector3(0, cos(a), sin(a)) * (0.025 + fmin(0.025, fmax(0.0, cos(t) * 0.1)));
ll_goal->setPosition(center + dl + dg);
lr_goal->setPosition(center + dl - dg);
rl_goal->setPosition(center + dr + dg);
rr_goal->setPosition(center + dr - dg);
double ro = 0;
ll_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro));
lr_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro));
rl_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro));
rr_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro));
lookat_goal->setAxis(tf::Vector3(1, 0, 0));
lookat_goal->setTarget(rr_goal->getPosition());
// "advanced" bio_ik usage. The call parameters for the end-effector
// poses and end-effector link names are left empty; instead the
// requested goals and weights are passed via the ik_options object.
//
robot_state.setFromIK(
joint_model_group, // active PR2 joints
EigenSTL::vector_Affine3d(), // no explicit poses here
std::vector<std::string>(), // no end effector links here
0, 0.0, // take values from YAML file
moveit::core::GroupStateValidityCallbackFn(),
ik_options // four gripper goals and secondary goals
);
... // check solution validity and actually move the robot
}
Cuando ejecute el código, el PR2 alcanzará la rueda de la válvula y la girará. De vez en cuando no puede alcanzar la válvula con su configuración actual del brazo y vuelve a agarrar la rueda.
Consulte [3] y [4] para obtener más ejemplos.
BioIK ha sido desarrollado para encontrar eficientemente buenas soluciones para problemas de cinemática inversa no convexa con múltiples objetivos y mínimos locales. Sin embargo, para algunas aplicaciones, esto puede dar lugar a resultados poco intuitivos. Si hay múltiples soluciones posibles para un problema de CI determinado, y si el usuario no ha especificado explícitamente cuál elegir, se puede seleccionar un resultado aleatoriamente del conjunto de todas las soluciones válidas. Cuando se sigue de forma incremental una trayectoria cartesiana, esto puede dar lugar a saltos no deseados, sacudidas, etc. Para generar de forma incremental una trayectoria suave utilizando BioIK, el comportamiento deseado debe especificarse explícitamente, lo que se puede hacer de dos maneras.
BioIK ofrece varios solucionadores diferentes, incluidos optimizadores globales y optimizadores locales. De forma predeterminada, BioIK utiliza un optimizador global memético ( bio2_memetic ). Se puede seleccionar una clase de solucionador diferente configurando el parámetro mode en el archivo kinematics.yaml de la configuración de su robot MoveIt.
Ejemplo:
all:
kinematics_solver: bio_ik/BioIKKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.02
kinematics_solver_attempts: 1
mode: gd_c
Optimizadores locales disponibles actualmente:
gd, gd_2, gd_4, gd_8
gd_r, gd_r_2, gd_r_4, gd_r_8
gd_c, gd_c_2, gd_c_4, gd_c_8
jac, jac_2, jac_4, jac_8
Convención de nomenclatura: <solver type>_[<variant>_]<number of threads>
Notas:
gd_* admiten tipos de objetivos arbitrarios.jac_* solo admiten objetivos de pose, pero en teoría podrían ser más estables en algunos casos.gd_c generalmente superaron a los otros solucionadores locales.gd_c , gd o jac ).Puede forzar a un optimizador global a devolver un mínimo local mediante la regularización.
Para el caso específico de movimientos incrementales del robot (también conocido como jogging ), la solución más simple es especificar tanto un PoseGoal como el RegularizationGoal especial, que intenta mantener la solución IK del espacio articular lo más cerca posible de la configuración inicial del robot dada. Normalmente, usarías un peso alto para PoseGoal y un peso menor para el regularizador.
También puede agregar un MinimalDisplacementGoal en lugar de RegularizationGoal . Ambos objetivos intentan mantener la solución IK cerca del estado actual/robot semilla, pero difieren ligeramente en el manejo de las articulaciones del robot rápidas y lentas (por ejemplo, el PR2 tiene articulaciones de brazos rápidas y una articulación de elevación del torso bastante lenta). Quizás quieras jugar con ambos objetivos para ver cuál se adapta mejor a tus necesidades.
Algunos controladores de robots industriales con brazos de 7 grados de libertad se comportan como si trabajaran en un brazo de 6 grados de libertad con una articulación adicional. Normalmente, se puede especificar el valor de la junta adicional y luego se busca una solución IK para las seis juntas restantes. Este comportamiento se puede lograr en bio_ik combinando un PoseGoal para el efector final con un JointPositionGoal para una (cualquiera) de las articulaciones del robot.
Otro truco útil es intentar mantener las articulaciones del robot centradas, ya que esto permitirá movimientos (articulaciones) del robot en ambas direcciones. Simplemente combine PoseGoal con CenterJointsGoal y, opcionalmente, también con RegularizationGaol .
También puede combinar la regularización con un solucionador gd_* local.
El solucionador bio_ik se basa en un algoritmo memético que combina la optimización basada en gradientes con la optimización genética y de enjambre de partículas.
Internamente, los vectores de todos los valores de las articulaciones del robot se utilizan para codificar diferentes soluciones intermedias (el genotipo del algoritmo genético). Durante la optimización, los valores de las articulaciones siempre se comparan con los límites de las articulaciones superior e inferior activos, de modo que solo se generen configuraciones de robot válidas.
Para calcular la aptitud de los individuos, se calcula el error acumulativo de todos los objetivos individuales determinados. Cualquier individuo con error cero es una solución exacta para el problema IK, mientras que los individuos con error pequeño corresponden a soluciones aproximadas.
Los individuos se clasifican según su condición física y se prueba la optimización basada en gradientes en las mejores configuraciones, lo que da como resultado una convergencia rápida y un buen rendimiento para muchos problemas. Si no se encuentra ninguna solución a partir de la optimización basada en gradientes, un conjunto de operadores de mutación y recombinación crean nuevos individuos, lo que da como resultado una buena exploración del espacio de búsqueda.
Consulte [3] y [4] para más detalles. Consulte [5] y [6] para obtener una explicación detallada de un algoritmo evolutivo anterior para animar personajes de videojuegos.
Hemos probado bio_ik en muchos brazos robóticos diferentes, tanto utilizando la API tradicional de efector final único como la API avanzada de efector final múltiple basada en KinematicsQueryOptions.
Una autoprueba simple consiste en generar configuraciones de robot válidas aleatorias, ejecutar cinemática directa para calcular la pose del efector final resultante y consultar el complemento IK para encontrar una configuración de articulación de robot adecuada. Luego se verifica el éxito ejecutando nuevamente la cinemática directa y verificando que la pose del efector final para la solución IK generada coincida con la pose objetivo. Este enfoque se puede ejecutar fácilmente para miles o millones de poses aleatorias, muestrea todo el espacio de trabajo del robot y permite generar rápidamente estimaciones de tasa de éxito y tiempo de solución para el solucionador de IK seleccionado.
Por supuesto, ejecutar las pruebas requiere instalar los modelos de robot correspondientes y agrega muchas dependencias. Por lo tanto, esas pruebas no están incluidas en el paquete bio_ik estándar, sino que se empaquetan por separado.
Para mayor comodidad, proporcionamos el paquete pr2_bioik_moveit , que también incluye algunas demostraciones de bio_ik para el robot de servicio PR2. Estas son demostraciones únicamente de cinemática; pero, por supuesto, también puedes intentar ejecutar las demostraciones en el robot real (si tienes uno) o en el simulador de Gazebo (si instalaste Gazebo).
Simplemente clone el paquete de descripción PR2 (dentro de pr2_common ) y el paquete pr2_bioik_moveit en su espacio de trabajo catkin:
roscd
cd src
git clone https://github.com/PR2/pr2_common.git
git clone https://github.com/TAMS-Group/bioik_pr2.git
catkin_make
Para la prueba de rendimiento FK-IK-FK, ejecute
roslaunch pr2_bioik_moveit env_pr2.launch
roslaunch pr2_bioik_moveit test_fk_ik.launch
... // wait for test completion and results summary
Cinemática y dinámica de Orocos, http://www.orocos.org
P. Beeson y B. Ames, TRAC-IK: Una biblioteca de código abierto para mejorar la resolución de cinemática inversa genérica , Actas de la Conferencia IEEE RAS Humanoids, Seúl, Corea, noviembre de 2015.
Philipp Ruppel, Norman Hendrich, Sebastian Starke, Jianwei Zhang, Funciones de costos para especificar el movimiento del cuerpo completo y tareas de manipulación de múltiples objetivos , Conferencia internacional IEEE sobre robótica y automatización, (ICRA-2018), Brisbane, Australia. DOI: 10.1109/ICRA.2018.8460799
Philipp Ruppel, Optimización del rendimiento e implementación de cinemática inversa evolutiva en ROS , tesis de maestría, Universidad de Hamburgo, 2017 PDF
Sebastian Starke, Norman Hendrich, Jianwei Zhang, Un algoritmo evolutivo memético para el movimiento cinemático articulado en tiempo real , IEEE Intl. Congreso sobre Computación Evolutiva (CEC-2017), p.2437-2479, 4-8 de junio de 2017, San Sebastián, España. DOI: 10.1109/CEC.2017.7969605
Sebastian Starke, Norman Hendrich, Dennis Krupke, Jianwei Zhang, Optimización evolutiva multiobjetivo para cinemática inversa en robots humanoides y altamente articulados , IEEE Intl. Conferencia sobre robots y sistemas inteligentes (IROS-2017), 24 al 28 de septiembre de 2017, Vancouver, Canadá
