private TransformationMatrix ComputeWristFrame(TransformationMatrix currentTarget)
{
DenseMatrix addInverse = (DenseMatrix)Joints.Last().Value.JointTransformation.DenseMatrix *Joint6ToFlangeTrafo.DenseMatrix;
DenseMatrix wrist = currentTarget.DenseMatrix * (DenseMatrix)addInverse.Inverse();
return(new TransformationMatrix(wrist));
}