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
@@ -57,11 +57,11 @@ class ChainIkSolverVelMimicSVD : public ChainIkSolverVel
* @param num_mimic_joints The number of joints that are setup to follow other joints
* @param position_ik false if you want to solve for the full 6 dof end-effector pose,
* true if you want to solve only for the 3 dof end-effector position.
* @param threshold if a singular value is below this value, its inverse is set to zero, default: 0.00001
* @param threshold if a singular value is below this value, its inverse is set to zero, default: 0.001
*/
explicit ChainIkSolverVelMimicSVD(const Chain& chain_,
const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints,
bool position_ik = false, double threshold = 0.00001);
bool position_ik = false, double threshold = 0.001);

// TODO: simplify after kinetic support is dropped
#define KDL_VERSION_LESS(a, b, c) (KDL_VERSION < ((a << 16) | (b << 8) | c))
@@ -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
@@ -161,6 +170,9 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase
void getRandomConfiguration(const Eigen::VectorXd& seed_state, const std::vector<double>& consistency_limits,
Eigen::VectorXd& jnt_array) const;

/// clip q_delta such that joint limits will not be violated
void clipToJointLimits(const KDL::JntArray& q, KDL::JntArray& q_delta, Eigen::ArrayXd& weighting) const;

bool initialized_; ///< Internal variable that indicates whether solver is configured and ready

unsigned int dimension_; ///< Dimension of the group
@@ -171,11 +183,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;
}
ProTip! Use n and p to navigate between commits in a pull request.
You can’t perform that action at this time.