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.

Sign up for GitHub

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

use undamped jacobian in nullspace projection #78

Merged
merged 2 commits into from
Jan 26, 2016
Merged

use undamped jacobian in nullspace projection #78

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
10cf6d0
minor renaming
fmessmer Jan 12, 2016
1dfbe92
use undamped jacobian in nullspace projection
fmessmer Jan 16, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion ...ntroller/include/cob_twist_controller/constraint_solvers/solvers/constraint_solver_base.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ class ConstraintSolver
public:
/**
* The interface method to solve the inverse kinematics problem. Has to be implemented in inherited classes.
* @param inCartVelocities The input velocities vector (in cartesian space).
* @param in_cart_velocities The input velocities vector (in cartesian space).
* @param joint_states The joint states with history.
* @return The calculated new joint velocities.
*/
Expand Down
23 changes: 12 additions & 11 deletions cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,18 +37,18 @@
* In addtion to the partial solution q_dot = J^+ * v the homogeneous solution (I - J^+ * J) q_dot_0 is calculated.
* The q_dot_0 results from the sum of the constraint cost function gradients. The terms of the sum are weighted with a factor k_H separately.
*/
Eigen::MatrixXd GradientProjectionMethodSolver::solve(const Vector6d_t& inCartVelocities,
Eigen::MatrixXd GradientProjectionMethodSolver::solve(const Vector6d_t& in_cart_velocities,
const JointStates& joint_states)
{
Eigen::MatrixXd damped_jpi = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
Eigen::MatrixXd jpi = pinv_calc_.calculate(this->jacobian_data_);
Eigen::MatrixXd damped_pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
Eigen::MatrixXd pinv = pinv_calc_.calculate(this->jacobian_data_);

Eigen::MatrixXd particular_solution = damped_jpi * inCartVelocities;
Eigen::MatrixXd particular_solution = damped_pinv * in_cart_velocities;

Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(damped_jpi.rows(), this->jacobian_data_.cols());
Eigen::MatrixXd projector = ident - damped_jpi * this->jacobian_data_;
// Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(jpi.rows(), this->jacobian_data_.cols());
// Eigen::MatrixXd projector = ident - jpi * this->jacobian_data_;
// Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(damped_pinv.rows(), this->jacobian_data_.cols());
// Eigen::MatrixXd projector = ident - damped_pinv * this->jacobian_data_;
Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(pinv.rows(), this->jacobian_data_.cols());
Eigen::MatrixXd projector = ident - pinv * this->jacobian_data_;

Eigen::MatrixXd homogeneous_solution = Eigen::MatrixXd::Zero(particular_solution.rows(), particular_solution.cols());
KDL::JntArrayVel predict_jnts_vel(joint_states.current_q_.rows());
Expand All @@ -58,14 +58,15 @@ Eigen::MatrixXd GradientProjectionMethodSolver::solve(const Vector6d_t& inCartVe
ROS_DEBUG_STREAM("task id: " << (*it)->getTaskId());
(*it)->update(joint_states, predict_jnts_vel, this->jacobian_data_);
Eigen::VectorXd q_dot_0 = (*it)->getPartialValues();
Eigen::MatrixXd tmpHomogeneousSolution = projector * q_dot_0;
Eigen::MatrixXd tmp_projection = projector * q_dot_0;
double activation_gain = (*it)->getActivationGain(); // contribution of the homo. solution to the part. solution
double constraint_k_H = (*it)->getSelfMotionMagnitude(particular_solution, tmpHomogeneousSolution); // gain of homogenous solution (if active)
homogeneous_solution += (constraint_k_H * activation_gain * tmpHomogeneousSolution);
double constraint_k_H = (*it)->getSelfMotionMagnitude(particular_solution, tmp_projection); // gain of homogenous solution (if active)
homogeneous_solution += (constraint_k_H * activation_gain * tmp_projection);
}

Eigen::MatrixXd qdots_out = particular_solution + this->params_.k_H * homogeneous_solution; // weighting with k_H is done in loop

// //DEBUG: for verification of nullspace projection
// std::stringstream ss_part;
// ss_part << "particular_solution: ";
// for(unsigned int i=0; i<particular_solution.rows(); i++)
Expand Down
25 changes: 14 additions & 11 deletions cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,10 +40,13 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities,
double cycle = (now - this->last_time_).toSec();
this->last_time_ = now;

Eigen::MatrixXd jacobianPseudoInverse = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(jacobianPseudoInverse.rows(), this->jacobian_data_.cols());
Eigen::MatrixXd projector = ident - jacobianPseudoInverse * this->jacobian_data_;
Eigen::MatrixXd particular_solution = jacobianPseudoInverse * in_cart_velocities;
Eigen::MatrixXd damped_pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
Eigen::MatrixXd pinv = pinv_calc_.calculate(this->jacobian_data_);

Eigen::MatrixXd particular_solution = damped_pinv * in_cart_velocities;

Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(pinv.rows(), this->jacobian_data_.cols());
Eigen::MatrixXd projector = ident - pinv * this->jacobian_data_;

Eigen::MatrixXd projector_i = Eigen::MatrixXd::Identity(this->jacobian_data_.cols(), this->jacobian_data_.cols());
Eigen::VectorXd q_i = Eigen::VectorXd::Zero(this->jacobian_data_.cols());
Expand Down Expand Up @@ -102,7 +105,7 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities,
Eigen::MatrixXd J_task = it->task_jacobian_;
Eigen::MatrixXd J_temp = J_task * projector_i;
Eigen::VectorXd v_task = it->task_;
Eigen::MatrixXd J_temp_inv = pinv_calc_.calculate(it->tcp_, it->db_, J_temp);
Eigen::MatrixXd J_temp_inv = pinv_calc_.calculate(J_temp);
q_i = q_i + J_temp_inv * (v_task - J_task * q_i);
projector_i = projector_i - J_temp_inv * J_temp;
}
Expand All @@ -113,15 +116,15 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities,


void StackOfTasksSolver::processState(std::set<ConstraintBase_t>::iterator& it,
const Eigen::MatrixXd& projector,
const Eigen::MatrixXd& particular_solution,
double inv_sum_of_prios,
Eigen::VectorXd& sum_of_gradient)
const Eigen::MatrixXd& projector,
const Eigen::MatrixXd& particular_solution,
double inv_sum_of_prios,
Eigen::VectorXd& sum_of_gradient)
{
Eigen::VectorXd q_dot_0 = (*it)->getPartialValues();
const double activation_gain = (*it)->getActivationGain();
Eigen::MatrixXd tmpHomogeneousSolution = projector * q_dot_0;
const double magnitude = (*it)->getSelfMotionMagnitude(particular_solution, tmpHomogeneousSolution);
Eigen::MatrixXd tmp_projection = projector * q_dot_0;
const double magnitude = (*it)->getSelfMotionMagnitude(particular_solution, tmp_projection);
ConstraintState cstate = (*it)->getState();

const double constr_prio = (*it)->getPriorityAsNum();
Expand Down
13 changes: 8 additions & 5 deletions cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,10 +48,13 @@ Eigen::MatrixXd TaskPrioritySolver::solve(const Vector6d_t& in_cart_velocities,

Eigen::MatrixXd qdots_out = Eigen::MatrixXd::Zero(this->jacobian_data_.cols(), 1);
Eigen::VectorXd partial_cost_func = Eigen::VectorXd::Zero(this->jacobian_data_.cols());
Eigen::MatrixXd jacobianPseudoInverse = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(jacobianPseudoInverse.rows(), this->jacobian_data_.cols());
Eigen::MatrixXd projector = ident - jacobianPseudoInverse * this->jacobian_data_;
Eigen::MatrixXd particular_solution = jacobianPseudoInverse * in_cart_velocities;
Eigen::MatrixXd damped_pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
Eigen::MatrixXd pinv = pinv_calc_.calculate(this->jacobian_data_);

Eigen::MatrixXd particular_solution = damped_pinv * in_cart_velocities;

Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(pinv.rows(), this->jacobian_data_.cols());
Eigen::MatrixXd projector = ident - pinv * this->jacobian_data_;

KDL::JntArrayVel predict_jnts_vel(joint_states.current_q_.rows());

Expand Down Expand Up @@ -81,7 +84,7 @@ Eigen::MatrixXd TaskPrioritySolver::solve(const Vector6d_t& in_cart_velocities,
if (activation_gain > 0.0)
{
Eigen::MatrixXd tmp_matrix = partial_cost_func.transpose() * projector;
jac_inv_2nd_term = pinv_calc_.calculate(this->params_, this->damping_, tmp_matrix);
jac_inv_2nd_term = pinv_calc_.calculate(tmp_matrix);
}

Eigen::MatrixXd m_derivative_cost_func_value = derivative_cost_func_value * Eigen::MatrixXd::Identity(1, 1);
Expand Down
6 changes: 3 additions & 3 deletions cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,11 +35,11 @@
* It calculates the pseudo-inverse of the Jacobian via the base implementation of calculatePinvJacobianBySVD.
* With the pseudo-inverse the joint velocity vector is calculated.
*/
Eigen::MatrixXd UnconstraintSolver::solve(const Vector6d_t& inCartVelocities,
Eigen::MatrixXd UnconstraintSolver::solve(const Vector6d_t& in_cart_velocities,
const JointStates& joint_states)
{
Eigen::MatrixXd jacobianPseudoInverse = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
Eigen::MatrixXd qdots_out = jacobianPseudoInverse * inCartVelocities;
Eigen::MatrixXd pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
Eigen::MatrixXd qdots_out = pinv * in_cart_velocities;
return qdots_out;
}

9 changes: 5 additions & 4 deletions cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,10 +43,11 @@ Eigen::MatrixXd WeightedLeastNormSolver::solve(const Vector6d_t& in_cart_velocit
Eigen::MatrixXd inv_root_W_WLN = root_W_WLN.inverse(); // -> W^(-1/2)

// SVD of JLA weighted Jacobian: Damping will be done later in calculatePinvJacobianBySVD for pseudo-inverse Jacobian with additional truncation etc.
Eigen::MatrixXd weighted_jacobian_pseudoinverse = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_ * inv_root_W_WLN);
Eigen::MatrixXd weighted_jacobian = this->jacobian_data_ * inv_root_W_WLN;
Eigen::MatrixXd pinv = pinv_calc_.calculate(this->params_, this->damping_, weighted_jacobian);

// Take care: W^(1/2) * q_dot = weighted_pinv_J * x_dot -> One must consider the weighting!!!
Eigen::MatrixXd qdots_out = inv_root_W_WLN * weighted_jacobian_pseudoinverse * in_cart_velocities;
Eigen::MatrixXd qdots_out = inv_root_W_WLN * pinv * in_cart_velocities;
return qdots_out;
}

Expand All @@ -56,6 +57,6 @@ Eigen::MatrixXd WeightedLeastNormSolver::solve(const Vector6d_t& in_cart_velocit
Eigen::MatrixXd WeightedLeastNormSolver::calculateWeighting(const JointStates& joint_states) const
{
uint32_t cols = this->jacobian_data_.cols();
Eigen::VectorXd output = Eigen::VectorXd::Ones(cols);
return output.asDiagonal();
Eigen::VectorXd weighting = Eigen::VectorXd::Ones(cols);
return weighting.asDiagonal();
}
8 changes: 4 additions & 4 deletions cob_twist_controller/src/constraint_solvers/solvers/wln_joint_limit_avoidance_solver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,14 +41,14 @@ Eigen::MatrixXd WLN_JointLimitAvoidanceSolver::calculateWeighting(const JointSta
std::vector<double> limits_min = this->limiter_params_.limits_min;
std::vector<double> limits_max = this->limiter_params_.limits_max;
uint32_t cols = this->jacobian_data_.cols();
Eigen::VectorXd output = Eigen::VectorXd::Zero(cols);
Eigen::VectorXd weighting = Eigen::VectorXd::Zero(cols);

KDL::JntArray q = joint_states.current_q_;
KDL::JntArray q_dot = joint_states.current_q_dot_;

for (uint32_t i = 0; i < cols ; ++i)
{
output(i) = 1.0; // in the else cases -> output always 1
weighting(i) = 1.0; // in the else cases -> weighting always 1
if (i < q.rows())
{
// See Chan paper ISSN 1042-296X [Page 288]
Expand All @@ -61,11 +61,11 @@ Eigen::MatrixXd WLN_JointLimitAvoidanceSolver::calculateWeighting(const JointSta
if (denominator != 0.0)
{
double partialPerformanceCriterion = fabs(nominator / denominator);
output(i) = 1 + partialPerformanceCriterion;
weighting(i) = 1 + partialPerformanceCriterion;
}
}
}
}

return output.asDiagonal();
return weighting.asDiagonal();
}
50 changes: 25 additions & 25 deletions cob_twist_controller/src/inverse_jacobian_calculations/inverse_jacobian_calculation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,9 +50,9 @@ Eigen::MatrixXd PInvBySVD::calculate(const Eigen::MatrixXd& jacobian) const
singularValuesInv(i) = (singularValues(i) < eps_truncation) ? 0.0 : singularValues(i) / denominator;
}

Eigen::MatrixXd pseudoInverseJacobian = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
Eigen::MatrixXd result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();

return pseudoInverseJacobian;
return result;
}

/**
Expand All @@ -68,24 +68,24 @@ Eigen::MatrixXd PInvBySVD::calculate(const TwistControllerParams& params,
Eigen::VectorXd singularValues = svd.singularValues();
Eigen::VectorXd singularValuesInv = Eigen::VectorXd::Zero(singularValues.rows());
double lambda = db->getDampingFactor(singularValues, jacobian);
uint32_t i = 0;

if (params.numerical_filtering)
{
// Formula 20 Singularity-robust Task-priority Redundandancy Resolution
// Sum part
for (; i < singularValues.rows()-1; ++i)
for (uint32_t i = 0; i < singularValues.rows()-1; ++i)
{
// pow(beta, 2) << pow(lambda, 2)
singularValuesInv(i) = singularValues(i) / (pow(singularValues(i), 2) + pow(params.beta, 2));
}
// Formula 20 - additional part
singularValuesInv(i) = singularValues(i) / (pow(singularValues(i), 2) + pow(params.beta, 2) + pow(lambda, 2));
// Formula 20 - additional part - numerical filtering for least singular value m
uint32_t m = singularValues.rows();
singularValuesInv(m) = singularValues(m) / (pow(singularValues(m), 2) + pow(params.beta, 2) + pow(lambda, 2));
}
else
{
// small change to ref: here quadratic damping due to Control of Redundant Robot Manipulators : R.V. Patel, 2005, Springer [Page 13-14]
for (; i < singularValues.rows(); ++i)
for (uint32_t i = 0; i < singularValues.rows(); ++i)
{
double denominator = (singularValues(i) * singularValues(i) + pow(lambda, 2) );
// singularValuesInv(i) = (denominator < eps_truncation) ? 0.0 : singularValues(i) / denominator;
Expand All @@ -100,9 +100,9 @@ Eigen::MatrixXd PInvBySVD::calculate(const TwistControllerParams& params,
// }
}

Eigen::MatrixXd pseudoInverseJacobian = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
Eigen::MatrixXd result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();

return pseudoInverseJacobian;
return result;
}

/**
Expand All @@ -111,17 +111,17 @@ Eigen::MatrixXd PInvBySVD::calculate(const TwistControllerParams& params,
Eigen::MatrixXd PInvDirect::calculate(const Eigen::MatrixXd& jacobian) const
{
Eigen::MatrixXd result;
Eigen::MatrixXd j_t = jacobian.transpose();
uint32_t jac_rows = jacobian.rows();
uint32_t jac_cols = jacobian.cols();
Eigen::MatrixXd jac_t = jacobian.transpose();
uint32_t rows = jacobian.rows();
uint32_t cols = jacobian.cols();

if (jac_cols >= jac_rows)
if (cols >= rows)
{
result = j_t * (jacobian * j_t).inverse();
result = jac_t * (jacobian * jac_t).inverse();
}
else
{
result = (j_t * jacobian).inverse() * j_t;
result = (jac_t * jacobian).inverse() * jac_t;
}

return result;
Expand All @@ -135,26 +135,26 @@ Eigen::MatrixXd PInvDirect::calculate(const TwistControllerParams& params,
const Eigen::MatrixXd& jacobian) const
{
Eigen::MatrixXd result;
Eigen::MatrixXd j_t = jacobian.transpose();
uint32_t jac_rows = jacobian.rows();
uint32_t jac_cols = jacobian.cols();
Eigen::MatrixXd jac_t = jacobian.transpose();
uint32_t rows = jacobian.rows();
uint32_t cols = jacobian.cols();
if (params.damping_method == LEAST_SINGULAR_VALUE)
{
ROS_ERROR("PInvDirect does not support SVD. Use PInvBySVD class instead!");
}

double lambda = db->getDampingFactor(Eigen::VectorXd::Zero(1, 1), jacobian);
if (jac_cols >= jac_rows)
if (cols >= rows)
{
Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(jac_rows, jac_rows);
Eigen::MatrixXd toBeInv = jacobian * j_t + lambda * lambda * ident;
result = j_t * toBeInv.inverse();
Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(rows, rows);
Eigen::MatrixXd temp = jacobian * jac_t + lambda * lambda * ident;
result = jac_t * temp.inverse();
}
else
{
Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(jac_cols, jac_cols);
Eigen::MatrixXd toBeInv = j_t * jacobian + lambda * lambda * ident;
result = toBeInv.inverse() * j_t;
Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(cols, cols);
Eigen::MatrixXd temp = jac_t * jacobian + lambda * lambda * ident;
result = temp.inverse() * jac_t;
}

return result;
Expand Down