Need help with Matrix calculations for robotic place operation

[JT3D]

Hi guys,

Just trying o get my head around the math’s for the following situation

I have picked up a part in a gripper with a robot using a vision system. The pose I have at this stage is rough so I refine it with a vision system and I get a more accurate pose with respect to the robot base frame while it’s attached to the gripper.

Now I need to place the part in some precision tooling that is in a fixed location and rotation with respect to the robot base frame. I know where the fixed tooling is and I know what rotations I want the part to be placed at, but I’m unsure of the maths to tell the robot where it needs go and place the part, that takes into account the parts refined pose with respect to the robot base that I calculated above while in the gripper?

I believe the translation part is just the difference between the two translation sets but I’m not sure on rotation. Do I multiply my refined pose by the inverse of the precision tooling pose or am I totally wrong?

The refined pose I have is a 4 x 4 matrix with respect to the robot base frame. All done in c++ but it’s more the pseudo maths I need help with

Thanks in advance

[crackwitz]

C++ is not a language for prototyping/exploring/learning. I’d suggest you figure this out in a more amenable language, such as Python. after you’ve figured it out, you can formulate it in C++. that would be a more sensible approach than simultaneously having to fight C++ and laying out the math.

[JT3D]

Fair point on above. No problems using Python as the prototyping tool. It more the pseudo code / approach I am looking for here so c++ implementation is not important. I can port it at a later date. It looks this should be relatively simple to solve but I am stuck on the rotation part of the maths

[crackwitz]

I’d suggest a schematic. make it look like a graph. nodes are frames. edges are transformations.

[JT3D]

Yeah - good idea - I presume you mean something like what is in the middle part of this image? (This is just an example)

[crackwitz]

actually, no, I find that graphic impossible to decipher.

I’m thinking something like this. I assumed a few things. those arrows are the transformations you have, e.g. pose of base in camera frame (or transformation from base to camera frame).

you can calculate other transformations from those, such as the poses of wrist, gripper, part, … in the base frame.

the point of this is to give you a cheat sheet so you get the orders of transformations right, and you can see when you need an inverted transformation.

base
cam
arm_wrist
gripper
part
tooling
cam base observed
cam arm_wrist observed
cam part observed
arm_wrist gripper construction
gripper part construction
base tooling defined

on CS Academy

[JT3D]

Very good - nice and simple. I’ll use this to explain what I am trying to work out

[JT3D]

Ok - hopefully this better explains the setup

Base = Robot Base Frame
Gripper = end effector
Part - part we are picking - We know it’s pose with respect to the robot base frame when it’s in the gripper
Place - Fixed tooling - known location with respect to the base frame of the robot
Camera - vision system that gets the pose of the object while it’s in the gripper with respect to the robot base frame

So assuming I know the part pose with respect to the base frame when it’s still being held in the gripper, and I know where I want to place my part (‘Place node’) at a certain orientation. How do I calculate the transformation to tell the robot to move so that the part ends up in the correct rotation and translation on the place node, while taking into account the part to base pose?

Is it?

Part2Base * inv (Place2Base) = transform I want?

[crackwitz]

note the direction of those arrows. I remember the situation from the last discussion. I realize I’m coming up with notations ad hoc and they might look conflicting because they’re taking shape as I deal with this. I don’t have formal education in robotics/mechatronics, just the usual university math and “introductory” computer graphics where they handwave over lots of the “craft” of math.

my sense for arrows in this thread is that an arrow goes from base to gripper, if you have the pose of the gripper relative to the base frame. I would say the transformation “originates” from the base and describes the gripper’s pose (goes forward/outward). the matrix describes the gripper’s pose in/relative to the base frame, but it also translates coordinates from the gripper frame to the base frame (is pulled “back down”).

now the camera/base situation… initially you have the base’s pose in the camera frame because that’s what the camera gives you. I’ll call that camera_base and my arrow in the graphic would go camera → base. it transforms coordinates from the base frame (right side of the matrix) into the camera frame (left side of matrix).

if you need base_cam (a matrix inversion), which expresses the cam’s pose in the base frame, or equivalently the transformation that converts things from the cam frame into the base frame.

I’d always think in terms of frames (forward/outward) rather than how things are converted from one frame to another (pulled back down).

now, assuming

• your robot (its API) can handle the math for its “changed” topology due to the attached gripper, i.e. its topology can be treated as transparent to you
• base is the native frame of the robot and its gripper
• you want the part pose equaling the place pose
• base_gripper is the variable to solve for, it commands the robot to move such that you get your desired outcome

place == part // goal in arbitrary frames
base_place == base_part // goal in the base frame
base_place == base_gripper * gripper_part // expanded into values we have and the variable we want
// note how     ..._gripper*gripper_...., the matrices lock into each other
// now we need to isolate base_gripper on the right side, so we "subtract"/divide gripper_part
(base_place) * (gripper_part⁻¹) == (base_gripper * gripper_part) * (gripper_part⁻¹) // multiply from the right, both sides
base_place * (gripper_part⁻¹) == base_gripper * (gripper_part * gripper_part⁻¹) // moving parens, you'll see...
base_place * (gripper_part⁻¹) == base_gripper * I // poof, an identity
base_place * (gripper_part⁻¹) == base_gripper

that’s the calculation:

base_gripper := (base_place) * (gripper_part⁻¹)

some more explanation:

base_place * (gripper_part⁻¹) == base_gripper
base_place *  part_gripper == base_gripper // that's the same but naming the legs "forward"
base_place * place_gripper == base_gripper // since place == part
base_part  *  part_gripper == base_gripper // since place == part

(gripper_part⁻¹) == part_gripper is where the gripper goes, if you are in the part frame, which equals the place frame… so that’s where the gripper goes in the place frame.

now checking your question…

@JT3D

Part2Base * inv (Place2Base) = transform I want?

// translating into my notation
base_part * inv (base_place) = transform // ?
base_part * place_base = transform
base_place * place_base = transform
base_base = transform

// assuming senses are mixed up
Part_Base * inv (Place_Base) = transform
Part_Base * base_place = transform
Part_place = transform

not quite then. you’d be getting an identity in the first part (base_base), or should want an identity (part = place) in the second half.

[JT3D]

OK - just trying to get my head around what you have here!! I’ll get back to you with some questions once I think I better understand what you are saying - probably the morning

Thanks again for the help

[Alejandro_Silvestri]

Hi @JT3D

Let’s talk about the math.

Review of what I understand your problem is:

• the robot base is the main reference system, let’s call it World, or w
• you can ask the robot to move the gripper, passing the desired gripper pose, a 4x4 matrix (or some other form with the same info) I want to name Twg, meaning Gripper pose in the World reference (w first, g second)
• the vision system tells you the part pose Twp, meaning Part pose in World reference
• you want the robot to place the part at the target pose in the machine Twt
• you need to compute Twg, in order to the part be placed in the exact right place (the target)

So, we have two important gripper poses:

• Twg1, the known gripper pose at the time the camera takes the photo to compute the part place Twp
• Twg2, the desired gripper pose you must compute to place the part in the target

You can get Twg2 in the following way:

# part displacement from the gripper
Tgp = Twg1⁻¹ * Twp

# Twt == Twg2 * Tgp
Twg2 = Twt * Tgp⁻¹