chainiksolver_pos_nr_jl_mimic.cpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2 // Copyright (C) 2008 Mikael Mayer
3 // Copyright (C) 2008 Julia Jesse
4 
5 // Version: 1.0
6 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
7 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
8 // URL: http://www.orocos.org/kdl
9 
10 // This library is free software; you can redistribute it and/or
11 // modify it under the terms of the GNU Lesser General Public
12 // License as published by the Free Software Foundation; either
13 // version 2.1 of the License, or (at your option) any later version.
14 
15 // This library is distributed in the hope that it will be useful,
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18 // Lesser General Public License for more details.
19 
20 // You should have received a copy of the GNU Lesser General Public
21 // License along with this library; if not, write to the Free Software
22 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
23 
24 // Modified to account for "mimic" joints, i.e. joints whose motion has a
25 // linear relationship to that of another joint.
26 // Copyright (C) 2013 Sachin Chitta, Willow Garage
27 
29 #include <ros/console.h>
30 
31 namespace KDL
32 {
34  const JntArray& _q_max, ChainFkSolverPos& _fksolver,
35  ChainIkSolverVel& _iksolver, unsigned int _maxiter,
36  double _eps, bool _position_ik)
37  : chain(_chain)
38  , q_min(_q_min)
39  , q_min_mimic(chain.getNrOfJoints())
40  , q_max(_q_max)
41  , q_max_mimic(chain.getNrOfJoints())
42  , q_temp(chain.getNrOfJoints())
43  , fksolver(_fksolver)
44  , iksolver(_iksolver)
45  , delta_q(_chain.getNrOfJoints())
46  , maxiter(_maxiter)
47  , eps(_eps)
48  , position_ik(_position_ik)
49 {
51  for (std::size_t i = 0; i < mimic_joints.size(); ++i)
52  {
53  mimic_joints[i].reset(i);
54  }
55  ROS_DEBUG_NAMED("kdl", "Limits");
56  for (std::size_t i = 0; i < q_min.rows(); ++i)
57  {
58  ROS_DEBUG_NAMED("kdl", "%ld: Min: %f, Max: %f", long(i), q_min(i), q_max(i));
59  }
60  ROS_DEBUG_NAMED("kdl", " ");
61 }
62 
63 bool ChainIkSolverPos_NR_JL_Mimic::setMimicJoints(const std::vector<kdl_kinematics_plugin::JointMimic>& _mimic_joints)
64 {
65  if (_mimic_joints.size() != chain.getNrOfJoints())
66  {
67  ROS_ERROR_NAMED("kdl", "Mimic Joint info should be same size as number of joints in chain: %d",
69  return false;
70  }
71 
72  for (std::size_t i = 0; i < _mimic_joints.size(); ++i)
73  {
74  if (_mimic_joints[i].map_index >= chain.getNrOfJoints())
75  {
76  ROS_ERROR_NAMED("kdl", "Mimic Joint index should be less than number of joints in chain: %d",
78  return false;
79  }
80  }
81  mimic_joints = _mimic_joints;
82 
83  // Note that q_min and q_max will be of size chain.getNrOfJoints() - num_mimic_joints
84  // qToqMimic(q_min,q_min_mimic);
85  // qToqMimic(q_max,q_max_mimic);
86 
87  ROS_DEBUG_NAMED("kdl", "Set mimic joints");
88  return true;
89 }
90 
92 {
93  for (std::size_t i = 0; i < chain.getNrOfJoints(); ++i)
94  {
95  q_result(i) = mimic_joints[i].offset + mimic_joints[i].multiplier * q(mimic_joints[i].map_index);
96  }
97 }
98 
100 {
101  for (std::size_t i = 0; i < chain.getNrOfJoints(); ++i)
102  {
103  if (mimic_joints[i].active) // This is not a mimic joint
104  {
105  q_result(mimic_joints[i].map_index) = q(i);
106  }
107  }
108 }
109 
110 int ChainIkSolverPos_NR_JL_Mimic::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
111 {
112  return CartToJntAdvanced(q_init, p_in, q_out, false);
113 }
114 
116  bool lock_redundant_joints)
117 {
118  // Note that q_init and q_out will be of size chain.getNrOfJoints()
119  // qToqMimic(q_init,q_temp);
120 
121  q_temp = q_init;
122  ROS_DEBUG_STREAM_NAMED("kdl", "Input:");
123  for (std::size_t i = 0; i < q_out.rows(); ++i)
124  ROS_DEBUG_NAMED("kdl", "%d: %f", (int)i, q_out(i));
125 
126  unsigned int i;
127  for (i = 0; i < maxiter; ++i)
128  {
130  delta_twist = diff(f, p_in);
131 
132  if (position_ik)
133  {
134  if (fabs(delta_twist(0)) < eps && fabs(delta_twist(1)) < eps && fabs(delta_twist(2)) < eps)
135  break;
136  }
137  else
138  {
139  if (Equal(delta_twist, Twist::Zero(), eps))
140  break;
141  }
142 
143  ROS_DEBUG_STREAM_NAMED("kdl", "delta_twist");
144  for (std::size_t i = 0; i < 6; ++i)
145  ROS_DEBUG_NAMED("kdl", "%d: %f", (int)i, delta_twist(i));
146 
148 
150 
151  ROS_DEBUG_STREAM_NAMED("kdl", "delta_q");
152  for (std::size_t i = 0; i < delta_q.rows(); ++i)
153  ROS_DEBUG_NAMED("kdl", "%d: %f", (int)i, delta_q(i));
154 
155  ROS_DEBUG_STREAM_NAMED("kdl", "q_temp");
156  for (std::size_t i = 0; i < q_temp.rows(); ++i)
157  ROS_DEBUG_NAMED("kdl", "%d: %f", (int)i, q_temp(i));
158 
159  for (std::size_t j = 0; j < q_min.rows(); ++j)
160  {
161  // if(mimic_joints[j].active)
162  if (q_temp(j) < q_min(j))
163  q_temp(j) = q_min(j);
164  }
165  for (std::size_t j = 0; j < q_max.rows(); ++j)
166  {
167  // if(mimic_joints[j].active)
168  if (q_temp(j) > q_max(j))
169  q_temp(j) = q_max(j);
170  }
171 
172  // q_out = q_temp;
173  // Make sure limits are applied on the mimic joints to
174  // qMimicToq(q_temp,q_out);
175  // qToqMimic(q_out,q_temp);
176  }
177 
178  // qMimicToq(q_temp, q_out);
179  q_out = q_temp;
180  ROS_DEBUG_STREAM_NAMED("kdl", "Full Solution:");
181  for (std::size_t i = 0; i < q_temp.rows(); ++i)
182  ROS_DEBUG_NAMED("kdl", "%d: %f", (int)i, q_temp(i));
183 
184  ROS_DEBUG_STREAM_NAMED("kdl", "Actual Solution:");
185  for (std::size_t i = 0; i < q_out.rows(); ++i)
186  ROS_DEBUG_NAMED("kdl", "%d: %f", (int)i, q_out(i));
187 
188  if (i != maxiter)
189  return 0;
190  else
191  return -3;
192 }
193 
195 {
196 }
197 }
IMETHOD doubleVel diff(const doubleVel &a, const doubleVel &b, double dt=1.0)
void qMimicToq(const JntArray &q, JntArray &q_result)
static Twist Zero()
#define ROS_DEBUG_STREAM_NAMED(name, args)
unsigned int rows() const
ChainIkSolverPos_NR_JL_Mimic(const Chain &chain, const JntArray &q_min, const JntArray &q_max, ChainFkSolverPos &fksolver, ChainIkSolverVel &iksolver, unsigned int maxiter=100, double eps=1e-6, bool position_ik=false)
virtual int CartToJntAdvanced(const JntArray &q_init, const Frame &p_in, JntArray &q_out, bool lock_redundant_joints)
std::vector< kdl_kinematics_plugin::JointMimic > mimic_joints
void qToqMimic(const JntArray &q, JntArray &q_result)
void Add(const JntArrayVel &src1, const JntArrayVel &src2, JntArrayVel &dest)
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)=0
#define ROS_DEBUG_NAMED(name,...)
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)=0
virtual int CartToJnt(const JntArray &q_init, const Frame &p_in, JntArray &q_out)
bool setMimicJoints(const std::vector< kdl_kinematics_plugin::JointMimic > &mimic_joints)
unsigned int getNrOfJoints() const
#define ROS_ERROR_NAMED(name,...)


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 10 2019 04:03:41