Fix swig wrapping of kinematic utils (#32)

* Fix swig wrapping of kinematic utils

* Use ptrdiff_t for Eigen::Index, add redundant solutions test

Author: johnwason

tesseract_python/swig/eigen_types.i

@@ -40,14 +40,22 @@ namespace Eigen
 %eigen_typemaps(Eigen::Vector3d);
 %eigen_typemaps(Eigen::Vector4d);
 %eigen_typemaps(Eigen::VectorXd);
+%eigen_typemaps(%arg(Eigen::VectorX<double>));
 %eigen_typemaps(Eigen::MatrixXd);
 %eigen_typemaps(%arg(Eigen::Matrix3Xd));
+%eigen_typemaps(%arg(Eigen::MatrixX2d));
 %eigen_typemaps(%arg(Eigen::Matrix<uint32_t,3,Eigen::Dynamic>));
 %eigen_typemaps(%arg(Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>));
 %eigen_typemaps(Eigen::VectorXi);
 %eigen_typemaps(Eigen::Matrix3d);
 %eigen_typemaps(Eigen::Matrix4d);
 
+// Assume that Eigen::Index can be treated as int64_t
+namespace Eigen {
+typedef ptrdiff_t Index;
+}
+%template(vector_eigen_index) std::vector<Eigen::Index>;
+
 /*
 //Workaround typemaps for Isometry3d
 

tesseract_python/swig/tesseract_common_python.i

@@ -244,4 +244,6 @@ namespace std
   %}
 }
 
-
+%template(isWithinPositionLimits) tesseract_common::isWithinPositionLimits<double>;
+%template(satisfiesPositionLimits) tesseract_common::satisfiesPositionLimits<double>;
+//%template(enforcePositionLimits) tesseract_common::enforcePositionLimits<double>;

tesseract_python/swig/tesseract_kinematics_python.i

@@ -63,6 +63,7 @@
 #include <tesseract_kinematics/core/kinematics_plugin_factory.h>
 #include <tesseract_kinematics/core/rep_factory.h>
 #include <tesseract_kinematics/core/rop_factory.h>
+#include <tesseract_kinematics/core/utils.h>
 
 #include <tesseract_kinematics/kdl/kdl_factories.h>
 #include <tesseract_kinematics/opw/opw_factory.h>
@@ -111,6 +112,8 @@ struct KinGroupIKInput;
 %shared_ptr(tesseract_kinematics::InvKinFactory)
 %include "tesseract_kinematics/core/kinematics_plugin_factory.h"
 
+
+
 // TODO
 //%shared_ptr(tesseract_kinematics::RobotWithExternalPositionerInvKin)
 //%shared_ptr(tesseract_kinematics::RobotOnPositionerInvKin)
@@ -120,7 +123,9 @@ struct KinGroupIKInput;
 //%shared_ptr(tesseract_kinematics::KDLInvKinChainNR)
 //%shared_ptr(tesseract_kinematics::OPWInvKin)
 
-
+%eigen_typemaps(%arg(tesseract_kinematics::VectorX<double>));
+%include "tesseract_kinematics/core/utils.h"
+%template(getRedundantSolutions) tesseract_kinematics::getRedundantSolutions<double>;
 
 
 

tesseract_python/tests/tesseract_environment/test_tesseract_environment_kingroup.py

@@ -3,7 +3,7 @@
 
 from tesseract_robotics.tesseract_environment import Environment
 from tesseract_robotics.tesseract_common import FilesystemPath, ManipulatorInfo
-from tesseract_robotics.tesseract_kinematics import KinGroupIKInput, KinGroupIKInputs
+from tesseract_robotics.tesseract_kinematics import KinGroupIKInput, KinGroupIKInputs, getRedundantSolutions
 from ..tesseract_support_resource_locator import TesseractSupportResourceLocator
 
 def get_environment():
@@ -46,4 +46,23 @@ def test_kinematic_group():
 
     np.testing.assert_allclose(invkin.flatten(),joint_vals)
 
+def test_tesseract_redundant_solutions_tesseract_function():
+    
+    env, manip_info, joint_names = get_environment()
+
+    kin_group = env.getKinematicGroup(manip_info.manipulator).release()
+
+    limits = kin_group.getLimits()
+    redundancy_indices = list(kin_group.getRedundancyCapableJointIndices())
+
+    import tesseract_robotics.tesseract_kinematics as tes_com
+    sol = np.ones((6,1))*np.deg2rad(5)
+    redun_sol = tes_com.getRedundantSolutions(sol, limits.joint_limits, redundancy_indices)
+    
+    assert len(redun_sol) == 2
+
+    assert np.allclose(redun_sol[0].flatten(), 
+        np.array([0.08726646, 0.08726646,  0.08726646, 0.08726646, 0.08726646, -6.19591884])) or \
+            np.allclose(redun_sol[0].flatten(), 
+        np.array([0.08726646, 0.08726646,  0.08726646, 0.08726646, 0.08726646, 6.19591884]))