Added C++ Client (WPI-AIM#143)

* Added C++ Client

* Updated License

* Removing std namespace from C++ client package following best practices

* Restoring library files

* Adding missing return statment to Sensor client

CMakeLists.txt

@@ -125,11 +125,11 @@ if (${CMAKE_SYSTEM_NAME} MATCHES Linux)
         add_subdirectory(${PROJECT_SOURCE_DIR}/external/tf_function)
         # Call find catkin again, with REQUIRED this time, it's okay to write over previously
         # imported catkin variables
-        find_package(catkin QUIET COMPONENTS ambf_server ambf_msgs dvrk_arm razer_hydra cv_bridge image_transport)
+        find_package(catkin QUIET COMPONENTS ambf_server ambf_msgs dvrk_arm razer_hydra cv_bridge image_transport ambf_client)
               find_package(tf_function REQUIRED)
 
-        if(${ambf_server_FOUND} AND ${ambf_msgs_FOUND} AND ${dvrk_arm_FOUND})
-            message("-- *** FOUND AMBF_COMM, AMBF_MSGS and DVRK_ARM, ENABLING DVRK DEVICE SUPPORT")
+        if(${ambf_server_FOUND} AND ${ambf_msgs_FOUND} AND ${dvrk_arm_FOUND} AND ${ambf_client_FOUND})
+            message("-- *** FOUND AMBF_COMM, AMBF_MSGS, AMBF_CLIENT and DVRK_ARM, ENABLING DVRK DEVICE SUPPORT")
 
             set(AMBF_ROS_INCLUDE_DIRS ${AMBF_ROS_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
             set(AMBF_ROS_LIBRARIES ${AMBF_ROS_LIBRARIES} ${catkin_LIBRARIES})
@@ -138,7 +138,7 @@ if (${CMAKE_SYSTEM_NAME} MATCHES Linux)
             list(APPEND CHAI3D_DEFINITIONS -DC_ENABLE_AMBF_DVRK_DEVICE_SUPPORT)
 
         else()
-            message("-- *** AMBF_COMM, AMBF_MSGS and DVRK_ARM NOT FOUND")
+            message("-- *** AMBF_COMM, AMBF_MSGS, AMBF_CLIENT and DVRK_ARM NOT FOUND")
         endif()
 
         find_package(OpenCV QUIET)

ambf_controller/CMakeLists.txt

@@ -38,7 +38,8 @@
 
 
 # common settings required to build examples
-include_directories (raven2/include)
+include_directories (raven2/include
+		     dvrk/cpp_controller/include )
 
 add_executable (raven_controller raven2/include/ambf_defines.h
                                  raven2/include/ambf_controller.h
@@ -49,3 +50,30 @@ add_executable (raven_controller raven2/include/ambf_defines.h
                                  raven2/src/main.cpp )
 
 target_link_libraries (raven_controller ${AMBF_LIBRARIES})
+
+set (PSM_IK_HDR_FILES
+    dvrk/cpp_controller/include/psm_ik_test.h
+    dvrk/cpp_controller/include/DH.h
+    dvrk/cpp_controller/include/Utilities.h)
+
+set(PSM_IK_SOURCE_FILES
+    dvrk/cpp_controller/src/psm_ik_test.cpp
+    dvrk/cpp_controller/src/DH.cpp
+    dvrk/cpp_controller/src/Utilities.cpp)
+
+add_executable (psm_ik_test  ${PSM_IK_HDR_FILES} ${PSM_IK_SOURCE_FILES})
+target_link_libraries (psm_ik_test ${AMBF_LIBRARIES} ${AMBF_CLIEMT_LIBRARIES} ${GLFW_LIBRARIES} ${Boost_LIBRARIES} ambf_client_cpp)
+
+
+set (ECM_IK_HDR_FILES
+    dvrk/cpp_controller/include/ecm_ik_test.h
+    dvrk/cpp_controller/include/DH.h
+    dvrk/cpp_controller/include/Utilities.h)
+
+set(ECM_IK_SOURCE_FILES
+    dvrk/cpp_controller/src/ecm_ik_test.cpp
+    dvrk/cpp_controller/src/DH.cpp
+    dvrk/cpp_controller/src/Utilities.cpp)
+
+add_executable (ecm_ik_test  ${ECM_IK_HDR_FILES} ${ECM_IK_SOURCE_FILES})
+target_link_libraries (ecm_ik_test ${AMBF_LIBRARIES} ${AMBF_CLIEMT_LIBRARIES} ${GLFW_LIBRARIES} ${Boost_LIBRARIES} ambf_client_cpp)

ambf_controller/dvrk/cpp_controller/include/DH.h

@@ -0,0 +1,29 @@
+#ifndef DH_H
+#define DH_H
+
+#include<iostream>
+#include<Eigen/Dense>
+
+using namespace Eigen;
+
+class DH
+{
+public:
+    DH(const double alpha, const double a, const double theta, const double d, const double offset, const std::string joint_type)
+        : alpha_(alpha), a_(a), theta_(theta), d_(d), offset_(offset), joint_type_(joint_type) {}
+
+    Matrix4f mat_from_dh(double alpha, double a, double theta, double d, double offset);
+    Matrix4f get_trans();
+
+    ~DH(void);
+private:
+    const double alpha_{0.0};
+    const double a_{0.0};
+    const double theta_{0.0};
+    const double d_{0.0};
+    const double offset_{0.0};
+    const std::string joint_type_;
+};
+
+
+#endif // DH_H

ambf_controller/dvrk/cpp_controller/include/Utilities.h

@@ -0,0 +1,24 @@
+#ifndef UTILITIES_H
+#define UTILITIES_H
+
+#include<iostream>
+#include<Eigen/Dense>
+#include<Eigen/Eigen>
+#include<Eigen/Core>
+#include<cmath>
+#include <stdlib.h>
+
+using namespace Eigen;
+class Utilities
+{
+public:
+    Utilities();
+    float get_angle(Vector3f vec_a, Vector3f vec_b, Vector3f up_vector);
+    float get_random_between_range(float low, float high);
+    Eigen::Matrix3f rotation_from_euler(float roll, float pitch, float yaw);
+    Eigen::Vector3f rpy_from_rotation(Eigen::Matrix3f R);
+    Eigen::Matrix4f get_frame(Eigen::Matrix3f R, Vector3f T);
+    ~Utilities(void);
+};
+
+#endif // UTILITIES_H

ambf_controller/dvrk/cpp_controller/include/ecm_ik_test.h

@@ -0,0 +1,52 @@
+#ifndef ECM_H
+#define ECM_H
+
+#include<vector>
+#include<cmath>
+#include<iostream>
+
+#include "ambf_client/ambf_client.h"
+#include "DH.h"
+#include "Utilities.h"
+
+enum class JointType{
+    ROTATIONAL,
+    PRISMATIC,
+};
+
+
+class ECM
+{
+public:
+    ECM();
+    Matrix4f computeFK(std::vector<float> joint_pos);
+    std::vector<float> computeIK(Matrix4f T_4_0);
+    void testIK();
+
+    void cleanup();
+    ~ECM(void);
+
+private:
+    std::vector<DH *> DH_Vector_;
+    const float L_rcc_ = 0.3822;
+    const float L_scopelen_ = 0.385495;
+
+
+    float dh_params_[4][6] = {
+   //     alpha,      a,      theta,                d,                  offset,     joint_type
+        { M_PI_2,     0.0,    0.0,                  0.0,                M_PI_2,     0},
+        { -M_PI_2,    0.0,    0.0,                  0.0,                -M_PI_2,    0},
+        { M_PI_2,     0.0,    0.0,                  0.0,                -L_rcc_,    1},
+        { 0.0,        0.0,    0.0,                  L_scopelen_,            0.0,    0}
+    };
+
+    std::vector<std::vector<float>> ECM_JOINT_LIMITS_ = {
+        {  (float) (-91.96 * (M_PI / 180.0)),  (float) (91.96 * (M_PI / 180.0))},
+        {  (float) (-60.00 * (M_PI / 180.0)),  (float) (60.00 * (M_PI / 180.0))},
+        {                    0.0,   0.24               },
+        { (float) (-175.00 * (M_PI / 180.0)), (float) (175.00 * (M_PI / 180.0))}
+    };
+
+};
+
+#endif // ECM_H

ambf_controller/dvrk/cpp_controller/include/psm_ik_test.h

@@ -0,0 +1,79 @@
+#ifndef PSM_H
+#define PSM_H
+
+#include "DH.h"
+#include "Utilities.h"
+#include "ambf_client/ambf_client.h"
+#include<vector>
+#include<cmath>
+#include<iostream>
+
+enum class JointType{
+    ROTATIONAL,
+    PRISMATIC,
+};
+
+
+//THIS IS THE FK FOR THE PSM MOUNTED WITH THE LARGE NEEDLE DRIVER TOOL. THIS IS THE
+//SAME KINEMATIC CONFIGURATION FOUND IN THE DVRK MANUAL. NOTE, JUST LIKE A FAULT IN THE
+//MTM's DH PARAMETERS IN THE MANUAL, THERE IS A FAULT IN THE PSM's DH AS WELL. BASED ON
+//THE FRAME ATTACHMENT IN THE DVRK MANUAL THE CORRECT DH CAN FOUND IN THIS FILE
+
+//ALSO, NOTICE THAT AT HOME CONFIGURATION THE TIP OF THE PSM HAS THE FOLLOWING
+//ROTATION OFFSET W.R.T THE BASE. THIS IS IMPORTANT FOR IK PURPOSES.
+//R_7_0 = [ 0,  1,  0 ]
+//      = [ 1,  0,  0 ]
+//      = [ 0,  0, -1 ]
+//Basically, x_7 is along y_0, y_7 is along x_0 and z_7 is along -z_0.
+
+//You need to provide a list of joint positions. If the list is less that the number of joint
+//i.e. the robot has 6 joints, but only provide 3 joints. The FK till the 3+1 link will be provided
+
+
+
+class PSM
+{
+public:
+    PSM();
+    Matrix4f computeFK(std::vector<float> joint_pos);
+    std::vector<float> computeIK(Matrix4f T_7_0);
+    void testAmbfPsm();
+    std::vector<float> testIK(const std::vector<float> joint_angles);
+
+    void cleanup();
+//    std::vector<std::vector<float>> getJointsLimit() { return PSM_JOINT_LIMITS_; }
+
+    ~PSM(void);
+private:
+    std::vector<DH *> DH_Vector_;
+
+    const float L_rcc_ = 0.4389;
+    const float L_tool_ = 0.416;
+    const float L_pitch2yaw_ = 0.009; //Fixed length from the palm joint to the pinch joint
+    const float L_yaw2ctrlpnt_ = 0.0106; //Fixed length from the pinch joint to the pinch tip
+    const float L_tool2rcm_offset_= 0.0229; //Delta between tool tip and the Remote Center of Motion
+
+    float dh_params_[7][6] = {
+   //     alpha,      a,             theta,  d,              offset,     joint_type
+        { M_PI_2,     0.0,           0.0,    0.0,             M_PI_2,     0},
+        { -M_PI_2,    0.0,           0.0,    0.0,            -M_PI_2,     0},
+        { M_PI_2,     0.0,           0.0,    0.0,            -L_rcc_,     1},
+        { 0.0,        0.0,           0.0,    L_tool_,            0.0,     0},
+        { -M_PI_2,    0.0,           0.0,    0.0,            -M_PI_2,     0},
+        { -M_PI_2,    L_pitch2yaw_,  0.0,    0.0,            -M_PI_2,     0},
+        { -M_PI_2,    0.0,           0.0,    L_yaw2ctrlpnt_,  M_PI_2,     0}
+    };
+
+    std::vector<std::vector<float>> PSM_JOINT_LIMITS_ = {
+        {  (float) (-91.96 * (M_PI / 180.0)),  (float) (91.96 * (M_PI / 180.0))},
+        {  (float) (-60.00 * (M_PI / 180.0)),  (float) (60.00 * (M_PI / 180.0))},
+        {                                0.0,                              0.24},
+        { (float) (-175.00 * (M_PI / 180.0)), (float) (175.00 * (M_PI / 180.0))},
+        { (float) ( -90.00 * (M_PI / 180.0)), (float) ( 90.00 * (M_PI / 180.0))},
+        { (float) ( -85.00 * (M_PI / 180.0)), (float) ( 85.00 * (M_PI / 180.0))},
+        {                                0.0,                               0.0}
+    };
+};
+
+
+#endif // PSM_H

ambf_controller/dvrk/cpp_controller/src/DH.cpp

@@ -0,0 +1,34 @@
+#include "DH.h"
+
+
+Matrix4f DH::mat_from_dh(double alpha, double a, double theta, double d, double offset) {
+    Matrix4f mat;
+
+
+    float ca = cos(alpha);
+    float sa = sin(alpha);
+
+    if(joint_type_.compare("ROTATIONAL") == 0) {
+        theta += offset;
+    } else if(joint_type_.compare("PRISMATIC") == 0) {
+        d += offset;
+    } else nullptr;
+
+    float ct = cos(theta);
+    float st = sin(theta);
+
+
+    mat <<  ct     ,    -st    ,      0,          a,
+            st * ca,    ct * ca,    -sa,    -d * sa,
+            st * sa,    ct * sa,     ca,     d * ca,
+            0      ,          0,      0,           1;
+
+   return mat;
+}
+
+Matrix4f DH::get_trans() {
+    return mat_from_dh(alpha_, a_, theta_, d_, offset_);
+
+}
+
+DH::~DH(void) {}

ambf_controller/dvrk/cpp_controller/src/Utilities.cpp

@@ -0,0 +1,86 @@
+#include "Utilities.h"
+
+Utilities::Utilities() {}
+
+float Utilities::get_angle(Vector3f vec_a, Vector3f vec_b, Vector3f up_vector) {
+    float angle = 0.0;
+    vec_a.normalize();
+    vec_b.normalize();
+
+    Vector3f cross_ab = vec_a.cross(vec_b);
+    float vdot = vec_a.dot(vec_b);
+
+//    # Check if the vectors are in the same direction
+    if(1.0 - vdot < 0.000001) angle = 0.0;
+    else if (1.0 + vdot < 0.000001) angle = M_PI;
+    else angle = acos(vdot);
+
+    if(up_vector(0) != std::numeric_limits<float>::min() &&
+       up_vector(1) != std::numeric_limits<float>::min() &&
+       up_vector(2) != std::numeric_limits<float>::min()) {
+        float same_dir = cross_ab.dot(up_vector);
+
+        if(same_dir < 0.0) angle = -angle;
+    }
+
+    return angle;
+}
+
+float Utilities::get_random_between_range(float low, float high) {
+    if(high < low) {
+        std::cout << "Make sure low is equal or less than high" << std::endl;
+        return  std::numeric_limits<float>::min();
+    }
+
+    std::srand (time(NULL));
+    float seed = ((double)rand()) / ((double)RAND_MAX) * high + low;
+
+    return seed;
+}
+
+
+
+Eigen::Matrix3f Utilities::rotation_from_euler(float roll, float pitch, float yaw) {
+
+    // roll and pitch and yaw in radians
+    float su = std::sin(roll);
+    float cu = std::cos(roll);
+    float sv = std::sin(pitch);
+    float cv = std::cos(pitch);
+    float sw = std::sin(yaw);
+    float cw = std::cos(yaw);
+
+    Eigen::Matrix3f Rot_matrix(3, 3);
+    Rot_matrix(0, 0) = cv*cw;
+    Rot_matrix(0, 1) = su*sv*cw - cu*sw;
+    Rot_matrix(0, 2) = su*sw + cu*sv*cw;
+    Rot_matrix(1, 0) = cv*sw;
+    Rot_matrix(1, 1) = cu*cw + su*sv*sw;
+    Rot_matrix(1, 2) = cu*sv*sw - su*cw;
+    Rot_matrix(2, 0) = -sv;
+    Rot_matrix(2, 1) = su*cv;
+    Rot_matrix(2, 2) = cu*cv;
+
+    return Rot_matrix;
+}
+
+Eigen::Vector3f Utilities::rpy_from_rotation(Eigen::Matrix3f R) {
+    Eigen::Vector3f rpy;
+    rpy[0] = std::atan2(R(2, 1), R(2, 2));
+    rpy[1] = std::atan2(-R(2, 0), std::pow( R(2, 1)*R(2, 1) +R(2, 2)*R(2, 2), 0.5));
+    rpy[2] = std::atan2( R(1, 0), R(0, 0));
+
+    return rpy;
+}
+
+Eigen::Matrix4f Utilities::get_frame(Eigen::Matrix3f R, Vector3f T){
+    Eigen::Matrix4f Trans;
+    Trans.setIdentity();
+
+    Trans.block<3,3>(0,0) = R;
+    Trans.block<3,1>(0,3) = T;
+
+    return Trans;
+}
+
+Utilities::~Utilities(void) {}