Skip to content

Added SNS Acceleration IK Solver Interface with UnitTest #102

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

Open
wants to merge 4 commits into
base: indigo-devel
Choose a base branch
from
Open

Added SNS Acceleration IK Solver Interface with UnitTest #102

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
ef410a6
Added SNS Acceleration IK Solver Interface with UnitTest
Sep 27, 2018
9e6dc91
Added a missing unit test file that caused Travis to fail
Sep 28, 2018
c3e21d6
Correct a log level in the code
Sep 28, 2018
66ad233
Addressed the reviewers' comments
Oct 3, 2018
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
19 changes: 3 additions & 16 deletions sns_ik_lib/include/sns_ik/sns_acceleration_ik.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,10 +52,9 @@ class SNSAccelerationIK {

// SNS Acceleration IK
Copy link
Contributor

@forrest-rm forrest-rm Oct 1, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This function needs more documentation for inputs and outputs

Copy link
Contributor

@forrest-rm forrest-rm Nov 6, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Overall, more documentation is still needed.

int getJointAcceleration(Eigen::VectorXd *jointAcceleration, const std::vector<TaskAcc> &sot,
const Eigen::VectorXd &jointConfiguration, const Eigen::VectorXd &jointVelocities);
const Eigen::VectorXd &jointConfiguration, const Eigen::VectorXd &jointVelocities);

std::vector<double> getTasksScaleFactor()
{ return scaleFactors; }
std::vector<double> getTasksScaleFactor() const { return scaleFactors; }

Eigen::VectorXd getJointLimitLow() { return jointLimit_low; }
Eigen::VectorXd getJointLimitHigh() { return jointLimit_high; }
Expand All @@ -67,7 +66,7 @@ class SNSAccelerationIK {

// Shape the joint acceleration bound ddotQmin and ddotQmax
void shapeJointAccelerationBound(const Eigen::VectorXd &actualJointConfiguration,
const Eigen::VectorXd &actualJointVelocities, double margin = 0.98);
const Eigen::VectorXd &actualJointVelocities, double margin = 0.98);

int n_dof; //manipulator degree of freedom
int n_tasks; //number of tasks
Expand All @@ -84,19 +83,7 @@ class SNSAccelerationIK {
std::vector<double> scaleFactors;

// variables for base acc ik solver
Copy link
Contributor

@forrest-rm forrest-rm Oct 1, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It looks like most of these variables are only used in one function. They should not be member variables.

Copy link
Author

@robodreamer robodreamer Oct 3, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That is right. I moved them inside the function now.

Copy link
Contributor

@forrest-rm forrest-rm Nov 6, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

👍

Eigen::ArrayXd ddqLow;
Eigen::ArrayXd ddqUpp;
Eigen::MatrixXd J;
Eigen::VectorXd dJdq;
Eigen::VectorXd ddx;
Eigen::VectorXd ddqCS;
Eigen::VectorXd ddqSol;

double taskScale, taskScaleCS;

SnsAccIkBase::uPtr baseIkSolver;

SnsIkBase::ExitCode exitCode;
};

} // namespace sns_ik
Expand Down
9 changes: 2 additions & 7 deletions sns_ik_lib/include/sns_ik/sns_ik.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,7 @@ namespace sns_ik {
SNS_OptimalScaleMargin,
SNS_Fast,
SNS_FastOptimal,
SNS_Base,
SNS_Base_Acc
SNS_Base
};


Expand Down Expand Up @@ -153,11 +152,7 @@ namespace sns_ik {
bool getJacobian(const KDL::JntArray& jnt_pos_in, Eigen::MatrixXd *jacobianOut);

bool getJacobianDot(const KDL::JntArray& jnt_pos_in, const KDL::JntArray& jnt_vel_in,
Eigen::MatrixXd *jacobianDotOut, const std::string & tipName);

bool getJacobianDot(const KDL::JntArray& jnt_pos_in, const KDL::JntArray& jnt_vel_in,
Eigen::MatrixXd *jacobianDotOut)
{ return getJacobianDot(jnt_pos_in, jnt_vel_in, jacobianDotOut, "right_hand"); }
Eigen::MatrixXd *jacobianDotOut, const std::string& tipName = "right_hand");

int CartToJntAcc(const KDL::JntArray& q_in,
const KDL::JntArray& qdot_in,
Expand Down
3 changes: 3 additions & 0 deletions sns_ik_lib/include/sns_ik/sns_ik_base.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -117,6 +117,9 @@ class SnsIkBase {
// Tolerance for checks on the velocity/acceleration limits
static const double BOUND_TOLERANCE;

// A small scale factor used during sns ik algorithm
static const double SMALL_SCALE_FACTOR;

// Nice formatting option from Eigen
static const Eigen::IOFormat EigArrFmt;

Expand Down
62 changes: 40 additions & 22 deletions sns_ik_lib/src/sns_acceleration_ik.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@ SNSAccelerationIK::SNSAccelerationIK(int dof, double loop_period) :
setNumberOfDOF(dof);
setLoopPeriod(loop_period);

// initialize the sns acc ik solver
baseIkSolver = SnsAccIkBase::create(n_dof);
}

Expand Down Expand Up @@ -91,10 +92,17 @@ bool SNSAccelerationIK::setMaxJointAcceleration(const Eigen::VectorXd maxAcceler
return true;
}

// The box constraints (ddotQmin and ddotQmax) in acceleration level are calculated
// from joint position, velocity and acceleration limits based on the equations
// in the following paper:
// Flacco, Fabrizio, Alessandro De Luca, and Oussama Khatib.
// "Motion control of redundant robots under joint constraints:
// Saturation in the null space." In IEEE International Conference on Robotics
// and Automation (ICRA), pp. 285-292, 2012.
void SNSAccelerationIK::shapeJointAccelerationBound(const Eigen::VectorXd &actualJointConfiguration,
const Eigen::VectorXd &actualJointVelocities, double margin) {

// it could be written using the Eigen::Array potentiality
// TODO: rewrite this using the Eigen::Array
double step, max, stop;

for (int i = 0; i < n_dof; i++) {
Expand All @@ -109,12 +117,6 @@ void SNSAccelerationIK::shapeJointAccelerationBound(const Eigen::VectorXd &actua
ddotQmin(i) = max;
}

if (ddotQmin(i) > 0) {
ROS_ERROR_STREAM("Limit has been reversed for "<< i <<"th joint!\n"<< "jointLimit_Low: " << jointLimit_low(i)<<
", actualJointConfiguration: " << actualJointConfiguration(i));
ROS_ERROR_STREAM("minJointVelocity: " << -maxJointVelocity(i)<< ", actualJointVelocities: " << actualJointVelocities(i));
}

// for the maximum bound
max = maxJointAcceleration(i);
if (m_usePositionLimits) {
Expand All @@ -126,10 +128,18 @@ void SNSAccelerationIK::shapeJointAccelerationBound(const Eigen::VectorXd &actua
ddotQmax(i) = max;
}

if (ddotQmax(i) < 0) {
ROS_ERROR_STREAM("Limit has been reversed for "<< i <<"th joint!\n"<< "jointLimit_high: " << jointLimit_high(i)<<
", actualJointConfiguration: " << actualJointConfiguration(i));
ROS_ERROR_STREAM("maxJointVelocity: " << maxJointVelocity(i)<< ", actualJointVelocities: " << actualJointVelocities(i));
// check whether the acceleration limits are correctly shaped
if (ddotQmin(i) > ddotQmax(i)) {
if (ddotQmin(i) > 0) {
ROS_ERROR_STREAM("Lower limit has been reversed for J"<< i <<"!\n"<< "jointLimit_Low: " << jointLimit_low(i)
Copy link
Contributor

@forrest-rm forrest-rm Nov 6, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Need additional indentation

Copy link
Contributor

@forrest-rm forrest-rm Nov 6, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should this return an error code and/or revert to the previous values?

<< ", actualJointConfiguration: " << actualJointConfiguration(i) <<"\nminJointVelocity: " << -maxJointVelocity(i)
<< ", actualJointVelocities: " << actualJointVelocities(i));
}
if (ddotQmax(i) < 0) {
ROS_ERROR_STREAM("Upper limit has been reversed for J"<< i <<"!\n"<< "jointLimit_high: " << jointLimit_high(i)
<< ", actualJointConfiguration: " << actualJointConfiguration(i) << "\nmaxJointVelocity: " << maxJointVelocity(i)
<< ", actualJointVelocities: " << actualJointVelocities(i));
}
}
}

Expand All @@ -140,24 +150,36 @@ void SNSAccelerationIK::shapeJointAccelerationBound(const Eigen::VectorXd &actua
int SNSAccelerationIK::getJointAcceleration(Eigen::VectorXd *jointAcceleration,
const std::vector<TaskAcc> &sot, const Eigen::VectorXd &jointConfiguration, const Eigen::VectorXd &jointVelocities)
{
// check whether tasks are defined or not
if (sot.size()==0) {
ROS_ERROR("Tasks have not been correctly set!");
return -1;
}

// This will only reset member variables if different from previous values
Copy link
Contributor

@forrest-rm forrest-rm Oct 1, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Check that sot.size() > 0

Copy link
Author

@robodreamer robodreamer Oct 3, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good suggestion!

setNumberOfTasks(sot.size(), sot[0].jacobian.cols());

// calculate box constraints
shapeJointAccelerationBound(jointConfiguration, jointVelocities);

// solve using SNS base IK solver (andy)
ddqLow = ddotQmin;
ddqUpp = ddotQmax;
J = sot[0].jacobian;
dJdq = sot[0].dJdq;
ddx = sot[0].desired;
// define variables
Eigen::ArrayXd ddqLow = ddotQmin;
Eigen::ArrayXd ddqUpp = ddotQmax;
Eigen::MatrixXd J = sot[0].jacobian;
Eigen::VectorXd dJdq = sot[0].dJdq;
Eigen::VectorXd ddx = sot[0].desired;
Eigen::VectorXd ddqSol;
Eigen::VectorXd ddqCS;
double taskScale, taskScaleCS;
SnsIkBase::ExitCode exitCode;

// set default values
ddqSol.resize(n_dof);
taskScale = 1.0;
taskScaleCS = 1.0;

if (n_tasks > 1) {
// if the tasks include a nullspace bias task
taskScaleCS = 1.0;
ddqCS = sot[1].desired;
}

Expand All @@ -178,13 +200,9 @@ int SNSAccelerationIK::getJointAcceleration(Eigen::VectorXd *jointAcceleration,
*jointAcceleration = ddqSol;

if (exitCode != SnsIkBase::ExitCode::Success)
{
return -1;
Copy link
Contributor

@forrest-rm forrest-rm Oct 1, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is there a reason not to return the actual exitCode?

Copy link
Author

@robodreamer robodreamer Oct 3, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It was because all other existing codes were written based on integer return values.

}

return 1;
}



} // namespace sns_ik
74 changes: 31 additions & 43 deletions sns_ik_lib/src/sns_ik.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,10 @@

namespace sns_ik {

// define some constants
static const size_t CARTESIAN_DOF = 6;
static const size_t NUM_LINKS = 9; // the number of links in robot kinematic chain
Copy link
Contributor

@forrest-rm forrest-rm Nov 6, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why is this hard-coded and will this break things if the the DOF is different?


std::string toStr(const sns_ik::VelocitySolveType& type) {
switch (type) {
case sns_ik::VelocitySolveType::SNS:
Expand All @@ -42,8 +46,6 @@ namespace sns_ik {
return "SNS_FastOptimal";
case sns_ik::VelocitySolveType::SNS_Base:
return "SNS_Base";
case sns_ik::VelocitySolveType::SNS_Base_Acc:
return "SNS_Base_Acc";
default:
return "SNS_Unknown";
}
Expand Down Expand Up @@ -234,13 +236,6 @@ bool SNS_IK::setVelocitySolveType(VelocitySolveType type) {
m_ik_vel_solver = std::shared_ptr<SNSVelIKBaseInterface>(new SNSVelIKBaseInterface(m_chain.getNrOfJoints(), m_loopPeriod));
ROS_INFO("SNS_IK: Set Velocity solver to Base SNS solver.");
break;
case sns_ik::SNS_Base_Acc:
m_ik_vel_solver = std::shared_ptr<SNSVelIKBaseInterface>(new SNSVelIKBaseInterface(m_chain.getNrOfJoints(), m_loopPeriod));
m_ik_acc_solver = std::shared_ptr<SNSAccelerationIK>(new SNSAccelerationIK(m_chain.getNrOfJoints(), m_loopPeriod));
m_ik_acc_solver->setJointsCapabilities(m_lower_bounds.data, m_upper_bounds.data,
m_velocity.data, m_acceleration.data);
ROS_INFO("SNS_IK: Set Acceleration solver to Base SNS solver.");
break;
default:
ROS_ERROR("SNS_IK: Unknown Velocity solver type requested.");
return false;
Expand All @@ -249,6 +244,10 @@ bool SNS_IK::setVelocitySolveType(VelocitySolveType type) {
m_velocity.data, m_acceleration.data);
m_ik_pos_solver = std::shared_ptr<SNSPositionIK>(new SNSPositionIK(m_chain, m_ik_vel_solver, m_eps));

// set default acceleration IK solver
m_ik_acc_solver = std::shared_ptr<SNSAccelerationIK>(new SNSAccelerationIK(m_chain.getNrOfJoints(), m_loopPeriod));
m_ik_acc_solver->setJointsCapabilities(m_lower_bounds.data, m_upper_bounds.data, m_velocity.data, m_acceleration.data);

m_solvetype = type;
m_initialized = true;
return true;
Expand Down Expand Up @@ -308,9 +307,9 @@ int SNS_IK::CartToJntVel(const KDL::JntArray& q_in, const KDL::Twist& v_in,
std::vector<Task> sot;
Task task;
task.jacobian = jacobian.data;
task.desired = Eigen::VectorXd::Zero(6);
task.desired = Eigen::VectorXd::Zero(CARTESIAN_DOF);
// twistEigenToKDL
for(size_t i = 0; i < 6; i++)
for(size_t i = 0; i < CARTESIAN_DOF; i++)
task.desired(i) = v_in[i];
sot.push_back(task);

Expand Down Expand Up @@ -426,7 +425,7 @@ bool SNS_IK::getJacobianDot(const KDL::JntArray& jnt_pos_in, const KDL::JntArray
}

// Get Linear/Angular Jacobians
Eigen::MatrixXd Jv, Jw;
Eigen::Matrix3Xd Jv, Jw;
Jv = jacobian.data.block(0,0,3,numJnt);
Jw = jacobian.data.block(3,0,3,numJnt);

Expand All @@ -435,49 +434,43 @@ bool SNS_IK::getJacobianDot(const KDL::JntArray& jnt_pos_in, const KDL::JntArray
jacobianDot.resize(numJnt);

// initialization
Eigen::MatrixXd p_0_i_1_i = Eigen::MatrixXd::Zero(3, numJnt);
Eigen::MatrixXd pdot_0_i_1_i = Eigen::MatrixXd::Zero(3, numJnt);
Eigen::MatrixXd p_0_i_1 = Eigen::MatrixXd::Zero(3, numJnt);
Eigen::MatrixXd p_0_i_1_k = Eigen::MatrixXd::Zero(3, numJnt);
Eigen::MatrixXd pdot_0_i_1_k = Eigen::MatrixXd::Zero(3, numJnt);
Eigen::Matrix3Xd p_0_i_1_i = Eigen::MatrixXd::Zero(3, numJnt);
Eigen::Matrix3Xd pdot_0_i_1_i = Eigen::MatrixXd::Zero(3, numJnt);
Eigen::Matrix3Xd p_0_i_1 = Eigen::MatrixXd::Zero(3, numJnt);
Eigen::Matrix3Xd p_0_i_1_k = Eigen::MatrixXd::Zero(3, numJnt);
Eigen::Matrix3Xd pdot_0_i_1_k = Eigen::MatrixXd::Zero(3, numJnt);

Eigen::MatrixXd w = Eigen::MatrixXd::Zero(3, numJnt);
Eigen::Matrix3Xd w = Eigen::MatrixXd::Zero(3, numJnt);

Eigen::MatrixXd Jdw = Eigen::MatrixXd::Zero(3, numJnt);
Eigen::MatrixXd Jdv = Eigen::MatrixXd::Zero(3, numJnt);
Eigen::Matrix3Xd Jdw = Eigen::MatrixXd::Zero(3, numJnt);
Eigen::Matrix3Xd Jdv = Eigen::MatrixXd::Zero(3, numJnt);

// get FK of the intermediate links
std::vector<KDL::Frame> transforms;
KDL::ChainFkSolverPos_recursive fwdKin(m_chain);
KDL::JntArray q(jnt_pos_in.rows());
q.data << 1,1,1,1,1,1,1;

for (size_t i = 0; i < 9; i++) {
for (size_t i = 0; i < NUM_LINKS; i++) {
KDL::Frame transformTemp;
fwdKin.JntToCart(q, transformTemp, i+1);
fwdKin.JntToCart(jnt_pos_in, transformTemp, i+1);
if (i < numJnt)
p_0_i_1.col(i) << transformTemp.p(0),transformTemp.p(1),transformTemp.p(2);
transforms.push_back(transformTemp);
}

// get the tip position vector
Eigen::Vector3d p_0_k(transforms[8].p(0),transforms[8].p(1),transforms[8].p(2));
// get the tip position vector (the last link frame index is NUM_LINKS-1)
Eigen::Vector3d p_0_k(transforms[NUM_LINKS-1].p(0),transforms[NUM_LINKS-1].p(1),transforms[NUM_LINKS-1].p(2));

// first pass -- compute Jdw and quantities needed for Jdv
Eigen::Vector3d w_i(0,0,0);
Eigen::Vector3d w_i_1(0,0,0);
for (size_t i = 0; i < numJnt; i++) {
for (size_t i = 1; i < numJnt; i++) {
if (i > 0) {
w_i_1 = w.col(i-1);
}

// compute w_i and w_i_1
w_i = jnt_vel_in(i)*Jw.col(i) + w_i_1;
w.col(i) = w_i;

w.col(i) = jnt_vel_in(i)*Jw.col(i) + w_i_1;
// compute Jdw
Eigen::Vector3d Jw_i = Jw.col(i);
Jdw.col(i) = w_i_1.cross(Jw_i);
Jdw.col(i) = w_i_1.cross(Jw.col(i));

/// compute quantities for Jdv

Expand All @@ -490,9 +483,7 @@ bool SNS_IK::getJacobianDot(const KDL::JntArray& jnt_pos_in, const KDL::JntArray
}

// compute its derivative
Eigen::Vector3d p_0_i_1_i_ = p_0_i_1_i.col(i);
pdot_0_i_1_i.col(i) = w_i.cross(p_0_i_1_i_);

pdot_0_i_1_i.col(i) = w.col(i).cross(p_0_i_1_i.col(i));
// compute the position vector between joint coordinate frame and tip
p_0_i_1_k.col(i) = p_0_k - p_0_i_1.col(i);
}
Expand All @@ -504,11 +495,7 @@ bool SNS_IK::getJacobianDot(const KDL::JntArray& jnt_pos_in, const KDL::JntArray
pdot_0_i_1_k.col(i) = pdot_0_i_1_k.col(i) + pdot_0_i_1_i.col(j);
}
// compute Jdv
Eigen::Vector3d Jw_i = Jw.col(i);
Eigen::Vector3d Jdw_i = Jdw.col(i);
Eigen::Vector3d p_0_i_1_k_ = p_0_i_1_k.col(i);
Eigen::Vector3d pdot_0_i_1_k_ = pdot_0_i_1_k.col(i);
Jdv.col(i) = Jdw_i.cross(p_0_i_1_k_) + Jw_i.cross(pdot_0_i_1_k_);
Jdv.col(i) = Jdw.col(i).cross(p_0_i_1_k.col(i)) + Jw.col(i).cross(pdot_0_i_1_k.col(i));
}

// assign Jdv and Jdw into Jacobian dot
Expand Down Expand Up @@ -555,10 +542,10 @@ int SNS_IK::CartToJntAcc(const KDL::JntArray& q_in, const KDL::JntArray& qdot_in
task.jacobian = jacobian.data;
Eigen::VectorXd dJdq = jacobianDot*qdot_in.data;
task.dJdq = dJdq;
task.desired = Eigen::VectorXd::Zero(6);
task.desired = Eigen::VectorXd::Zero(CARTESIAN_DOF);

// twistEigenToKDL
for(size_t i = 0; i < 6; i++)
for(size_t i = 0; i < CARTESIAN_DOF; i++)
task.desired(i) = a_in[i];
sot.push_back(task);

Expand Down Expand Up @@ -617,6 +604,7 @@ bool SNS_IK::setMaxJointAcceleration(const KDL::JntArray& accel) {
bool SNS_IK::getTaskScaleFactors(std::vector<double>& scaleFactors) {
scaleFactors = m_ik_vel_solver->getTasksScaleFactor();

// if scaleFactors from velocity IK solver are empty, get them from acc IK solver
if (scaleFactors.empty())
scaleFactors = m_ik_acc_solver->getTasksScaleFactor();
return m_initialized && !scaleFactors.empty();
Expand Down
6 changes: 4 additions & 2 deletions sns_ik_lib/src/sns_ik_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@ const double SnsIkBase::NEG_INF = std::numeric_limits<double>::lowest();
const double SnsIkBase::MINIMUM_FINITE_SCALE_FACTOR = 1e-10;
const double SnsIkBase::MAXIMUM_FINITE_SCALE_FACTOR = 1e10;
const double SnsIkBase::BOUND_TOLERANCE = 1e-8;
const double SnsIkBase::SMALL_SCALE_FACTOR = 1e-3;
const Eigen::IOFormat SnsIkBase::EigArrFmt(4, 0, ", ", "\n", "[", "]");

/*************************************************************************************************
Expand Down Expand Up @@ -193,8 +194,9 @@ SnsIkBase::ExitCode SnsIkBase::computeTaskScalingFactor(const Eigen::MatrixXd& J
for (int i = 0; i < nJnt_; i++) {
if (jntIsFree[i]) {
jntScaleFactorArr(i) = SnsIkBase::findScaleFactor(lowMargin(i), uppMargin(i), a(i));
if (jntScaleFactorArr(i) == 1 && (jointOut(i) < (qLow_(i) - SnsIkBase::BOUND_TOLERANCE) || jointOut(i) > (qUpp_(i) + SnsIkBase::BOUND_TOLERANCE)))
jntScaleFactorArr(i) = 1e-3;
if (jntScaleFactorArr(i) == 1 && (jointOut(i) < (qLow_(i) - SnsIkBase::BOUND_TOLERANCE)
|| jointOut(i) > (qUpp_(i) + SnsIkBase::BOUND_TOLERANCE)))
jntScaleFactorArr(i) = SMALL_SCALE_FACTOR;
} else { // joint is constrained
jntScaleFactorArr(i) = POS_INF;
}
Expand Down
Loading