Skip to content

Commit 048fe97

Browse files
Levi ArmstrongLevi-Armstrong
Levi Armstrong
authored and
Levi-Armstrong
committed
Update forward kinematics interface to return solutions versus out parameters
1 parent 9bd9b23 commit 048fe97

File tree

11 files changed

+114
-175
lines changed

11 files changed

+114
-175
lines changed

‎tesseract_kinematics/include/tesseract_kinematics/core/forward_kinematics.h‎

Lines changed: 13 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -74,51 +74,49 @@ class ForwardKinematics
7474

7575
/**
7676
* @brief Calculates tool pose of robot chain
77+
* @details Throws an exception on failures (including uninitialized)
7778
* @param pose Transform of end-of-tip relative to root
7879
* @param joint_angles Vector of joint angles (size must match number of joints in robot chain)
79-
* @return True if calculation successful, False if anything is wrong (including uninitialized BasicKin)
8080
*/
81-
virtual bool calcFwdKin(Eigen::Isometry3d& pose, const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const = 0;
81+
virtual Eigen::Isometry3d calcFwdKin(const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const = 0;
8282

8383
/**
8484
* @brief Calculates pose for all links of robot chain
85+
* @details Throws an exception on failures (including uninitialized)
8586
* @param poses Transform of each link relative to root. Same order as getLinkNames()
8687
* @param joint_angles Vector of joint angles (size must match number of joints in robot chain)
87-
* @return True if calculation successful, False if anything is wrong (including uninitialized BasicKin)
8888
*/
89-
virtual bool calcFwdKin(tesseract_common::VectorIsometry3d& poses,
90-
const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const = 0;
89+
virtual tesseract_common::VectorIsometry3d
90+
calcFwdKinAll(const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const = 0;
9191

9292
/**
9393
* @brief Calculates pose for a given link
94+
* @details Throws an exception on failures (including uninitialized)
9495
* @param pose Transform of link relative to root
9596
* @param joint_angles Vector of joint angles (size must match number of joints in robot chain)
9697
* @param link_name Name of link to calculate pose which is part of the kinematics
97-
* @return True if calculation successful, False if anything is wrong (including uninitialized BasicKin)
9898
*/
99-
virtual bool calcFwdKin(Eigen::Isometry3d& pose,
100-
const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
101-
const std::string& link_name) const = 0;
99+
virtual Eigen::Isometry3d calcFwdKin(const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
100+
const std::string& link_name) const = 0;
102101

103102
/**
104103
* @brief Calculated jacobian of robot given joint angles
104+
* @details Throws an exception on failures (including uninitialized)
105105
* @param jacobian Output jacobian
106106
* @param joint_angles Input vector of joint angles
107-
* @return True if calculation successful, False if anything is wrong (including uninitialized BasicKin)
108107
*/
109-
virtual bool calcJacobian(Eigen::Ref<Eigen::MatrixXd> jacobian,
110-
const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const = 0;
108+
virtual Eigen::MatrixXd calcJacobian(const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const = 0;
111109

112110
/**
113111
* @brief Calculated jacobian at a link given joint angles
112+
* @details Throws an exception on failures (including uninitialized)
114113
* @param jacobian Output jacobian for a given link
115114
* @param joint_angles Input vector of joint angles
116115
* @param link_name Name of link to calculate jacobian
117116
* @return True if calculation successful, False if anything is wrong (including uninitialized BasicKin)
118117
*/
119-
virtual bool calcJacobian(Eigen::Ref<Eigen::MatrixXd> jacobian,
120-
const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
121-
const std::string& link_name) const = 0;
118+
virtual Eigen::MatrixXd calcJacobian(const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
119+
const std::string& link_name) const = 0;
122120

123121
/**
124122
* @brief Check for consistency in # and limits of joints

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

Lines changed: 12 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
4040

4141
namespace tesseract_kinematics
4242
{
43+
template <typename FloatType>
44+
using VectorX = Eigen::Matrix<FloatType, Eigen::Dynamic, 1>;
45+
4346
/**
4447
* @brief Change the base coordinate system of the jacobian
4548
* @param jacobian The current Jacobian which gets modified in place
@@ -90,16 +93,14 @@ inline static void numericalJacobian(Eigen::Ref<Eigen::MatrixXd> jacobian,
9093
{
9194
Eigen::VectorXd njvals;
9295
double delta = 0.001;
93-
Eigen::Isometry3d pose;
94-
kin.calcFwdKin(pose, joint_values, link_name);
96+
Eigen::Isometry3d pose = kin.calcFwdKin(joint_values, link_name);
9597
pose = change_base * pose;
9698

9799
for (int i = 0; i < static_cast<int>(joint_values.size()); ++i)
98100
{
99101
njvals = joint_values;
100102
njvals[i] += delta;
101-
Eigen::Isometry3d updated_pose;
102-
kin.calcFwdKin(updated_pose, njvals, link_name);
103+
Eigen::Isometry3d updated_pose = kin.calcFwdKin(njvals, link_name);
103104
updated_pose = change_base * updated_pose;
104105

105106
Eigen::Vector3d temp = pose * link_point;
@@ -286,8 +287,7 @@ createKinematicsMap(const tesseract_scene_graph::SceneGraph::ConstPtr& scene_gra
286287
* @return True if joint values are within the joint limits, otherwise false
287288
*/
288289
template <typename FloatType>
289-
inline bool isWithinLimits(const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& joint_values,
290-
const Eigen::MatrixX2d& limits)
290+
inline bool isWithinLimits(const Eigen::Ref<const VectorX<FloatType>>& joint_values, const Eigen::MatrixX2d& limits)
291291
{
292292
for (int i = 0; i < limits.rows(); ++i)
293293
if ((joint_values[i] < limits(i, 0)) || (joint_values[i] > limits(i, 1)))
@@ -302,28 +302,25 @@ inline bool isWithinLimits(const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen
302302
* @param limits The joint limits of the robot
303303
*/
304304
template <typename FloatType>
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)
305+
inline std::vector<VectorX<FloatType>> getRedundantSolutions(const Eigen::Ref<const VectorX<FloatType>>& sol,
306+
const Eigen::MatrixX2d& limits)
308307
{
309308
FloatType val;
310-
std::vector<Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> redundant_sols;
309+
std::vector<VectorX<FloatType>> redundant_sols;
311310
for (Eigen::Index i = 0; i < static_cast<Eigen::Index>(sol.size()); ++i)
312311
{
313312
val = sol[i];
314313
while ((val -= (2 * M_PI)) > limits(i, 0))
315314
{
316-
Eigen::Matrix<FloatType, Eigen::Dynamic, 1> new_sol =
317-
Eigen::Map<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>(sol.data(), 6);
315+
VectorX<FloatType> new_sol = sol;
318316
new_sol[i] = val;
319317
redundant_sols.push_back(new_sol);
320318
}
321319

322320
val = sol[i];
323321
while ((val += (static_cast<FloatType>(2.0 * M_PI))) < limits(i, 1))
324322
{
325-
Eigen::Matrix<FloatType, Eigen::Dynamic, 1> new_sol =
326-
Eigen::Map<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>(sol.data(), 6);
323+
VectorX<FloatType> new_sol = sol;
327324
new_sol[i] = val;
328325
redundant_sols.push_back(new_sol);
329326
}
@@ -357,7 +354,7 @@ inline bool isValid(const std::array<FloatType, 6>& qs)
357354
* @param dof The length of the float array
358355
*/
359356
template <typename FloatType>
360-
inline void harmonizeTowardZero(Eigen::Ref<Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> qs)
357+
inline void harmonizeTowardZero(Eigen::Ref<VectorX<FloatType>> qs)
361358
{
362359
const static auto pi = FloatType(M_PI);
363360
const static auto two_pi = FloatType(2.0 * M_PI);

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

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -140,11 +140,11 @@ inline bool checkKinematics(const tesseract_kinematics::ForwardKinematics::Const
140140
{
141141
joint_angles2[t] = M_PI / 2;
142142

143-
fwd_kin->calcFwdKin(test1, joint_angles2);
143+
test1 = fwd_kin->calcFwdKin(joint_angles2);
144144
IKSolutions sols = inv_kin->calcInvKin(test1, seed_angles);
145145
for (const auto& sol : sols)
146146
{
147-
fwd_kin->calcFwdKin(test2, sol);
147+
test2 = fwd_kin->calcFwdKin(sol);
148148

149149
if ((test1.translation() - test2.translation()).norm() > tol)
150150
{
@@ -267,8 +267,8 @@ inline bool checkKinematics(const tesseract_kinematics::ForwardKinematics::Const
267267
{
268268
joint_angles2[t] = M_PI / 2;
269269

270-
fwd_kin1->calcFwdKin(test1, joint_angles2);
271-
fwd_kin2->calcFwdKin(test2, joint_angles2);
270+
test1 = fwd_kin1->calcFwdKin(joint_angles2);
271+
test2 = fwd_kin2->calcFwdKin(joint_angles2);
272272

273273
if ((test1.translation() - test2.translation()).norm() > tol)
274274
{

‎tesseract_kinematics/include/tesseract_kinematics/kdl/kdl_fwd_kin_chain.h‎

Lines changed: 11 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -74,21 +74,18 @@ class KDLFwdKinChain : public ForwardKinematics
7474

7575
bool update() override;
7676

77-
bool calcFwdKin(Eigen::Isometry3d& pose, const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const override;
77+
Eigen::Isometry3d calcFwdKin(const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const override;
7878

79-
bool calcFwdKin(tesseract_common::VectorIsometry3d& poses,
80-
const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const override;
79+
tesseract_common::VectorIsometry3d
80+
calcFwdKinAll(const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const override;
8181

82-
bool calcFwdKin(Eigen::Isometry3d& pose,
83-
const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
84-
const std::string& link_name) const override;
82+
Eigen::Isometry3d calcFwdKin(const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
83+
const std::string& link_name) const override;
8584

86-
bool calcJacobian(Eigen::Ref<Eigen::MatrixXd> jacobian,
87-
const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const override;
85+
Eigen::MatrixXd calcJacobian(const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const override;
8886

89-
bool calcJacobian(Eigen::Ref<Eigen::MatrixXd> jacobian,
90-
const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
91-
const std::string& link_name) const override;
87+
Eigen::MatrixXd calcJacobian(const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
88+
const std::string& link_name) const override;
9289

9390
bool checkJoints(const Eigen::Ref<const Eigen::VectorXd>& vec) const override;
9491

@@ -162,14 +159,11 @@ class KDLFwdKinChain : public ForwardKinematics
162159
bool init(const KDLFwdKinChain& kin);
163160

164161
/** @brief calcFwdKin helper function */
165-
bool calcFwdKinHelper(Eigen::Isometry3d& pose,
166-
const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
167-
int segment_num = -1) const;
162+
Eigen::Isometry3d calcFwdKinHelper(const Eigen::Ref<const Eigen::VectorXd>& joint_angles, int segment_num = -1) const;
168163

169164
/** @brief calcFwdKin helper function */
170-
bool calcFwdKinHelper(tesseract_common::VectorIsometry3d& poses,
171-
const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
172-
int segment_num = -1) const;
165+
tesseract_common::VectorIsometry3d calcFwdKinHelperAll(const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
166+
int segment_num = -1) const;
173167

174168
/** @brief calcJacobian helper function */
175169
bool calcJacobianHelper(KDL::Jacobian& jacobian,

‎tesseract_kinematics/include/tesseract_kinematics/kdl/kdl_fwd_kin_tree.h‎

Lines changed: 9 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -72,21 +72,18 @@ class KDLFwdKinTree : public ForwardKinematics
7272

7373
bool update() override;
7474

75-
bool calcFwdKin(Eigen::Isometry3d& pose, const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const override;
75+
Eigen::Isometry3d calcFwdKin(const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const override;
7676

77-
bool calcFwdKin(tesseract_common::VectorIsometry3d& poses,
78-
const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const override;
77+
tesseract_common::VectorIsometry3d
78+
calcFwdKinAll(const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const override;
7979

80-
bool calcFwdKin(Eigen::Isometry3d& pose,
81-
const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
82-
const std::string& link_name) const override;
80+
Eigen::Isometry3d calcFwdKin(const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
81+
const std::string& link_name) const override;
8382

84-
bool calcJacobian(Eigen::Ref<Eigen::MatrixXd> jacobian,
85-
const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const override;
83+
Eigen::MatrixXd calcJacobian(const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const override;
8684

87-
bool calcJacobian(Eigen::Ref<Eigen::MatrixXd> jacobian,
88-
const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
89-
const std::string& link_name) const override;
85+
Eigen::MatrixXd calcJacobian(const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
86+
const std::string& link_name) const override;
9087

9188
bool checkJoints(const Eigen::Ref<const Eigen::VectorXd>& vec) const override;
9289

@@ -164,7 +161,7 @@ class KDLFwdKinTree : public ForwardKinematics
164161
const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const;
165162

166163
/** @brief calcFwdKin helper function */
167-
bool calcFwdKinHelper(Eigen::Isometry3d& pose, const KDL::JntArray& kdl_joints, const std::string& link_name) const;
164+
Eigen::Isometry3d calcFwdKinHelper(const KDL::JntArray& kdl_joints, const std::string& link_name) const;
168165

169166
/** @brief calcJacobian helper function */
170167
bool calcJacobianHelper(KDL::Jacobian& jacobian, const KDL::JntArray& kdl_joints, const std::string& link_name) const;

‎tesseract_kinematics/src/core/rep_inverse_kinematics.cpp‎

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -83,10 +83,7 @@ void RobotWithExternalPositionerInvKin::ikAt(IKSolutions& solutions,
8383
Eigen::VectorXd& positioner_pose,
8484
const Eigen::Ref<const Eigen::VectorXd>& seed) const
8585
{
86-
Eigen::Isometry3d positioner_tf;
87-
if (!positioner_fwd_kin_->calcFwdKin(positioner_tf, positioner_pose))
88-
return;
89-
86+
Eigen::Isometry3d positioner_tf = positioner_fwd_kin_->calcFwdKin(positioner_pose);
9087
Eigen::Isometry3d robot_target_pose = manip_base_to_positioner_base_ * positioner_tf * target_pose;
9188
if (robot_target_pose.translation().norm() > manip_reach_)
9289
return;

‎tesseract_kinematics/src/core/rop_inverse_kinematics.cpp‎

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -83,10 +83,7 @@ void RobotOnPositionerInvKin::ikAt(IKSolutions& solutions,
8383
Eigen::VectorXd& positioner_pose,
8484
const Eigen::Ref<const Eigen::VectorXd>& seed) const
8585
{
86-
Eigen::Isometry3d positioner_tf;
87-
if (!positioner_fwd_kin_->calcFwdKin(positioner_tf, positioner_pose))
88-
return;
89-
86+
Eigen::Isometry3d positioner_tf = positioner_fwd_kin_->calcFwdKin(positioner_pose);
9087
Eigen::Isometry3d robot_target_pose = positioner_tf.inverse() * target_pose;
9188
if (robot_target_pose.translation().norm() > manip_reach_)
9289
return;

0 commit comments

Comments
 (0)