moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
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#include "pseudoinverse.hpp"
29
30namespace
31{
32unsigned int countMimicJoints(const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints)
33{
34 unsigned int num_mimic = 0;
35 for (const auto& item : mimic_joints)
36 {
37 if (!item.active)
38 ++num_mimic;
39 }
40 return num_mimic;
41}
42} // namespace
43
44namespace KDL
45{
47 const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints,
48 bool position_ik, double threshold)
49 : mimic_joints_(mimic_joints)
50 , num_mimic_joints_(countMimicJoints(mimic_joints))
51 , chain_(chain)
52 , jnt2jac_(chain)
53 , position_ik_(position_ik)
54 , threshold_(threshold)
55 , jac_(chain_.getNrOfJoints())
56 , jac_reduced_(chain_.getNrOfJoints() - num_mimic_joints_)
57{
58 assert(mimic_joints_.size() == chain.getNrOfJoints());
59#ifndef NDEBUG
60 for (const auto& item : mimic_joints)
61 assert(item.map_index < chain_.getNrOfJoints());
62#endif
63}
64
66{
67 // TODO: move (re)allocation of any internal data structures here
68 // to react to changes in chain
69}
70
72
73bool 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)
87int 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 {
100 jnt2jac_.JntToJac(q_in, jac_reduced_);
101 }
102
103 // weight Jacobian
104 auto& jac = jac_reduced_.data;
105 const Eigen::Index rows = position_ik_ ? 3 : 6;
106 jac.topRows(rows) *= joint_weights.asDiagonal();
107 jac.topRows(rows).transpose() *= cartesian_weights.topRows(rows).asDiagonal();
108
109 // transform v_in to 6D Eigen::Vector
110 Eigen::Matrix<double, 6, 1> vin;
111 vin.topRows<3>() = Eigen::Map<const Eigen::Array3d>(v_in.vel.data, 3) * cartesian_weights.topRows<3>().array();
112 vin.bottomRows<3>() = Eigen::Map<const Eigen::Array3d>(v_in.rot.data, 3) * cartesian_weights.bottomRows<3>().array();
113
114 const auto jacobian = jac.topRows(rows);
115 const Eigen::VectorXd x_sol =
116 kdl_kinematics_plugin::internal::solvePseudoinverse(jacobian, vin.topRows(rows), threshold_);
117
118 if (num_mimic_joints_ > 0)
119 {
120 qdot_out_reduced_.noalias() = x_sol;
121 qdot_out_reduced_.array() *= joint_weights.array();
122 for (unsigned int i = 0; i < chain_.getNrOfJoints(); ++i)
123 qdot_out(i) = qdot_out_reduced_[mimic_joints_[i].map_index] * mimic_joints_[i].multiplier;
124 }
125 else
126 {
127 qdot_out.data.noalias() = x_sol;
128 qdot_out.data.array() *= joint_weights.array();
129 }
130
131 return 0;
132}
133} // 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)
Eigen::VectorXd solvePseudoinverse(const Eigen::Ref< const Eigen::MatrixXd > &jacobian, const Eigen::Ref< const Eigen::VectorXd > &input, double threshold)