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)));
}
