Skip to content

Commit 71a93d1

Browse files
Levi ArmstrongLevi-Armstrong
Levi Armstrong
authored and
Levi-Armstrong
committed
Update inverse kinematics interface to return solutions versus out parameters
1 parent f33ba34 commit 71a93d1

17 files changed

+183
-256
lines changed

‎tesseract_kinematics/include/tesseract_kinematics/core/inverse_kinematics.h‎

Lines changed: 9 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
4545

4646
namespace tesseract_kinematics
4747
{
48+
using IKSolutions = std::vector<Eigen::VectorXd>;
49+
4850
/** @brief Inverse kinematics functions. */
4951
class InverseKinematics
5052
{
@@ -74,23 +76,20 @@ class InverseKinematics
7476
* @param solutions A vector of solutions, so check the size of the vector to determine the number of solutions
7577
* @param pose Transform of end-of-tip relative to root (base link)
7678
* @param seed Vector of seed joint angles (size must match number of joints in robot chain)
77-
* @return True if calculation successful, False if anything is wrong (including uninitialized)
79+
* @return A vector of solutions, If empty it failed to find a solution (including uninitialized)
7880
*/
79-
virtual bool calcInvKin(Eigen::VectorXd& solutions,
80-
const Eigen::Isometry3d& pose,
81-
const Eigen::Ref<const Eigen::VectorXd>& seed) const = 0;
81+
virtual IKSolutions calcInvKin(const Eigen::Isometry3d& pose,
82+
const Eigen::Ref<const Eigen::VectorXd>& seed) const = 0;
8283

8384
/**
8485
* @brief Calculates joint solutions given a pose for a specific link.
85-
* @param solutions A vector of solutions, so check the size of the vector to determine the number of solutions
8686
* @param pose Transform of end-of-tip relative to root (base link)
8787
* @param seed Vector of seed joint angles (size must match number of joints in robot chain)
88-
* @return True if calculation successful, False if anything is wrong (including uninitialized)
88+
* @return A vector of solutions, If empty it failed to find a solution (including uninitialized)
8989
*/
90-
virtual bool calcInvKin(Eigen::VectorXd& solutions,
91-
const Eigen::Isometry3d& pose,
92-
const Eigen::Ref<const Eigen::VectorXd>& seed,
93-
const std::string& link_name) const = 0;
90+
virtual IKSolutions calcInvKin(const Eigen::Isometry3d& pose,
91+
const Eigen::Ref<const Eigen::VectorXd>& seed,
92+
const std::string& link_name) const = 0;
9493

9594
/**
9695
* @brief Check for consistency in # and limits of joints

‎tesseract_kinematics/include/tesseract_kinematics/core/rep_inverse_kinematics.h‎

Lines changed: 7 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -69,14 +69,11 @@ class RobotWithExternalPositionerInvKin : public InverseKinematics
6969

7070
bool update() override;
7171

72-
bool calcInvKin(Eigen::VectorXd& solutions,
73-
const Eigen::Isometry3d& pose,
74-
const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
72+
IKSolutions calcInvKin(const Eigen::Isometry3d& pose, const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
7573

76-
bool calcInvKin(Eigen::VectorXd& solutions,
77-
const Eigen::Isometry3d& pose,
78-
const Eigen::Ref<const Eigen::VectorXd>& seed,
79-
const std::string& link_name) const override;
74+
IKSolutions calcInvKin(const Eigen::Isometry3d& pose,
75+
const Eigen::Ref<const Eigen::VectorXd>& seed,
76+
const std::string& link_name) const override;
8077

8178
bool checkJoints(const Eigen::Ref<const Eigen::VectorXd>& vec) const override;
8279

@@ -181,18 +178,16 @@ class RobotWithExternalPositionerInvKin : public InverseKinematics
181178
bool init(const RobotWithExternalPositionerInvKin& kin);
182179

183180
/** @brief calcFwdKin helper function */
184-
bool calcInvKinHelper(std::vector<double>& solutions,
185-
const Eigen::Isometry3d& pose,
186-
const Eigen::Ref<const Eigen::VectorXd>& seed) const;
181+
IKSolutions calcInvKinHelper(const Eigen::Isometry3d& pose, const Eigen::Ref<const Eigen::VectorXd>& seed) const;
187182

188-
void nested_ik(std::vector<double>& solutions,
183+
void nested_ik(IKSolutions& solutions,
189184
int loop_level,
190185
const std::vector<Eigen::VectorXd>& dof_range,
191186
const Eigen::Isometry3d& target_pose,
192187
Eigen::VectorXd& positioner_pose,
193188
const Eigen::Ref<const Eigen::VectorXd>& seed) const;
194189

195-
bool ikAt(std::vector<double>& solutions,
190+
void ikAt(IKSolutions& solutions,
196191
const Eigen::Isometry3d& target_pose,
197192
Eigen::VectorXd& positioner_pose,
198193
const Eigen::Ref<const Eigen::VectorXd>& seed) const;

‎tesseract_kinematics/include/tesseract_kinematics/core/rop_inverse_kinematics.h‎

Lines changed: 7 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -67,14 +67,11 @@ class RobotOnPositionerInvKin : public InverseKinematics
6767

6868
bool update() override;
6969

70-
bool calcInvKin(Eigen::VectorXd& solutions,
71-
const Eigen::Isometry3d& pose,
72-
const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
70+
IKSolutions calcInvKin(const Eigen::Isometry3d& pose, const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
7371

74-
bool calcInvKin(Eigen::VectorXd& solutions,
75-
const Eigen::Isometry3d& pose,
76-
const Eigen::Ref<const Eigen::VectorXd>& seed,
77-
const std::string& link_name) const override;
72+
IKSolutions calcInvKin(const Eigen::Isometry3d& pose,
73+
const Eigen::Ref<const Eigen::VectorXd>& seed,
74+
const std::string& link_name) const override;
7875

7976
bool checkJoints(const Eigen::Ref<const Eigen::VectorXd>& vec) const override;
8077

@@ -141,18 +138,16 @@ class RobotOnPositionerInvKin : public InverseKinematics
141138
bool init(const RobotOnPositionerInvKin& kin);
142139

143140
/** @brief calcFwdKin helper function */
144-
bool calcInvKinHelper(std::vector<double>& solutions,
145-
const Eigen::Isometry3d& pose,
146-
const Eigen::Ref<const Eigen::VectorXd>& seed) const;
141+
IKSolutions calcInvKinHelper(const Eigen::Isometry3d& pose, const Eigen::Ref<const Eigen::VectorXd>& seed) const;
147142

148-
void nested_ik(std::vector<double>& solutions,
143+
void nested_ik(IKSolutions& solutions,
149144
int loop_level,
150145
const std::vector<Eigen::VectorXd>& dof_range,
151146
const Eigen::Isometry3d& target_pose,
152147
Eigen::VectorXd& positioner_pose,
153148
const Eigen::Ref<const Eigen::VectorXd>& seed) const;
154149

155-
bool ikAt(std::vector<double>& solutions,
150+
void ikAt(IKSolutions& solutions,
156151
const Eigen::Isometry3d& target_pose,
157152
Eigen::VectorXd& positioner_pose,
158153
const Eigen::Ref<const Eigen::VectorXd>& seed) const;

‎tesseract_kinematics/include/tesseract_kinematics/core/utils.h‎

Lines changed: 20 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828

2929
#include <tesseract_common/macros.h>
3030
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
31+
#include <vector>
3132
#include <Eigen/Core>
3233
#include <Eigen/Geometry>
3334
#include <console_bridge/console.h>
@@ -285,7 +286,7 @@ createKinematicsMap(const tesseract_scene_graph::SceneGraph::ConstPtr& scene_gra
285286
* @return True if joint values are within the joint limits, otherwise false
286287
*/
287288
template <typename FloatType>
288-
inline bool isWithinLimits(const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1> >& joint_values,
289+
inline bool isWithinLimits(const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& joint_values,
289290
const Eigen::MatrixX2d& limits)
290291
{
291292
for (int i = 0; i < limits.rows(); ++i)
@@ -301,27 +302,30 @@ inline bool isWithinLimits(const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen
301302
* @param limits The joint limits of the robot
302303
*/
303304
template <typename FloatType>
304-
inline std::vector<FloatType> getRedundantSolutions(const FloatType* sol, const Eigen::MatrixX2d& limits)
305+
inline std::vector<Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>
306+
getRedundantSolutions(const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& sol,
307+
const Eigen::MatrixX2d& limits)
305308
{
306-
auto dof = static_cast<int>(limits.rows());
307309
FloatType val;
308-
std::vector<FloatType> redundant_sols;
309-
for (int i = 0; i < dof; ++i)
310+
std::vector<Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> redundant_sols;
311+
for (Eigen::Index i = 0; i < static_cast<Eigen::Index>(sol.size()); ++i)
310312
{
311313
val = sol[i];
312314
while ((val -= (2 * M_PI)) > limits(i, 0))
313315
{
314-
std::vector<FloatType> new_sol(sol, sol + dof);
315-
new_sol[static_cast<size_t>(i)] = val;
316-
redundant_sols.insert(redundant_sols.end(), new_sol.begin(), new_sol.end());
316+
Eigen::Matrix<FloatType, Eigen::Dynamic, 1> new_sol =
317+
Eigen::Map<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>(sol.data(), 6);
318+
new_sol[i] = val;
319+
redundant_sols.push_back(new_sol);
317320
}
318321

319322
val = sol[i];
320323
while ((val += (static_cast<FloatType>(2.0 * M_PI))) < limits(i, 1))
321324
{
322-
std::vector<FloatType> new_sol(sol, sol + dof);
323-
new_sol[static_cast<size_t>(i)] = val;
324-
redundant_sols.insert(redundant_sols.end(), new_sol.begin(), new_sol.end());
325+
Eigen::Matrix<FloatType, Eigen::Dynamic, 1> new_sol =
326+
Eigen::Map<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>(sol.data(), 6);
327+
new_sol[i] = val;
328+
redundant_sols.push_back(new_sol);
325329
}
326330
}
327331

@@ -338,10 +342,10 @@ inline std::vector<FloatType> getRedundantSolutions(const FloatType* sol, const
338342
* @return True if the array is valid, otherwise false
339343
*/
340344
template <typename FloatType>
341-
inline bool isValid(const FloatType* qs, int dof)
345+
inline bool isValid(const std::array<FloatType, 6>& qs)
342346
{
343-
for (int i = 0; i < dof; ++i)
344-
if (!std::isfinite(qs[i]))
347+
for (const auto& q : qs)
348+
if (!std::isfinite(q))
345349
return false;
346350

347351
return true;
@@ -353,12 +357,12 @@ inline bool isValid(const FloatType* qs, int dof)
353357
* @param dof The length of the float array
354358
*/
355359
template <typename FloatType>
356-
inline void harmonizeTowardZero(FloatType* qs, int dof)
360+
inline void harmonizeTowardZero(Eigen::Ref<Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> qs)
357361
{
358362
const static auto pi = FloatType(M_PI);
359363
const static auto two_pi = FloatType(2.0 * M_PI);
360364

361-
for (int i = 0; i < dof; i++)
365+
for (Eigen::Index i = 0; i < qs.rows(); ++i)
362366
{
363367
FloatType diff = std::fmod(qs[i] + pi, two_pi);
364368
qs[i] = (diff < 0) ? (diff + pi) : (diff - pi);

‎tesseract_kinematics/include/tesseract_kinematics/core/validate.h‎

Lines changed: 3 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -141,12 +141,10 @@ inline bool checkKinematics(const tesseract_kinematics::ForwardKinematics::Const
141141
joint_angles2[t] = M_PI / 2;
142142

143143
fwd_kin->calcFwdKin(test1, joint_angles2);
144-
Eigen::VectorXd sols;
145-
inv_kin->calcInvKin(sols, test1, seed_angles);
146-
const int num_sols = static_cast<int>(sols.size()) / nj;
147-
for (int i = 0; i < num_sols; ++i)
144+
IKSolutions sols = inv_kin->calcInvKin(test1, seed_angles);
145+
for (const auto& sol : sols)
148146
{
149-
fwd_kin->calcFwdKin(test2, sols.middleRows(nj * i, nj));
147+
fwd_kin->calcFwdKin(test2, sol);
150148

151149
if ((test1.translation() - test2.translation()).norm() > tol)
152150
{

‎tesseract_kinematics/include/tesseract_kinematics/ikfast/ikfast_inv_kin.h‎

Lines changed: 5 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -103,14 +103,11 @@ class IKFastInvKin : public InverseKinematics
103103

104104
bool update() override;
105105

106-
bool calcInvKin(Eigen::VectorXd& solutions,
107-
const Eigen::Isometry3d& pose,
108-
const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
109-
110-
bool calcInvKin(Eigen::VectorXd& solutions,
111-
const Eigen::Isometry3d& pose,
112-
const Eigen::Ref<const Eigen::VectorXd>& seed,
113-
const std::string& link_name) const override;
106+
IKSolutions calcInvKin(const Eigen::Isometry3d& pose, const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
107+
108+
IKSolutions calcInvKin(const Eigen::Isometry3d& pose,
109+
const Eigen::Ref<const Eigen::VectorXd>& seed,
110+
const std::string& link_name) const override;
114111

115112
bool checkJoints(const Eigen::Ref<const Eigen::VectorXd>& vec) const override;
116113
unsigned int numJoints() const override;

‎tesseract_kinematics/include/tesseract_kinematics/ikfast/impl/ikfast_inv_kin.hpp‎

Lines changed: 4 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -55,9 +55,7 @@ bool IKFastInvKin::update()
5555
return init(name_, base_link_name_, tip_link_name_, joint_names_, link_names_, active_link_names_, limits_);
5656
}
5757

58-
bool IKFastInvKin::calcInvKin(Eigen::VectorXd& solutions,
59-
const Eigen::Isometry3d& pose,
60-
const Eigen::Ref<const Eigen::VectorXd>& seed) const
58+
IKSolutions IKFastInvKin::calcInvKin(const Eigen::Isometry3d& pose, const Eigen::Ref<const Eigen::VectorXd>& seed) const
6159
{
6260
// Convert to ikfast data type
6361
Eigen::Transform<IkReal, 3, Eigen::Isometry> ikfast_tcp = pose.cast<IkReal>();
@@ -126,10 +124,9 @@ bool IKFastInvKin::calcInvKin(Eigen::VectorXd& solutions,
126124
return !solution_set.empty();
127125
}
128126

129-
bool IKFastInvKin::calcInvKin(Eigen::VectorXd& solutions,
130-
const Eigen::Isometry3d& pose,
131-
const Eigen::Ref<const Eigen::VectorXd>& seed,
132-
const std::string& link_name) const
127+
IKSolutions IKFastInvKin::calcInvKin(const Eigen::Isometry3d& pose,
128+
const Eigen::Ref<const Eigen::VectorXd>& seed,
129+
const std::string& link_name) const
133130
{
134131
throw std::runtime_error("IKFastInvKin::calcInvKin(Eigen::VectorXd&, const Eigen::Isometry3d&, const "
135132
"Eigen::Ref<const Eigen::VectorXd>&, const std::string&) Not Supported!");

‎tesseract_kinematics/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_lma.h‎

Lines changed: 7 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -69,14 +69,11 @@ class KDLInvKinChainLMA : public InverseKinematics
6969

7070
bool update() override;
7171

72-
bool calcInvKin(Eigen::VectorXd& solutions,
73-
const Eigen::Isometry3d& pose,
74-
const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
72+
IKSolutions calcInvKin(const Eigen::Isometry3d& pose, const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
7573

76-
bool calcInvKin(Eigen::VectorXd& solutions,
77-
const Eigen::Isometry3d& pose,
78-
const Eigen::Ref<const Eigen::VectorXd>& seed,
79-
const std::string& link_name) const override;
74+
IKSolutions calcInvKin(const Eigen::Isometry3d& pose,
75+
const Eigen::Ref<const Eigen::VectorXd>& seed,
76+
const std::string& link_name) const override;
8077

8178
bool checkJoints(const Eigen::Ref<const Eigen::VectorXd>& vec) const override;
8279

@@ -149,10 +146,9 @@ class KDLInvKinChainLMA : public InverseKinematics
149146
bool init(const KDLInvKinChainLMA& kin);
150147

151148
/** @brief calcFwdKin helper function */
152-
bool calcInvKinHelper(Eigen::VectorXd& solutions,
153-
const Eigen::Isometry3d& pose,
154-
const Eigen::Ref<const Eigen::VectorXd>& seed,
155-
int segment_num = -1) const;
149+
IKSolutions calcInvKinHelper(const Eigen::Isometry3d& pose,
150+
const Eigen::Ref<const Eigen::VectorXd>& seed,
151+
int segment_num = -1) const;
156152
};
157153

158154
} // namespace tesseract_kinematics

‎tesseract_kinematics/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr.h‎

Lines changed: 7 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -71,14 +71,11 @@ class KDLInvKinChainNR : public InverseKinematics
7171

7272
bool update() override;
7373

74-
bool calcInvKin(Eigen::VectorXd& solutions,
75-
const Eigen::Isometry3d& pose,
76-
const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
74+
IKSolutions calcInvKin(const Eigen::Isometry3d& pose, const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
7775

78-
bool calcInvKin(Eigen::VectorXd& solutions,
79-
const Eigen::Isometry3d& pose,
80-
const Eigen::Ref<const Eigen::VectorXd>& seed,
81-
const std::string& link_name) const override;
76+
IKSolutions calcInvKin(const Eigen::Isometry3d& pose,
77+
const Eigen::Ref<const Eigen::VectorXd>& seed,
78+
const std::string& link_name) const override;
8279

8380
bool checkJoints(const Eigen::Ref<const Eigen::VectorXd>& vec) const override;
8481

@@ -153,10 +150,9 @@ class KDLInvKinChainNR : public InverseKinematics
153150
bool init(const KDLInvKinChainNR& kin);
154151

155152
/** @brief calcFwdKin helper function */
156-
bool calcInvKinHelper(Eigen::VectorXd& solutions,
157-
const Eigen::Isometry3d& pose,
158-
const Eigen::Ref<const Eigen::VectorXd>& seed,
159-
int segment_num = -1) const;
153+
IKSolutions calcInvKinHelper(const Eigen::Isometry3d& pose,
154+
const Eigen::Ref<const Eigen::VectorXd>& seed,
155+
int segment_num = -1) const;
160156
};
161157

162158
} // namespace tesseract_kinematics

‎tesseract_kinematics/include/tesseract_kinematics/opw/opw_inv_kin.h‎

Lines changed: 5 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -61,14 +61,11 @@ class OPWInvKin : public InverseKinematics
6161

6262
bool update() override;
6363

64-
bool calcInvKin(Eigen::VectorXd& solutions,
65-
const Eigen::Isometry3d& pose,
66-
const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
67-
68-
bool calcInvKin(Eigen::VectorXd& solutions,
69-
const Eigen::Isometry3d& pose,
70-
const Eigen::Ref<const Eigen::VectorXd>& seed,
71-
const std::string& link_name) const override;
64+
IKSolutions calcInvKin(const Eigen::Isometry3d& pose, const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
65+
66+
IKSolutions calcInvKin(const Eigen::Isometry3d& pose,
67+
const Eigen::Ref<const Eigen::VectorXd>& seed,
68+
const std::string& link_name) const override;
7269

7370
bool checkJoints(const Eigen::Ref<const Eigen::VectorXd>& vec) const override;
7471
unsigned int numJoints() const override;

0 commit comments

Comments
 (0)