chainiksolver_pos_nr_jl_mimic.cpp

Go to the documentation of this file.

// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Copyright  (C)  2008  Mikael Mayer
// Copyright  (C)  2008  Julia Jesse

// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

// Modified to account for "mimic" joints, i.e. joints whose motion has a
// linear relationship to that of another joint.
// Copyright  (C)  2013  Sachin Chitta, Willow Garage

#include "moveit/kdl_kinematics_plugin/chainiksolver_pos_nr_jl_mimic.hpp"
#include <ros/console.h>

namespace KDL
{

ChainIkSolverPos_NR_JL_Mimic::ChainIkSolverPos_NR_JL_Mimic(const Chain& _chain,
                                                           const JntArray& _q_min,
                                                           const JntArray& _q_max,
                                                           ChainFkSolverPos& _fksolver,
                                                           ChainIkSolverVel& _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("kdl","Limits");
  for(std::size_t i=0; i < q_min.rows(); ++i)
  { 
    ROS_DEBUG_NAMED("kdl","%ld: Min: %f, Max: %f", long(i), q_min(i), q_max(i));
  }
  ROS_DEBUG_NAMED("kdl"," ");
}

bool ChainIkSolverPos_NR_JL_Mimic::setMimicJoints(const std::vector<kdl_kinematics_plugin::JointMimic>& _mimic_joints)
{
  if(_mimic_joints.size() != chain.getNrOfJoints())
  {
    ROS_ERROR_NAMED("kdl","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("kdl","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("kdl","Set mimic joints");
  return true;
}

void ChainIkSolverPos_NR_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_NR_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_NR_JL_Mimic::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
{
  return CartToJntAdvanced(q_init,p_in,q_out,false);
}

int ChainIkSolverPos_NR_JL_Mimic::CartToJntAdvanced(const JntArray& q_init, const Frame& p_in, JntArray& q_out, bool lock_redundant_joints)
{
  //  Note that q_init and q_out will be of size chain.getNrOfJoints()
  //  qToqMimic(q_init,q_temp);

  q_temp = q_init;
  ROS_DEBUG_STREAM_NAMED("kdl","Input:");
  for(std::size_t i=0; i < q_out.rows(); ++i)
    ROS_DEBUG_NAMED("kdl","%d: %f",(int) i,q_out(i));

  unsigned int i;
  for(i=0;i<maxiter;++i)
  {
    fksolver.JntToCart(q_temp,f);
    delta_twist = diff(f,p_in);

    if(position_ik)
    {
      if(fabs(delta_twist(0)) < eps && fabs(delta_twist(1)) < eps && fabs(delta_twist(2)) < eps)
        break;
    }
    else
    {
      if(Equal(delta_twist,Twist::Zero(),eps))
        break;
    }

    ROS_DEBUG_STREAM_NAMED("kdl","delta_twist");
    for(std::size_t i=0; i < 6; ++i)
      ROS_DEBUG_NAMED("kdl","%d: %f",(int) i, delta_twist(i));

    iksolver.CartToJnt(q_temp,delta_twist,delta_q);

    Add(q_temp,delta_q,q_temp);

    ROS_DEBUG_STREAM_NAMED("kdl","delta_q");
    for(std::size_t i=0; i < delta_q.rows(); ++i)
      ROS_DEBUG_NAMED("kdl","%d: %f",(int) i, delta_q(i));

    ROS_DEBUG_STREAM_NAMED("kdl","q_temp");
    for(std::size_t i=0; i < q_temp.rows(); ++i)
      ROS_DEBUG_NAMED("kdl","%d: %f",(int) i, q_temp(i));

    for(std::size_t j=0; j<q_min.rows(); ++j)
    {
      //      if(mimic_joints[j].active)
        if(q_temp(j) < q_min(j))
          q_temp(j) = q_min(j);
    }
    for(std::size_t j=0; j<q_max.rows(); ++j)
    {
      //      if(mimic_joints[j].active)
        if(q_temp(j) > q_max(j))
          q_temp(j) = q_max(j);
    }

    //    q_out = q_temp;
    //Make sure limits are applied on the mimic joints to
    //    qMimicToq(q_temp,q_out);
    //    qToqMimic(q_out,q_temp);

  }

  //  qMimicToq(q_temp, q_out);
  q_out = q_temp;
  ROS_DEBUG_STREAM_NAMED("kdl","Full Solution:");
  for(std::size_t i=0; i < q_temp.rows(); ++i)
    ROS_DEBUG_NAMED("kdl","%d: %f",(int) i,q_temp(i));

  ROS_DEBUG_STREAM_NAMED("kdl","Actual Solution:");
  for(std::size_t i=0; i < q_out.rows(); ++i)
    ROS_DEBUG_NAMED("kdl","%d: %f",(int) i,q_out(i));

  if(i!=maxiter)
    return 0;
  else
    return -3;
}

ChainIkSolverPos_NR_JL_Mimic::~ChainIkSolverPos_NR_JL_Mimic()
{
}
}