Updated wrapped for parameterization input

.gitignore

@@ -1,3 +1,4 @@
 build*
 *.pyc
+*.asv
 **/.DS_Store

gpmp2.i

@@ -108,11 +108,11 @@ class GaussianProcessInterpolatorLinear {
 #include <gpmp2/kinematics/Arm.h>
 
 class Arm {
-  Arm(size_t dof, Vector a, Vector alpha, Vector d);
+  Arm(size_t dof, Vector a, Vector alpha, Vector d, gpmp2::Parameterization parameterization);
   Arm(size_t dof, Vector a, Vector alpha, Vector d,
-      const gtsam::Pose3& base_pose);
+      const gtsam::Pose3& base_pose, gpmp2::Parameterization parameterization);
   Arm(size_t dof, Vector a, Vector alpha, Vector d,
-      const gtsam::Pose3& base_pose, Vector theta_bias);
+      const gtsam::Pose3& base_pose, Vector theta_bias, gpmp2::Parameterization parameterization);
   // full forward kinematics
   Matrix forwardKinematicsPose(Vector jp) const;
   Matrix forwardKinematicsPosition(Vector jp) const;
@@ -123,6 +123,7 @@ class Arm {
   Vector d() const;
   Vector alpha() const;
   gtsam::Pose3 base_pose() const;
+  gpmp2::Parameterization parameterization() const;
 };
 
 // abstract pose2 mobile base class

gpmp2/kinematics/Arm.h

@@ -96,6 +96,7 @@ class GPMP2_EXPORT Arm
   const gtsam::Vector &d() const { return d_; }
   const gtsam::Vector &alpha() const { return alpha_; }
   const gtsam::Pose3 &base_pose() const { return base_pose_; }
+  const Parameterization &parameterization() const {return parameterization_; }
 
 private:
   /// Calculate the homogenous transformation and matrix for joint j with angle

gpmp2/kinematics/tests/testArm.cpp

@@ -595,11 +595,11 @@ TEST(Arm, KinovaGen3) {
 
   // expected joint velocities
   vi_exp.push_back(Vector3(0, 0, 0));                   // J1
-  vi_exp.push_back(Vector3(0, 0, 0));              // J2
-  vi_exp.push_back(Vector3(-0.0298, 0, -0.0298));  // J3
-  vi_exp.push_back(Vector3(-0.0294, -0.0006, -0.0285));  // J4
-  vi_exp.push_back(Vector3(-0.0916, -0.0006, -0.0493));  // J5
-  vi_exp.push_back(Vector3(-0.0916, -0.0006, -0.0493));  // J6
+  vi_exp.push_back(Vector3(0, 0, 0));                   // J2
+  vi_exp.push_back(Vector3(-0.0298, 0, -0.0298));       // J3
+  vi_exp.push_back(Vector3(-0.0294, -0.0006, -0.0285)); // J4
+  vi_exp.push_back(Vector3(-0.0916, -0.0006, -0.0493)); // J5
+  vi_exp.push_back(Vector3(-0.0916, -0.0006, -0.0493)); // J6
   vi_exp.push_back(Vector3(-0.0968, -0.0380, -0.0068)); // J7
 
   // fk no velocity

matlab/+gpmp2/generateArm.m

@@ -3,9 +3,13 @@
 %
 %   Usage: arm_model = GENERATEARM(arm_str)
 %   @arm_str       dataset string, existing datasets:
-%                  'SimpleTwoLinksArm', 'SimpleThreeLinksArm', 'WAMArm', 'PR2Arm'
+%                  'SimpleTwoLinksArm', 'SimpleThreeLinksArm', 'WAMArm',
+%                  'PR2Arm', 'KinovaGen3'
 %   @base_pose     arm's base pose, default is origin with no rotation
 %
+%   @theta_bias    bias of joint angle inputs
+%   
+%   @
 %   Output Format:
 %   arm_model      an ArmModel object, contains kinematics and model information
 
@@ -84,8 +88,8 @@
     alpha = [-pi/2,pi/2,-pi/2,pi/2,-pi/2,pi/2,0]';
     a = [0,0,0.045,-0.045,0,0,0]';
     d = [0,0,0.55,0,0.3,0,0.06]';
-    theta = [0, 0, 0, 0, 0, 0, 0]';
-    abs_arm = Arm(7, a, alpha, d, base_pose, theta);
+    theta_bias = [0, 0, 0, 0, 0, 0, 0]';
+    abs_arm = Arm(7, a, alpha, d, base_pose, theta_bias);
 
     % physical arm
     % sphere data [id x y z r]
@@ -122,8 +126,8 @@
     alpha = [-1.5708, 1.5708, -1.5708, 1.5708, -1.5708, 1.5708, 0]';
     a = [0.1, 0, 0, 0, 0, 0, 0]';
     d = [0, 0, 0.4, 0, 0.321, 0, 0]';
-    theta = [0, 1.5708, 0, 0, 0, 0, 0]';
-    abs_arm = Arm(7, a, alpha, d, base_pose, theta);
+    theta_bias = [0, 1.5708, 0, 0, 0, 0, 0]';
+    abs_arm = Arm(7, a, alpha, d, base_pose, theta_bias);
     % physical arm
     % sphere data [id x y z r]
     spheres_data = [...
@@ -171,8 +175,8 @@
     alpha = [pi/2, pi, pi/2, 1.0472, 1.0472, pi]';
     a = [0, 0.41, 0, 0, 0, 0]';
     d = [0.2755, 0, -0.0098, -0.2501, -0.0856, -0.2228]';
-    theta = [0, 0, 0, 0, 0, 0]';
-    abs_arm = Arm(6, a, alpha, d, base_pose, theta);
+    theta_bias = [0, 0, 0, 0, 0, 0]';
+    abs_arm = Arm(6, a, alpha, d, base_pose, theta_bias);
     % physical arm
     % sphere data [id x y z r]
     spheres_data = [...
@@ -225,10 +229,18 @@
             Point3(spheres_data(i,2:4)')));
     end
     arm_model = ArmModel(abs_arm, sphere_vec);
+
+elseif strcmp(arm_str, 'KinovaGen3')
+    % arm: Kinova Gen3 7DOF arm (modified DH parameterization)
+    alpha = [pi, pi/2, -pi/2, pi/2, -pi/2, pi/2, -pi/2]';
+    a = zeros(7,1);
+    d = [-0.2848, -0.0118, -0.4208, -0.0128, -0.3143, 0.0, -0.1674]';
+    theta_bias = [0, 0, 0, 0, 0, 0]';
+    abs_arm = Arm(7, a, alpha, d, base_pose, theta_bias);
 
 % no such dataset
 else
-    error('No such arm exist');
+    error('No such arm model exist');
 end
 
 end