unit tests for spatial vectors

Author: petercorke

unit_test/featherstone_test/ID.m

@@ -5,11 +5,14 @@
 
     for i = 1:model.NB
         [ XJ, S(:,i) ] = jcalc( model.pitch(i), q(i) );
+        XJ
+        S(:,i)
         vJ = SpatialVelocity(S(:,i)*qd(i));
         Xup(i) = XJ*model.Xtree(i);
         if model.parent(i) == 0
             v(i) = vJ;
             a(i) = Xup(i)*(-a_grav) + SpatialAcceleration(S(:,i)*qdd(i));
+            a
         else
             v(i) = Xup(i)*v(model.parent(i)) + vJ;
             a(i) = Xup(i)*a(model.parent(i)) ...
@@ -18,6 +21,10 @@
         end
         f(i) = model.I(i)*a(i) + cross( v(i), model.I(i)*v(i) );
     end
+
+    v
+    a
+    f
 
     for i = model.NB:-1:1
         tau(i,1) = S(:,i)' * double(f(i));

unit_test/featherstone_test/compare.m

@@ -1,5 +1,5 @@
-q = [0 0]
+q = [90 -90] * pi/180
 qd = [2 -3]
-qdd = [3 4];
+qdd = [3 4] 
 
 [idyn_rtb(q, qd, qdd); idyn_roy(q, qd, qdd)]

unit_test/featherstone_test/idyn_roy.m

@@ -13,6 +13,11 @@
 robot.I(1) = SpatialInertia( 1, [0.5 0 0], diag([0 0 0]) );
 robot.I(2) = SpatialInertia( 1, [0.5 0 0], diag([0 0 0]) );
 
+robot.Xtree(1)
+%robot.I(1)
+robot.Xtree(2)
+%robot.I(2)
+
 %% compute inverse dynamics
 tau = ID( robot, q, qd, qdd)' %, f_ext, grav_accn )
 end

unit_test/featherstone_test/idyn_rtb.m

@@ -1,6 +1,8 @@
 function tau = idyn_rtb(q, qd, qdd)
     twolink = [];
     mdl_twolink
+    
+    twolink.fast = false
 
-tau = twolink.rne(q, qd, qdd)
+    tau = twolink.rne(q, qd, qdd)
 end