Additional testing.

.travis.yml

@@ -120,4 +120,4 @@ script:
   - catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS )
   # Run the tests, ensuring the path is set correctly.
   - source devel/setup.bash
-  - catkin_make run_tests && catkin_make test
+  - travis_wait 400 catkin_make run_tests && catkin_make test

CMakeLists.txt

@@ -77,6 +77,14 @@ target_link_libraries(ik_demo
   ${catkin_LIBRARIES}
 )
 
+add_executable(one_to_many_demo examples/one_to_many_demo.cpp)
+
+target_link_libraries(one_to_many_demo
+  davinci_kinematics
+  davinci_kinematic_definitions
+  ${catkin_LIBRARIES}
+)
+
 
 file(GLOB_RECURSE SRC_FILES_CPP src/*.cpp)
 file(GLOB_RECURSE TEST_FILES_CPP test/*.cpp)
@@ -95,4 +103,4 @@ target_link_libraries(full_kinematics_test
   davinci_kinematics
   davinci_kinematic_definitions
   ${catkin_LIBRARIES}
-)
+)

examples/one_to_many_demo.cpp

@@ -0,0 +1,50 @@
+#include <cwru_davinci_kinematics/davinci_fwd_kinematics.h>
+#include <cwru_davinci_kinematics/davinci_inv_kinematics.h>
+#include <Eigen/Geometry>
+
+int main(int argc, char **argv){
+	Eigen::Vector3d x_vec = Eigen::Vector3d(0.0, 0.0,  1.0);
+	Eigen::Vector3d z_vec = Eigen::Vector3d(1.0, 0.0,  0.0);
+	Eigen::Vector3d t_vec = Eigen::Vector3d(0.0, 0.0, -0.1);
+
+	Eigen::Vector3d y_vec = z_vec.cross(x_vec);
+	Eigen::Affine3d des_gripper_affine;
+	Eigen::Matrix3d R;
+	R.col(0) = x_vec;
+	R.col(1) = y_vec;
+	R.col(2) = z_vec;
+	des_gripper_affine.linear() = R;
+	des_gripper_affine.translation() = t_vec;
+
+	std::cout << "What came in was\n" << des_gripper_affine.linear() << "\n\n" << des_gripper_affine.translation() << "\n\n";
+
+	davinci_kinematics::Inverse kin = davinci_kinematics::Inverse();
+	int s = kin.ik_solve(des_gripper_affine);
+	if(s < 1){
+		std::cout << "Inverse kinematics is rejecting.\n\n";
+	} else{
+		std::cout << "Inverse kinematics is OK.\n\n";
+	}
+
+	davinci_kinematics::Vectorq7x1 q_vec1 = kin.get_soln();
+
+	std::cout << "Joint vector\n" << q_vec1 << "\n\n";
+
+	davinci_kinematics::Forward fkin = davinci_kinematics::Forward();
+	Eigen::Affine3d output = fkin.fwd_kin_solve(q_vec1);
+
+	std::cout << "What came out was\n" << output.linear() << "\n\n" << output.translation() << "\n\n";
+
+	s = kin.ik_solve(des_gripper_affine);
+	if(s < 1){
+		std::cout << "Inverse kinematics is rejecting.\n\n";
+	} else{
+		std::cout << "Inverse kinematics is OK.\n\n";
+	}
+
+	q_vec1 = kin.get_soln();
+
+	std::cout << "Joint vector, again\n" << q_vec1 << "\n\n";
+
+	return 0;
+}

test/full_kinematics.cpp

@@ -127,7 +127,8 @@ TEST(davinci_kinematics, Full_Kinematics_JPJ)
   davinci_kinematics::Forward dvrk_forward;
   davinci_kinematics::Inverse dvrk_inverse;
 
-  double joint_steps(4);
+//TES: This only iterated over 4 steps for each joint. I took it up to 20.
+  double joint_steps(20);
   double dj[7];
   for (int index(0); index < 7; index++)
   {
@@ -234,7 +235,7 @@ TEST(davinci_kinematics, Full_Kinematics_JPJ)
   printf("The linear error was %f\n", ik_lin_error);
   printf("The rotational error was %f\n", ik_rot_error);
   // fully random
-  for (int i(0); i < 25; i++)
+  for (int i(0); i < 100; i++)
   {
     dvrk_forward.gen_rand_legal_jnt_vals(q_vec);