search

Robotics010 / ar600e-default

AR-600E Humanoid Robot Open Source Project. AR-600E has default hardware and firmware without any modifications.
GNU General Public License v3.0
2 stars 0 forks source link

Какой способ ОК используется в MoveIt! у нас? #14

Closed Robotics010 closed 5 years ago

Robotics010 commented 5 years ago

Какой способ решения задачи обратной кинематики (ОК) используется по умолчанию в MoveIt! у нас?

VladimirAlD commented 5 years ago

При решении задач кинематики использовали стандартный решатель библиотеки Orocos_KDL нооо он не работает если <6DOFs. Альтернива, для <6DOF, IKFast и Trac-IK. Orocos-KDL и Track-IK – числовые, IKFast – аналитический. ссылки на код: Orocos_KDL Track-IK IKFast // открывал через расширение Browsec

пы.сы. дублирую kdl.docx matlab.docx

Robotics010 commented 5 years ago

В MoveIt! Concepts указано, что

The default inverse kinematics plugin for MoveIt! is configured using the KDL numerical jacobian-based solver. This plugin is automatically configured by the MoveIt! Setup Assistant.

Т.е. Levenberg–Marquardt и Newton-Raphson - это Jacobian Methods

Это семейство методов на основе нелинейной регрессии (Анжела)

В уроке Generate IKFast Plugin Tutorial указано, что

The IKFast plugin should function identically to the default KDL IK Solver, but with greatly increased performance.

На базе этого можно предположить, что по умолчанию используется KDL IK Solver.

Robotics010 commented 5 years ago

У нас в нашем kinematics.yaml (ar600e-default/moveit_arbot/config/kinematics.yaml) явно указан текущий решатель:

kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin

Robotics010 commented 5 years ago

В Kinematics Configuration Tutorial явно сказано что

The KDL Kinematics Plugin is the default kinematics plugin currently used by MoveIt!

Тут же сказано, что

The KDL kinematics plugin wraps around the numerical inverse kinematics solver provided by the Orocos KDL package.

Т.е. решатель построен на базе численного метода (как и предполагалось)

Как, в принципе, @VladimirAlD и писал выше:

При решении задач кинематики использовали стандартный решатель библиотеки Orocos_KDL

Robotics010 commented 5 years ago

Сам решатель в ROS описан здесь - orocos_kdl (ранее здесь - kdl)

Robotics010 commented 5 years ago

Судя по описанию KDL::ChainIkSolverPos Class Reference для решения ОК используется

или

Robotics010 commented 5 years ago

Исходные файлы решателей kdl_kinematics_plugin.cpp kdl_kinematics_plugin.h

Robotics010 commented 5 years ago

Levenberg-Marquardt algorithm is iterative, gradient-based optimization methods that start from an initial guess at the solution and seek to minimize a specific cost function. If either algorithm converges to a configuration where the cost is close to zero within a specified tolerance, it has found a solution to the inverse kinematics problem. However, for some combinations of initial guesses and desired end effector poses, the algorithm may exit without finding an ideal robot configuration. To handle this, the algorithm utilizes a random restart mechanism. If enabled, the random restart mechanism restarts the iterative search from a random robot configuration whenever that search fails to find a configuration that achieves the desired end effector pose. These random restarts continue until either a qualifying IK solution is found, the maximum time has elapsed, or the iteration limit is reached.

The Levenberg-Marquardt (LM) algorithm is an error-damped least-squares method. The error-damped factor helps to prevent the algorithm from escaping a local minimum. The LM algorithm is optimized to converge much faster if the initial guess is close to the solution. However the algorithm does not handle arbitrary initial guesses well.

Consider using this algorithm for finding IK solutions for a series of poses along a desired trajectory of the end effector. Once a robot configuration is found for one pose, that configuration is often a good initial guess at an IK solution for the next pose in the trajectory. In this situation, the LM algorithm may yield faster results.

Источник https://de.mathworks.com/help/robotics/ug/inverse-kinematics-algorithms.html

Robotics010 commented 5 years ago

https://www.mi.ams.eng.osaka-u.ac.jp/member/sugihara/pub/ichr09.pdf

Robotics010 commented 5 years ago

http://people.duke.edu/~hpgavin/ce281/lm.pdf здесь подробно про сам метод (Анжела)

Robotics010 commented 5 years ago

В книге Bertsekas, Dimitri P. Nonlinear Programming. Belmont, MA: Athena Scientific, 1999 есть упоминание о Levenberg-Marquardt method на странице 91

Robotics010 commented 5 years ago

В книге Nocedal, Jorge, and Stephen Wright. Numerical Optimization. New York, NY: Springer, 2006 есть целый раздел, посвященный The Levenberg–Marquardt Method и еще раздел, посвященный его реализации

Robotics010 commented 5 years ago

Newton-Raphson - Engineering_Mathematics_4thed-_J_Bird - с. 123 по оглавлению (Анжела)

Robotics010 commented 5 years ago

Выполняю уроки из Indigo MoveIt! Tutorials, чтобы разобраться какой из методов выше используется по умолчанию.

Robotics010 commented 5 years ago

http://docs.ros.org/indigo/api/moveit_tutorials/html/doc/ros_visualization/visualization_tutorial.html https://github.com/ros-planning/moveit_tutorials/issues/74

I attempted Setup Assistant Tutorial followed by the RViz Plugin Tutorial No interactive marker was displayed in RViz and the following error was logged to terminal:

[ INFO] [1508344563.774111009]: Loading robot model 'pr2'... [ERROR] [1508344563.785211118]: Group 'right_arm' is not a chain [ERROR] [1508344563.785262245]: Kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for group 'right_arm' [ERROR] [1508344563.785979936]: Kinematics solver could not be instantiated for joint group right_arm. [ INFO] [1508344563.799430428]: Loading robot model 'pr2'... [ERROR] [1508344563.810831186]: Group 'left_arm' is not a chain [ERROR] [1508344563.810856901]: Kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for group 'left_arm' [ERROR] [1508344563.811354335]: Kinematics solver could not be instantiated for joint group left_arm.

I solved this by modifying MoveIt Setup Assistant Planning Groups for right_arm (and left_arm) to be a chain instead of joints.

right_arm > Chain > r_shoulder_pan_link -> r_wrist_roll_link right_gripper > Links > r_gripper_palm_link -> r_gripper_tool_frame left_arm > Chain > l_shoulder_pan_link -> l_wrist_roll_link left_gripper > Links > l_gripper_palm_link -> l_gripper_tool_frame

Robotics010 commented 5 years ago

Довольно подробное и понятное объяснение по IK Solvers в ROS и MoveIt! "Performance optimization and implementation of evolutionary inverse kinematics in ROS" https://tams.informatik.uni-hamburg.de/publications/2017/MSc_Philipp_Ruppel.pdf

Robotics010 commented 5 years ago

Вопрос аналогичный нашему, но ответа по реализованному алгоритму нет - http://www.orocos.org/forum/orocos/orocos-users/kdl-algorithm-details

Robotics010 commented 5 years ago

Да, действительно.

При решении задач кинематики использовали стандартный решатель библиотеки Orocos_KDL нооо он не работает если <6DOFs.

Хороший пост что с этим делать KDL Kinematics Solver Limitations

Тогда почему у нас в качестве решателя используется kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin ?

Robotics010 commented 5 years ago

Итого, по используется KDLKinematicsPlugin - решатель обратной позиционной задачи кинематики на базе Orocos_KDL. Это численный метод на базе матриц Якоби. В документации к KDLKinematicsPlugin было описано, что используется либо Levenberg–Marquardt либо Newton-Raphson. Эти методы не рассчитаны только для манипуляторов с 6 или более степенями свободы. В нашем роботе сейчас руки-манипуляторы 5 DOF.

Нужно уточнить чем вызваны такие ограничения, во что это выливается. Рекомендуется использовать вместо KDLKinematicsPlugin - MoveIt! ikfast plugin. Или можно сделать свой решатель и импортировать его в ROS. Есть инструкции по этому поводу.