Extended GeometricFrame with euler angle parameterization, issue #13

OrebroUniversity · Dec 28, 2016 · 19a9ba5 · 19a9ba5
1 parent 8203605
commit 19a9ba5
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 12 deletions.
diff --git a/hiqp_core/include/hiqp/geometric_primitives/geometric_frame.h b/hiqp_core/include/hiqp/geometric_primitives/geometric_frame.h
@@ -29,7 +29,9 @@ namespace hiqp
 namespace geometric_primitives
 {
 
-  /*! \brief Parameters: [x, y, z, qw, qx, qy, qz]
+  /*! \brief Parameters: [x, y, z]
+   *                     [x, y, z, ax, ay, az]
+   *                     [x, y, z, qw, qx, qy, qz]
    *  \author Marcus A Johansson */ 
   class GeometricFrame : public GeometricPrimitive {
   public:
@@ -43,8 +45,8 @@ namespace geometric_primitives
 
     int init(const std::vector<double>& parameters) {
       int size = parameters.size();
-      if (size != 7) {
-        printHiqpWarning("GeometricFrame requires 7 parameters, got " 
+      if (!(size == 3 || size == 6 || size == 7)) {
+        printHiqpWarning("GeometricFrame requires 3, 6 or 7 parameters, got " 
           + std::to_string(size) + "! Initialization failed!");
         return -1;
       }
@@ -55,12 +57,27 @@ namespace geometric_primitives
 
       eigen_c_ << kdl_c_(0), kdl_c_(1), kdl_c_(2);
 
-      double w = parameters.at(3);
-      double x = parameters.at(4);
-      double y = parameters.at(5);
-      double z = parameters.at(6);
-
-      q_ = Eigen::Quaternion<double>(w, x, y, z);
+      if (size == 6) {
+        double angle1 = parameters.at(3);
+        double angle2 = parameters.at(4);
+        double angle3 = parameters.at(5);
+
+        Eigen::Matrix3d m;
+        m = Eigen::AngleAxisd(angle1, Eigen::Vector3d::UnitX())
+            * Eigen::AngleAxisd(angle2, Eigen::Vector3d::UnitY())
+            * Eigen::AngleAxisd(angle3, Eigen::Vector3d::UnitZ());
+        q_ = Eigen::Quaternion<double>(m);
+
+      } else if (size == 7) {
+        double w = parameters.at(3);
+        double x = parameters.at(4);
+        double y = parameters.at(5);
+        double z = parameters.at(6);
+        q_ = Eigen::Quaternion<double>(w, x, y, z);
+
+      } else {
+        q_ = Eigen::Quaternion<double>(1.0, 0.0, 0.0, 0.0);
+      }
 
       Eigen::Vector3d ax = q_._transformVector(Eigen::Vector3d(1,0,0));
       axis_x_ = KDL::Vector(ax(0), ax(1), ax(2));

diff --git a/hiqp_ros/scripts/functional_tests/TDefGeomAlign/frame_with_frame_alignment.sh b/hiqp_ros/scripts/functional_tests/TDefGeomAlign/frame_with_frame_alignment.sh
@@ -4,15 +4,15 @@ type: 'frame'
 frame_id: 'gripper_r_base'
 visible: true
 color: [0, 0, 0, 1]
-parameters: [0, 0, 0.1, 0, 0, 0, 1]"
+parameters: [0, 0, 0.1, 0, 0, 0]"
 
 rosservice call /yumi/hiqp_joint_velocity_controller/set_primitive \
 "name: 'myframe2'
 type: 'frame'
-frame_id: 'gripper_l_base'
+frame_id: 'world'
 visible: true
 color: [0, 0, 0, 1]
-parameters: [0, 0, 0.1, 0, 0, 0, 1]"
+parameters: [0.5, 0, 0.3, 1.57079, 0, 0]"
 
 sleep 1.0