[complete unitesting of robot_util]

Author: fbailly

unitTesting/test_robot_utils.cpp

@@ -28,15 +28,23 @@ int main( void )
   /*Test set and get joint_limits_for_id */
   const double upper_lim(1);
   const double lower_lim(2);
-  robot_util->set_joint_limits_for_id(1.,lower_lim,upper_lim);
+  robot_util->set_joint_limits_for_id(1,lower_lim,upper_lim);
   if (robot_util->get_joint_limits_from_id(1).upper == upper_lim && robot_util->get_joint_limits_from_id(1).lower == lower_lim)
   {
       std::cout << "joint_limits_for_id works !" << std::endl;  }
   else 
   {
       std::cout << "ERROR: joint_limits_for_id does not work !" << std::endl;
   }
+  if (robot_util->cp_get_joint_limits_from_id(1).upper == upper_lim && robot_util->cp_get_joint_limits_from_id(1).lower == lower_lim)
+  {
+      std::cout << "cp_get_joint_limits_for_id works !" << std::endl;  }
+  else 
+  {
+      std::cout << "ERROR: cp_get_joint_limits_for_id does not work !" << std::endl;
+  }
 
+ 
   /*Test set and get name_to_id */
   const std::string joint_name("test_joint");
   const double joint_id(10);  
@@ -50,10 +58,13 @@ int main( void )
       std::cout << "ERROR: name_to_id does not work !" << std::endl;
   }
 
+  /*Test create_id_to_name_map */
+  robot_util->create_id_to_name_map();  
+
   /*Test set urdf_to_sot */
 
   dg::Vector urdf_to_sot(3);
-  urdf_to_sot << 1,3,2;
+  urdf_to_sot << 0,2,1;
   robot_util->set_urdf_to_sot(urdf_to_sot);
   if (urdf_to_sot == robot_util->m_dgv_urdf_to_sot )
   {
@@ -63,5 +74,94 @@ int main( void )
   {
       std::cout << "ERROR: urdf_to_sot does not work !" << std::endl;
   }
+  /*Test joints_urdf_to_sot and joints_sot_to_urdf */
+  dg::Vector q_urdf(3);
+  q_urdf << 10,20,30;
+  dg::Vector q_sot(3);
+  dg::Vector q_test_urdf(3);
+  robot_util->joints_urdf_to_sot(q_urdf, q_sot);
+  robot_util->joints_sot_to_urdf(q_sot, q_test_urdf);
+  if (q_urdf == q_test_urdf )
+  {
+      std::cout << "joints_urdf_to_sot and joints_sot_to_urdf work !" << std::endl; 
+  }
+  else 
+  {
+      std::cout << "ERROR: joints_urdf_to_sot or joints_sot_to_urdf do not work !" << std::endl;
+  }  
+
+  /*Test velocity_sot_to_urdf and velocity_urdf_to_sot */
+  dg::Vector q2_urdf(10);
+  dg::Vector v_urdf(9);
+  dg::Vector v_sot(9);
+  robot_util->velocity_urdf_to_sot(q2_urdf, v_urdf, v_sot);
+  robot_util->velocity_sot_to_urdf(q2_urdf, v_sot, v_urdf);
+  std::cout << "velocity_sot_to_urdf and velocity_urdf_to_sot work !" << std::endl;
+ 
+  /*Test base_urdf_to_sot and base_sot_to_urdf */
+  dg::Vector base_q_urdf(7);
+  dg::Vector base_q_sot(6);
+  robot_util->base_urdf_to_sot(base_q_urdf, base_q_sot);
+  robot_util->base_sot_to_urdf(base_q_sot, base_q_urdf);
+  std::cout << "base_urdf_to_sot and base_sot_to_urdf work !" << std::endl;
+
+  /*Test config_urdf_to_sot and config_sot_to_urdf */
+  dg::Vector q2_sot(9);
+  robot_util->config_urdf_to_sot(q2_sot,q2_urdf);
+  robot_util->config_sot_to_urdf(q2_urdf,q2_sot);
+  std::cout << "config_urdf_to_sot and config_sot_to_urdf work !" << std::endl;
+  
+  robot_util->display(std::cout);
+  robot_util->sendMsg("test",MSG_TYPE_ERROR_STREAM);
+
+  /*Test set_name_to_force_id of forceutil */
+  const std::string rf("rf");
+  const std::string lf("lf");
+  const std::string lh("lh");
+  const std::string rh("rh");
+
+  robot_util->m_force_util.set_name_to_force_id(rf,0);
+  robot_util->m_force_util.set_name_to_force_id(lf,1);
+  robot_util->m_force_util.set_name_to_force_id(lh,2);
+  robot_util->m_force_util.set_name_to_force_id(rh,3);
+
+  dg::Vector lf_lim(6);
+  lf_lim << 1,2,3,4,5,6;  
+  dg::Vector uf_lim(6);
+  uf_lim << 10,20,30,40,50,60;
+  robot_util->m_force_util.set_force_id_to_limits(1, lf_lim, uf_lim);
+  if( robot_util->m_force_util.get_id_from_name(rf) == 0 &&
+      robot_util->m_force_util.get_id_from_name(lf) == 1 &&
+      robot_util->m_force_util.get_id_from_name(lh) == 2 &&
+      robot_util->m_force_util.get_id_from_name(rh) == 3)
+  {
+      std::cout << "force_util set and get id_from_name work !" << std::endl; 
+  }
+  else 
+  {
+      std::cout << "ERROR: force_util set and get id_from_name do not work !" << std::endl;
+  } 
+  if( robot_util->m_force_util.get_name_from_id(0) == rf &&
+      robot_util->m_force_util.get_name_from_id(1) == lf &&
+      robot_util->m_force_util.get_name_from_id(2) == lh &&
+      robot_util->m_force_util.get_name_from_id(3) == rh)
+  {
+      std::cout << "force_util get name_from_id works !" << std::endl; 
+  }
+  else 
+  {
+      std::cout << "ERROR: force_util get name_from_id does not work !" << std::endl;
+  } 
+  if (robot_util->m_force_util.get_limits_from_id(1).upper == uf_lim &&
+      robot_util->m_force_util.get_limits_from_id(1).lower == lf_lim)
+  {
+      std::cout << "force_util set and get id to limits work !" << std::endl; 
+  }
+  else 
+  {
+      std::cout << "ERROR: force_util set and get id to limits works do not work !" << std::endl;
+  } 
+  robot_util->m_force_util.display(std::cout);
+  robot_util->m_foot_util.display(std::cout);
   return 0;
 }