Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

KDL IK solver improvements #1321

Merged
merged 7 commits into from Feb 4, 2019

weighted Jacobian

  • Loading branch information...
rhaschke committed Jan 9, 2019
commit 60d9a3cea8afbb747da2ffe5c9d923dbfe1b6397
@@ -74,14 +74,31 @@ class ChainIkSolverVelMimicSVD : public ChainIkSolverVel

~ChainIkSolverVelMimicSVD() override;

int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out) override;
int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out) override
{
return CartToJnt(q_in, v_in, qdot_out, Eigen::VectorXd::Constant(svd_.cols(), 1.0),
Eigen::Matrix<double, 6, 1>::Constant(1.0));
}

/** Compute qdot_out = W_q * (W_x * J * W_q)^# * W_x * v_in
*
* where W_q and W_x are joint- and Cartesian weights respectively.
* A smaller joint weight (< 1.0) will reduce the contribution of this joint to the solution. */
int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out, const Eigen::VectorXd& joint_weights,
const Eigen::Matrix<double, 6, 1>& cartesian_weights);

/// not implemented.
int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out) override
{
return -1;
}

/// Return true iff we ignore orientation but only consider position for inverse kinematics
bool isPositionOnly() const
{
return svd_.rows() == 3;
}

private:
bool jacToJacReduced(const Jacobian& jac, Jacobian& jac_reduced);

@@ -49,7 +49,7 @@ class JointMimic
JointMimic()
{
this->reset(0);
};
}

/** \brief Offset for this joint value from the joint that it mimics */
double offset;
@@ -68,7 +68,7 @@ class JointMimic
multiplier = 1.0;
map_index = index;
active = false;
};
}
};
}

@@ -59,6 +59,11 @@
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

namespace KDL
{
class ChainIkSolverVelMimicSVD;
}

namespace kdl_kinematics_plugin
{
/**
@@ -135,11 +140,15 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase
moveit_msgs::MoveItErrorCodes& error_code, const std::vector<double>& consistency_limits,
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const;

typedef Eigen::Matrix<double, 6, 1> Twist;

/// Solve position IK given initial joint values
int CartToJnt(KDL::ChainIkSolverVel& ik_solver, const KDL::JntArray& q_init, const KDL::Frame& p_in,
KDL::JntArray& q_out, const unsigned int max_iter) const;
int CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, const KDL::JntArray& q_init, const KDL::Frame& p_in,
KDL::JntArray& q_out, const unsigned int max_iter, const Eigen::VectorXd& joint_weights,
const Twist& cartesian_weights) const;

private:
void getJointWeights();
bool timedOut(const ros::WallTime& start_time, double duration) const;

/** @brief Check whether the solution lies within the consistency limits of the seed state
@@ -171,11 +180,17 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase
KDL::Chain kdl_chain_;
std::unique_ptr<KDL::ChainFkSolverPos> fk_solver_;
std::vector<JointMimic> mimic_joints_;
std::vector<double> joint_weights_;
Eigen::VectorXd joint_min_, joint_max_; ///< joint limits

bool position_ik_; ///< whether this solver is only being used for position ik
int max_solver_iterations_;
double epsilon_;
/** weight of orientation error vs position error
*
* < 1.0: orientation has less importance than position
* > 1.0: orientation has more importance than position
* = 0.0: perform position-only IK */
double orientation_vs_position_weight_;
};
}

@@ -87,7 +87,9 @@ bool ChainIkSolverVelMimicSVD::jacToJacReduced(const Jacobian& jac, Jacobian& ja
return true;
}

int ChainIkSolverVelMimicSVD::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
int ChainIkSolverVelMimicSVD::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out,
const Eigen::VectorXd& joint_weights,
const Eigen::Matrix<double, 6, 1>& cartesian_weights)
{
// Let the ChainJntToJacSolver calculate the Jacobian for the current joint positions q_in.
if (num_mimic_joints_ > 0)
@@ -99,24 +101,32 @@ int ChainIkSolverVelMimicSVD::CartToJnt(const JntArray& q_in, const Twist& v_in,
else
jnt2jac_.JntToJac(q_in, jac_reduced_);

// weight Jacobian
auto& J = jac_reduced_.data;
const Eigen::Index rows = svd_.rows(); // only operate on position rows?
J.topRows(rows) *= joint_weights.asDiagonal();
J.topRows(rows).transpose() *= cartesian_weights.topRows(rows).asDiagonal();

// transform v_in to 6D Eigen::Vector
Eigen::Matrix<double, 6, 1> vin;
vin.topRows<3>() = Eigen::Map<const Eigen::Vector3d>(v_in.vel.data, 3);
vin.bottomRows<3>() = Eigen::Map<const Eigen::Vector3d>(v_in.rot.data, 3);

const Eigen::MatrixXd& J = jac_reduced_.data;
vin.topRows<3>() = Eigen::Map<const Eigen::Array3d>(v_in.vel.data, 3) * cartesian_weights.topRows<3>().array();
vin.bottomRows<3>() = Eigen::Map<const Eigen::Array3d>(v_in.rot.data, 3) * cartesian_weights.bottomRows<3>().array();

// Do a singular value decomposition: J = U*S*V^t
svd_.compute(J.topRows(svd_.rows()));
svd_.compute(J.topRows(rows));

if (num_mimic_joints_ > 0)
{
qdot_out_reduced_.noalias() = svd_.solve(vin.topRows(svd_.rows()));
qdot_out_reduced_.noalias() = svd_.solve(vin.topRows(rows));
qdot_out_reduced_.array() *= joint_weights.array();
for (unsigned int i = 0; i < chain_.getNrOfJoints(); ++i)
qdot_out(i) = qdot_out_reduced_[mimic_joints_[i].map_index] * mimic_joints_[i].multiplier;
}
else
qdot_out.data.noalias() = svd_.solve(vin.topRows(svd_.rows()));
{
qdot_out.data.noalias() = svd_.solve(vin.topRows(rows));
qdot_out.data.array() *= joint_weights.array();
}

return 0;
}
@@ -83,6 +83,61 @@ bool KDLKinematicsPlugin::checkConsistency(const Eigen::VectorXd& seed_state,
return true;
}

void KDLKinematicsPlugin::getJointWeights()
{
const std::vector<std::string>& active_names = joint_model_group_->getActiveJointModelNames();
std::vector<std::string> names;
std::vector<double> weights;
if (lookupParam("joint_weights/weights", weights, weights))
{
if (!lookupParam("joint_weights/names", names, names) || names.size() != weights.size())
{
ROS_ERROR_NAMED("kdl", "Expecting list parameter joint_weights/names of same size as list joint_weights/weights");
// fall back to default weights
weights.clear();
}
}
else if (lookupParam("joint_weights", weights, weights)) // try reading weight lists (for all active joints) directly
{
std::size_t num_active = active_names.size();
if (weights.size() == num_active)
{
joint_weights_ = weights;
return;
}
else if (!weights.empty())
{
ROS_ERROR_NAMED("kdl", "Expecting parameter joint_weights to list weights for all active joints (%zu) in order",
num_active);
// fall back to default weights
weights.clear();
}
}

// by default assign weights of 1.0 to all joints
joint_weights_ = std::vector<double>(active_names.size(), 1.0);
if (weights.empty()) // indicates default case
return;

// modify weights of listed joints
assert(names.size() == weights.size());
for (size_t i = 0; i != names.size(); ++i)
{
auto it = std::find(active_names.begin(), active_names.end(), names[i]);
if (it == active_names.cend())
ROS_WARN_NAMED("kdl", "Joint '%s' is not an active joint in group '%s'", names[i].c_str(),
joint_model_group_->getName().c_str());
else if (weights[i] < 0.0)
ROS_WARN_NAMED("kdl", "Negative weight %f for joint '%s' will be ignored", weights[i], names[i].c_str());
else
joint_weights_[it - active_names.begin()] = weights[i];
}
ROS_INFO_STREAM_NAMED("kdl", "Joint weights for group '"
<< getGroupName() << "': \n"
<< Eigen::Map<const Eigen::VectorXd>(joint_weights_.data(), joint_weights_.size())
.transpose());
}

bool KDLKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model, const std::string& group_name,
const std::string& base_frame, const std::vector<std::string>& tip_frames,
double search_discretization)
@@ -148,11 +203,17 @@ bool KDLKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model
// Get Solver Parameters
lookupParam("max_solver_iterations", max_solver_iterations_, 500);
lookupParam("epsilon", epsilon_, 1e-5);
lookupParam("position_only_ik", position_ik_, false);
lookupParam("orientation_vs_position", orientation_vs_position_weight_, 1.0);

if (position_ik_)
bool position_ik;
lookupParam("position_only_ik", position_ik, false);
if (position_ik) // position_only_ik overrules orientation_vs_position
orientation_vs_position_weight_ = 0.0;
if (orientation_vs_position_weight_ == 0.0)
ROS_INFO_NAMED("kdl", "Using position only ik");

getJointWeights();

// Check for mimic joints
unsigned int joint_counter = 0;
for (std::size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i)
@@ -307,14 +368,17 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c
consistency_limits_mimic.push_back(consistency_limits[i]);
}
}
Eigen::Matrix<double, 6, 1> cartesian_weights;
cartesian_weights.topRows<3>().setConstant(1.0);
cartesian_weights.bottomRows<3>().setConstant(orientation_vs_position_weight_);

KDL::JntArray jnt_seed_state(dimension_);
KDL::JntArray jnt_pos_in(dimension_);
KDL::JntArray jnt_pos_out(dimension_);
jnt_seed_state.data = Eigen::Map<const Eigen::VectorXd>(ik_seed_state.data(), ik_seed_state.size());
jnt_pos_in = jnt_seed_state;

KDL::ChainIkSolverVelMimicSVD ik_solver_vel(kdl_chain_, mimic_joints_, position_ik_);
KDL::ChainIkSolverVelMimicSVD ik_solver_vel(kdl_chain_, mimic_joints_, orientation_vs_position_weight_ == 0.0);
solution.resize(dimension_);

KDL::Frame pose_desired;
@@ -338,7 +402,9 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c
ROS_DEBUG_STREAM_NAMED("kdl", "New random configuration (" << attempt << "): " << jnt_pos_in);
}

int ik_valid = CartToJnt(ik_solver_vel, jnt_pos_in, pose_desired, jnt_pos_out, max_solver_iterations_);
int ik_valid =
CartToJnt(ik_solver_vel, jnt_pos_in, pose_desired, jnt_pos_out, max_solver_iterations_,
Eigen::Map<const Eigen::VectorXd>(joint_weights_.data(), joint_weights_.size()), cartesian_weights);
if (ik_valid == 0 || options.return_approximate_solution) // found acceptable solution
{
if (!consistency_limits_mimic.empty() &&
@@ -367,8 +433,9 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c
return false;
}

int KDLKinematicsPlugin::CartToJnt(KDL::ChainIkSolverVel& ik_solver, const KDL::JntArray& q_init,
const KDL::Frame& p_in, KDL::JntArray& q_out, const unsigned int max_iter) const
int KDLKinematicsPlugin::CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, const KDL::JntArray& q_init,
const KDL::Frame& p_in, KDL::JntArray& q_out, const unsigned int max_iter,
const Eigen::VectorXd& joint_weights, const Twist& cartesian_weights) const
{
double last_delta_twist_norm = DBL_MAX;
double step_size = 1.0;
@@ -389,7 +456,7 @@ int KDLKinematicsPlugin::CartToJnt(KDL::ChainIkSolverVel& ik_solver, const KDL::

// check norms of position and orientation errors
const double position_error = delta_twist.vel.Norm();
const double orientation_error = position_ik_ ? 0 : delta_twist.rot.Norm();
const double orientation_error = ik_solver.isPositionOnly() ? 0 : delta_twist.rot.Norm();
const double delta_twist_norm = std::max(position_error, orientation_error);
if (delta_twist_norm <= epsilon_)
{
@@ -413,7 +480,7 @@ int KDLKinematicsPlugin::CartToJnt(KDL::ChainIkSolverVel& ik_solver, const KDL::
step_size = 1.0; // reset step size
last_delta_twist_norm = delta_twist_norm;

ik_solver.CartToJnt(q_out, delta_twist, delta_q);
ik_solver.CartToJnt(q_out, delta_twist, delta_q, joint_weights, cartesian_weights);
}
const double delta_q_norm = delta_q.data.lpNorm<1>();
ROS_INFO_NAMED("kdl", "[%3d] pos err: %f rot err: %f delta_q: %f", i, position_error, orientation_error,
ProTip! Use n and p to navigate between commits in a pull request.
You can’t perform that action at this time.