moveit2
The MoveIt Motion Planning Framework for ROS 2.
chainiksolver_vel_mimic_svd.cpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
6 // URL: http://www.orocos.org/kdl
7 
8 // This library is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 2.1 of the License, or (at your option) any later version.
12 
13 // This library is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // Lesser General Public License for more details.
17 
18 // You should have received a copy of the GNU Lesser General Public
19 // License along with this library; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
21 
22 // Modified to account for "mimic" joints, i.e. joints whose motion has a
23 // linear relationship to that of another joint.
24 // Copyright (C) 2013 Sachin Chitta, Willow Garage
25 
27 
28 namespace
29 {
30 unsigned int countMimicJoints(const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints)
31 {
32  unsigned int num_mimic = 0;
33  for (const auto& item : mimic_joints)
34  {
35  if (!item.active)
36  ++num_mimic;
37  }
38  return num_mimic;
39 }
40 } // namespace
41 
42 namespace KDL
43 {
45  const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints,
46  bool position_ik, double threshold)
47  : mimic_joints_(mimic_joints)
48  , num_mimic_joints_(countMimicJoints(mimic_joints))
49  , chain_(chain)
50  , jnt2jac_(chain)
51  // Performing a position-only IK, we just need to consider the first 3 rows of the Jacobian for SVD
52  // SVD doesn't consider mimic joints, but only their driving joints
53  , svd_(position_ik ? 3 : 6, chain_.getNrOfJoints() - num_mimic_joints_, Eigen::ComputeThinU | Eigen::ComputeThinV)
54  , jac_(chain_.getNrOfJoints())
55  , jac_reduced_(svd_.cols())
56 {
57  assert(mimic_joints_.size() == chain.getNrOfJoints());
58 #ifndef NDEBUG
59  for (const auto& item : mimic_joints)
60  assert(item.map_index < chain_.getNrOfJoints());
61 #endif
62  svd_.setThreshold(threshold);
63 }
64 
66 {
67  // TODO: move (re)allocation of any internal data structures here
68  // to react to changes in chain
69 }
70 
72 
73 bool ChainIkSolverVelMimicSVD::jacToJacReduced(const Jacobian& jac, Jacobian& jac_reduced)
74 {
75  jac_reduced.data.setZero();
76  for (std::size_t i = 0; i < chain_.getNrOfJoints(); ++i)
77  {
78  Twist vel1 = jac_reduced.getColumn(mimic_joints_[i].map_index);
79  Twist vel2 = jac.getColumn(i);
80  Twist result = vel1 + (mimic_joints_[i].multiplier * vel2);
81  jac_reduced.setColumn(mimic_joints_[i].map_index, result);
82  }
83  return true;
84 }
85 
86 // NOLINTNEXTLINE(readability-identifier-naming)
87 int ChainIkSolverVelMimicSVD::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out,
88  const Eigen::VectorXd& joint_weights,
89  const Eigen::Matrix<double, 6, 1>& cartesian_weights)
90 {
91  // Let the ChainJntToJacSolver calculate the Jacobian for the current joint positions q_in.
92  if (num_mimic_joints_ > 0)
93  {
94  jnt2jac_.JntToJac(q_in, jac_);
95  // Now compute the actual jacobian that involves only the active DOFs
96  jacToJacReduced(jac_, jac_reduced_);
97  }
98  else
99  jnt2jac_.JntToJac(q_in, jac_reduced_);
100 
101  // weight Jacobian
102  auto& jac = jac_reduced_.data;
103  const Eigen::Index rows = svd_.rows(); // only operate on position rows?
104  jac.topRows(rows) *= joint_weights.asDiagonal();
105  jac.topRows(rows).transpose() *= cartesian_weights.topRows(rows).asDiagonal();
106 
107  // transform v_in to 6D Eigen::Vector
108  Eigen::Matrix<double, 6, 1> vin;
109  vin.topRows<3>() = Eigen::Map<const Eigen::Array3d>(v_in.vel.data, 3) * cartesian_weights.topRows<3>().array();
110  vin.bottomRows<3>() = Eigen::Map<const Eigen::Array3d>(v_in.rot.data, 3) * cartesian_weights.bottomRows<3>().array();
111 
112  // Do a singular value decomposition: J = U*S*V^t
113  svd_.compute(jac.topRows(rows));
114 
115  if (num_mimic_joints_ > 0)
116  {
117  qdot_out_reduced_.noalias() = svd_.solve(vin.topRows(rows));
118  qdot_out_reduced_.array() *= joint_weights.array();
119  for (unsigned int i = 0; i < chain_.getNrOfJoints(); ++i)
120  qdot_out(i) = qdot_out_reduced_[mimic_joints_[i].map_index] * mimic_joints_[i].multiplier;
121  }
122  else
123  {
124  qdot_out.data.noalias() = svd_.solve(vin.topRows(rows));
125  qdot_out.data.array() *= joint_weights.array();
126  }
127 
128  return 0;
129 }
130 } // namespace KDL
int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out) override
ChainIkSolverVelMimicSVD(const Chain &chain_, const std::vector< kdl_kinematics_plugin::JointMimic > &mimic_joints, bool position_ik=false, double threshold=0.001)