chainiksolver_vel_mimic_svd.hpp

Go to the documentation of this file.

// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
 
// 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
 
#pragma once
 
#include <kdl/config.h>
#include <kdl/chainiksolver.hpp>
#include <kdl/chainjnttojacsolver.hpp>
 
#include <moveit/kdl_kinematics_plugin/joint_mimic.hpp>
#include <Eigen/SVD>
 
namespace KDL
{
class ChainIkSolverVelMimicSVD : public ChainIkSolverVel
{
public:
  explicit ChainIkSolverVelMimicSVD(const Chain& chain_,
                                    const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints,
                                    bool position_ik = false, double threshold = 0.001);
 
// 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
 
  ~ChainIkSolverVelMimicSVD() override;
 
  int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out) override
  {
    return CartToJnt(q_in, v_in, qdot_out, Eigen::VectorXd::Constant(svd_.cols(), 1.0),
                     Eigen::Matrix<double, 6, 1>::Constant(1.0));
  }
 
  // NOLINTNEXTLINE(readability-identifier-naming)
  int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out, const Eigen::VectorXd& joint_weights,
                const Eigen::Matrix<double, 6, 1>& cartesian_weights);
 
  int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/) override
  {
    return -1;
  }
 
  bool isPositionOnly() const
  {
    return svd_.rows() == 3;
  }
 
private:
  bool jacToJacReduced(const Jacobian& jac, Jacobian& jac_reduced);
 
  // Mimic joint specific
  const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints_;
  int num_mimic_joints_;
 
  const Chain& chain_;
  ChainJntToJacSolver jnt2jac_;
 
  Eigen::JacobiSVD<Eigen::MatrixXd> svd_;
  Eigen::VectorXd qdot_out_reduced_;
 
  Jacobian jac_;          // full Jacobian
  Jacobian jac_reduced_;  // reduced Jacobian with contributions of mimic joints mapped onto active DoFs
};
}  // namespace KDL