chainiksolver_pos_nr_jl_mimic.cpp
Go to the documentation of this file.
00001 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00002 // Copyright  (C)  2008  Mikael Mayer
00003 // Copyright  (C)  2008  Julia Jesse
00004 
00005 // Version: 1.0
00006 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00007 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00008 // URL: http://www.orocos.org/kdl
00009 
00010 // This library is free software; you can redistribute it and/or
00011 // modify it under the terms of the GNU Lesser General Public
00012 // License as published by the Free Software Foundation; either
00013 // version 2.1 of the License, or (at your option) any later version.
00014 
00015 // This library is distributed in the hope that it will be useful,
00016 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00017 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018 // Lesser General Public License for more details.
00019 
00020 // You should have received a copy of the GNU Lesser General Public
00021 // License along with this library; if not, write to the Free Software
00022 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00023 
00024 // Modified to account for "mimic" joints, i.e. joints whose motion has a
00025 // linear relationship to that of another joint.
00026 // Copyright  (C)  2013  Sachin Chitta, Willow Garage
00027 
00028 #include "moveit/kdl_kinematics_plugin/chainiksolver_pos_nr_jl_mimic.hpp"
00029 #include <ros/console.h>
00030 
00031 namespace KDL
00032 {
00033 ChainIkSolverPos_NR_JL_Mimic::ChainIkSolverPos_NR_JL_Mimic(const Chain& _chain, const JntArray& _q_min,
00034                                                            const JntArray& _q_max, ChainFkSolverPos& _fksolver,
00035                                                            ChainIkSolverVel& _iksolver, unsigned int _maxiter,
00036                                                            double _eps, bool _position_ik)
00037   : chain(_chain)
00038   , q_min(_q_min)
00039   , q_min_mimic(chain.getNrOfJoints())
00040   , q_max(_q_max)
00041   , q_max_mimic(chain.getNrOfJoints())
00042   , q_temp(chain.getNrOfJoints())
00043   , fksolver(_fksolver)
00044   , iksolver(_iksolver)
00045   , delta_q(_chain.getNrOfJoints())
00046   , maxiter(_maxiter)
00047   , eps(_eps)
00048   , position_ik(_position_ik)
00049 {
00050   mimic_joints.resize(chain.getNrOfJoints());
00051   for (std::size_t i = 0; i < mimic_joints.size(); ++i)
00052   {
00053     mimic_joints[i].reset(i);
00054   }
00055   ROS_DEBUG_NAMED("kdl", "Limits");
00056   for (std::size_t i = 0; i < q_min.rows(); ++i)
00057   {
00058     ROS_DEBUG_NAMED("kdl", "%ld: Min: %f, Max: %f", long(i), q_min(i), q_max(i));
00059   }
00060   ROS_DEBUG_NAMED("kdl", " ");
00061 }
00062 
00063 bool ChainIkSolverPos_NR_JL_Mimic::setMimicJoints(const std::vector<kdl_kinematics_plugin::JointMimic>& _mimic_joints)
00064 {
00065   if (_mimic_joints.size() != chain.getNrOfJoints())
00066   {
00067     ROS_ERROR_NAMED("kdl", "Mimic Joint info should be same size as number of joints in chain: %d",
00068                     chain.getNrOfJoints());
00069     return false;
00070   }
00071 
00072   for (std::size_t i = 0; i < _mimic_joints.size(); ++i)
00073   {
00074     if (_mimic_joints[i].map_index >= chain.getNrOfJoints())
00075     {
00076       ROS_ERROR_NAMED("kdl", "Mimic Joint index should be less than number of joints in chain: %d",
00077                       chain.getNrOfJoints());
00078       return false;
00079     }
00080   }
00081   mimic_joints = _mimic_joints;
00082 
00083   // Note that q_min and q_max will be of size chain.getNrOfJoints() - num_mimic_joints
00084   //  qToqMimic(q_min,q_min_mimic);
00085   //  qToqMimic(q_max,q_max_mimic);
00086 
00087   ROS_DEBUG_NAMED("kdl", "Set mimic joints");
00088   return true;
00089 }
00090 
00091 void ChainIkSolverPos_NR_JL_Mimic::qToqMimic(const JntArray& q, JntArray& q_result)
00092 {
00093   for (std::size_t i = 0; i < chain.getNrOfJoints(); ++i)
00094   {
00095     q_result(i) = mimic_joints[i].offset + mimic_joints[i].multiplier * q(mimic_joints[i].map_index);
00096   }
00097 }
00098 
00099 void ChainIkSolverPos_NR_JL_Mimic::qMimicToq(const JntArray& q, JntArray& q_result)
00100 {
00101   for (std::size_t i = 0; i < chain.getNrOfJoints(); ++i)
00102   {
00103     if (mimic_joints[i].active)  // This is not a mimic joint
00104     {
00105       q_result(mimic_joints[i].map_index) = q(i);
00106     }
00107   }
00108 }
00109 
00110 int ChainIkSolverPos_NR_JL_Mimic::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
00111 {
00112   return CartToJntAdvanced(q_init, p_in, q_out, false);
00113 }
00114 
00115 int ChainIkSolverPos_NR_JL_Mimic::CartToJntAdvanced(const JntArray& q_init, const Frame& p_in, JntArray& q_out,
00116                                                     bool lock_redundant_joints)
00117 {
00118   //  Note that q_init and q_out will be of size chain.getNrOfJoints()
00119   //  qToqMimic(q_init,q_temp);
00120 
00121   q_temp = q_init;
00122   ROS_DEBUG_STREAM_NAMED("kdl", "Input:");
00123   for (std::size_t i = 0; i < q_out.rows(); ++i)
00124     ROS_DEBUG_NAMED("kdl", "%d: %f", (int)i, q_out(i));
00125 
00126   unsigned int i;
00127   for (i = 0; i < maxiter; ++i)
00128   {
00129     fksolver.JntToCart(q_temp, f);
00130     delta_twist = diff(f, p_in);
00131 
00132     if (position_ik)
00133     {
00134       if (fabs(delta_twist(0)) < eps && fabs(delta_twist(1)) < eps && fabs(delta_twist(2)) < eps)
00135         break;
00136     }
00137     else
00138     {
00139       if (Equal(delta_twist, Twist::Zero(), eps))
00140         break;
00141     }
00142 
00143     ROS_DEBUG_STREAM_NAMED("kdl", "delta_twist");
00144     for (std::size_t i = 0; i < 6; ++i)
00145       ROS_DEBUG_NAMED("kdl", "%d: %f", (int)i, delta_twist(i));
00146 
00147     iksolver.CartToJnt(q_temp, delta_twist, delta_q);
00148 
00149     Add(q_temp, delta_q, q_temp);
00150 
00151     ROS_DEBUG_STREAM_NAMED("kdl", "delta_q");
00152     for (std::size_t i = 0; i < delta_q.rows(); ++i)
00153       ROS_DEBUG_NAMED("kdl", "%d: %f", (int)i, delta_q(i));
00154 
00155     ROS_DEBUG_STREAM_NAMED("kdl", "q_temp");
00156     for (std::size_t i = 0; i < q_temp.rows(); ++i)
00157       ROS_DEBUG_NAMED("kdl", "%d: %f", (int)i, q_temp(i));
00158 
00159     for (std::size_t j = 0; j < q_min.rows(); ++j)
00160     {
00161       //      if(mimic_joints[j].active)
00162       if (q_temp(j) < q_min(j))
00163         q_temp(j) = q_min(j);
00164     }
00165     for (std::size_t j = 0; j < q_max.rows(); ++j)
00166     {
00167       //      if(mimic_joints[j].active)
00168       if (q_temp(j) > q_max(j))
00169         q_temp(j) = q_max(j);
00170     }
00171 
00172     //    q_out = q_temp;
00173     // Make sure limits are applied on the mimic joints to
00174     //    qMimicToq(q_temp,q_out);
00175     //    qToqMimic(q_out,q_temp);
00176   }
00177 
00178   //  qMimicToq(q_temp, q_out);
00179   q_out = q_temp;
00180   ROS_DEBUG_STREAM_NAMED("kdl", "Full Solution:");
00181   for (std::size_t i = 0; i < q_temp.rows(); ++i)
00182     ROS_DEBUG_NAMED("kdl", "%d: %f", (int)i, q_temp(i));
00183 
00184   ROS_DEBUG_STREAM_NAMED("kdl", "Actual Solution:");
00185   for (std::size_t i = 0; i < q_out.rows(); ++i)
00186     ROS_DEBUG_NAMED("kdl", "%d: %f", (int)i, q_out(i));
00187 
00188   if (i != maxiter)
00189     return 0;
00190   else
00191     return -3;
00192 }
00193 
00194 ChainIkSolverPos_NR_JL_Mimic::~ChainIkSolverPos_NR_JL_Mimic()
00195 {
00196 }
00197 }


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