private Vector3 TransHeadToRobot(Vector3 vectorInKinectCoords, double headPan)
{
//Transforms a coordinate in the kinect system to robot system considering only the head pan.
//Vision returns the coordinates considering the kinect in an horizontal position, i.e.
//it corrects the head tilt using the kinect accelerometers.
Vector3 temp = new Vector3(vectorInKinectCoords.Z, -vectorInKinectCoords.X, vectorInKinectCoords.Y);
//The order Z,-X, Y is to transform from kinect system to head system
//1.75 is the Jutina's height
//0.1 is the horizontal distance between Justinas kinematic center (point between tire axis)
// and the front of kinect
HomogeneousM R = new HomogeneousM(headPan, 1.75, 0.1, 0);
return(R.Transform(temp));
}
private HomogeneousM robot2neckMatrix()
{
HomogeneousM robot2neck;
HomogeneousM robot2torso;
HomogeneousM torso2neck;
double torsoElevation = 0.0;
double torsoPan = 0.0;
// 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 neck2headKinectMatrix()
{
HomogeneousM neck2HeadKinect;
HomogeneousM neckPan2Tilt;
HomogeneousM tilt2xTraslation;
HomogeneousM xTraslation2zTraslation;
double headPan = this.headPan;
double headTilt = this.headTilt;
// Neck to Head Pan
neckPan2Tilt = new HomogeneousM(this.headPan, .25975, .0417, MathUtil.PiOver2);
// Head Tilt to Traslation in X ROBOT axis
tilt2xTraslation = new HomogeneousM(this.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 neck2RightArmMatrix()
{
HomogeneousM neck2armMatrix;
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);
neck2armMatrix = new HomogeneousM(translationY.Matrix * translationX.Matrix * translationZ.Matrix);
return neck2armMatrix;
}
private Vector3 CalculateKinectToArm(double x, double y, double z, double headPan, double headTilt)
{
double x4, y4, z4;
x4 = -y;
y4 = z;
z4 = -x;
HomogeneousM headTiltM = new HomogeneousM(-Math.PI / 2 + headTilt, 0, -0.065, 0);
HomogeneousM headPanM = new HomogeneousM(headPan, 0.23, 0.0425, Math.PI / 2);
HomogeneousM result = new HomogeneousM(headPanM.Matrix * headTiltM.Matrix);
Vector3 pos = new Vector3(x4, y4, z4);
Vector3 resultPos = result.Transform(pos);
resultPos.X += 0.03;
resultPos.Y += 0.145;
Vector3 armPos = new Vector3(-resultPos.Z, resultPos.X, -resultPos.Y);
return armPos;
}
public Vector3 TransRightArm2Robot(Vector3 vector)
{
HomogeneousM robot2ra = new HomogeneousM(this.robot2neckMatrix().Matrix * this.neck2RightArmMatrix().Matrix);
return robot2ra.Transform(vector);
}
public Vector3 TransHeadKinect2Robot(Vector3 vector)
{
HomogeneousM robot2HeadkinectMatrix = new HomogeneousM(robot2neckMatrix().Matrix * neck2headKinectMatrix().Matrix);
return robot2HeadkinectMatrix.Transform(vector);
}
public Vector3 TransHeadKinect2RightArm(Vector3 vector)
{
HomogeneousM ra2hk = new HomogeneousM(this.neck2RightArmMatrix().Inverse * this.neck2headKinectMatrix().Matrix);
return ra2hk.Transform(vector);
}
private Vector3 TransHeadToRobot(Vector3 vectorInKinectCoords, double headPan)
{
//Transforms a coordinate in the kinect system to robot system considering only the head pan.
//Vision returns the coordinates considering the kinect in an horizontal position, i.e.
//it corrects the head tilt using the kinect accelerometers.
Vector3 temp = new Vector3(vectorInKinectCoords.Z, -vectorInKinectCoords.X, vectorInKinectCoords.Y);
//The order Z,-X, Y is to transform from kinect system to head system
//1.75 is the Jutina's height
//0.1 is the horizontal distance between Justinas kinematic center (point between tire axis)
// and the front of kinect
HomogeneousM R = new HomogeneousM(headPan, 1.75, 0.1, 0);
return R.Transform(temp);
}