chainiksolver_pos_lma_jl_mimic.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2016, CRI group, NTU, Singapore
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of CRI group nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Francisco Suarez-Ruiz */
36 
38 #include <ros/console.h>
39 
40 namespace KDL
41 {
43  const JntArray& _q_max, ChainFkSolverPos& _fksolver,
44  ChainIkSolverPos_LMA& _iksolver, unsigned int _maxiter,
45  double _eps, bool _position_ik)
46  : chain(_chain)
47  , q_min(_q_min)
48  , q_min_mimic(chain.getNrOfJoints())
49  , q_max(_q_max)
50  , q_max_mimic(chain.getNrOfJoints())
51  , q_temp(chain.getNrOfJoints())
52  , fksolver(_fksolver)
53  , iksolver(_iksolver)
54  , delta_q(_chain.getNrOfJoints())
55  , maxiter(_maxiter)
56  , eps(_eps)
57  , position_ik(_position_ik)
58 {
60  for (std::size_t i = 0; i < mimic_joints.size(); ++i)
61  {
62  mimic_joints[i].reset(i);
63  }
64  ROS_DEBUG_NAMED("lma", "Limits");
65  for (std::size_t i = 0; i < q_min.rows(); ++i)
66  {
67  ROS_DEBUG_NAMED("lma", "%ld: Min: %f, Max: %f", long(i), q_min(i), q_max(i));
68  }
69  ROS_DEBUG_NAMED("lma", " ");
70 }
71 
72 bool ChainIkSolverPos_LMA_JL_Mimic::setMimicJoints(const std::vector<lma_kinematics_plugin::JointMimic>& _mimic_joints)
73 {
74  if (_mimic_joints.size() != chain.getNrOfJoints())
75  {
76  ROS_ERROR_NAMED("lma", "Mimic Joint info should be same size as number of joints in chain: %d",
78  return false;
79  }
80 
81  for (std::size_t i = 0; i < _mimic_joints.size(); ++i)
82  {
83  if (_mimic_joints[i].map_index >= chain.getNrOfJoints())
84  {
85  ROS_ERROR_NAMED("lma", "Mimic Joint index should be less than number of joints in chain: %d",
87  return false;
88  }
89  }
90  mimic_joints = _mimic_joints;
91 
92  // Note that q_min and q_max will be of size chain.getNrOfJoints() - num_mimic_joints
93  // qToqMimic(q_min,q_min_mimic);
94  // qToqMimic(q_max,q_max_mimic);
95 
96  ROS_DEBUG_NAMED("lma", "Set mimic joints");
97  return true;
98 }
99 
101 {
102  for (std::size_t i = 0; i < chain.getNrOfJoints(); ++i)
103  {
104  q_result(i) = mimic_joints[i].offset + mimic_joints[i].multiplier * q(mimic_joints[i].map_index);
105  }
106 }
107 
109 {
110  for (std::size_t i = 0; i < chain.getNrOfJoints(); ++i)
111  {
112  if (mimic_joints[i].active) // This is not a mimic joint
113  {
114  q_result(mimic_joints[i].map_index) = q(i);
115  }
116  }
117 }
118 
119 int ChainIkSolverPos_LMA_JL_Mimic::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
120 {
121  return CartToJntAdvanced(q_init, p_in, q_out, false);
122 }
123 
125 {
126  for (size_t i = 0; i < chain.getNrOfJoints(); ++i)
127  {
128  // Harmonize
129  while (q_out(i) > 2 * M_PI)
130  q_out(i) -= 2 * M_PI;
131 
132  while (q_out(i) < -2 * M_PI)
133  q_out(i) += 2 * M_PI;
134  }
135 }
136 
138 {
139  bool obeys_limits = true;
140  for (size_t i = 0; i < chain.getNrOfJoints(); i++)
141  {
142  if ((q_out(i) < (q_min(i) - 0.0001)) || (q_out(i) > (q_max(i) + 0.0001)))
143  {
144  // One element of solution is not within limits
145  obeys_limits = false;
146  ROS_DEBUG_STREAM_NAMED("lma", "Not in limits! " << i << " value " << q_out(i) << " has limit being " << q_min(i)
147  << " to " << q_max(i));
148  break;
149  }
150  }
151  return obeys_limits;
152 }
153 
155  bool lock_redundant_joints)
156 {
157  int ik_valid = iksolver.CartToJnt(q_init, p_in, q_out);
158  harmonize(q_out);
159 
160  if (!obeysLimits(q_out))
161  ik_valid = -4; // Doesn't obey the joint limits
162 
163  return ik_valid;
164 }
165 
167 {
168 }
169 
170 } // namespace
std::vector< lma_kinematics_plugin::JointMimic > mimic_joints
#define ROS_DEBUG_STREAM_NAMED(name, args)
unsigned int rows() const
#define M_PI
ChainIkSolverPos_LMA_JL_Mimic(const Chain &chain, const JntArray &q_min, const JntArray &q_max, ChainFkSolverPos &fksolver, ChainIkSolverPos_LMA &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)
#define ROS_DEBUG_NAMED(name,...)
void qMimicToq(const JntArray &q, JntArray &q_result)
bool setMimicJoints(const std::vector< lma_kinematics_plugin::JointMimic > &mimic_joints)
void qToqMimic(const JntArray &q, JntArray &q_result)
unsigned int getNrOfJoints() const
#define ROS_ERROR_NAMED(name,...)
virtual int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &T_base_goal, KDL::JntArray &q_out)
virtual int CartToJnt(const JntArray &q_init, const Frame &p_in, JntArray &q_out)


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:17:53