unit tests for spatial vectors

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