johnny-five/lib/kinect-arm.js

Firmata based Arduino Programming Framework.

var five = require("../lib/johnny-five.js"),
    temporal = require("temporal"),
    OpenNI = require("openni");

Object.mixin = function( receiver, supplier ) {
  return Object.getOwnPropertyNames( supplier ).reduce(function( receiver, property ) {
    return Object.defineProperty(
      receiver, property, Object.getOwnPropertyDescriptor( supplier, property )
    );
  }, receiver );
};

function Initial( defaults, initializer ) {
  [ defaults, initializer || {} ].reduce( Object.mixin, this );
}

function Vector( initializer ) {
  Initial.call(
    this, Vector.DEFAULTS, initializer || {}
  );
}

Vector.DEFAULTS = {
  x: 0, y: 0, z: 0
};

Vector.degrees = function( angle ) {
  return (angle * 180) / Math.PI;
};

Vector.sub = function( vec1, vec2 ) {
  return new Vector({
    x: vec1.x - vec2.x,
    y: vec1.y - vec2.y,
    z: vec1.z - vec2.z
  });
};

Vector.between = function( vec1, vec2 ) {
  return Math.acos(
    vec1.dot(vec2) / (vec1.mag() * vec2.mag())
  );
};

Vector.prototype.mag = function() {
  var x, y, z;
  x = this.x;
  y = this.y;
  z = this.z;
  return Math.sqrt( x * x + y * y + z * z );
};

Vector.prototype.distance = function( vec ) {
  var dx, dy, dz;
  dx = this.x - v.x;
  dy = this.y - v.y;
  dz = this.z - v.z;
  return Math.sqrt( dx * dx + dy * dy + dz * dz );
};

Vector.prototype.dot = function( vec ) {
  return this.x * vec.x + this.y * vec.y + this.z * vec.z;
};

Vector.prototype.cross = function( vec ) {
  var x = this.x,
      y = this.y,
      z = this.z;

  return new Vector({
    x: y * vec.z - vec.y * z,
    y: z * vec.x - vec.z * x,
    z: x * vec.y - vec.x * y
  });
};

Vector.prototype.angleTo = function( vec ) {
  return Math.atan2(
    this.y - vec.y, this.x - vec.x
  );
};

Vector.prototype.between = function( vec ) {
  return Math.acos(
    this.dot(vec) / (this.mag() * vec.mag())
  );
};

function Skeleton( initializer ) {
  Initial.call(
    this, Skeleton.DEFAULTS, initializer || {}
  );

  Skeleton.Joints.forEach(function( joint ) {
    this.points[ joint ] = new Vector();

    this.kinect.on( joint, function( id, x, y, z ) {
      var vec = this.points[ joint ];
      if ( vec ) {
        vec.x = x;
        vec.y = y;
        vec.z = z;
      }
    }.bind(this));
  }, this);

  Skeletons.push( this );
}

Skeleton.DEFAULTS = {
  points: {}
};

Skeleton.Joints = [
  "head",
  "neck",
  // "torso",
  // "waist",
  "left_shoulder",
  "left_elbow",
  "left_hand",
  "right_shoulder",
  "right_elbow",
  "right_hand",
  "left_hip",
  "left_knee",
  "left_foot",
  "right_hip",
  "right_knee",
  "right_foot"
];

var Skeletons  = [];


five.Board().on("ready", function() {
  var defs, servos, kinect;

  // http://www.ranchbots.com/robot_arm/images/arm_diagram.jpg
  defs = [
    // Pivot/Rotator
    { id: "rotator", pin:  6, range: [  0, 180 ], startAt: 90 },
    // Shoulder
    { id: "upper", pin:  9, range: [ 0, 180 ], startAt: 180 },
    // Elbow
    { id: "fore", pin: 10, range: [  90, 180 ], startAt: 90 },
    // Wrist
    { id: "wrist", pin: 11, range: [  10, 170 ], startAt: 60 },
    // Grip
    { id: "claw", pin: 12, range: [  10, 170 ], startAt: 0 }
  ];

  // Reduce the des array into an object of servo instances,
  // where the servo id is the property name
  servos = defs.reduce(function( accum, def ) {
    return (accum[ def.id ] = five.Servo( def )) && accum;
  }, {});

  // Make servos accessible by name in the REPL
  this.repl.inject( servos );

  kinect = OpenNI();

  [
    "newuser",
    "lostuser",
    "posedetected",
    "calibrationstart",
    "calibrationsuccess",
    "calibrationfail"
  ].forEach(function( type ) {
    kinect.on( type , function( id ) {
      console.log( "%s (%d)", type, id );

      if ( type === "newuser" ) {
        if ( Skeletons.length === 0 ) {
          new Skeleton({
            kinect: kinect
          });
        } else {
          Skeletons[0].inFrame = true;
        }
      }
      if ( type === "lostuser" ) {
        console.log( "OUT OF FRAME" );
        if ( Skeletons.length ) {
          Skeletons[0].inFrame = false;
        }
      }
    });
  });

  function scale() {
    return Math.round(
      five.Fn.scale.apply(null, arguments)
    );
  };

  function angleOf( vec1, vec2, axis ) {
    console.log( "args", vec1, vec2, axis  );

    var sub = Vector.sub( vec2, vec1 );
    var bet = Vector.between( sub, axis );
    var deg = Vector.degrees( deg );

    console.log( "products", sub, bet, deg );


    return deg;
    // return Vector.degrees(
    //   Vector.between(
    //     Vector.sub( vec2, vec1 ), axis
    //   )
    // );
  }

  function Change() {
    this.last = 0;
  }

  Change.prototype.isNoticeable = function(value) {
    if ( !Number.isFinite(value) ) {
      return false;
    }
    if ( (value > this.last + 2) || (value < this.last - 2) ) {
      this.last = value;
      return true;
    }
    return false;
  };

  var last = Date.now();
  var interval = 1000 / 4;
  var rlow = 0;
  var rhigh = 0;
  var change = {
    upper: new Change(),
    fore: new Change(),
    rotator: new Change()
  };


  void function main() {
    setImmediate( main );

    var values, right, orientation, angle, upper, fore, rotator, axis;

    values = {
      upper: 0,
      fore: 0,
      rotator: 0
    };

    angle = {
      upper: 0,
      fore: 0
    };

    if ( Skeletons.length && (points = Skeletons[0].points) ) {
      if ( points.right_shoulder && points.right_elbow ) {

        upper = points.right_shoulder;
        fore = points.right_elbow;
        rotator = points.right_elbow;
        axis = points.right_hip;

        if ( Date.now() > (last + interval) ) {

          if ( rlow === 0 || rlow > rotator.z ) {
            rlow = Math.round( rotator.z );
          }
          if ( rhigh === 0 || rhigh < rotator.z ) {
            rhigh = Math.round( rotator.z );
          }

          right = {
            upper: new Vector({ x: upper.x, y: upper.y }),
            fore: new Vector({ x: fore.x, y: fore.y }),
            rotator: new Vector({ x: rotator.x, y: rotator.y }),
            axis: new Vector({ x: axis.x, y: axis.y })
          };

          // console.log( right.axis );

          orientation = {
            torso: Vector.sub( right.upper, right.axis ),
            arm: Vector.sub( right.fore, right.upper )
          };

          angle.upper = Math.round(
            angleOf( right.fore, right.upper, orientation.torso )
          );


          angle.fore = Math.round(
            angleOf( right.rotator, right.fore, orientation.arm )
          );


          console.log("angleOf",  angleOf( right.rotator, right.fore, orientation.arm ) );



          values.rotator = scale( rotator.z, rlow, rhigh, 0, 180 );
          values.upper = scale( angle.upper, 0, 180, 180, 0 );
          values.fore = scale( angle.fore, 180, 0, 90, 180 );


          // console.log( angle.fore );

          // if ( change.rotator.isNoticeable( values.rotator ) ) {
          //   console.log( values.rotator );
          //   servos.rotator.move( values.rotator );
          // }

          // if ( change.upper.isNoticeable( values.upper ) ) {
          //   console.log( values.rotator );
          //   servos.upper.move( values.upper );
          // }

          if ( change.fore.isNoticeable( values.fore ) ) {
            console.log( values.rotator );
            servos.fore.move( values.fore );
          }
          last = Date.now();
        }
      }
    }
  }();


// References
// http://www.ranchbots.com/robot_arm/images/arm_diagram.jpg
// https://github.com/OpenNI/OpenNI/blob/master/Include/XnCppWrapper.h
// http://www.mrtmrcn.com/en/post/2011/11/08/Kinect-Part-5-Kinect-Skeleton-Tracking.aspx
// http://code.google.com/p/bikinect/source/browse/trunk/MappInect/Skeleton.pde
// https://github.com/Sensebloom/OSCeleton-examples/blob/master/processing/Stickmanetic/Stickmanetic.pde
// http://www.pcl-users.org/openni-device-h-47-26-fatal-error-XnCppWrapper-h-No-such-file-or-directory-td3174297.html
// http://kinectcar.ronsper.com/docs/openni/_xn_cpp_wrapper_8h_source.html

});