diff --git a/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt index 5d9aa3c0ff0..49d1849b312 100644 --- a/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt @@ -1,8 +1,6 @@ set(MOVEIT_LIB_NAME moveit_lma_kinematics_plugin) -add_library(${MOVEIT_LIB_NAME} src/lma_kinematics_plugin.cpp - src/chainiksolver_pos_lma_jl_mimic.cpp - src/chainiksolver_vel_pinv_mimic.cpp) +add_library(${MOVEIT_LIB_NAME} src/lma_kinematics_plugin.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES}) diff --git a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/chainiksolver_pos_lma_jl_mimic.h b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/chainiksolver_pos_lma_jl_mimic.h deleted file mode 100644 index bb7d22552da..00000000000 --- a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/chainiksolver_pos_lma_jl_mimic.h +++ /dev/null @@ -1,121 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, CRI group, NTU, Singapore - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of CRI group nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Francisco Suarez-Ruiz */ - -#ifndef KDLCHAINIKSOLVERPOS_LMA_JL_MIMIC_H -#define KDLCHAINIKSOLVERPOS_LMA_JL_MIMIC_H - -#include "kdl/config.h" -#include "kdl/chainiksolverpos_lma.hpp" // Solver for the inverse position kinematics that uses Levenberg-Marquardt. -#include "kdl/chainfksolver.hpp" - -#include - -namespace KDL -{ -/** - * Implementation of a general inverse position kinematics - * algorithm based on Levenberg-Marquardt method to calculate the - * position transformation from Cartesian to joint space of a general - * KDL::Chain. Takes joint limits into account. - * - * @ingroup KinematicFamily - */ -class ChainIkSolverPos_LMA_JL_Mimic : public ChainIkSolverPos -{ -public: - /** - * Constructor of the solver, it needs the chain, a forward - * position kinematics solver and an inverse velocity - * kinematics solver for that chain. - * - * @param chain the chain to calculate the inverse position for - * @param q_max the maximum joint positions - * @param q_min the minimum joint positions - * @param fksolver a forward position kinematics solver - * @param iksolver an inverse velocity kinematics solver - * @param maxiter the maximum Levenberg-Marquardt iterations, - * default: 100 - * @param eps the precision for the position, used to end the - * iterations, default: epsilon (defined in kdl.hpp) - * - * @return - */ - ChainIkSolverPos_LMA_JL_Mimic(const Chain& chain, const JntArray& q_min, const JntArray& q_max, - ChainFkSolverPos& fksolver, ChainIkSolverPos_LMA& iksolver, unsigned int maxiter = 100, - double eps = 1e-6, bool position_ik = false); - -// TODO: simplify after kinetic support is dropped -#define KDL_VERSION_LESS(a, b, c) (KDL_VERSION < ((a << 16) | (b << 8) | c)) -#if KDL_VERSION_LESS(1, 4, 0) - void updateInternalDataStructures(); -#else - void updateInternalDataStructures() override; -#endif -#undef KDL_VERSION_LESS - - ~ChainIkSolverPos_LMA_JL_Mimic() override; - - int CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out) override; - - virtual int CartToJntAdvanced(const JntArray& q_init, const Frame& p_in, JntArray& q_out, bool lock_redundant_joints); - - bool setMimicJoints(const std::vector& mimic_joints); - -private: - const Chain chain; - JntArray q_min; // These are the limits for the "reduced" state consisting of only active DOFs - JntArray q_min_mimic; // These are the limits for the full state - JntArray q_max; // These are the limits for the "reduced" state consisting of only active DOFs - JntArray q_max_mimic; // These are the limits for the full state - JntArray q_temp; - ChainFkSolverPos& fksolver; - ChainIkSolverPos_LMA& iksolver; - JntArray delta_q; - Frame f; - Twist delta_twist; - unsigned int maxiter; - double eps; - std::vector mimic_joints; - void qToqMimic(const JntArray& q, - JntArray& q_result); // Convert from the "reduced" state (only active DOFs) to the "full" state - void qMimicToq(const JntArray& q, JntArray& q_result); // Convert from the "full" state to the "reduced" state - void harmonize(JntArray& q_out); // Puts the angles within [-2PI, 2PI] - bool obeysLimits(const KDL::JntArray& q_out); // Checks that a set of joint angles obey the urdf limits - bool position_ik; -}; -} - -#endif diff --git a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/chainiksolver_vel_pinv_mimic.h b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/chainiksolver_vel_pinv_mimic.h deleted file mode 100644 index 43e2230eb90..00000000000 --- a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/chainiksolver_vel_pinv_mimic.h +++ /dev/null @@ -1,195 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, CRI group, NTU, Singapore - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of CRI group nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Francisco Suarez-Ruiz */ - -#ifndef KDL_CHAIN_IKSOLVERVEL_PINV_MIMIC_H -#define KDL_CHAIN_IKSOLVERVEL_PINV_MIMIC_H - -#include "kdl/config.h" -#include "kdl/chainiksolver.hpp" -#include "kdl/chainjnttojacsolver.hpp" -#include "kdl/utilities/svd_HH.hpp" -#include "kdl/utilities/svd_eigen_HH.hpp" - -#include - -namespace KDL -{ -/** - * Implementation of a inverse velocity kinematics algorithm based - * on the generalize pseudo inverse to calculate the velocity - * transformation from Cartesian to joint space of a general - * KDL::Chain. It uses a svd-calculation based on householders - * rotations. - * - * @ingroup KinematicFamily - */ -class ChainIkSolverVel_pinv_mimic : public ChainIkSolverVel -{ -public: - /** - * Constructor of the solver - * - * @param chain the chain to calculate the inverse velocity - * kinematics for - * @param num_mimic_joints The number of joints that are setup to - * follow other joints - * @param num_redundant_joints The number of redundant dofs - * @param position_ik false if you want to solve for the full 6 dof - * end-effector pose, true if you want to solve only for the 3 dof - * end-effector position. - * @param eps if a singular value is below this value, its - * inverse is set to zero, default: 0.00001 - * @param maxiter maximum iterations for the svd calculation, - * default: 150 - */ - explicit ChainIkSolverVel_pinv_mimic(const Chain& chain, int num_mimic_joints = 0, int num_redundant_joints = 0, - bool position_ik = false, double eps = 0.00001, int maxiter = 150); - -// TODO: simplify after kinetic support is dropped -#define KDL_VERSION_LESS(a, b, c) (KDL_VERSION < ((a << 16) | (b << 8) | c)) -#if KDL_VERSION_LESS(1, 4, 0) - void updateInternalDataStructures(); -#else - void updateInternalDataStructures() override; -#endif -#undef KDL_VERSION_LESS - - ~ChainIkSolverVel_pinv_mimic() override; - - int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out) override; - - virtual int CartToJntRedundant(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); - - /** - * not (yet) implemented. - * - */ - int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out) override - { - return -1; - }; - - /** - * @brief Set a vector of indices that map each (and every) joint in the chain onto the corresponding joint in a - * reduced set of joints that do not include the mimic joints. This vector must be of size chain.getNrOfJoints(). - * E.g. if an arm has 7 joints: j0 to j6. Say j2 mimics (follows) j0. Then, mimic_joints should be: - * [0 1 0 3 4 5 6] - * @param mimic_joints Vector of size chain.getNrOfJoints() that maps every joint in the chain onto (a) itself - * if its not a mimic joint or (b) onto the active dof that it is mimicking - */ - bool setMimicJoints(const std::vector& _mimic_joints); - - /** - * @brief Set a mapping between a reduced set of joints (numbering either 6 or 3) and the full set of active (i.e - * excluding the mimic joints) DOFs in the robot. - * As an example, consider an arm with 7 joints: j0 to j6. If j2 represents the redundancy, then - * redundant_joints_map_index - * will be a 6 dimensional vector - [0 1 3 4 5 6], - * i.e. joint_value_full(redundant_joints_map_index[i]) = joint_value_reduced(i), for i=0,...5 - */ - bool setRedundantJointsMapIndex(const std::vector& redundant_joints_map_index); - - void lockRedundantJoints() - { - redundant_joints_locked = true; - } - - void unlockRedundantJoints() - { - redundant_joints_locked = false; - } - -private: - bool jacToJacReduced(const Jacobian& jac, Jacobian& jac_mimic); - bool jacToJacLocked(const Jacobian& jac, Jacobian& jac_locked); - - const Chain chain; - ChainJntToJacSolver jnt2jac; - - // This set of variables are all used in the default case, i.e. where we are solving for the - // full end-effector pose - Jacobian jac; - std::vector U; - JntArray S; - std::vector V; - JntArray tmp; - - // This is the "reduced" jacobian, i.e. the contributions of the mimic joints have been mapped onto - // the active DOFs here - Jacobian jac_reduced; - JntArray qdot_out_reduced; - - // This is the set of variable used when solving for position only inverse kinematics - Eigen::MatrixXd U_translate; - Eigen::VectorXd S_translate; - Eigen::MatrixXd V_translate; - Eigen::VectorXd tmp_translate; - - // This is the jacobian when the redundant joint is "locked" and plays no part - Jacobian jac_locked; - JntArray qdot_out_reduced_locked, qdot_out_locked; - - SVD_HH svd; - double eps; - int maxiter; - - // Mimic joint specific - std::vector mimic_joints_; - int num_mimic_joints; - - bool position_ik; - - // This is the set of variable used when solving for inverse kinematics - // for the case where the redundant joint is "locked" and plays no part - Eigen::MatrixXd U_locked; - Eigen::VectorXd S_locked; - Eigen::MatrixXd V_locked; - Eigen::VectorXd tmp_locked; - - // This is the set of variable used when solving for position only inverse kinematics - // for the case where the redundant joint is "locked" and plays no part - Eigen::MatrixXd U_translate_locked; - Eigen::VectorXd S_translate_locked; - Eigen::MatrixXd V_translate_locked; - Eigen::VectorXd tmp_translate_locked; - - // Internal storage for a map from the "locked" state to the full active state - std::vector locked_joints_map_index; - unsigned int num_redundant_joints; - bool redundant_joints_locked; -}; -} -#endif diff --git a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/joint_mimic.h b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/joint_mimic.h deleted file mode 100644 index 27e1906e7b5..00000000000 --- a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/joint_mimic.h +++ /dev/null @@ -1,75 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, CRI group, NTU, Singapore - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of CRI group nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Francisco Suarez-Ruiz */ - -#ifndef MOVEIT_LMA_KINEMATICS_PLUGIN_JOINT_MIMIC_H -#define MOVEIT_LMA_KINEMATICS_PLUGIN_JOINT_MIMIC_H - -namespace lma_kinematics_plugin -{ -/** \brief A model of a mimic joint. Mimic joints are typically unactuated joints -that are constrained to follow the motion of another joint. The constraint is linear, i.e. -joint_angle_constrained_joint = joint_angle_mimicked_joint*multiplier + offset -*/ -class JointMimic -{ -public: - JointMimic() - { - this->reset(0); - }; - - /** \brief Offset for this joint value from the joint that it mimics */ - double offset; - /** \brief Multiplier for this joint value from the joint that it mimics */ - double multiplier; - /** \brief Index of the joint that this joint mimics in the vector of active degrees of freedom */ - unsigned int map_index; - /** \brief Name of this joint */ - std::string joint_name; - /** \brief If true, this joint is an active DOF and not a mimic joint*/ - bool active; - - void reset(unsigned int index) - { - offset = 0.0; - multiplier = 1.0; - map_index = index; - active = false; - }; -}; -} - -#endif diff --git a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h index 9fbfc69dbfa..c83dcef90ec 100644 --- a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h +++ b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h @@ -49,23 +49,20 @@ #include // KDL -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include // MoveIt! #include +#include #include namespace lma_kinematics_plugin { /** - * @brief Specific implementation of kinematics using Levenberg-Marquardt method available at KDL. - * This version can be used with any robot. + * @brief Implementation of kinematics using Levenberg-Marquardt (LMA) solver from KDL. + * This version supports any kinematic chain without mimic joints. */ class LMAKinematicsPlugin : public kinematics::KinematicsBase { @@ -122,18 +119,14 @@ class LMAKinematicsPlugin : public kinematics::KinematicsBase protected: /** * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. - * This particular method is intended for "searching" for a solutions by stepping through the redundancy - * (or other numerical routines). + * This particular method is intended for "searching" for a solutions by randomly re-seeding on failure. * @param ik_pose the desired pose of the link * @param ik_seed_state an initial guess solution for the inverse kinematics * @param timeout The amount of time (in seconds) available to the solver * @param solution the solution vector - * @param solution_callback A callback solution for the IK solution + * @param solution_callback A callback to validate an IK solution * @param error_code an error code that encodes the reason for failure or success - * @param check_consistency Set to true if consistency check needs to be performed - * @param redundancy The index of the redundant joint - * @param consistency_limit The returned solutuion will contain a value for the redundant joint in the range - * [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit] + * @param consistency_limits The returned solutuion will not deviate more than these from the seed * @return True if a valid solution was found, false otherwise */ bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, @@ -141,65 +134,47 @@ class LMAKinematicsPlugin : public kinematics::KinematicsBase moveit_msgs::MoveItErrorCodes& error_code, const std::vector& consistency_limits, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; - bool setRedundantJoints(const std::vector& redundant_joint_indices) override; - private: bool timedOut(const ros::WallTime& start_time, double duration) const; - /** @brief Check whether the solution lies within the consistency limit of the seed state + /** @brief Check whether the solution lies within the consistency limits of the seed state * @param seed_state Seed state - * @param redundancy Index of the redundant joint within the chain - * @param consistency_limit The returned state for redundant joint should be in the range - * [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit] + * @param consistency_limits * @param solution solution configuration * @return true if check succeeds */ - bool checkConsistency(const KDL::JntArray& seed_state, const std::vector& consistency_limit, - const KDL::JntArray& solution) const; - - int getJointIndex(const std::string& name) const; + bool checkConsistency(const Eigen::VectorXd& seed_state, const std::vector& consistency_limits, + const Eigen::VectorXd& solution) const; - int getKDLSegmentIndex(const std::string& name) const; + void getRandomConfiguration(Eigen::VectorXd& jnt_array) const; - void getRandomConfiguration(KDL::JntArray& jnt_array, bool lock_redundancy) const; - - /** @brief Get a random configuration within joint limits close to the seed state + /** @brief Get a random configuration within consistency limits close to the seed state * @param seed_state Seed state - * @param redundancy Index of the redundant joint within the chain - * @param consistency_limit The returned state will contain a value for the redundant joint in the range - * [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit] + * @param consistency_limits * @param jnt_array Returned random configuration */ - void getRandomConfiguration(const KDL::JntArray& seed_state, const std::vector& consistency_limits, - KDL::JntArray& jnt_array, bool lock_redundancy) const; - - bool isRedundantJoint(unsigned int index) const; - - bool active_; /** Internal variable that indicates whether solvers are configured and ready */ - - moveit_msgs::KinematicSolverInfo ik_chain_info_; /** Stores information for the inverse kinematics solver */ - - moveit_msgs::KinematicSolverInfo fk_chain_info_; /** Store information for the forward kinematics solver */ + void getRandomConfiguration(const Eigen::VectorXd& seed_state, const std::vector& consistency_limits, + Eigen::VectorXd& jnt_array) const; - KDL::Chain kdl_chain_; - - unsigned int dimension_; /** Dimension of the group */ + bool initialized_; //< Internal variable that indicates whether solver is configured and ready - KDL::JntArray joint_min_, joint_max_; /** Joint limits */ - - mutable random_numbers::RandomNumberGenerator random_number_generator_; + unsigned int dimension_; //< Dimension of the group + moveit_msgs::KinematicSolverInfo solver_info_; //< Stores information for the inverse kinematics solver + const robot_model::JointModelGroup* joint_model_group_; robot_state::RobotStatePtr state_; + KDL::Chain kdl_chain_; + std::unique_ptr fk_solver; + Eigen::VectorXd joint_min_, joint_max_; // joint limits - int num_possible_redundant_joints_; - std::vector redundant_joints_map_index_; - - // Storage required for when the set of redundant joints is reset - bool position_ik_; // whether this solver is only being used for position ik - const robot_model::JointModelGroup* joint_model_group_; - double max_solver_iterations_; + int max_solver_iterations_; double epsilon_; - std::vector mimic_joints_; + /** weight of orientation error vs position error + * + * < 1.0: orientation has less importance than position + * > 1.0: orientation has more importance than position + * = 0.0: perform position-only IK */ + double orientation_vs_position_weight_; }; } diff --git a/moveit_kinematics/lma_kinematics_plugin/src/chainiksolver_pos_lma_jl_mimic.cpp b/moveit_kinematics/lma_kinematics_plugin/src/chainiksolver_pos_lma_jl_mimic.cpp deleted file mode 100644 index 12e8714cd46..00000000000 --- a/moveit_kinematics/lma_kinematics_plugin/src/chainiksolver_pos_lma_jl_mimic.cpp +++ /dev/null @@ -1,176 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, CRI group, NTU, Singapore - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of CRI group nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Francisco Suarez-Ruiz */ - -#include "moveit/lma_kinematics_plugin/chainiksolver_pos_lma_jl_mimic.h" -#include - -namespace KDL -{ -ChainIkSolverPos_LMA_JL_Mimic::ChainIkSolverPos_LMA_JL_Mimic(const Chain& _chain, const JntArray& _q_min, - const JntArray& _q_max, ChainFkSolverPos& _fksolver, - ChainIkSolverPos_LMA& _iksolver, unsigned int _maxiter, - double _eps, bool _position_ik) - : chain(_chain) - , q_min(_q_min) - , q_min_mimic(chain.getNrOfJoints()) - , q_max(_q_max) - , q_max_mimic(chain.getNrOfJoints()) - , q_temp(chain.getNrOfJoints()) - , fksolver(_fksolver) - , iksolver(_iksolver) - , delta_q(_chain.getNrOfJoints()) - , maxiter(_maxiter) - , eps(_eps) - , position_ik(_position_ik) -{ - mimic_joints.resize(chain.getNrOfJoints()); - for (std::size_t i = 0; i < mimic_joints.size(); ++i) - { - mimic_joints[i].reset(i); - } - ROS_DEBUG_NAMED("lma", "Limits"); - for (std::size_t i = 0; i < q_min.rows(); ++i) - { - ROS_DEBUG_NAMED("lma", "%ld: Min: %f, Max: %f", long(i), q_min(i), q_max(i)); - } - ROS_DEBUG_NAMED("lma", " "); -} - -void ChainIkSolverPos_LMA_JL_Mimic::updateInternalDataStructures() -{ - // TODO: move (re)allocation of any internal data structures here - // to react to changes in chain -} - -bool ChainIkSolverPos_LMA_JL_Mimic::setMimicJoints(const std::vector& _mimic_joints) -{ - if (_mimic_joints.size() != chain.getNrOfJoints()) - { - ROS_ERROR_NAMED("lma", "Mimic Joint info should be same size as number of joints in chain: %d", - chain.getNrOfJoints()); - return false; - } - - for (std::size_t i = 0; i < _mimic_joints.size(); ++i) - { - if (_mimic_joints[i].map_index >= chain.getNrOfJoints()) - { - ROS_ERROR_NAMED("lma", "Mimic Joint index should be less than number of joints in chain: %d", - chain.getNrOfJoints()); - return false; - } - } - mimic_joints = _mimic_joints; - - // Note that q_min and q_max will be of size chain.getNrOfJoints() - num_mimic_joints - // qToqMimic(q_min,q_min_mimic); - // qToqMimic(q_max,q_max_mimic); - - ROS_DEBUG_NAMED("lma", "Set mimic joints"); - return true; -} - -void ChainIkSolverPos_LMA_JL_Mimic::qToqMimic(const JntArray& q, JntArray& q_result) -{ - for (std::size_t i = 0; i < chain.getNrOfJoints(); ++i) - { - q_result(i) = mimic_joints[i].offset + mimic_joints[i].multiplier * q(mimic_joints[i].map_index); - } -} - -void ChainIkSolverPos_LMA_JL_Mimic::qMimicToq(const JntArray& q, JntArray& q_result) -{ - for (std::size_t i = 0; i < chain.getNrOfJoints(); ++i) - { - if (mimic_joints[i].active) // This is not a mimic joint - { - q_result(mimic_joints[i].map_index) = q(i); - } - } -} - -int ChainIkSolverPos_LMA_JL_Mimic::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out) -{ - return CartToJntAdvanced(q_init, p_in, q_out, false); -} - -void ChainIkSolverPos_LMA_JL_Mimic::harmonize(JntArray& q_out) -{ - for (size_t i = 0; i < chain.getNrOfJoints(); ++i) - { - // Harmonize - while (q_out(i) > 2 * M_PI) - q_out(i) -= 2 * M_PI; - - while (q_out(i) < -2 * M_PI) - q_out(i) += 2 * M_PI; - } -} - -bool ChainIkSolverPos_LMA_JL_Mimic::obeysLimits(const KDL::JntArray& q_out) -{ - bool obeys_limits = true; - for (size_t i = 0; i < chain.getNrOfJoints(); i++) - { - if ((q_out(i) < (q_min(i) - 0.0001)) || (q_out(i) > (q_max(i) + 0.0001))) - { - // One element of solution is not within limits - obeys_limits = false; - ROS_DEBUG_STREAM_NAMED("lma", "Not in limits! " << i << " value " << q_out(i) << " has limit being " << q_min(i) - << " to " << q_max(i)); - break; - } - } - return obeys_limits; -} - -int ChainIkSolverPos_LMA_JL_Mimic::CartToJntAdvanced(const JntArray& q_init, const Frame& p_in, JntArray& q_out, - bool lock_redundant_joints) -{ - int ik_valid = iksolver.CartToJnt(q_init, p_in, q_out); - harmonize(q_out); - - if (!obeysLimits(q_out)) - ik_valid = -4; // Doesn't obey the joint limits - - return ik_valid; -} - -ChainIkSolverPos_LMA_JL_Mimic::~ChainIkSolverPos_LMA_JL_Mimic() -{ -} - -} // namespace diff --git a/moveit_kinematics/lma_kinematics_plugin/src/chainiksolver_vel_pinv_mimic.cpp b/moveit_kinematics/lma_kinematics_plugin/src/chainiksolver_vel_pinv_mimic.cpp deleted file mode 100644 index fbdbdff8c9a..00000000000 --- a/moveit_kinematics/lma_kinematics_plugin/src/chainiksolver_vel_pinv_mimic.cpp +++ /dev/null @@ -1,343 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, CRI group, NTU, Singapore - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of CRI group nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Francisco Suarez-Ruiz */ - -#include -#include - -namespace KDL -{ -ChainIkSolverVel_pinv_mimic::ChainIkSolverVel_pinv_mimic(const Chain& _chain, int _num_mimic_joints, - int _num_redundant_joints, bool _position_ik, double _eps, - int _maxiter) - : chain(_chain) - , jnt2jac(chain) - , jac(chain.getNrOfJoints()) - , U(6, JntArray(chain.getNrOfJoints() - _num_mimic_joints)) - , S(chain.getNrOfJoints() - _num_mimic_joints) - , V(chain.getNrOfJoints() - _num_mimic_joints, JntArray(chain.getNrOfJoints() - _num_mimic_joints)) - , tmp(chain.getNrOfJoints() - _num_mimic_joints) - , jac_reduced(chain.getNrOfJoints() - _num_mimic_joints) - , qdot_out_reduced(chain.getNrOfJoints() - _num_mimic_joints) - , U_translate(MatrixXd::Zero(3, chain.getNrOfJoints() - _num_mimic_joints)) - , S_translate(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints)) - , V_translate(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints, chain.getNrOfJoints() - _num_mimic_joints)) - , tmp_translate(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints)) - , jac_locked(chain.getNrOfJoints() - _num_redundant_joints - _num_mimic_joints) - , qdot_out_reduced_locked(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints) - , qdot_out_locked(chain.getNrOfJoints() - _num_redundant_joints) - , svd(jac_reduced) - , eps(_eps) - , maxiter(_maxiter) - , num_mimic_joints(_num_mimic_joints) - , position_ik(_position_ik) - , U_locked(MatrixXd::Zero(6, chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints)) - , S_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints)) - , V_locked(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints, - chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints)) - , tmp_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints)) - , U_translate_locked(MatrixXd::Zero(3, chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints)) - , S_translate_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints)) - , V_translate_locked(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints, - chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints)) - , tmp_translate_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints)) - , num_redundant_joints(_num_redundant_joints) - , redundant_joints_locked(false) -{ - mimic_joints_.resize(chain.getNrOfJoints()); - for (std::size_t i = 0; i < mimic_joints_.size(); ++i) - mimic_joints_[i].reset(i); -} - -void ChainIkSolverVel_pinv_mimic::updateInternalDataStructures() -{ - // TODO: move (re)allocation of any internal data structures here - // to react to changes in chain -} - -ChainIkSolverVel_pinv_mimic::~ChainIkSolverVel_pinv_mimic() -{ -} - -bool ChainIkSolverVel_pinv_mimic::setMimicJoints(const std::vector& mimic_joints) -{ - if (mimic_joints.size() != chain.getNrOfJoints()) - return false; - - for (std::size_t i = 0; i < mimic_joints.size(); ++i) - { - if (mimic_joints[i].map_index >= chain.getNrOfJoints()) - return false; - } - mimic_joints_ = mimic_joints; - return true; -} - -bool ChainIkSolverVel_pinv_mimic::setRedundantJointsMapIndex( - const std::vector& redundant_joints_map_index) -{ - if (redundant_joints_map_index.size() != chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints) - { - ROS_ERROR("Map index size: %d does not match expected size. " - "No. of joints: %d, num_mimic_joints: %d, num_redundant_joints: %d", - (int)redundant_joints_map_index.size(), (int)chain.getNrOfJoints(), (int)num_mimic_joints, - (int)num_redundant_joints); - return false; - } - - for (std::size_t i = 0; i < redundant_joints_map_index.size(); ++i) - { - if (redundant_joints_map_index[i] >= chain.getNrOfJoints() - num_mimic_joints) - return false; - } - locked_joints_map_index = redundant_joints_map_index; - return true; -} - -bool ChainIkSolverVel_pinv_mimic::jacToJacReduced(const Jacobian& jac, Jacobian& jac_reduced_l) -{ - jac_reduced_l.data.setZero(); - for (std::size_t i = 0; i < chain.getNrOfJoints(); ++i) - { - Twist vel1 = jac_reduced_l.getColumn(mimic_joints_[i].map_index); - Twist vel2 = jac.getColumn(i); - Twist result = vel1 + (mimic_joints_[i].multiplier * vel2); - jac_reduced_l.setColumn(mimic_joints_[i].map_index, result); - } - return true; -} - -bool ChainIkSolverVel_pinv_mimic::jacToJacLocked(const Jacobian& jac, Jacobian& jac_locked) -{ - jac_locked.data.setZero(); - for (std::size_t i = 0; i < chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints; ++i) - { - jac_locked.setColumn(i, jac.getColumn(locked_joints_map_index[i])); - } - return true; -} - -int ChainIkSolverVel_pinv_mimic::CartToJntRedundant(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out) -{ - qdot_out.data.setZero(); - // Let the ChainJntToJacSolver calculate the jacobian "jac" for - // the current joint positions "q_in". This will include the mimic joints - if (num_mimic_joints > 0) - { - jnt2jac.JntToJac(q_in, jac); - // Now compute the actual jacobian that involves only the active DOFs - jacToJacReduced(jac, jac_reduced); - } - else - jnt2jac.JntToJac(q_in, jac_reduced); - - // Now compute the jacobian with redundant joints locked - jacToJacLocked(jac_reduced, jac_locked); - - // Do a singular value decomposition of "jac" with maximum - // iterations "maxiter", put the results in "U", "S" and "V" - // jac = U*S*Vt - - int ret; - if (!position_ik) - ret = svd_eigen_HH(jac_locked.data, U_locked, S_locked, V_locked, tmp_locked, maxiter); - else - ret = - svd_eigen_HH(jac_locked.data.topLeftCorner(3, chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints), - U_translate_locked, S_translate_locked, V_translate_locked, tmp_translate_locked, maxiter); - - double sum; - unsigned int i, j; - - // We have to calculate qdot_out = jac_pinv*v_in - // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut): - // qdot_out = V*S_pinv*Ut*v_in - - unsigned int rows; - if (!position_ik) - rows = jac_locked.rows(); - else - rows = 3; - - // first we calculate Ut*v_in - for (i = 0; i < jac_locked.columns(); i++) - { - sum = 0.0; - for (j = 0; j < rows; j++) - { - if (!position_ik) - sum += U_locked(j, i) * v_in(j); - else - sum += U_translate_locked(j, i) * v_in(j); - } - // If the singular value is too small ( 0) - qdot_out_reduced_locked(i) = sum; - else - qdot_out_locked(i) = sum; - } - ROS_DEBUG_STREAM_NAMED("lma", "Solution:"); - - if (num_mimic_joints > 0) - { - for (i = 0; i < chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints; ++i) - { - qdot_out_reduced(locked_joints_map_index[i]) = qdot_out_reduced_locked(i); - } - for (i = 0; i < chain.getNrOfJoints(); ++i) - { - qdot_out(i) = qdot_out_reduced(mimic_joints_[i].map_index) * mimic_joints_[i].multiplier; - } - } - else - { - for (i = 0; i < chain.getNrOfJoints() - num_redundant_joints; ++i) - { - qdot_out(locked_joints_map_index[i]) = qdot_out_locked(i); - } - } - // Reset the flag - // redundant_joints_locked = false; - // return the return value of the svd decomposition - return ret; -} - -int ChainIkSolverVel_pinv_mimic::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out) -{ - if (redundant_joints_locked) - return CartToJntRedundant(q_in, v_in, qdot_out); - - // Let the ChainJntToJacSolver calculate the jacobian "jac" for - // the current joint positions "q_in". This will include the mimic joints - if (num_mimic_joints > 0) - { - jnt2jac.JntToJac(q_in, jac); - // Now compute the actual jacobian that involves only the active DOFs - jacToJacReduced(jac, jac_reduced); - } - else - jnt2jac.JntToJac(q_in, jac_reduced); - - // Do a singular value decomposition of "jac" with maximum - // iterations "maxiter", put the results in "U", "S" and "V" - // jac = U*S*Vt - - int ret; - if (!position_ik) - ret = svd.calculate(jac_reduced, U, S, V, maxiter); - else - ret = svd_eigen_HH(jac_reduced.data.topLeftCorner(3, chain.getNrOfJoints() - num_mimic_joints), U_translate, - S_translate, V_translate, tmp_translate, maxiter); - - double sum; - unsigned int i, j; - - // We have to calculate qdot_out = jac_pinv*v_in - // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut): - // qdot_out = V*S_pinv*Ut*v_in - - unsigned int rows; - if (!position_ik) - rows = jac_reduced.rows(); - else - rows = 3; - - // first we calculate Ut*v_in - for (i = 0; i < jac_reduced.columns(); i++) - { - sum = 0.0; - for (j = 0; j < rows; j++) - { - if (!position_ik) - sum += U[j](i) * v_in(j); - else - sum += U_translate(j, i) * v_in(j); - } - // If the singular value is too small ( 0) - qdot_out_reduced(i) = sum; - else - qdot_out(i) = sum; - } - ROS_DEBUG_STREAM_NAMED("lma", "Solution:"); - if (num_mimic_joints > 0) - { - for (i = 0; i < chain.getNrOfJoints(); ++i) - { - qdot_out(i) = qdot_out_reduced(mimic_joints_[i].map_index) * mimic_joints_[i].multiplier; - } - } - // return the return value of the svd decomposition - return ret; -} -} diff --git a/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp b/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp index bc47905ba75..ad0d48f1d7d 100644 --- a/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp +++ b/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp @@ -35,86 +35,45 @@ /* Author: Francisco Suarez-Ruiz */ #include -#include +#include +#include #include #include +#include +#include // URDF, SRDF #include #include -// register KDLKinematics as a KinematicsBase implementation +// register as a KinematicsBase implementation +#include CLASS_LOADER_REGISTER_CLASS(lma_kinematics_plugin::LMAKinematicsPlugin, kinematics::KinematicsBase) namespace lma_kinematics_plugin { -LMAKinematicsPlugin::LMAKinematicsPlugin() : active_(false) +LMAKinematicsPlugin::LMAKinematicsPlugin() : initialized_(false) { } -void LMAKinematicsPlugin::getRandomConfiguration(KDL::JntArray& jnt_array, bool lock_redundancy) const +void LMAKinematicsPlugin::getRandomConfiguration(Eigen::VectorXd& jnt_array) const { - std::vector jnt_array_vector(dimension_, 0.0); state_->setToRandomPositions(joint_model_group_); - state_->copyJointGroupPositions(joint_model_group_, &jnt_array_vector[0]); - for (std::size_t i = 0; i < dimension_; ++i) - { - if (lock_redundancy) - if (isRedundantJoint(i)) - continue; - jnt_array(i) = jnt_array_vector[i]; - } + state_->copyJointGroupPositions(joint_model_group_, &jnt_array[0]); } -bool LMAKinematicsPlugin::isRedundantJoint(unsigned int index) const -{ - for (std::size_t j = 0; j < redundant_joint_indices_.size(); ++j) - if (redundant_joint_indices_[j] == index) - return true; - return false; -} - -void LMAKinematicsPlugin::getRandomConfiguration(const KDL::JntArray& seed_state, +void LMAKinematicsPlugin::getRandomConfiguration(const Eigen::VectorXd& seed_state, const std::vector& consistency_limits, - KDL::JntArray& jnt_array, bool lock_redundancy) const + Eigen::VectorXd& jnt_array) const { - std::vector values(dimension_, 0.0); - std::vector near(dimension_, 0.0); - for (std::size_t i = 0; i < dimension_; ++i) - near[i] = seed_state(i); - - // Need to resize the consistency limits to remove mimic joints - std::vector consistency_limits_mimic; - for (std::size_t i = 0; i < dimension_; ++i) - { - if (!mimic_joints_[i].active) - continue; - consistency_limits_mimic.push_back(consistency_limits[i]); - } - - joint_model_group_->getVariableRandomPositionsNearBy(state_->getRandomNumberGenerator(), values, near, - consistency_limits_mimic); - - for (std::size_t i = 0; i < dimension_; ++i) - { - bool skip = false; - if (lock_redundancy) - for (std::size_t j = 0; j < redundant_joint_indices_.size(); ++j) - if (redundant_joint_indices_[j] == i) - { - skip = true; - break; - } - if (skip) - continue; - jnt_array(i) = values[i]; - } + joint_model_group_->getVariableRandomPositionsNearBy(state_->getRandomNumberGenerator(), &jnt_array[0], + &seed_state[0], consistency_limits); } -bool LMAKinematicsPlugin::checkConsistency(const KDL::JntArray& seed_state, +bool LMAKinematicsPlugin::checkConsistency(const Eigen::VectorXd& seed_state, const std::vector& consistency_limits, - const KDL::JntArray& solution) const + const Eigen::VectorXd& solution) const { for (std::size_t i = 0; i < dimension_; ++i) if (fabs(seed_state(i) - solution(i)) > consistency_limits[i]) @@ -127,16 +86,16 @@ bool LMAKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model double search_discretization) { storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization); - const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name); - if (!joint_model_group) + joint_model_group_ = robot_model_->getJointModelGroup(group_name); + if (!joint_model_group_) return false; - if (!joint_model_group->isChain()) + if (!joint_model_group_->isChain()) { ROS_ERROR_NAMED("lma", "Group '%s' is not a chain", group_name.c_str()); return false; } - if (!joint_model_group->isSingleDOFJoints()) + if (!joint_model_group_->isSingleDOFJoints()) { ROS_ERROR_NAMED("lma", "Group '%s' includes joints that have more than 1 DOF", group_name.c_str()); return false; @@ -155,203 +114,57 @@ bool LMAKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model return false; } - dimension_ = joint_model_group->getActiveJointModels().size() + joint_model_group->getMimicJointModels().size(); - for (std::size_t i = 0; i < joint_model_group->getJointModels().size(); ++i) + dimension_ = joint_model_group_->getJointModels().size(); + for (std::size_t i = 0; i < joint_model_group_->getJointModels().size(); ++i) { - if (joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::REVOLUTE || - joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::PRISMATIC) + if (joint_model_group_->getJointModels()[i]->getType() == moveit::core::JointModel::REVOLUTE || + joint_model_group_->getJointModels()[i]->getType() == moveit::core::JointModel::PRISMATIC) { - ik_chain_info_.joint_names.push_back(joint_model_group->getJointModelNames()[i]); + solver_info_.joint_names.push_back(joint_model_group_->getJointModelNames()[i]); const std::vector& jvec = - joint_model_group->getJointModels()[i]->getVariableBoundsMsg(); - ik_chain_info_.limits.insert(ik_chain_info_.limits.end(), jvec.begin(), jvec.end()); + joint_model_group_->getJointModels()[i]->getVariableBoundsMsg(); + solver_info_.limits.insert(solver_info_.limits.end(), jvec.begin(), jvec.end()); } } - fk_chain_info_.joint_names = ik_chain_info_.joint_names; - fk_chain_info_.limits = ik_chain_info_.limits; - - if (!joint_model_group->hasLinkModel(getTipFrame())) + if (!joint_model_group_->hasLinkModel(getTipFrame())) { ROS_ERROR_NAMED("lma", "Could not find tip name in joint group '%s'", group_name.c_str()); return false; } - ik_chain_info_.link_names.push_back(getTipFrame()); - fk_chain_info_.link_names = joint_model_group->getLinkModelNames(); + solver_info_.link_names.push_back(getTipFrame()); - joint_min_.resize(ik_chain_info_.limits.size()); - joint_max_.resize(ik_chain_info_.limits.size()); + joint_min_.resize(solver_info_.limits.size()); + joint_max_.resize(solver_info_.limits.size()); - for (unsigned int i = 0; i < ik_chain_info_.limits.size(); i++) + for (unsigned int i = 0; i < solver_info_.limits.size(); i++) { - joint_min_(i) = ik_chain_info_.limits[i].min_position; - joint_max_(i) = ik_chain_info_.limits[i].max_position; + joint_min_(i) = solver_info_.limits[i].min_position; + joint_max_(i) = solver_info_.limits[i].max_position; } // Get Solver Parameters - int max_solver_iterations; - double epsilon; - bool position_ik; + lookupParam("max_solver_iterations", max_solver_iterations_, 500); + lookupParam("epsilon", epsilon_, 1e-5); + lookupParam("orientation_vs_position", orientation_vs_position_weight_, 0.01); - lookupParam("max_solver_iterations", max_solver_iterations, 500); - lookupParam("epsilon", epsilon, 1e-5); + bool position_ik; lookupParam("position_only_ik", position_ik, false); - ROS_DEBUG_NAMED("lma", "Looking for param name: position_only_ik"); - - if (position_ik) + if (position_ik) // position_only_ik overrules orientation_vs_position + orientation_vs_position_weight_ = 0.0; + if (orientation_vs_position_weight_ == 0.0) ROS_INFO_NAMED("lma", "Using position only ik"); - num_possible_redundant_joints_ = - kdl_chain_.getNrOfJoints() - joint_model_group->getMimicJointModels().size() - (position_ik ? 3 : 6); - - // Check for mimic joints - std::vector redundant_joints_map_index; - - std::vector mimic_joints; - unsigned int joint_counter = 0; - for (std::size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i) - { - const robot_model::JointModel* jm = robot_model_->getJointModel(kdl_chain_.segments[i].getJoint().getName()); - - // first check whether it belongs to the set of active joints in the group - if (jm->getMimic() == NULL && jm->getVariableCount() > 0) - { - JointMimic mimic_joint; - mimic_joint.reset(joint_counter); - mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName(); - mimic_joint.active = true; - mimic_joints.push_back(mimic_joint); - ++joint_counter; - continue; - } - if (joint_model_group->hasJointModel(jm->getName())) - { - if (jm->getMimic() && joint_model_group->hasJointModel(jm->getMimic()->getName())) - { - JointMimic mimic_joint; - mimic_joint.reset(joint_counter); - mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName(); - mimic_joint.offset = jm->getMimicOffset(); - mimic_joint.multiplier = jm->getMimicFactor(); - mimic_joints.push_back(mimic_joint); - continue; - } - } - } - for (std::size_t i = 0; i < mimic_joints.size(); ++i) - { - if (!mimic_joints[i].active) - { - const robot_model::JointModel* joint_model = - joint_model_group->getJointModel(mimic_joints[i].joint_name)->getMimic(); - for (std::size_t j = 0; j < mimic_joints.size(); ++j) - { - if (mimic_joints[j].joint_name == joint_model->getName()) - { - mimic_joints[i].map_index = mimic_joints[j].map_index; - } - } - } - } - mimic_joints_ = mimic_joints; - // Setup the joint state groups that we need state_.reset(new robot_state::RobotState(robot_model_)); - // Store things for when the set of redundant joints may change - position_ik_ = position_ik; - joint_model_group_ = joint_model_group; - max_solver_iterations_ = max_solver_iterations; - epsilon_ = epsilon; - - active_ = true; - ROS_DEBUG_NAMED("lma", "KDL solver initialized"); - return true; -} - -bool LMAKinematicsPlugin::setRedundantJoints(const std::vector& redundant_joints) -{ - if (num_possible_redundant_joints_ < 0) - { - ROS_ERROR_NAMED("lma", "This group cannot have redundant joints"); - return false; - } - if (int(redundant_joints.size()) > num_possible_redundant_joints_) - { - ROS_ERROR_NAMED("lma", "This group can only have %d redundant joints", num_possible_redundant_joints_); - return false; - } - /* - XmlRpc::XmlRpcValue joint_list; - if(private_handle.getParam(group_name+"/redundant_joints", joint_list)) - { - ROS_ASSERT(joint_list.getType() == XmlRpc::XmlRpcValue::TypeArray); - std::vector redundant_joints; - for (std::size_t i = 0; i < joint_list.size(); ++i) - { - ROS_ASSERT(joint_list[i].getType() == XmlRpc::XmlRpcValue::TypeString); - redundant_joints.push_back(static_cast(joint_list[i])); - ROS_INFO_NAMED("lma","Designated joint: %s as redundant joint", - redundant_joints.back().c_str()); - } - } - */ - std::vector redundant_joints_map_index; - unsigned int counter = 0; - for (std::size_t i = 0; i < dimension_; ++i) - { - bool is_redundant_joint = false; - for (std::size_t j = 0; j < redundant_joints.size(); ++j) - { - if (i == redundant_joints[j]) - { - is_redundant_joint = true; - counter++; - break; - } - } - if (!is_redundant_joint) - { - // check for mimic - if (mimic_joints_[i].active) - { - redundant_joints_map_index.push_back(counter); - counter++; - } - } - } - for (std::size_t i = 0; i < redundant_joints_map_index.size(); ++i) - ROS_DEBUG_NAMED("lma", "Redundant joint map index: %d %d", (int)i, (int)redundant_joints_map_index[i]); + fk_solver.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_)); - redundant_joints_map_index_ = redundant_joints_map_index; - redundant_joint_indices_ = redundant_joints; + initialized_ = true; + ROS_DEBUG_NAMED("lma", "LMA solver initialized"); return true; } -int LMAKinematicsPlugin::getJointIndex(const std::string& name) const -{ - for (unsigned int i = 0; i < ik_chain_info_.joint_names.size(); i++) - { - if (ik_chain_info_.joint_names[i] == name) - return i; - } - return -1; -} - -int LMAKinematicsPlugin::getKDLSegmentIndex(const std::string& name) const -{ - int i = 0; - while (i < (int)kdl_chain_.getNrOfSegments()) - { - if (kdl_chain_.getSegment(i).getName() == name) - { - return i + 1; - } - i++; - } - return -1; -} - bool LMAKinematicsPlugin::timedOut(const ros::WallTime& start_time, double duration) const { return ((ros::WallTime::now() - start_time).toSec() >= duration); @@ -361,11 +174,11 @@ bool LMAKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, cons std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const { - const IKCallbackFn solution_callback = 0; std::vector consistency_limits; - return searchPositionIK(ik_pose, ik_seed_state, default_timeout_, solution, solution_callback, error_code, - consistency_limits, options); + // limit search to a single attempt by setting a timeout of zero + return searchPositionIK(ik_pose, ik_seed_state, 0.0, solution, IKCallbackFn(), error_code, consistency_limits, + options); } bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, @@ -373,10 +186,9 @@ bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c moveit_msgs::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const { - const IKCallbackFn solution_callback = 0; std::vector consistency_limits; - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, IKCallbackFn(), error_code, consistency_limits, options); } @@ -385,8 +197,7 @@ bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const { - const IKCallbackFn solution_callback = 0; - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, IKCallbackFn(), error_code, consistency_limits, options); } @@ -418,10 +229,10 @@ bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c const std::vector& consistency_limits, const kinematics::KinematicsQueryOptions& options) const { - ros::WallTime n1 = ros::WallTime::now(); - if (!active_) + ros::WallTime start_time = ros::WallTime::now(); + if (!initialized_) { - ROS_ERROR_NAMED("lma", "kinematics not active"); + ROS_ERROR_NAMED("lma", "kinematics solver not initialized"); error_code.val = error_code.NO_IK_SOLUTION; return false; } @@ -442,39 +253,21 @@ bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c return false; } + Eigen::Matrix cartesian_weights; + cartesian_weights(0) = 1; + cartesian_weights(1) = 1; + cartesian_weights(2) = 1; + cartesian_weights(3) = orientation_vs_position_weight_; + cartesian_weights(4) = orientation_vs_position_weight_; + cartesian_weights(5) = orientation_vs_position_weight_; + KDL::JntArray jnt_seed_state(dimension_); KDL::JntArray jnt_pos_in(dimension_); KDL::JntArray jnt_pos_out(dimension_); + jnt_seed_state.data = Eigen::Map(ik_seed_state.data(), ik_seed_state.size()); + jnt_pos_in = jnt_seed_state; - // Build Solvers - Eigen::Matrix L; - L(0) = 1; - L(1) = 1; - L(2) = 1; - L(3) = 0.01; - L(4) = 0.01; - L(5) = 0.01; - - KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_); - KDL::ChainIkSolverPos_LMA ik_solver(kdl_chain_, L, epsilon_, max_solver_iterations_); - KDL::ChainIkSolverVel_pinv_mimic ik_solver_vel(kdl_chain_, joint_model_group_->getMimicJointModels().size(), - redundant_joint_indices_.size(), position_ik_); - KDL::ChainIkSolverPos_LMA_JL_Mimic ik_solver_pos(kdl_chain_, joint_min_, joint_max_, fk_solver, ik_solver, - max_solver_iterations_, epsilon_, position_ik_); - ik_solver_vel.setMimicJoints(mimic_joints_); - ik_solver_pos.setMimicJoints(mimic_joints_); - - if ((redundant_joint_indices_.size() > 0) && !ik_solver_vel.setRedundantJointsMapIndex(redundant_joints_map_index_)) - { - ROS_ERROR_NAMED("lma", "Could not set redundant joints"); - return false; - } - - if (options.lock_redundant_joints) - { - ik_solver_vel.lockRedundantJoints(); - } - + KDL::ChainIkSolverPos_LMA ik_solver_pos(kdl_chain_, cartesian_weights, epsilon_, max_solver_iterations_); solution.resize(dimension_); KDL::Frame pose_desired; @@ -484,67 +277,51 @@ bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c << ik_pose.position.x << " " << ik_pose.position.y << " " << ik_pose.position.z << " " << ik_pose.orientation.x << " " << ik_pose.orientation.y << " " << ik_pose.orientation.z << " " << ik_pose.orientation.w); - // Do the IK - for (unsigned int i = 0; i < dimension_; i++) - jnt_seed_state(i) = ik_seed_state[i]; - jnt_pos_in = jnt_seed_state; - - unsigned int counter(0); - while (1) + unsigned int attempt = 0; + do { - // ROS_DEBUG_NAMED("lma","Iteration: %d, time: %f, Timeout: - // %f",counter,(ros::WallTime::now()-n1).toSec(),timeout); - counter++; - if (timedOut(n1, timeout)) + ++attempt; + if (attempt > 1) // randomly re-seed after first attempt { - ROS_DEBUG_NAMED("lma", "IK timed out"); - error_code.val = error_code.TIMED_OUT; - ik_solver_vel.unlockRedundantJoints(); - return false; - } - int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out); - ROS_DEBUG_NAMED("lma", "IK valid: %d", ik_valid); - if (!consistency_limits.empty()) - { - getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_in, options.lock_redundant_joints); - if ((ik_valid < 0 && !options.return_approximate_solution) || - !checkConsistency(jnt_seed_state, consistency_limits, jnt_pos_out)) + if (options.return_consistent_solution) // don't allow re-seeding { - ROS_DEBUG_NAMED("lma", "Could not find IK solution: does not match consistency limits"); - continue; + ROS_DEBUG_STREAM_NAMED("lma", "Failed to find consistent IK solution"); + error_code.val = error_code.NO_IK_SOLUTION; + break; } + if (!consistency_limits.empty()) + getRandomConfiguration(jnt_seed_state.data, consistency_limits, jnt_pos_in.data); + else + getRandomConfiguration(jnt_pos_in.data); + ROS_DEBUG_STREAM_NAMED("lma", "New random configuration (" << attempt << "): " << jnt_pos_in); } - else + + int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out); + if (ik_valid == 0 || options.return_approximate_solution) // found acceptable solution { - getRandomConfiguration(jnt_pos_in, options.lock_redundant_joints); - ROS_DEBUG_NAMED("lma", "New random configuration"); - for (unsigned int j = 0; j < dimension_; j++) - ROS_DEBUG_NAMED("lma", "%d %f", j, jnt_pos_in(j)); + if (!consistency_limits.empty() && + !checkConsistency(jnt_seed_state.data, consistency_limits, jnt_pos_out.data)) + continue; - if (ik_valid < 0 && !options.return_approximate_solution) + Eigen::Map(solution.data(), solution.size()) = jnt_pos_out.data; + if (!solution_callback.empty()) { - ROS_DEBUG_NAMED("lma", "Could not find IK solution"); - continue; + solution_callback(ik_pose, solution, error_code); + if (error_code.val != error_code.SUCCESS) + continue; } - } - ROS_DEBUG_NAMED("lma", "Found IK solution"); - for (unsigned int j = 0; j < dimension_; j++) - solution[j] = jnt_pos_out(j); - if (!solution_callback.empty()) - solution_callback(ik_pose, solution, error_code); - else - error_code.val = error_code.SUCCESS; - if (error_code.val == error_code.SUCCESS) - { - ROS_DEBUG_STREAM_NAMED("lma", "Solved after " << counter << " iterations"); - ik_solver_vel.unlockRedundantJoints(); + // solution passed consistency check and solution callback + error_code.val = error_code.SUCCESS; + ROS_DEBUG_STREAM_NAMED("lma", "Solved after " << (ros::WallTime::now() - start_time).toSec() << " < " << timeout + << "s and " << attempt << " attempts"); return true; } - } - ROS_DEBUG_NAMED("lma", "An IK that satisifes the constraints and is collision free could not be found"); - error_code.val = error_code.NO_IK_SOLUTION; - ik_solver_vel.unlockRedundantJoints(); + } while (!timedOut(start_time, timeout)); + + ROS_DEBUG_STREAM_NAMED("lma", "IK timed out after " << (ros::WallTime::now() - start_time).toSec() << " > " << timeout + << "s and " << attempt << " attempts"); + error_code.val = error_code.TIMED_OUT; return false; } @@ -552,9 +329,9 @@ bool LMAKinematicsPlugin::getPositionFK(const std::vector& link_nam const std::vector& joint_angles, std::vector& poses) const { - if (!active_) + if (!initialized_) { - ROS_ERROR_NAMED("lma", "kinematics not active"); + ROS_ERROR_NAMED("lma", "kinematics solver not initialized"); return false; } poses.resize(link_names.size()); @@ -566,18 +343,12 @@ bool LMAKinematicsPlugin::getPositionFK(const std::vector& link_nam KDL::Frame p_out; KDL::JntArray jnt_pos_in(dimension_); - for (unsigned int i = 0; i < dimension_; i++) - { - jnt_pos_in(i) = joint_angles[i]; - } - - KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_); + jnt_pos_in.data = Eigen::Map(joint_angles.data(), joint_angles.size()); bool valid = true; for (unsigned int i = 0; i < poses.size(); i++) { - ROS_DEBUG_NAMED("lma", "End effector index: %d", getKDLSegmentIndex(link_names[i])); - if (fk_solver.JntToCart(jnt_pos_in, p_out, getKDLSegmentIndex(link_names[i])) >= 0) + if (fk_solver->JntToCart(jnt_pos_in, p_out) >= 0) { poses[i] = tf2::toMsg(p_out); } @@ -592,12 +363,12 @@ bool LMAKinematicsPlugin::getPositionFK(const std::vector& link_nam const std::vector& LMAKinematicsPlugin::getJointNames() const { - return ik_chain_info_.joint_names; + return solver_info_.joint_names; } const std::vector& LMAKinematicsPlugin::getLinkNames() const { - return ik_chain_info_.link_names; + return solver_info_.link_names; } } // namespace