chainiksolver_pos_lma_jl_mimic.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2016, CRI group, NTU, Singapore
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of CRI group nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Francisco Suarez-Ruiz */
00036 
00037 #include "moveit/lma_kinematics_plugin/chainiksolver_pos_lma_jl_mimic.h"
00038 #include <ros/console.h>
00039 
00040 namespace KDL
00041 {
00042 ChainIkSolverPos_LMA_JL_Mimic::ChainIkSolverPos_LMA_JL_Mimic(const Chain& _chain, const JntArray& _q_min,
00043                                                              const JntArray& _q_max, ChainFkSolverPos& _fksolver,
00044                                                              ChainIkSolverPos_LMA& _iksolver, unsigned int _maxiter,
00045                                                              double _eps, bool _position_ik)
00046   : chain(_chain)
00047   , q_min(_q_min)
00048   , q_min_mimic(chain.getNrOfJoints())
00049   , q_max(_q_max)
00050   , q_max_mimic(chain.getNrOfJoints())
00051   , q_temp(chain.getNrOfJoints())
00052   , fksolver(_fksolver)
00053   , iksolver(_iksolver)
00054   , delta_q(_chain.getNrOfJoints())
00055   , maxiter(_maxiter)
00056   , eps(_eps)
00057   , position_ik(_position_ik)
00058 {
00059   mimic_joints.resize(chain.getNrOfJoints());
00060   for (std::size_t i = 0; i < mimic_joints.size(); ++i)
00061   {
00062     mimic_joints[i].reset(i);
00063   }
00064   ROS_DEBUG_NAMED("lma", "Limits");
00065   for (std::size_t i = 0; i < q_min.rows(); ++i)
00066   {
00067     ROS_DEBUG_NAMED("lma", "%ld: Min: %f, Max: %f", long(i), q_min(i), q_max(i));
00068   }
00069   ROS_DEBUG_NAMED("lma", " ");
00070 }
00071 
00072 bool ChainIkSolverPos_LMA_JL_Mimic::setMimicJoints(const std::vector<lma_kinematics_plugin::JointMimic>& _mimic_joints)
00073 {
00074   if (_mimic_joints.size() != chain.getNrOfJoints())
00075   {
00076     ROS_ERROR_NAMED("lma", "Mimic Joint info should be same size as number of joints in chain: %d",
00077                     chain.getNrOfJoints());
00078     return false;
00079   }
00080 
00081   for (std::size_t i = 0; i < _mimic_joints.size(); ++i)
00082   {
00083     if (_mimic_joints[i].map_index >= chain.getNrOfJoints())
00084     {
00085       ROS_ERROR_NAMED("lma", "Mimic Joint index should be less than number of joints in chain: %d",
00086                       chain.getNrOfJoints());
00087       return false;
00088     }
00089   }
00090   mimic_joints = _mimic_joints;
00091 
00092   // Note that q_min and q_max will be of size chain.getNrOfJoints() - num_mimic_joints
00093   //  qToqMimic(q_min,q_min_mimic);
00094   //  qToqMimic(q_max,q_max_mimic);
00095 
00096   ROS_DEBUG_NAMED("lma", "Set mimic joints");
00097   return true;
00098 }
00099 
00100 void ChainIkSolverPos_LMA_JL_Mimic::qToqMimic(const JntArray& q, JntArray& q_result)
00101 {
00102   for (std::size_t i = 0; i < chain.getNrOfJoints(); ++i)
00103   {
00104     q_result(i) = mimic_joints[i].offset + mimic_joints[i].multiplier * q(mimic_joints[i].map_index);
00105   }
00106 }
00107 
00108 void ChainIkSolverPos_LMA_JL_Mimic::qMimicToq(const JntArray& q, JntArray& q_result)
00109 {
00110   for (std::size_t i = 0; i < chain.getNrOfJoints(); ++i)
00111   {
00112     if (mimic_joints[i].active)  // This is not a mimic joint
00113     {
00114       q_result(mimic_joints[i].map_index) = q(i);
00115     }
00116   }
00117 }
00118 
00119 int ChainIkSolverPos_LMA_JL_Mimic::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
00120 {
00121   return CartToJntAdvanced(q_init, p_in, q_out, false);
00122 }
00123 
00124 void ChainIkSolverPos_LMA_JL_Mimic::harmonize(JntArray& q_out)
00125 {
00126   for (size_t i = 0; i < chain.getNrOfJoints(); ++i)
00127   {
00128     // Harmonize
00129     while (q_out(i) > 2 * M_PI)
00130       q_out(i) -= 2 * M_PI;
00131 
00132     while (q_out(i) < -2 * M_PI)
00133       q_out(i) += 2 * M_PI;
00134   }
00135 }
00136 
00137 bool ChainIkSolverPos_LMA_JL_Mimic::obeysLimits(const KDL::JntArray& q_out)
00138 {
00139   bool obeys_limits = true;
00140   for (size_t i = 0; i < chain.getNrOfJoints(); i++)
00141   {
00142     if ((q_out(i) < (q_min(i) - 0.0001)) || (q_out(i) > (q_max(i) + 0.0001)))
00143     {
00144       // One element of solution is not within limits
00145       obeys_limits = false;
00146       ROS_DEBUG_STREAM_NAMED("lma", "Not in limits! " << i << " value " << q_out(i) << " has limit being  " << q_min(i)
00147                                                       << " to " << q_max(i));
00148       break;
00149     }
00150   }
00151   return obeys_limits;
00152 }
00153 
00154 int ChainIkSolverPos_LMA_JL_Mimic::CartToJntAdvanced(const JntArray& q_init, const Frame& p_in, JntArray& q_out,
00155                                                      bool lock_redundant_joints)
00156 {
00157   int ik_valid = iksolver.CartToJnt(q_init, p_in, q_out);
00158   harmonize(q_out);
00159 
00160   if (!obeysLimits(q_out))
00161     ik_valid = -4;  // Doesn't obey the joint limits
00162 
00163   return ik_valid;
00164 }
00165 
00166 ChainIkSolverPos_LMA_JL_Mimic::~ChainIkSolverPos_LMA_JL_Mimic()
00167 {
00168 }
00169 
00170 }  // namespace


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:21:29