bio_ik
1.0.0
Este repositório fornece uma implementação autônoma licenciada por BSD de uma variedade de métodos de otimização para resolver com eficiência problemas de cinemática inversa generalizada.
Todo o módulo foi implementado por Philipp Ruppel como parte de sua tese de mestrado.
Para uma reimplementação baseada em C++ do algoritmo "BioIK" original, conforme vendido originalmente na loja Unity, você pode usar o modo não padrão bio1 . O modo padrão bio2_memetic não compartilha nenhum código com esta implementação, demonstrou superá-lo em termos de taxa de sucesso, precisão e eficiência, e é realmente utilizável para aplicações robóticas precisas [4].
Você precisará da versão ROS Indigo ou mais recente (wiki.ros.org). O software foi desenvolvido no Ubuntu Linux 16.04 LTS com ROS Kinetic, mas também foi testado no Ubuntu Linux 14.04 LTS com ROS Indigo. Versões mais recentes do ROS devem funcionar, mas podem precisar de alguma adaptação. Veja abaixo as instruções específicas da versão.
Baixe o pacote bio_ik e descompacte-o em sua área de trabalho catkin.
Execute catkin_make para compilar seu espaço de trabalho:
roscd
cd src
git clone https://github.com/TAMS-Group/bio_ik.git
roscd
catkin_make
Configure o Moveit para usar bio_ik como solucionador cinemático (veja a próxima seção).
Use o Moveit para planejar e executar movimentos ou use seu próprio código junto com o nó move_group para mover seu robô.
Como de costume, a API pública é especificada nos arquivos de cabeçalho público do pacote bio_ik , localizados no subdiretório include/bio_ik ; as fontes, incluindo alguns arquivos de cabeçalho privados, estão no subdiretório src .
Para facilidade de uso e compatibilidade com o código existente, o algoritmo bio_ik é encapsulado como um plugin de cinemática Moveit. Portanto, bio_ik pode ser usado como um substituto direto do solucionador IK padrão baseado em Orocos/KDL. Dado o nome de um efetor final e uma pose de alvo de 6 DOF, o bio_ik procurará uma configuração de junta de robô válida que atinja o alvo determinado.
Em nossos testes (veja abaixo), tanto em termos de taxa de sucesso quanto de tempo de solução, o bio_ik superou regularmente o solucionador Orocos [1] e é competitivo com o trac-ik [2]. O algoritmo bio_ik também pode ser usado para sistemas de alto DOF, como cobras robóticas, e convergirá automaticamente para as melhores soluções aproximadas para braços de baixo DOF, onde algumas poses de alvo não são exatamente alcançáveis.
Embora você possa escrever os arquivos de configuração do Moveit manualmente, a maneira mais fácil é executar o assistente de configuração do Moveit para o seu robô e, em seguida, selecionar bio_ik como o solucionador IK ao configurar os efetores finais. Uma vez configurado, o solucionador pode ser chamado usando a API Moveit padrão ou usado interativamente no rviz usando o plugin MotionPlanning GUI.
Certifique-se de ter um modelo URDF (ou xacro) para o seu robô.
Execute o assistente de configuração do moveit para criar os arquivos de configuração do Moveit:
rosrun moveit_setup_assistant moveit_setup_assistant
O assistente de configuração procura automaticamente todos os plug-ins do solucionador IK disponíveis em seu espaço de trabalho. Portanto, você pode simplesmente selecionar bio_ik como o solucionador IK na lista suspensa para cada efetor final e então configurar os parâmetros cinemáticos, ou seja, a precisão da posição padrão (metros) e o tempo limite (em segundos). Para braços típicos de 6-DOF ou 7-DOF, uma precisão de 0,001 m (ou menor) e um tempo limite de 1 ms devem ser adequados. Robôs mais complexos podem precisar de um tempo limite maior.
Gere os arquivos de configuração do moveit no assistente de configuração. Claro, você também pode editar o arquivo de configuração config/kinematics.yaml com seu editor de texto favorito. Por exemplo, uma configuração para o robô PR2 pode ser assim:
# 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 um primeiro teste, execute o lançamento de demonstração criado pelo Moveit. Assim que o rviz estiver em execução, habilite o plugin de planejamento de movimento e selecione um dos efetores finais do seu robô. O Rviz deve mostrar um marcador interativo 6-D (posição e orientação) para o(s) efetor(es) final(is) selecionado(s). Mova o marcador interativo e observe o bio_ik calculando poses para o seu robô.
Se você também instalou a demonstração bio_ik (veja abaixo), deverá ser capaz de executar uma das demonstrações predefinidas:
roslaunch pr2_bioik_moveit demo.launch
roslaunch pr2_bioik_moveit valve.launch
roslaunch pr2_bioik_moveit dance.launch
Agora você está pronto para usar o bio_ik em seus programas C/C++ e Python, usando a API Moveit padrão. Para solicitar explicitamente uma solução IK em 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 muitas aplicações de robôs, é essencial especificar mais do que apenas uma posição do efetor final. Exemplos típicos incluem
No bio_ik, tais tarefas são especificadas como uma combinação de múltiplos objetivos individuais.
O algoritmo então tenta encontrar uma configuração do robô que cumpra todos os objetivos simultaneamente, minimizando uma função de erro quadrática construída a partir dos objetivos individuais ponderados. Embora a API Moveit atual não ofereça suporte direto a tarefas com vários objetivos, ela fornece a classe KinematicQueryOptions. Portanto, o bio_ik simplesmente fornece um conjunto de metas de movimento predefinidas e uma combinação das metas especificadas pelo usuário é passada via Moveit para o solucionador IK. Nenhuma alteração de API é necessária no Moveit, mas usar o solucionador IK agora consiste em passar as metas ponderadas por meio de KinematicQueryOptions. Os objetivos predefinidos incluem:
Para resolver um problema de movimento no seu robô, o truque agora é construir uma combinação adequada de objetivos individuais.
No exemplo a seguir, queremos agarrar e girar lentamente uma roda de válvula com as pinças esquerda e direita do robô 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);
Também definimos alguns objetivos secundários. Primeiro, queremos que a cabeça do PR2 fique voltada para o centro da válvula. Em segundo lugar, queremos evitar limites em todas as juntas, se possível. Terceiro, queremos que as soluções IK sejam o mais próximas possível da configuração conjunta anterior, o que significa movimentos pequenos e eficientes. Isso é resolvido adicionando MinimalDisplacementGoal. Quarto, queremos evitar movimentos de elevação do tronco, que são muito lentos no PR2. Tudo isso é especificado facilmente:
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 o movimento de rotação real, calculamos um conjunto de poses de garra necessárias em um loop:
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
}
Ao executar o código, o PR2 alcançará a roda da válvula e a girará. De vez em quando, ele não consegue alcançar a válvula com a configuração atual do braço e irá recolocar a roda.
Veja [3] e [4] para mais exemplos.
O BioIK foi desenvolvido para encontrar eficientemente boas soluções para problemas de cinemática inversa não convexa com múltiplos objetivos e mínimos locais. No entanto, para algumas aplicações, isso pode levar a resultados não intuitivos. Se houver múltiplas soluções possíveis para um determinado problema de CI, e se o usuário não tiver especificado explicitamente qual delas escolher, um resultado poderá ser selecionado aleatoriamente do conjunto de todas as soluções válidas. Ao rastrear incrementalmente um caminho cartesiano, isso pode resultar em saltos indesejados, tremores, etc. Para gerar incrementalmente uma trajetória suave usando BioIK, o comportamento desejado deve ser especificado explicitamente, o que pode ser feito de duas maneiras.
A BioIK oferece vários solucionadores diferentes, incluindo otimizadores globais e otimizadores locais. Por padrão, o BioIK usa um otimizador memético global ( bio2_memetic ). Uma classe de solucionador diferente pode ser selecionada definindo o parâmetro mode no arquivo kinematics.yaml da configuração do seu robô MoveIt.
Exemplo:
all:
kinematics_solver: bio_ik/BioIKKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.02
kinematics_solver_attempts: 1
mode: gd_c
Otimizadores locais atualmente disponíveis:
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
Convenção de nomenclatura: <solver type>_[<variant>_]<number of threads>
Notas:
gd_* suportam tipos de metas arbitrárias.jac_* suportam apenas metas de pose, mas podem, em teoria, ser mais estáveis em alguns casos.gd_c geralmente superaram os outros solucionadores locais.gd_c , gd ou jac ).Você pode forçar um otimizador global a retornar um mínimo local por meio de regularização.
Para o caso específico de movimentos incrementais do robô (também conhecido como jogging ), a solução mais simples é especificar um PoseGoal e o RegularizationGoal especial , que tenta manter a solução IK do espaço conjunto o mais próximo possível da configuração inicial do robô fornecida. Normalmente você usaria um peso alto para o PoseGoal e um peso menor para o regularizador.
Você também pode adicionar um MinimalDisplacementGoal em vez de RegularizationGoal . Ambos os objetivos tentam manter a solução IK próxima do estado atual/semente do robô, mas diferem ligeiramente no manuseio de articulações de robô rápidas e lentas (por exemplo, o PR2 possui articulações de braço rápidas e uma articulação de elevação de tronco bastante lenta). Você pode querer jogar com os dois objetivos para ver qual deles atende melhor às suas necessidades.
Alguns controladores de robôs industriais em braços de 7 DOF se comportam como se estivessem trabalhando em um braço de 6 DOF com uma junta extra. Normalmente, o valor da junta extra pode ser especificado e uma solução IK é então pesquisada para as seis juntas restantes. Este comportamento pode ser alcançado no bio_ik combinando um PoseGoal para o efetor final com um JointPositionGoal para uma (qualquer uma) das juntas do robô.
Outro truque útil é tentar manter as articulações do robô centralizadas, pois isso permitirá movimentos (juntas) do robô em ambas as direções. Basta combinar PoseGoal com CenterJointsGoal e, opcionalmente, também RegularizationGaol .
Você também pode combinar a regularização com um solucionador gd_* local.
O solucionador bio_ik é baseado em um algoritmo memético que combina otimização baseada em gradiente com otimização genética e de enxame de partículas.
Internamente, vetores de todos os valores conjuntos do robô são usados para codificar diferentes soluções intermediárias (o genótipo do algoritmo genético). Durante a otimização, os valores das juntas são sempre verificados em relação aos limites inferiores e superiores das juntas ativos, de modo que apenas configurações válidas do robô sejam geradas.
Para calcular a aptidão dos indivíduos, é calculado o erro cumulativo sobre todas as metas individuais fornecidas. Qualquer indivíduo com erro zero é uma solução exata para o problema IK, enquanto indivíduos com erro pequeno correspondem a soluções aproximadas.
Os indivíduos são classificados por sua aptidão e a otimização baseada em gradiente é testada nas melhores configurações, resultando em convergência rápida e bom desempenho para muitos problemas. Se nenhuma solução for encontrada a partir da otimização baseada em gradiente, novos indivíduos são criados por um conjunto de operadores de mutação e recombinação, resultando em uma boa exploração do espaço de busca.
Veja [3] e [4] para mais detalhes. Veja [5] e [6] para uma explicação detalhada de um algoritmo evolutivo anterior para animação de personagens de videogame.
Testamos o bio_ik em muitos braços de robô diferentes, ambos usando a API tradicional de efetor final único e a API avançada de efetor múltiplo baseada em KinematicsQueryOptions.
Um autoteste simples consiste em gerar configurações aleatórias válidas do robô, executar a cinemática direta para calcular a pose do efetor final resultante e consultar o plug-in IK para encontrar uma configuração de junta do robô adequada. O sucesso é então verificado executando novamente a cinemática direta e verificando se a pose do efetor final para a solução IK gerada corresponde à pose alvo. Essa abordagem pode ser executada facilmente para milhares ou milhões de poses aleatórias, amostra todo o espaço de trabalho do robô e permite gerar rapidamente estimativas de taxa de sucesso e tempo de solução para o solucionador IK selecionado.
É claro que a execução dos testes requer a instalação dos modelos de robô correspondentes e adiciona muitas dependências. Portanto, esses testes não estão incluídos no pacote bio_ik padrão, mas são embalados separadamente.
Por conveniência, fornecemos o pacote pr2_bioik_moveit , que também inclui algumas demonstrações bio_ik para o robô de serviço PR2. Estas são demonstrações apenas de cinemática; mas é claro que você também pode tentar executar as demonstrações no robô real (se tiver um) ou no simulador Gazebo (se você instalou o Gazebo).
Simplesmente clone o pacote de descrição PR2 (dentro de pr2_common ) e o pacote pr2_bioik_moveit em seu espaço de trabalho 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 o teste de desempenho FK-IK-FK, execute
roslaunch pr2_bioik_moveit env_pr2.launch
roslaunch pr2_bioik_moveit test_fk_ik.launch
... // wait for test completion and results summary
Orocos Cinemática e Dinâmica, http://www.orocos.org
P. Beeson e B. Ames, TRAC-IK: Uma biblioteca de código aberto para melhor resolução de cinemática inversa genérica , Proceedings of the IEEE RAS Humanoids Conference, Seul, Coreia, novembro de 2015.
Philipp Ruppel, Norman Hendrich, Sebastian Starke, Jianwei Zhang, Cost Functions to Specific Full-Body Motion and Multi-Goal Manipulation Tasks , Conferência Internacional IEEE sobre Robótica e Automação, (ICRA-2018), Brisbane, Austrália. DOI: 10.1109/ICRA.2018.8460799
Philipp Ruppel, Otimização de desempenho e implementação de cinemática inversa evolucionária em ROS , tese de mestrado, Universidade de Hamburgo, 2017 PDF
Sebastian Starke, Norman Hendrich, Jianwei Zhang, Um algoritmo evolutivo memético para movimento cinemático articulado em tempo real , IEEE Intl. Congresso sobre Computação Evolutiva (CEC-2017), p.2437-2479, 4 a 8 de junho de 2017, San Sebastian, Espanha. DOI: 10.1109/CEC.2017.7969605
Sebastian Starke, Norman Hendrich, Dennis Krupke, Jianwei Zhang, Otimização Evolutiva Multiobjetivo para Cinemática Inversa em Robôs Altamente Articulados e Humanóides , IEEE Intl. Conferência sobre Robôs e Sistemas Inteligentes (IROS-2017), 24 a 28 de setembro de 2017, Vancouver, Canadá
