servo_jr tests

Servo tests now include servo_jr functionality and testing. Only tests
1st joint on each arm for now. It's neat. Give it a spin.

src/crtk_servo_test/include/crtk_motion.h

@@ -67,24 +67,42 @@ class CRTK_motion{
   char send_servo_cp_distance(tf::Vector3,float,time_t);
   char send_servo_cr_rot_time(tf::Vector3,float,float,time_t);
   char send_servo_cp_rot_angle(tf::Vector3,float,time_t);
+
   char send_servo_cr(tf::Transform);
   char send_servo_cp(tf::Transform);
+  char send_servo_jr(float*);
+  char send_servo_jp(float*);
   char send_servo_jr_grasp(float);
+
   void reset_servo_cr_updated();
   void reset_servo_cp_updated();
+  void reset_servo_jr_updated();
+  void reset_servo_jp_updated();
   void reset_servo_jr_grasp_updated();
+
+  char get_servo_jr_updated();
+  char get_servo_jp_updated();
   char get_servo_jr_grasp_updated();
   char get_servo_cr_updated();
   char get_servo_cp_updated();
+
   tf::Transform get_servo_cr_command();
   tf::Transform get_servo_cp_command();
+  void get_servo_jr_command(float*, int);
+  void get_servo_jp_command(float*, int);
   float get_servo_jr_grasp_command();
 
   time_t get_start_time();
   tf::Transform get_start_tf();
 
   char start_motion( time_t curr_time);
 
+  char set_home_pos(tf::Quaternion, tf::Vector3);
+  tf::Transform get_home_pos();
+
+  char go_to_pos(tf::Transform, time_t);
+  char go_to_jpos(float, time_t);
+
 private:
   tf::Transform measured_cp;
   tf::Transform measured_cv;
@@ -98,12 +116,20 @@ class CRTK_motion{
   tf::Transform servo_cr_command;
   tf::Transform servo_cp_command;
   float servo_jr_grasp_command;
+  float servo_jr_command[MAX_JOINTS];
+  float servo_jp_command[MAX_JOINTS];
+
   char servo_cr_updated;
   char servo_cp_updated;
+  char servo_jr_updated;
+  char servo_jp_updated;
   char servo_jr_grasp_updated;
 
   time_t motion_start_time;
   tf::Transform motion_start_tf;
+  float motion_start_js_pos[MAX_JOINTS];
+
+  tf::Transform home_pos;
 };
 
 #endif

src/crtk_servo_test/include/crtk_robot.h

@@ -62,6 +62,7 @@ class CRTK_robot{
     void publish_servo_cr(char);
     void publish_servo_cp(char);
     void publish_servo_jr_grasp(char);
+    void publish_servo_jr(char);
     void run();
   private:
     ros::Subscriber sub_measured_cp1;
@@ -73,7 +74,12 @@ class CRTK_robot{
     ros::Publisher pub_servo_cr2; 
     ros::Publisher pub_servo_cp1;
     ros::Publisher pub_servo_cp2; 
+    ros::Publisher pub_servo_jr1;
+    ros::Publisher pub_servo_jr2; 
+    ros::Publisher pub_servo_jp1;
+    ros::Publisher pub_servo_jp2; 
     ros::Publisher pub_servo_jr_grasp1;
     ros::Publisher pub_servo_jr_grasp2; 
 };
+
 #endif

src/crtk_servo_test/include/defines.h

@@ -34,11 +34,21 @@
 #include "tf/tf.h"
 #include <cmath> 
 #define MM_TO_M   * 0.001
-#define LOOP_RATE 1100          // Hz (TODO increase to 1000Hz? NOOOOOOO! will fail for _cp)
+#define LOOP_RATE 950         // Hz (TODO increase to 1000Hz? NOOOOOOO! will fail for _cp)
 
+#define RAVEN
+//#define DVRK
 
+#ifdef RAVEN
 #define MAX_JOINTS 7
+#elif  
+#define MAX_JOINTS 15
+#endif
+
+
+
 #define DEG_TO_RAD * M_PI/180
+#define RAD_TO_DEG * 180/M_PI
 
 #define STEP_TRANS_LIMIT  2 MM_TO_M     // change this if loop rate is changed
 #define STEP_ROT_LIMIT    3 DEG_TO_RAD  // change this if loop rate is changed
@@ -50,4 +60,4 @@ enum CRTK_robot_state_enum {CRTK_ENABLED, CRTK_DISABLED, CRTK_PAUSED, CRTK_FAULT
 
 
 
-#endif
+#endif

src/crtk_servo_test/include/servo_tests.h

@@ -86,4 +86,17 @@ int test_3_1(CRTK_robot *, time_t);
 // (functionality) rotate along Z axis for 45 degrees (both arms)
 //    Pass: Ask user
 int test_3_2(CRTK_robot *, time_t);
+
+
+// 3-3 Go home (command: servo_cp) 
+// (functionality) move back to home pose (both arms)
+//    Pass: Ask user
+int test_3_3(CRTK_robot *, time_t);
+
+
+// 4-1 Go home (command: servo_jr) 
+// (functionality) move 10 degrees in the shoulder
+//    Pass: Ask user
+int test_4_1(CRTK_robot *, time_t);
+
 #endif