From ef410a6aa30dd6b527b0e6774f86fef157ff7e3d Mon Sep 17 00:00:00 2001 From: Andy Park Date: Thu, 27 Sep 2018 16:33:54 -0400 Subject: [PATCH 1/4] Added SNS Acceleration IK Solver Interface with UnitTest - added sns acceleration ik class and SNS_Base_Acc type - added Jacobian dot computation - created unit test with sawyer model --- sns_ik_lib/CMakeLists.txt | 3 + sns_ik_lib/include/sns_ik/sns_acc_ik_base.hpp | 2 +- .../include/sns_ik/sns_acceleration_ik.hpp | 104 ++++++++ sns_ik_lib/include/sns_ik/sns_ik.hpp | 49 +++- sns_ik_lib/src/sns_acc_ik_base.cpp | 15 +- sns_ik_lib/src/sns_acceleration_ik.cpp | 190 ++++++++++++++ sns_ik_lib/src/sns_ik.cpp | 240 +++++++++++++++++- sns_ik_lib/src/sns_ik_base.cpp | 3 + sns_ik_lib/src/sns_vel_ik_base.cpp | 9 +- sns_ik_lib/test/sns_ik_vel_test.cpp | 2 +- 10 files changed, 600 insertions(+), 17 deletions(-) create mode 100644 sns_ik_lib/include/sns_ik/sns_acceleration_ik.hpp create mode 100644 sns_ik_lib/src/sns_acceleration_ik.cpp diff --git a/sns_ik_lib/CMakeLists.txt b/sns_ik_lib/CMakeLists.txt index c7267a4..1843826 100644 --- a/sns_ik_lib/CMakeLists.txt +++ b/sns_ik_lib/CMakeLists.txt @@ -46,6 +46,7 @@ add_library(sns_ik src/sns_vel_ik_base.cpp src/sns_vel_ik_base_interface.cpp src/sns_velocity_ik.cpp + src/sns_acceleration_ik.cpp utilities/sns_ik_math_utils.cpp utilities/sns_linear_solver.cpp) target_link_libraries(sns_ik ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) @@ -74,6 +75,8 @@ if (CATKIN_ENABLE_TESTING) target_link_libraries(sns_ik_pos_test sns_ik sns_ik_test ${catkin_LIBRARIES}) catkin_add_gtest(sns_ik_vel_test test/sns_ik_vel_test.cpp) target_link_libraries(sns_ik_vel_test sns_ik sns_ik_test ${catkin_LIBRARIES}) + catkin_add_gtest(sns_ik_acc_test test/sns_ik_acc_test.cpp) + target_link_libraries(sns_ik_acc_test sns_ik sns_ik_test ${catkin_LIBRARIES}) catkin_add_gtest(sns_vel_ik_base_test test/sns_vel_ik_base_test.cpp) target_link_libraries(sns_vel_ik_base_test sns_ik sns_ik_test ${catkin_LIBRARIES}) catkin_add_gtest(sns_acc_ik_base_test test/sns_acc_ik_base_test.cpp) diff --git a/sns_ik_lib/include/sns_ik/sns_acc_ik_base.hpp b/sns_ik_lib/include/sns_ik/sns_acc_ik_base.hpp index f8d689b..4fd4c48 100644 --- a/sns_ik_lib/include/sns_ik/sns_acc_ik_base.hpp +++ b/sns_ik_lib/include/sns_ik/sns_acc_ik_base.hpp @@ -31,7 +31,7 @@ namespace sns_ik { -class SnsAccIkBase : SnsIkBase{ +class SnsAccIkBase : public SnsIkBase{ public: diff --git a/sns_ik_lib/include/sns_ik/sns_acceleration_ik.hpp b/sns_ik_lib/include/sns_ik/sns_acceleration_ik.hpp new file mode 100644 index 0000000..2119577 --- /dev/null +++ b/sns_ik_lib/include/sns_ik/sns_acceleration_ik.hpp @@ -0,0 +1,104 @@ +/*! \file sns_acceleration_ik.hpp + * \brief Basic SNS acceleration IK solver + * \author Andy Park + */ +/* + * Copyright 2018 Rethink Robotics + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SNS_IK_ACCELERATION_IK +#define SNS_IK_ACCELERATION_IK + +#include +#include +#include + +namespace sns_ik { + +/*! \struct Task + * A desired robot task + */ + +struct TaskAcc { + Eigen::MatrixXd jacobian; //!< the task Jacobian + Eigen::VectorXd dJdq; //!< the product of task Jacobian dot and joint velocity + Eigen::VectorXd desired; //!< desired velocity in task space +}; + +class SNSAccelerationIK { + public: + SNSAccelerationIK(int dof, double loop_period); + virtual ~SNSAccelerationIK() {}; + + bool setJointsCapabilities(const Eigen::VectorXd limit_low, const Eigen::VectorXd limit_high, + const Eigen::VectorXd maxVelocity, const Eigen::VectorXd maxAcceleration); + bool setMaxJointAcceleration(const Eigen::VectorXd maxAcceleration); + virtual void setNumberOfTasks(int ntasks, int dof = -1); + virtual void setNumberOfDOF(int dof); + + // The control loop period in seconds + void setLoopPeriod(double period) { loop_period = period; } + + // SNS Acceleration IK + int getJointAcceleration(Eigen::VectorXd *jointAcceleration, const std::vector &sot, + const Eigen::VectorXd &jointConfiguration, const Eigen::VectorXd &jointVelocities); + + std::vector getTasksScaleFactor() + { return scaleFactors; } + + Eigen::VectorXd getJointLimitLow() { return jointLimit_low; } + Eigen::VectorXd getJointLimitHigh() { return jointLimit_high; } + Eigen::VectorXd getJointVelocityMax() { return maxJointVelocity; } + + void usePositionLimits(bool use) { m_usePositionLimits = use; } + + protected: + + // Shape the joint acceleration bound ddotQmin and ddotQmax + void shapeJointAccelerationBound(const Eigen::VectorXd &actualJointConfiguration, + const Eigen::VectorXd &actualJointVelocities, double margin = 0.98); + + int n_dof; //manipulator degree of freedom + int n_tasks; //number of tasks + double loop_period; //needed to compute the bounds + + Eigen::VectorXd jointLimit_low; // low joint limits + Eigen::VectorXd jointLimit_high; // high joint limit + Eigen::VectorXd maxJointVelocity; // maximum joint velocity + Eigen::VectorXd maxJointAcceleration; // maximum joint acceleration + bool m_usePositionLimits; + + Eigen::ArrayXd ddotQmin; // lower joint velocity bound + Eigen::ArrayXd ddotQmax; // higher joint velocity bound + std::vector scaleFactors; + + // variables for base acc ik solver + 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 + +#endif diff --git a/sns_ik_lib/include/sns_ik/sns_ik.hpp b/sns_ik_lib/include/sns_ik/sns_ik.hpp index cae7c96..ba5333d 100644 --- a/sns_ik_lib/include/sns_ik/sns_ik.hpp +++ b/sns_ik_lib/include/sns_ik/sns_ik.hpp @@ -12,7 +12,7 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -// Author: Ian McMahon +// Author: Ian McMahon, Andy Park #ifndef SNS_IK_HPP #define SNS_IK_HPP @@ -24,6 +24,7 @@ #include #include #include +#include namespace sns_ik { @@ -32,7 +33,8 @@ namespace sns_ik { SNS_OptimalScaleMargin, SNS_Fast, SNS_FastOptimal, - SNS_Base + SNS_Base, + SNS_Base_Acc }; @@ -45,6 +47,8 @@ namespace sns_ik { // Forward declare SNS Velocity Base Class class SNSVelocityIK; + // Forward declare SNS Acceleration Base Class + class SNSAccelerationIK; class SNS_IK { public: @@ -146,6 +150,46 @@ namespace sns_ik { const KDL::JntArray& q_vel_bias, KDL::JntArray& qdot_out); + 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"); } + + int CartToJntAcc(const KDL::JntArray& q_in, + const KDL::JntArray& qdot_in, + const KDL::Twist& a_in, + KDL::JntArray& qddot_out) + { return CartToJntAcc(q_in, qdot_in, a_in, KDL::JntArray(0), std::vector(), + KDL::JntArray(0), qddot_out); } + + // Assumes the NS bias is for all the joints in the correct order + int CartToJntAcc(const KDL::JntArray& q_in, + const KDL::JntArray& qdot_in, + const KDL::Twist& a_in, + const KDL::JntArray& q_bias, + KDL::JntArray& qddot_out) + { return CartToJntAcc(q_in, qdot_in, a_in, q_bias, m_jointNames, KDL::JntArray(0), qddot_out); } + + int CartToJntAcc(const KDL::JntArray& q_in, + const KDL::JntArray& qdot_in, + const KDL::Twist& a_in, + const KDL::JntArray& q_bias, + const KDL::JntArray& q_acc_bias, + KDL::JntArray& qddot_out) + { return CartToJntAcc(q_in, qdot_in, a_in, q_bias, m_jointNames, q_acc_bias, qddot_out); } + + int CartToJntAcc(const KDL::JntArray& q_in, + const KDL::JntArray& qdot_in, + const KDL::Twist& a_in, + const KDL::JntArray& q_bias, + const std::vector& biasNames, + const KDL::JntArray& q_acc_bias, + KDL::JntArray& qddot_out); + // Nullspace gain should be specified between 0 and 1.0 double getNullspaceGain() { return m_nullspaceGain; } void setNullspaceGain(double gain) @@ -171,6 +215,7 @@ namespace sns_ik { std::vector m_solutions; std::shared_ptr m_ik_vel_solver; + std::shared_ptr m_ik_acc_solver; std::shared_ptr m_ik_pos_solver; std::shared_ptr m_jacobianSolver; diff --git a/sns_ik_lib/src/sns_acc_ik_base.cpp b/sns_ik_lib/src/sns_acc_ik_base.cpp index a0bf786..c9f1270 100644 --- a/sns_ik_lib/src/sns_acc_ik_base.cpp +++ b/sns_ik_lib/src/sns_acc_ik_base.cpp @@ -137,10 +137,6 @@ SnsIkBase::ExitCode SnsAccIkBase::solve(const Eigen::MatrixXd& J, const Eigen::V ROS_ERROR("Failed to compute task scale!"); return taskScaleExit; } - if (tmpScale < MINIMUM_FINITE_SCALE_FACTOR) { // check that the solver found a feasible solution - ROS_ERROR("Task is infeasible! scaling --> zero"); - return ExitCode::InfeasibleTask; - } if (tmpScale > 1.0) { ROS_ERROR("Task scale is %f, which is more than 1.0", tmpScale); @@ -172,10 +168,7 @@ SnsIkBase::ExitCode SnsAccIkBase::solve(const Eigen::MatrixXd& J, const Eigen::V ddqNull(jntIdx) = (getUpperBounds())(jntIdx); } else if ((*ddq)(jntIdx) < (getLowerBounds())(jntIdx)) { ddqNull(jntIdx) = (getLowerBounds())(jntIdx); - } else { - ROS_ERROR("Internal error in computing task scale! ddq(%d) = %f", jntIdx, (*ddq)(jntIdx)); - return ExitCode::InternalError; - } + } // Update the linear solver if (setLinearSolver(J*W) != ExitCode::Success) { @@ -189,6 +182,11 @@ SnsIkBase::ExitCode SnsAccIkBase::solve(const Eigen::MatrixXd& J, const Eigen::V W = bestW; ddqNull = bestDdqNull; + if (bestTaskScale < MINIMUM_FINITE_SCALE_FACTOR) { // check that the solver found a feasible solution + ROS_WARN("Task is infeasible! scaling --> zero"); + return ExitCode::InfeasibleTask; + } + // Update the linear solver if (setLinearSolver(J * W) != ExitCode::Success) { ROS_ERROR("Solver failed to set linear solver!"); @@ -283,6 +281,7 @@ SnsIkBase::ExitCode SnsAccIkBase::solve(const Eigen::MatrixXd& J, const Eigen::V // within the upper and lower bounds. Eigen::ArrayXd lowMargin = (getLowerBounds() - b); Eigen::ArrayXd uppMargin = (getUpperBounds() - b); + for (size_t i = 0; i < getNrOfJoints(); i++) { if (jointIsFree[i]) { jntScaleFactorArr(i) = findScaleFactor(lowMargin(i), uppMargin(i), a(i)); diff --git a/sns_ik_lib/src/sns_acceleration_ik.cpp b/sns_ik_lib/src/sns_acceleration_ik.cpp new file mode 100644 index 0000000..1f7194d --- /dev/null +++ b/sns_ik_lib/src/sns_acceleration_ik.cpp @@ -0,0 +1,190 @@ +/*! \file sns_acceleration_ik.cpp + * \brief Basic SNS acceleration IK solver + * \author Andy Park + */ +/* + * Copyright 2018 Rethink Robotics + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include + +#include "sns_ik_math_utils.hpp" + +namespace sns_ik { + +static const Eigen::IOFormat EigArrFmt(4, 0, ", ", "\n", "[", "]"); + +SNSAccelerationIK::SNSAccelerationIK(int dof, double loop_period) : + n_dof(0), + n_tasks(0), + m_usePositionLimits(true) +{ + setNumberOfDOF(dof); + setLoopPeriod(loop_period); + + baseIkSolver = SnsAccIkBase::create(n_dof); +} + +void SNSAccelerationIK::setNumberOfDOF(int dof) +{ + if (dof > 0 && dof != n_dof) { + n_dof = dof; + } +} + +void SNSAccelerationIK::setNumberOfTasks(int ntasks, int dof) +{ + setNumberOfDOF(dof); + + if (n_tasks != ntasks) { + n_tasks = ntasks; + Eigen::VectorXd ddq = Eigen::VectorXd::Zero(n_dof); + + double scale = 1.0; + scaleFactors.resize(n_tasks, scale); + } +} + + +bool SNSAccelerationIK::setJointsCapabilities(const Eigen::VectorXd limit_low, const Eigen::VectorXd limit_high, + const Eigen::VectorXd maxVelocity, const Eigen::VectorXd maxAcceleration) +{ + if (limit_low.rows() != n_dof || limit_high.rows() != n_dof + || maxVelocity.rows() != n_dof || maxAcceleration.rows() != n_dof) { + return false; + } + + jointLimit_low = limit_low; + jointLimit_high = limit_high; + maxJointVelocity = maxVelocity; + maxJointAcceleration = maxAcceleration; + + ddotQmin = -maxJointAcceleration.array(); + ddotQmax = maxJointAcceleration.array(); + + return true; +} + +bool SNSAccelerationIK::setMaxJointAcceleration(const Eigen::VectorXd maxAcceleration) +{ + if (maxAcceleration.rows() != n_dof) { + return false; + } + + maxJointAcceleration = maxAcceleration; + ddotQmin = -maxAcceleration.array(); + ddotQmax = maxAcceleration.array(); + + return true; +} + +void SNSAccelerationIK::shapeJointAccelerationBound(const Eigen::VectorXd &actualJointConfiguration, + const Eigen::VectorXd &actualJointVelocities, double margin) { + + // it could be written using the Eigen::Array potentiality + double step, max, stop; + + for (int i = 0; i < n_dof; i++) { + // for the minimum bound + max = -maxJointAcceleration(i); + if (m_usePositionLimits) { + step = 2 * (jointLimit_low(i) - actualJointConfiguration(i) - actualJointVelocities(i)*loop_period) / (loop_period * loop_period); + stop = (-maxJointVelocity(i) - actualJointVelocities(i)) / loop_period; + // take the maximum + ddotQmin(i) = std::max({step, max, stop}); + } else { + 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) { + step = 2 * (jointLimit_high(i) - actualJointConfiguration(i) - actualJointVelocities(i)*loop_period) / (loop_period * loop_period); + stop = (maxJointVelocity(i) - actualJointVelocities(i)) / loop_period; + // take the minimum + ddotQmax(i) = std::min({step, max, stop}); + } else { + 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)); + } + } + + ddotQmin *= margin; + ddotQmax *= margin; +} + +int SNSAccelerationIK::getJointAcceleration(Eigen::VectorXd *jointAcceleration, + const std::vector &sot, const Eigen::VectorXd &jointConfiguration, const Eigen::VectorXd &jointVelocities) +{ + // This will only reset member variables if different from previous values + 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; + ddqSol.resize(n_dof); + taskScale = 1.0; + + if (n_tasks > 1) { + // if the tasks include a nullspace bias task + taskScaleCS = 1.0; + ddqCS = sot[1].desired; + } + + // set box constraints + baseIkSolver->setBounds(ddqLow, ddqUpp); + + if (n_tasks == 1) { + exitCode = baseIkSolver->solve(J, dJdq, ddx, &ddqSol, &taskScale); + scaleFactors[0] = taskScale; + } + else { + exitCode = baseIkSolver->solve(J, dJdq, ddx, ddqCS, &ddqSol, &taskScale, &taskScaleCS); + scaleFactors[0] = taskScale; + scaleFactors[1] = taskScaleCS; + } + + // store solution and scale factor + *jointAcceleration = ddqSol; + + if (exitCode != SnsIkBase::ExitCode::Success) + { + return -1; + } + + return 1; +} + + + +} // namespace sns_ik diff --git a/sns_ik_lib/src/sns_ik.cpp b/sns_ik_lib/src/sns_ik.cpp index 0b91cc7..1e3e40f 100644 --- a/sns_ik_lib/src/sns_ik.cpp +++ b/sns_ik_lib/src/sns_ik.cpp @@ -24,6 +24,7 @@ #include #include #include +#include namespace sns_ik { @@ -41,6 +42,8 @@ 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"; } @@ -231,6 +234,13 @@ bool SNS_IK::setVelocitySolveType(VelocitySolveType type) { m_ik_vel_solver = std::shared_ptr(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(new SNSVelIKBaseInterface(m_chain.getNrOfJoints(), m_loopPeriod)); + m_ik_acc_solver = std::shared_ptr(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; @@ -238,6 +248,7 @@ bool SNS_IK::setVelocitySolveType(VelocitySolveType type) { m_ik_vel_solver->setJointsCapabilities(m_lower_bounds.data, m_upper_bounds.data, m_velocity.data, m_acceleration.data); m_ik_pos_solver = std::shared_ptr(new SNSPositionIK(m_chain, m_ik_vel_solver, m_eps)); + m_solvetype = type; m_initialized = true; return true; @@ -290,7 +301,7 @@ int SNS_IK::CartToJntVel(const KDL::JntArray& q_in, const KDL::Twist& v_in, jacobian.resize(q_in.rows()); if (m_jacobianSolver->JntToJac(q_in, jacobian) < 0) { - std::cout << "JntToJac failed" << std::endl; + ROS_ERROR("JntToJac failed!"); return -1; } @@ -362,6 +373,230 @@ bool SNS_IK::nullspaceBiasTask(const KDL::JntArray& q_bias, return true; } +/** + * Method to return the jacobian matrix. + * + * @param jnt_pos_in: vector of joint positions + * @param jacobianOut: the jacobian dot matrix for the given joint angles and velocities + * @param tipName: std string holding the tip name. + * + * @return true: if jacobianOut is successfully loaded with the desired matrix. + * + */ +bool SNS_IK::getJacobian(const KDL::JntArray& jnt_pos_in, Eigen::MatrixXd *jacobianOut) +{ + // Get the Jacobian corresponding to current joint position + KDL::Jacobian jacobian; + jacobian.resize(jnt_pos_in.rows()); + if (m_jacobianSolver->JntToJac(jnt_pos_in, jacobian) < 0) + { + ROS_ERROR("JntToJac failed!"); + return false; + } + + *jacobianOut = jacobian.data; + + return true; +} + +/** + * Method to return the jacobian dot matrix. + * + * @param jnt_pos_in: vector of joint positions + * @param jnt_vel_in: vector of joint velocities + * @param jacobianDotOut: the jacobian dot matrix for the given joint angles and velocities + * @param tipName: std string holding the tip name. + * + * @return true: if jacobianDotOut is successfully loaded with the desired matrix. + * + */ +bool SNS_IK::getJacobianDot(const KDL::JntArray& jnt_pos_in, const KDL::JntArray& jnt_vel_in, + Eigen::MatrixXd *jacobianDotOut, const std::string & tipName) +{ + // copy joint positions and velocities into kdl JntArray + size_t numJnt = jnt_pos_in.rows(); + + // Get the Jacobian corresponding to current joint position + KDL::Jacobian jacobian; + jacobian.resize(jnt_pos_in.rows()); + if (m_jacobianSolver->JntToJac(jnt_pos_in, jacobian) < 0) + { + ROS_ERROR("JntToJac failed!"); + return false; + } + + // Get Linear/Angular Jacobians + Eigen::MatrixXd Jv, Jw; + Jv = jacobian.data.block(0,0,3,numJnt); + Jw = jacobian.data.block(3,0,3,numJnt); + + /// Compute Jacobian Dot + KDL::Jacobian jacobianDot; + 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::MatrixXd w = Eigen::MatrixXd::Zero(3, numJnt); + + Eigen::MatrixXd Jdw = Eigen::MatrixXd::Zero(3, numJnt); + Eigen::MatrixXd Jdv = Eigen::MatrixXd::Zero(3, numJnt); + + // get FK of the intermediate links + std::vector 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++) { + KDL::Frame transformTemp; + fwdKin.JntToCart(q, 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)); + + // 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++) { + 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; + + // compute Jdw + Eigen::Vector3d Jw_i = Jw.col(i); + Jdw.col(i) = w_i_1.cross(Jw_i); + + /// compute quantities for Jdv + + // compute the position vector between consecutive links w.r.t base + if (i < numJnt - 1) { + p_0_i_1_i.col(i) = p_0_i_1.col(i+1) - p_0_i_1.col(i); + } + else { + p_0_i_1_i.col(i) = p_0_k - p_0_i_1.col(i); + } + + // 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_); + + // 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); + } + + // second pass -- compute Jdv + for (size_t i = 0; i < numJnt; i++) { + // sum of derivative + for (size_t j = i; j < numJnt; j++) { + 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_); + } + + // assign Jdv and Jdw into Jacobian dot + jacobianDot.data.block(0, 0, 3, numJnt) = Jdv; + jacobianDot.data.block(3, 0, 3, numJnt) = Jdw; + + // Get the jacobian + *jacobianDotOut = jacobianDot.data; + + return true; +} + +int SNS_IK::CartToJntAcc(const KDL::JntArray& q_in, const KDL::JntArray& qdot_in, + const KDL::Twist& a_in, + const KDL::JntArray& q_bias, + const std::vector& biasNames, + const KDL::JntArray& q_acc_bias, + KDL::JntArray& qddot_out) +{ + if (!m_initialized) { + ROS_ERROR("SNS_IK was not properly initialized with a valid chain or limits."); + return -1; + } + + // compute Jacobian + KDL::Jacobian jacobian; + jacobian.resize(q_in.rows()); + if (m_jacobianSolver->JntToJac(q_in, jacobian) < 0) + { + ROS_ERROR("JntToJac failed!"); + return -1; + } + + // compute Jacobian dot + Eigen::MatrixXd jacobianDot; + if (!getJacobianDot(q_in, qdot_in, &jacobianDot)) + { + ROS_ERROR("Jacobian dot computation failed!"); + return -1; + } + + std::vector sot; + TaskAcc task; + task.jacobian = jacobian.data; + Eigen::VectorXd dJdq = jacobianDot*qdot_in.data; + task.dJdq = dJdq; + task.desired = Eigen::VectorXd::Zero(6); + + // twistEigenToKDL + for(size_t i = 0; i < 6; i++) + task.desired(i) = a_in[i]; + sot.push_back(task); + + // Calculate the nullspace goal as a configuration-space task. + // Creates a task Jacobian which maps the provided nullspace joints to + // the full joint state. + if (q_bias.rows()) { + TaskAcc task2; + std::vector indicies; + if (!nullspaceBiasTask(q_bias, biasNames, &(task2.jacobian), &indicies)) { + ROS_ERROR("Could not create nullspace bias task"); + return -1; + } + task2.desired = Eigen::VectorXd::Zero(q_bias.rows()); + for (size_t ii = 0; ii < q_bias.rows(); ++ii) { + // This calculates a "nullspace velocity". + // There is an arbitrary scale factor which will be set by the max scale factor. + task2.desired(ii) = m_nullspaceGain * (q_bias(ii) - q_in(indicies[ii])) / m_loopPeriod; + // TODO: may want to limit the NS velocity to 70-90% of max joint velocity + } + sot.push_back(task2); + } + + // Bias the joint velocities + // If the bias is the previous joint velocities, this is velocity damping + if(q_acc_bias.rows() == q_in.rows()) { + TaskAcc task2; + task2.jacobian = Eigen::MatrixXd::Identity(q_acc_bias.rows(), q_acc_bias.rows()); + task2.desired = Eigen::VectorXd::Zero(q_acc_bias.rows()); + for (size_t ii = 0; ii < q_acc_bias.rows(); ++ii) { + task2.desired(ii) = q_acc_bias(ii); + } + sot.push_back(task2); + } + + return m_ik_acc_solver->getJointAcceleration(&qddot_out.data, sot, q_in.data, qdot_in.data); +} + SNS_IK::~SNS_IK(){} bool SNS_IK::setMaxJointVelocity(const KDL::JntArray& vel) { @@ -381,6 +616,9 @@ bool SNS_IK::setMaxJointAcceleration(const KDL::JntArray& accel) { bool SNS_IK::getTaskScaleFactors(std::vector& scaleFactors) { scaleFactors = m_ik_vel_solver->getTasksScaleFactor(); + + if (scaleFactors.empty()) + scaleFactors = m_ik_acc_solver->getTasksScaleFactor(); return m_initialized && !scaleFactors.empty(); } diff --git a/sns_ik_lib/src/sns_ik_base.cpp b/sns_ik_lib/src/sns_ik_base.cpp index 37e4598..e4d0bcc 100644 --- a/sns_ik_lib/src/sns_ik_base.cpp +++ b/sns_ik_lib/src/sns_ik_base.cpp @@ -189,9 +189,12 @@ SnsIkBase::ExitCode SnsIkBase::computeTaskScalingFactor(const Eigen::MatrixXd& J Eigen::ArrayXd jntScaleFactorArr(nJnt_); Eigen::ArrayXd lowMargin = (qLow_ - b); Eigen::ArrayXd uppMargin = (qUpp_ - b); + 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; } else { // joint is constrained jntScaleFactorArr(i) = POS_INF; } diff --git a/sns_ik_lib/src/sns_vel_ik_base.cpp b/sns_ik_lib/src/sns_vel_ik_base.cpp index f7235aa..0f0ac62 100644 --- a/sns_ik_lib/src/sns_vel_ik_base.cpp +++ b/sns_ik_lib/src/sns_vel_ik_base.cpp @@ -136,10 +136,6 @@ SnsIkBase::ExitCode SnsVelIkBase::solve(const Eigen::MatrixXd& J, const Eigen::V ROS_ERROR("Failed to compute task scale!"); return taskScaleExit; } - if (tmpScale < MINIMUM_FINITE_SCALE_FACTOR) { // check that the solver found a feasible solution - ROS_ERROR("Task is infeasible! scaling --> zero"); - return ExitCode::InfeasibleTask; - } if (tmpScale > 1.0) { ROS_ERROR("Task scale is %f, which is more than 1.0", tmpScale); @@ -177,6 +173,11 @@ SnsIkBase::ExitCode SnsVelIkBase::solve(const Eigen::MatrixXd& J, const Eigen::V W = bestW; dqNull = bestDqNull; + if (bestTaskScale < MINIMUM_FINITE_SCALE_FACTOR) { // check that the solver found a feasible solution + ROS_ERROR("Task is infeasible! scaling --> zero"); + return ExitCode::InfeasibleTask; + } + // Update the linear solver if (setLinearSolver(J * W) != ExitCode::Success) { ROS_ERROR("Solver failed to set linear solver!"); diff --git a/sns_ik_lib/test/sns_ik_vel_test.cpp b/sns_ik_lib/test/sns_ik_vel_test.cpp index bdf4b28..e397b52 100644 --- a/sns_ik_lib/test/sns_ik_vel_test.cpp +++ b/sns_ik_lib/test/sns_ik_vel_test.cpp @@ -1,6 +1,6 @@ /*! \file sns_ik_vel_test.cpp * \brief Unit Test: sns_ik velocity solver - * \author Matthew Kelly + * \author Matthew Kelly, Andy Park */ /* * Copyright 2018 Rethink Robotics From 9e6dc9118f1a77a98e4c5a7e528877e7586c06da Mon Sep 17 00:00:00 2001 From: Andy Park Date: Fri, 28 Sep 2018 12:21:50 -0400 Subject: [PATCH 2/4] Added a missing unit test file that caused Travis to fail --- sns_ik_lib/test/sns_ik_acc_test.cpp | 327 ++++++++++++++++++++++++++++ 1 file changed, 327 insertions(+) create mode 100644 sns_ik_lib/test/sns_ik_acc_test.cpp diff --git a/sns_ik_lib/test/sns_ik_acc_test.cpp b/sns_ik_lib/test/sns_ik_acc_test.cpp new file mode 100644 index 0000000..3dab796 --- /dev/null +++ b/sns_ik_lib/test/sns_ik_acc_test.cpp @@ -0,0 +1,327 @@ +/*! \file sns_ik_acc_test.cpp + * \brief Unit Test: sns_ik acceleration solver + * \author Andy Park + */ +/* + * Copyright 2018 Rethink Robotics + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rng_utilities.hpp" +#include "sawyer_model.hpp" +#include + +/* + * Define a common interface to call both SNS and KDL solvers for acceleration IK + */ +typedef std::function IkSolver; + +/* + * This file provides a unit and regression test for the top-level SNS-IK solver, along with the + * various solvers that are derived from it. + */ + +/************************************************************************************************* + * Acceleration IK Test Params * + *************************************************************************************************/ + +// How many acceleration IK tests should be run? +static const int ACC_IK_TEST_COUNT = 250; + +// Should the acceleration IK tests print the results (beyond pass/fail) to terminal? +static const bool ACC_IK_TEST_VERBOSE = true; + +// Tolerance for checks on forward kinematics +static const double ACC_IK_TEST_FK_LIN_TOL = 1e-8; // meters per second^2 +static const double ACC_IK_TEST_FK_ANG_TOL = 1e-8; // radians per second^2 + +// Margins for joint angles and rates with respect to their limits +static const double TEST_ANGLE_MARGIN = 1e-1; +static const double TEST_RATE_MARGIN = 1e-1; + +/************************************************************************************************* + * Utilities Functions * + *************************************************************************************************/ + +struct AccTestResult { + ros::Duration solveTime; // duration spent in the solver across all calls + int exitCode; // exit code returned by the solver + double fkLinErr; // error in the linear speed + double fkAngErr; // error in the angular speed + bool fkValid; // did the forward-kinematics on the solution meet tolerance? + int nPass; // number of tests that passed + int nFail; // number of tests that failed + double taskScale; // scaling applied to the task +}; + +/*************************************************************************************************/ + +void forwardKinematicsAcceleration(const KDL::JntArray& q, const KDL::JntArray& dq, + const KDL::JntArray& ddq, sns_ik::SNS_IK pSolver, KDL::Twist *accFK) +{ + // compute Jacobian + Eigen::MatrixXd jacobian; + + if (!pSolver.getJacobian(q, &jacobian)) + { + ROS_ERROR("Jacobian computation failed!"); + } + + // compute Jacobian dot + Eigen::MatrixXd jacobianDot; + if (!pSolver.getJacobianDot(q, dq, &jacobianDot)) + { + ROS_ERROR("Jacobian dot computation failed!"); + } + + // compute the goal endpoint acc + Eigen::VectorXd accFKtmp = jacobian*ddq.data + jacobianDot*dq.data; + + accFK->vel(0) = accFKtmp(0); + accFK->vel(1) = accFKtmp(1); + accFK->vel(2) = accFKtmp(2); + accFK->rot(0) = accFKtmp(3); + accFK->rot(1) = accFKtmp(4); + accFK->rot(2) = accFKtmp(5); +} + + +/* + * Computes the maximum absolute error in the forward kinematics at the acceleration level. + * @param q: joint angles + * @param dq: joint rates + * @param ddq: joint accelerations + * @param accSns: expected acc at the end-effector + * @param[out] linErr: linear acc error + * @param[out] angErr: angular acc error + */ +void checkForwardKinematicAcceleration( const KDL::JntArray& q, const KDL::JntArray& dq, const KDL::JntArray& ddq, + const KDL::Twist& accSns, sns_ik::SNS_IK pSolver, double* linErr, double* angErr) +{ + if (!linErr) { ROS_ERROR("linErr is nullptr!"); return; } + if (!angErr) { ROS_ERROR("angErr is nullptr!"); return; } + + // get forward kinematics in acc + KDL::Twist accFk; + forwardKinematicsAcceleration(q, dq, ddq, pSolver, &accFk); + + // compute the difference + KDL::Twist twistErr = accSns - accFk; + + // compute the error in each component: + KDL::Vector rot = twistErr.rot; + KDL::Vector vel = twistErr.vel; + *linErr = rot.Norm(); + *angErr = vel.Norm(); +} + +/*************************************************************************************************/ + +/* + * A function to run a benchmarking test on the acceleration IK solvers. + * + * Structure: + * - generate N test poses in joint space + * - use forward kinematics to compute end-effector acc + * - solve the IK problem using ikSolver + * - measure and report solve time and success rate + * + * @param seed: seed to pass to the RNG on each call + * if seed == 0, then RNG seed is not updated + * @param chain: kinematic chain for the model system + * @param qLow: lower joint limits + * @param qUpp: upper joint limits + * @param vMax: maximum joint speed + * @param aMax: maximum joint acc + * @param fwdKin: forward kinematics solver + * @param ikSolver: inverse kinematics solver + * @return: acceleration solver test result + */ +AccTestResult runAccIkSingleTest(int seed, const KDL::JntArray& qLow, const KDL::JntArray& qUpp, + const KDL::JntArray& vMax, const KDL::JntArray& aMax, IkSolver& ikSolver, sns_ik::SNS_IK pSolver) +{ + // compute a random joint acceleration and acceleration for the solution and the corresponding test pose + KDL::JntArray qLowTmp = qLow; + KDL::JntArray qUppTmp = qUpp; + for (int i = 0; i < int(qLow.rows()); i++) { qLowTmp(i) = qLowTmp(i) + TEST_ANGLE_MARGIN; } + for (int i = 0; i < int(qUpp.rows()); i++) { qUppTmp(i) = qUppTmp(i) - TEST_ANGLE_MARGIN; } + KDL::JntArray qTest = sns_ik::rng_util::getRngBoundedJoints(seed, qLowTmp, qUppTmp); + seed = 0; // let the RNG update the sequence automatically after the first call + + KDL::JntArray vMin = vMax; + for (int i = 0; i < int(vMin.rows()); i++) { vMin(i) = -vMin(i); } + + KDL::JntArray qdLowTmp = vMin; + KDL::JntArray qdUppTmp = vMax; + for (int i = 0; i < int(qdLowTmp.rows()); i++) { qdLowTmp(i) = qdLowTmp(i) + TEST_RATE_MARGIN; } + for (int i = 0; i < int(qdUppTmp.rows()); i++) { qdUppTmp(i) = qdUppTmp(i) - TEST_RATE_MARGIN; } + KDL::JntArray dqTest = sns_ik::rng_util::getRngBoundedJoints(seed, qdLowTmp, qdUppTmp); + + KDL::JntArray aMin = aMax; + for (int i = 0; i < int(aMin.rows()); i++) { aMin(i) = -aMin(i); } + KDL::JntArray ddqTest = sns_ik::rng_util::getRngBoundedJoints(seed, aMin, aMax); + + // get forward kinematics in acc + KDL::Twist accFkTmp; + forwardKinematicsAcceleration(qTest, dqTest, ddqTest, pSolver, &accFkTmp); + + // initialize the output for the IK solver + KDL::JntArray ddqSolve(qTest.rows()); + double taskScale; + + // initialize the test result: + AccTestResult result; + + // run benchmarking: + ros::Time startTime = ros::Time::now(); + result.exitCode = ikSolver(qTest, dqTest, accFkTmp, ddqSolve, taskScale); + + result.solveTime += ros::Time::now() - startTime; + result.taskScale = taskScale; // primary task scale + KDL::Twist scaledAcc = accFkTmp * result.taskScale; + checkForwardKinematicAcceleration(qTest, dqTest, ddqSolve, scaledAcc, pSolver, &(result.fkLinErr), &(result.fkAngErr)); + result.fkValid = result.fkLinErr <= ACC_IK_TEST_FK_LIN_TOL && result.fkAngErr <= ACC_IK_TEST_FK_ANG_TOL; + if (result.fkValid && result.exitCode >= 0) { + result.nPass = 1; result.nFail = 0; + } else { + result.nPass = 0; result.nFail = 1; + } + return result; +} + + +/*************************************************************************************************/ + +/* + * @param seed: seed to pass to the RNG on each call + * if seed == 0, then seed is ignored + * @param fwdKin: forward kinematics solver for acceleration + * @param ikSolver: inverse kinematics solver for acceleration (to be tested) + * @param qLow: lower bounds on joints (for generating the test problem) + * @param qLow: upper bounds on joints (for generating the test problem) + * @param vMax: maximum joint speed (for generating the test problem) + * @param aMax: maximum joint acc (for generating the test problem) + * @param solverName: solver name, used for logging only + * @param nTest: number of tests to run + */ +void runGeneralAccIkTest(int seed, sns_ik::SNS_IK pSolver, IkSolver& ikSolver, + const KDL::JntArray& qLow, const KDL::JntArray& qUpp, + const KDL::JntArray& vMax, const KDL::JntArray& aMax, + std::string solverName, int nTest = ACC_IK_TEST_COUNT) +{ + // Set up the data structure for the results: + AccTestResult results; // accumulate results here + AccTestResult tmp; // individual test output here + + // run the test many times and accumulate the results + bool success = true; + for (int iTest = 0; iTest < nTest; iTest++) { + seed++; + tmp = runAccIkSingleTest(seed, qLow, qUpp, vMax, aMax, ikSolver, pSolver); + if (iTest == 0) { + results = tmp; + } else { + results.nPass += tmp.nPass; + results.nFail += tmp.nFail; + results.solveTime += tmp.solveTime; + results.fkLinErr += tmp.fkLinErr; + results.fkAngErr += tmp.fkAngErr; + ASSERT_LE(tmp.fkLinErr, ACC_IK_TEST_FK_LIN_TOL); + ASSERT_LE(tmp.fkAngErr, ACC_IK_TEST_FK_ANG_TOL); + } + } + + // Print out the results: + if (ACC_IK_TEST_VERBOSE) { + int nPass = results.nPass; + int nFail = results.nFail; + int nTotal = nPass + nFail; + double total = static_cast(nTotal); + ROS_INFO("Acceleration IK Test --> pass: %d, fail: %d -- %f ms -- linErr: %e, angErr: %e -- %s", + nPass, nFail, 1000.0 * results.solveTime.toSec() / total, + results.fkLinErr / total, results.fkAngErr / total, + solverName.c_str()); + }; + + // Final check: + ASSERT_TRUE(success); +} + +/*************************************************************************************************/ + +void runSnsAccIkTest(int seed, sns_ik::VelocitySolveType solverType) { + + // Create a sawyer model: + std::vector jointNames; + KDL::Chain sawyerChain = sns_ik::sawyer_model::getSawyerKdlChain(&jointNames); + KDL::JntArray qLow, qUpp, vMax, aMax; + sns_ik::sawyer_model::getSawyerJointLimits(&qLow, &qUpp, &vMax, &aMax); + + // Create a SNS-IK solver: + sns_ik::SNS_IK ikSolver(sawyerChain, qLow, qUpp, vMax, aMax, jointNames); + ikSolver.setVelocitySolveType(solverType); + + // Function template for acceleration IK solver + IkSolver invKin = [&ikSolver](const KDL::JntArray& qInit, const KDL::JntArray& dqInit, + const KDL::Twist& ddpGoal, KDL::JntArray& ddqSoln, double& taskScale) { + int exitCode = ikSolver.CartToJntAcc(qInit, dqInit, ddpGoal, ddqSoln); + std::vector taskScaleAcc; + ikSolver.getTaskScaleFactors(taskScaleAcc); + if (taskScaleAcc.empty()) { + ROS_ERROR("No task scale!"); + taskScale = -1.0; // This is what SNS-IK uses as an error flag... + } else { + taskScale = taskScaleAcc.front(); + } + return exitCode; + }; + + // Run the test: + /* + * Note: This seems to cause a segfault in ubuntu 16.04 with Eigen 3.3.4, but it + * works fine in Ubuntu 14.04 with Eigen 3.2.0. FIXME + */ + runGeneralAccIkTest(seed, ikSolver, invKin, qLow, qUpp, vMax, aMax, sns_ik::toStr(solverType)); +} + +/************************************************************************************************* + * Tests * + *************************************************************************************************/ + +/* + * Run the benchmark test on all five versions of the acceleration solver. + * Note: each test uses the same seed so that the test problems are identical for each solver. + */ +TEST(sns_ik, acc_ik_SNS_test) { + runSnsAccIkTest(23539, sns_ik::VelocitySolveType::SNS_Base_Acc); } + +/*************************************************************************************************/ + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv){ + ros::Time::init(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From c3e21d60c7888ea96be138c20e06f57c67b6c4d4 Mon Sep 17 00:00:00 2001 From: Andy Park Date: Fri, 28 Sep 2018 13:06:02 -0400 Subject: [PATCH 3/4] Correct a log level in the code --- sns_ik_lib/src/sns_acc_ik_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sns_ik_lib/src/sns_acc_ik_base.cpp b/sns_ik_lib/src/sns_acc_ik_base.cpp index c9f1270..1e7c03a 100644 --- a/sns_ik_lib/src/sns_acc_ik_base.cpp +++ b/sns_ik_lib/src/sns_acc_ik_base.cpp @@ -183,7 +183,7 @@ SnsIkBase::ExitCode SnsAccIkBase::solve(const Eigen::MatrixXd& J, const Eigen::V ddqNull = bestDdqNull; if (bestTaskScale < MINIMUM_FINITE_SCALE_FACTOR) { // check that the solver found a feasible solution - ROS_WARN("Task is infeasible! scaling --> zero"); + ROS_ERROR("Task is infeasible! scaling --> zero"); return ExitCode::InfeasibleTask; } From 66ad233af6c853e08fd9a1e37956ce49c0dc7828 Mon Sep 17 00:00:00 2001 From: Andy Park Date: Wed, 3 Oct 2018 11:20:31 -0400 Subject: [PATCH 4/4] Addressed the reviewers' comments - removed unnecessary variables in jacobian dot computation - removed hard-coded numbers - enabled sns acc ik by default (only called upon request) - other cleanups --- .../include/sns_ik/sns_acceleration_ik.hpp | 19 +---- sns_ik_lib/include/sns_ik/sns_ik.hpp | 9 +-- sns_ik_lib/include/sns_ik/sns_ik_base.hpp | 3 + sns_ik_lib/src/sns_acceleration_ik.cpp | 62 ++++++++++------ sns_ik_lib/src/sns_ik.cpp | 74 ++++++++----------- sns_ik_lib/src/sns_ik_base.cpp | 6 +- sns_ik_lib/test/sns_ik_acc_test.cpp | 18 ++--- 7 files changed, 92 insertions(+), 99 deletions(-) diff --git a/sns_ik_lib/include/sns_ik/sns_acceleration_ik.hpp b/sns_ik_lib/include/sns_ik/sns_acceleration_ik.hpp index 2119577..eb7afa1 100644 --- a/sns_ik_lib/include/sns_ik/sns_acceleration_ik.hpp +++ b/sns_ik_lib/include/sns_ik/sns_acceleration_ik.hpp @@ -52,10 +52,9 @@ class SNSAccelerationIK { // SNS Acceleration IK int getJointAcceleration(Eigen::VectorXd *jointAcceleration, const std::vector &sot, - const Eigen::VectorXd &jointConfiguration, const Eigen::VectorXd &jointVelocities); + const Eigen::VectorXd &jointConfiguration, const Eigen::VectorXd &jointVelocities); - std::vector getTasksScaleFactor() - { return scaleFactors; } + std::vector getTasksScaleFactor() const { return scaleFactors; } Eigen::VectorXd getJointLimitLow() { return jointLimit_low; } Eigen::VectorXd getJointLimitHigh() { return jointLimit_high; } @@ -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 @@ -84,19 +83,7 @@ class SNSAccelerationIK { std::vector scaleFactors; // variables for base acc ik solver - 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 diff --git a/sns_ik_lib/include/sns_ik/sns_ik.hpp b/sns_ik_lib/include/sns_ik/sns_ik.hpp index ba5333d..81748a8 100644 --- a/sns_ik_lib/include/sns_ik/sns_ik.hpp +++ b/sns_ik_lib/include/sns_ik/sns_ik.hpp @@ -33,8 +33,7 @@ namespace sns_ik { SNS_OptimalScaleMargin, SNS_Fast, SNS_FastOptimal, - SNS_Base, - SNS_Base_Acc + SNS_Base }; @@ -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, diff --git a/sns_ik_lib/include/sns_ik/sns_ik_base.hpp b/sns_ik_lib/include/sns_ik/sns_ik_base.hpp index 7e84c78..cb9ba6a 100644 --- a/sns_ik_lib/include/sns_ik/sns_ik_base.hpp +++ b/sns_ik_lib/include/sns_ik/sns_ik_base.hpp @@ -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; diff --git a/sns_ik_lib/src/sns_acceleration_ik.cpp b/sns_ik_lib/src/sns_acceleration_ik.cpp index 1f7194d..3d9f5c7 100644 --- a/sns_ik_lib/src/sns_acceleration_ik.cpp +++ b/sns_ik_lib/src/sns_acceleration_ik.cpp @@ -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); } @@ -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++) { @@ -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) { @@ -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) + << ", 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)); + } } } @@ -140,24 +150,36 @@ void SNSAccelerationIK::shapeJointAccelerationBound(const Eigen::VectorXd &actua int SNSAccelerationIK::getJointAcceleration(Eigen::VectorXd *jointAcceleration, const std::vector &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 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; } @@ -178,13 +200,9 @@ int SNSAccelerationIK::getJointAcceleration(Eigen::VectorXd *jointAcceleration, *jointAcceleration = ddqSol; if (exitCode != SnsIkBase::ExitCode::Success) - { return -1; - } return 1; } - - } // namespace sns_ik diff --git a/sns_ik_lib/src/sns_ik.cpp b/sns_ik_lib/src/sns_ik.cpp index 1e3e40f..c95b1b7 100644 --- a/sns_ik_lib/src/sns_ik.cpp +++ b/sns_ik_lib/src/sns_ik.cpp @@ -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 + std::string toStr(const sns_ik::VelocitySolveType& type) { switch (type) { case sns_ik::VelocitySolveType::SNS: @@ -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"; } @@ -234,13 +236,6 @@ bool SNS_IK::setVelocitySolveType(VelocitySolveType type) { m_ik_vel_solver = std::shared_ptr(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(new SNSVelIKBaseInterface(m_chain.getNrOfJoints(), m_loopPeriod)); - m_ik_acc_solver = std::shared_ptr(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; @@ -249,6 +244,10 @@ bool SNS_IK::setVelocitySolveType(VelocitySolveType type) { m_velocity.data, m_acceleration.data); m_ik_pos_solver = std::shared_ptr(new SNSPositionIK(m_chain, m_ik_vel_solver, m_eps)); + // set default acceleration IK solver + m_ik_acc_solver = std::shared_ptr(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; @@ -308,9 +307,9 @@ int SNS_IK::CartToJntVel(const KDL::JntArray& q_in, const KDL::Twist& v_in, std::vector 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); @@ -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); @@ -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 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 @@ -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); } @@ -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 @@ -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); @@ -617,6 +604,7 @@ bool SNS_IK::setMaxJointAcceleration(const KDL::JntArray& accel) { bool SNS_IK::getTaskScaleFactors(std::vector& 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(); diff --git a/sns_ik_lib/src/sns_ik_base.cpp b/sns_ik_lib/src/sns_ik_base.cpp index e4d0bcc..cd1281a 100644 --- a/sns_ik_lib/src/sns_ik_base.cpp +++ b/sns_ik_lib/src/sns_ik_base.cpp @@ -33,6 +33,7 @@ const double SnsIkBase::NEG_INF = std::numeric_limits::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", "[", "]"); /************************************************************************************************* @@ -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; } diff --git a/sns_ik_lib/test/sns_ik_acc_test.cpp b/sns_ik_lib/test/sns_ik_acc_test.cpp index 3dab796..3384713 100644 --- a/sns_ik_lib/test/sns_ik_acc_test.cpp +++ b/sns_ik_lib/test/sns_ik_acc_test.cpp @@ -78,10 +78,10 @@ struct AccTestResult { /*************************************************************************************************/ -void forwardKinematicsAcceleration(const KDL::JntArray& q, const KDL::JntArray& dq, +void forwardKinematicsAcceleration(const KDL::JntArray& q, const KDL::JntArray& dq, const KDL::JntArray& ddq, sns_ik::SNS_IK pSolver, KDL::Twist *accFK) { - // compute Jacobian + // compute Jacobian Eigen::MatrixXd jacobian; if (!pSolver.getJacobian(q, &jacobian)) @@ -98,7 +98,7 @@ void forwardKinematicsAcceleration(const KDL::JntArray& q, const KDL::JntArray& // compute the goal endpoint acc Eigen::VectorXd accFKtmp = jacobian*ddq.data + jacobianDot*dq.data; - + accFK->vel(0) = accFKtmp(0); accFK->vel(1) = accFKtmp(1); accFK->vel(2) = accFKtmp(2); @@ -197,7 +197,7 @@ AccTestResult runAccIkSingleTest(int seed, const KDL::JntArray& qLow, const KDL: // run benchmarking: ros::Time startTime = ros::Time::now(); result.exitCode = ikSolver(qTest, dqTest, accFkTmp, ddqSolve, taskScale); - + result.solveTime += ros::Time::now() - startTime; result.taskScale = taskScale; // primary task scale KDL::Twist scaledAcc = accFkTmp * result.taskScale; @@ -226,9 +226,9 @@ AccTestResult runAccIkSingleTest(int seed, const KDL::JntArray& qLow, const KDL: * @param solverName: solver name, used for logging only * @param nTest: number of tests to run */ -void runGeneralAccIkTest(int seed, sns_ik::SNS_IK pSolver, IkSolver& ikSolver, +void runGeneralAccIkTest(int seed, sns_ik::SNS_IK pSolver, IkSolver& ikSolver, const KDL::JntArray& qLow, const KDL::JntArray& qUpp, - const KDL::JntArray& vMax, const KDL::JntArray& aMax, + const KDL::JntArray& vMax, const KDL::JntArray& aMax, std::string solverName, int nTest = ACC_IK_TEST_COUNT) { // Set up the data structure for the results: @@ -284,7 +284,7 @@ void runSnsAccIkTest(int seed, sns_ik::VelocitySolveType solverType) { ikSolver.setVelocitySolveType(solverType); // Function template for acceleration IK solver - IkSolver invKin = [&ikSolver](const KDL::JntArray& qInit, const KDL::JntArray& dqInit, + IkSolver invKin = [&ikSolver](const KDL::JntArray& qInit, const KDL::JntArray& dqInit, const KDL::Twist& ddpGoal, KDL::JntArray& ddqSoln, double& taskScale) { int exitCode = ikSolver.CartToJntAcc(qInit, dqInit, ddpGoal, ddqSoln); std::vector taskScaleAcc; @@ -311,11 +311,11 @@ void runSnsAccIkTest(int seed, sns_ik::VelocitySolveType solverType) { *************************************************************************************************/ /* - * Run the benchmark test on all five versions of the acceleration solver. + * Run the benchmark test on the acceleration solvers. * Note: each test uses the same seed so that the test problems are identical for each solver. */ TEST(sns_ik, acc_ik_SNS_test) { - runSnsAccIkTest(23539, sns_ik::VelocitySolveType::SNS_Base_Acc); } + runSnsAccIkTest(23539, sns_ik::VelocitySolveType::SNS_Base); } /*************************************************************************************************/