Added SNS Acceleration IK Solver Interface with UnitTest

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)

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:
 

sns_ik_lib/include/sns_ik/sns_acceleration_ik.hpp

@@ -0,0 +1,91 @@
+/*! \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 <Eigen/Dense>
+#include <vector>
+#include <sns_ik/sns_acc_ik_base.hpp>
+
+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<TaskAcc> &sot,
+                             const Eigen::VectorXd &jointConfiguration, const Eigen::VectorXd &jointVelocities);
+
+    std::vector<double> getTasksScaleFactor() const { 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<double> scaleFactors;
+
+    // variables for base acc ik solver
+    SnsAccIkBase::uPtr baseIkSolver;
+};
+
+}  // namespace sns_ik
+
+#endif

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 <kdl/jntarray.hpp>
 #include <kdl/chainfksolverpos_recursive.hpp>
 #include <sns_ik/sns_position_ik.hpp>
+#include <sns_ik/sns_acceleration_ik.hpp>
 
 namespace sns_ik {
 
@@ -45,6 +46,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 +149,42 @@ 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 = "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<std::string>(),
+                          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<std::string>& 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 +210,7 @@ namespace sns_ik {
 
     std::vector<KDL::JntArray> m_solutions;
     std::shared_ptr<SNSVelocityIK> m_ik_vel_solver;
+    std::shared_ptr<SNSAccelerationIK> m_ik_acc_solver;
     std::shared_ptr<SNSPositionIK> m_ik_pos_solver;
     std::shared_ptr<KDL::ChainJntToJacSolver> m_jacobianSolver;
 

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;
 

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_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!");
@@ -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));