How to ensure the tip of the end effector is facing toward the center of the object

I'm using kuka youbot now. Current situation is that I have set the coordinate for the tip of it's end effector and an object is situated at the center of the workspace. Supposingly, when the tip of the end effector reaches the coordinate I set, it should face toward the center of my object. After I set the xyz coordinate in the inverse kinematic code, the tip of the end effector do move to the coordinate I set, but it do not face towards the center point of the object I set. So, may I know is there any way to ensure the tip of the end effector is facing toward the center of the object? Or is there a way to ensure the joint 3 to the tip of the end effector is in 90 degree from the x-axis?

Asked by Shuu on 2019-05-15 08:40:14 UTC

Comments

Answers

From kuka's website at youbot inverse kinematic :

The input data is a 6D-Pose consisting of the x-y-z coordinates and the roll-pitch-yaw orientation of the gripper

So if you know the coordinates of the center of your object you can find the roll, pitch and yaw required to face it and just specify it in the input.

EDIT as response to your comment :

do 6-D pose means that if the end effector move to a certain coordinate, it has 6 solution?

No, it means that the pose is not only { x; y; z} (3-D) but it's { x; y; z; roll; pitch; yaw} (6-D).

but how do I find the roll pitch and yaw required from the coordinate I put

If you want to find the orientation required to face the object you need the coordinates of your object and your end effector frame coordinates. You will need to do some steps :

Using the transformation of arm link 1 to arm link 4 that you've calculated, you have to convert the object coordinates from your global frame to the end_effector frame.
Once you have the object coordinates in end effector frame, you need to convert them from carthesian to spherical (you can have the conversion formulas here to have coordinates as { z; theta; phi}. theta being the angle around z axis and phi the angle around x axis).
Assuming that it's the direction of the x axis of your end effector frame that determine what your end effector faces : You know that if the end effector is facing the object then theta = 0 and phi=pi/2. You also know that phi and theta are the same as respectively roll and theta (you don't need the pitch).
Just by calculating the spherical coordinates you directly find the roll and yaw that needs to be applied to properly set the end effector, so just by substracting those roll and yaw to the roll and yaw calculated at step 1, you get the roll, pitch and yaw required in the input data of your inverse kinematic code.

Asked by Delb on 2019-05-15 09:39:54 UTC

Comments

do 6-D pose means that if the end effector move to a certain coordinate, it has 6 solution?
Next, I do know the coordinates of the center of my object, but how do I find the roll pitch and yaw required from the coordinate I put, in order for it to face it? I do get the roll pitch and yaw from the transformation of arm link 1 to arm link 4 though, which do not point toward the center point. So, do I change the rpy of joint 3 based on the rpy I got from there?

Asked by Shuu on 2019-05-15 10:09:26 UTC

I kind of have the idea of that, but I do not know how to write in my cpp code, since my coding is not that good. However, from my understanding, if the direction of x-axis of the end effector and joint 3 is parallel, I can use trigonometry function to calculate the angle I want for the joint 3 to bend in order to point toward the center point right? I have used trigonometry function to obtain the angle and add it into my joint 3, after measuring, I found out that the measured angle and my calculated angle is almost similar. So, I can apply trigonometry function for my joint 3 right? Just as the another question I asked, but my z-axis is different from the rosrun tf.

Asked by Shuu on 2019-05-16 22:55:49 UTC

x-axis of the end effector and joint 3 is parallel

Is it always true ?

I can apply trigonometry function for my joint 3 right?

If you managed to calculate the angle required with trigonomerty yes it's fine, there are always different methods to achieve something.

z-axis is different from the rosrun tf.

Which z-axis and what are the inputs tf of tf_echo ?

Same comment as in #q323393, maybe you should open a new question detailling your calculations and ask where it failed instead of adding a new question within another one.

Asked by Delb on 2019-05-17 02:18:06 UTC

The x axis of end effector and joint 3 will be parallel if only if the positive and negative distance from the reference frame is same.

Sorry, is z-coordinate*. The inputs of tf is from the source frame of arm link 0 to arm link 5.

Asked by Shuu on 2019-05-17 09:31:01 UTC