private HomogeneousM neck2headKinectMatrix() { HomogeneousM neck2HeadKinect; double headPan = head.Pan; double headTilt = head.Tilt; HomogeneousM neckPan2Tilt; HomogeneousM tilt2xTraslation; HomogeneousM xTraslation2zTraslation; // Neck to Head Pan neckPan2Tilt = new HomogeneousM(headPan, 0.112, 0.0, MathUtil.PiOver2); // Head Tilt to Traslation in X ROBOT axis tilt2xTraslation = new HomogeneousM(headTilt, 0, 0.0337, -MathUtil.PiOver2); // X Traslation to Traslation in -Z ROBOT axis xTraslation2zTraslation = new HomogeneousM(-MathUtil.PiOver2, 0.052, 0, -MathUtil.PiOver2); return(neck2HeadKinect = new HomogeneousM(neckPan2Tilt.Matrix * tilt2xTraslation.Matrix * xTraslation2zTraslation.Matrix)); //HomogeneousM transNeckToServo = new HomogeneousM(0.0, 0.0, 0.0, 0.0, 0.0, 0.112); //HomogeneousM rotations = new HomogeneousM(headPan, headTilt, 0.0, 0.0, 0.0, 0.0); //HomogeneousM transServoToKnct = new HomogeneousM(0.0, 0.0, 0.0, 0.0337, 0.0, 0.052); //HomogeneousM rotToKnctRef = new HomogeneousM(-MathUtil.PiOver2, 0.0, -MathUtil.PiOver2, 0.0, 0.0, 0.0); //neck2HeadKinect = new HomogeneousM( transNeckToServo.Matrix * // rotations.Matrix * // transServoToKnct.Matrix * // rotToKnctRef.Matrix); return(neck2HeadKinect); }
public Vector3 TransRobot2LeftArm(Vector3 vector) { HomogeneousM leftArm2neck = new HomogeneousM(neck2LeftArmMatrix().Inverse); HomogeneousM neck2robot = new HomogeneousM(robot2neckMatrix().Inverse); HomogeneousM arm2robot = new HomogeneousM(leftArm2neck.Matrix * neck2robot.Matrix); return(arm2robot.Transform(vector)); }
private HomogeneousM neck2LeftArmMatrix() { HomogeneousM neck2leftArmMatrix; HomogeneousM translation = new HomogeneousM(0.0, 0.0, 0.0, 0.045, 0.213, -0.117); HomogeneousM rotation = new HomogeneousM(0.0, MathUtil.PiOver2, MathUtil.PiOver2, 0.0, 0.0, 0.0); neck2leftArmMatrix = new HomogeneousM(translation.Matrix * rotation.Matrix); return(neck2leftArmMatrix); }
private HomogeneousM neck2chestKinectMatrix() { HomogeneousM neck2ChestKinect; HomogeneousM xTranslation; HomogeneousM zTranslation; // Only translation in X and -Z robot axis //xzTranslation = new HomogeneousM(0, -0.08175, 0.0479, 0); xTranslation = new HomogeneousM(0, 0, 0.0479, 0); zTranslation = new HomogeneousM(MathUtil.PiOver2, -0.08175, 0, MathUtil.PiOver2); return(neck2ChestKinect = new HomogeneousM(xTranslation.Matrix * zTranslation.Matrix)); }
private HomogeneousM neck2chestKinectMatrix() { HomogeneousM neck2ChestKinect; double angle = MathUtil.ToRadians(56); HomogeneousM neck2rot = new HomogeneousM(0.0, -0.02, 0.05, MathUtil.PiOver2); HomogeneousM rot2edgeKnct = new HomogeneousM(-angle, 0.0, 0.09, -MathUtil.PiOver2); HomogeneousM edgeKnct2refKnct = new HomogeneousM(-MathUtil.PiOver2, 0.0, 0.0, -MathUtil.PiOver2); neck2ChestKinect = new HomogeneousM(neck2rot.Matrix * rot2edgeKnct.Matrix * edgeKnct2refKnct.Matrix); return(neck2ChestKinect); }
private HomogeneousM neck2LeftArmMatrix() { HomogeneousM neck2leftArmMatrix; HomogeneousM translationY; HomogeneousM translationX; HomogeneousM translationZ; translationY = new HomogeneousM(MathUtil.PiOver2, 0, 0.175, MathUtil.PiOver2); translationX = new HomogeneousM(-MathUtil.PiOver2, -0.027, 0, MathUtil.PiOver2); translationZ = new HomogeneousM(0, 0, 0.0974, 0); neck2leftArmMatrix = new HomogeneousM(translationY.Matrix * translationX.Matrix * translationZ.Matrix); return(neck2leftArmMatrix); }
private HomogeneousM robot2neckMatrix() { HomogeneousM robot2neck; HomogeneousM robot2torso; HomogeneousM torso2neck; double torsoElevation = this.torso.Elevation; double torsoPan = this.torso.Pan; // robot to torso transf ( traslation to star) robot2torso = new HomogeneousM(0, 0.4165, -0.11085, 0); // torso to neck ( torso pan , torso elevation, traslation ) torso2neck = new HomogeneousM(torsoPan, torsoElevation + .34195 + .04, .14125, 0); return(robot2neck = new HomogeneousM(robot2torso.Matrix * torso2neck.Matrix)); }
private HomogeneousM robot2neckMatrix() { HomogeneousM robot2neck; HomogeneousM robot2torso; HomogeneousM torso2neck; double torsoElevation = this.torso.Elevation; double torsoPan = this.torso.Pan; // -0.107 in X .- wheel To center of the post // 0.283 in Z .- floor to base of the post robot2torso = new HomogeneousM(0, 0.283, -0.107, 0); torsoElevation = 0.936; // base of the post to Neck torsoPan = 0.0; torso2neck = new HomogeneousM(torsoPan, torsoElevation, 0.0, 0); return(robot2neck = new HomogeneousM(robot2torso.Matrix * torso2neck.Matrix)); }
private HomogeneousM neck2headKinectMatrix() { HomogeneousM neck2HeadKinect; HomogeneousM neckPan2Tilt; HomogeneousM tilt2xTraslation; HomogeneousM xTraslation2zTraslation; double headPan = head.Pan; double headTilt = head.Tilt; // Neck to Head Pan neckPan2Tilt = new HomogeneousM(headPan, .25975, .0417, MathUtil.PiOver2); // Head Tilt to Traslation in X ROBOT axis tilt2xTraslation = new HomogeneousM(headTilt, 0, 0.0321, -MathUtil.PiOver2); // X Traslation to Traslation in -Z ROBOT axis xTraslation2zTraslation = new HomogeneousM(-MathUtil.PiOver2, 0.0688, 0, -MathUtil.PiOver2); return(neck2HeadKinect = new HomogeneousM(neckPan2Tilt.Matrix * tilt2xTraslation.Matrix * xTraslation2zTraslation.Matrix)); }
private HomogeneousM neck2minoruMatrix() { HomogeneousM neck2minoru; HomogeneousM neckPan2Tilt; HomogeneousM tilt2xTraslation; HomogeneousM xTraslation2zTraslation; double headPan = this.head.Pan; double headTilt = this.head.Tilt; // Neck to Head Pan neckPan2Tilt = new HomogeneousM(headPan, .25975, .0417, MathUtil.PiOver2); // Head Tilt to Traslation in X ROBOT axis tilt2xTraslation = new HomogeneousM(headTilt, 0, .08135, -MathUtil.PiOver2); // X Traslation to Traslation in -Z ROBOT axis xTraslation2zTraslation = new HomogeneousM(0, -0.0145, 0, 0); return(neck2minoru = new HomogeneousM(neckPan2Tilt.Matrix * tilt2xTraslation.Matrix * xTraslation2zTraslation.Matrix)); }
private HomogeneousM robot2chestKinectMatrix() { HomogeneousM robot2chestNeck = new HomogeneousM(this.robot2neckMatrix().Matrix *neck2chestKinectMatrix().Matrix); return(robot2chestNeck); }
public Vector3 TransHeadKinect2Robot(Vector3 vector) { HomogeneousM robot2HeadkinectMatrix = new HomogeneousM(robot2neckMatrix().Matrix *neck2headKinectMatrix().Matrix); return(robot2HeadkinectMatrix.Transform(vector)); }
public Vector3 TransLeftArm2Robot(Vector3 vector) { HomogeneousM robot2leftArm = new HomogeneousM(robot2neckMatrix().Matrix *neck2LeftArmMatrix().Matrix); return(robot2leftArm.Transform(vector)); }
public Vector3 TransMinoru2Robot(Vector3 vector) { HomogeneousM robot2minoruMatrix = new HomogeneousM(robot2neckMatrix().Matrix *neck2minoruMatrix().Matrix); return(new Vector3(robot2minoruMatrix.Transform(vector))); }
public Vector3 TransChestKinect2Robot(Vector3 vector) { HomogeneousM chestKinect2robot = new HomogeneousM(robot2neckMatrix().Matrix *neck2chestKinectMatrix().Matrix); return(new Vector3(chestKinect2robot.Transform(vector))); }
public Vector3 TransRobot2Neck(Vector3 vector) { HomogeneousM neck2robotMatrix = new HomogeneousM(robot2neckMatrix().Inverse); return(new Vector3(neck2robotMatrix.Transform(vector))); }