Ok nearly there!! - it’s going to the right point now but the gripper is not going to the right orientation around the point any more
so what i did is…
I moved the offset that I wanted to the rhs like this
cv::Mat Pose::CalculateRobotPose(cv::Mat calibrationMat, cv::Mat offsetMat, cv::Mat cameraPose)
{
Mat r = (calibrationMat * (cameraPose * **offsetMat** ));
return r.clone();
}
I also adjusted the offset matrix so that the rotation part of the cameraPose is is now part of this matrix.
But now the problem is the gripper of the robot is not going to the correct orientation around the new offset point. Is that because I have left the rotation in the Camera Pose mat as well as the object pose mat rotation -
Should the cameraPose have all 0’s for the 3x3 rotation part and the only place there should be a rotation is in the object pose?
My head is wrecked but I think we are close!!