4
$\begingroup$

How do i compute all transformation matrices which places a robot endeffector at the shell of this sphere, with the end effector pointing toward the object in the center.

enter image description here

I know at all time how far the object is relative to the endeffector, and radius of the sphere is the desired distance i want between the object and endeffector.

I want by using inverse kinematics pan around this object in a sphere shaped trajectory.

Each transformation matrix should contain different positions on the sphere and the rotation should be oriented such that the arm looks at the object.

The position should be relative easy to compute, as i already know the distance to to object, and radius of the sphere.

But the rotation matrix for each position is still a mystery for me.

$\endgroup$
  • $\begingroup$ Maybe it's easier in spherical coordinates. $\endgroup$ – Bending Unit 22 Apr 23 '16 at 17:50
  • 1
    $\begingroup$ @BendingUnit22 Would you care to elaborate a bit more on your idea, how would you use spherical coordinates to form the rotational matrix? $\endgroup$ – test Apr 23 '16 at 20:16
  • 2
    $\begingroup$ Which end-effector axis do you want pointed at the center? $\endgroup$ – Ben Apr 24 '16 at 11:46
  • 1
    $\begingroup$ The endeffector has a camera on it, so i want to camera facing the object.. $\endgroup$ – test Apr 24 '16 at 12:03
  • 1
    $\begingroup$ @Ben the Z-axis of the camera should be facing the object $\endgroup$ – test Apr 24 '16 at 18:01
4
$\begingroup$

Remember that the columns of a rotation matrix are simply unit vectors indicating where each axis points. Lets say your end-effector is at $p_e = (x_e, y_e, z_e)$, and the center of the circle is at $p_o = (x_o, y_o, z_o)$. You didn't fully answer my comment about your end-effector axes, so I will assume you want $z_e$ pointed at the object. Here are the steps you should take.

First, some notation. The transform from the world frame to your desired end-effector is: $$ T^{world}_{ee} = \begin{bmatrix} R_e & p_e \\ 0 & 1 \end{bmatrix} $$

Where $R_e$ is the rotation matrix in question. It is composed of 3 column vectors:

$$ R_e = \begin{bmatrix} R_{x_e} & R_{y_e} & R_{z_e} \end{bmatrix} $$

  1. The third (Z) column of the rotation matrix is simply $R_{z_e} = \widehat{p_o - p_e}$.
  2. Pick a natural orientation for your camera. Perhaps $y_e$ down. So think about a plane created by the world down axis and $R_{z_e}$. Now $x_e$ must be normal to that plane. Assuming the world down axis is $down = (0,0,-1)$. Now $R_{x_e} = \widehat{down \times R_{z_e}}$. This is the first (X) column of your rotation matrix. (Note: be careful to avoid degenerate cases, like when the camera is directly above the object ($z_e = z_o$). You need to test for this case and handle this differently.)
  3. Lastly, the Y axis of your new frame must be the cross product of the other two, so: $R_{y_e} = R_{z_e} \times R_{x_e}$. This is the second (Y) column of your new rotation matrix.

This gives you a single orientation for each position on your sphere in which the object will be upright in the camera image.

Your question was a little vague on whether this is sufficient, or if you want many orientations for each point on the sphere. Since the camera can rotate about its focal axis, the arm and camera will still be pointing at the object. However, the object will be rotated in the image.

There are an infinite number of orientations that satisfy the constrains for each point, but you can get a discrete set by picking some angles from 0 to 360 degrees then simply rotating your end-effector frame about its Z axis by this amount.

$$ T^{world}_{newee} = T^{world}_{ee} * \begin{bmatrix} \cos \theta & -\sin \theta & 0 & 0\\ \sin \theta & \cos \theta & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ \end{bmatrix} $$

So now $T^{world}_{newee}$ will have the object upside down if you chose $\theta = 180^{\circ}$ for example.

Here is some simple Python code which implements this solution (without the arbitrary rotation), and displays the frames with OpenRave.

#! /usr/bin/env python

import numpy as np
from openravepy import *

# set up OpenRave environment
env = Environment()
env.SetViewer('qtcoin')

# center of sphere
p_o = np.array([0.0, 0.0, 0.0])

# radius of sphere
r = 1.0

# down vector
down = np.array([0.0, 0.0, -1.0])

# large coordinate frame at the center of the sphere
originaxes = misc.DrawAxes(env, [1, 0, 0, 0, p_o[0], p_o[1], p_o[2]], dist=1, linewidth=2)

# all other frames
axes = []

for i in range(200):

    # get a random point on the sphere
    p_e = np.random.uniform(-1, 1, 3)
    p_e *= r / np.linalg.norm(p_e)
    p_e += p_o

    # Z column
    R_z = (p_o - p_e)
    R_z /= np.linalg.norm(R_z)

    # X column
    R_x = np.cross(down, R_z)
    R_x /= np.linalg.norm(R_x)

    # Y column
    R_y = np.cross(R_z, R_x)

    # Full transformation matrix
    T = np.array([[ R_x[0], R_y[0], R_z[0], p_e[0] ],
                  [ R_x[1], R_y[1], R_z[1], p_e[1] ],
                  [ R_x[2], R_y[2], R_z[2], p_e[2] ],
                  [      0,      0,      0,      1]])

    # draw it
    axes.append(misc.DrawAxes(env, T, dist=0.2, linewidth=1))

This should give you frames all around the sphere like this: Random frames around a circle

$\endgroup$
  • 1
    $\begingroup$ thanks for the answer :).. There is a few things i am questioning... First of all.. Why $| p_0 - p_e|$ and not $p_0 - p_e$ for $R_z$? $\endgroup$ – test Apr 24 '16 at 18:26
  • 1
    $\begingroup$ The columns of the rotation matrix must be normalized to have a length of 1. I'll switch to the hat notation which is more proper. $\endgroup$ – Ben Apr 24 '16 at 18:39
  • $\begingroup$ ahh.. Makes sense :) Most of the things make sense, Just a last thing. If i understand it correctly $T_{world}^{ee}$ is the transformation matrix describing the desired position and orientation.. I am not sure i understand the effect of the last matrix in the last multiplication. Aren't you already doing that with $T_{world}^{ee}$. $\endgroup$ – test Apr 24 '16 at 18:58
  • $\begingroup$ doing as in rotation about the z-axis. $\endgroup$ – test Apr 24 '16 at 19:22
  • $\begingroup$ See expanded answer. the last step gives you an arbitrary (but discrete) rotation about the camera's axis. Steps 1 through 3 give you the one transform where the object is upright. I guess you could use a different vector in step 2 instead of world-down to get different orientations. But that seems difficult to me and not as useful. $\endgroup$ – Ben Apr 25 '16 at 23:45
1
$\begingroup$

Currently your problem still has one degree of freedom, namely the orientation of the robot can still roll around the vector. For now I will assume that you would want the top of the robot to keep facing upwards as best as possible.

A rotation matrix can be seen as an second order tensor, which can be constructed by summing multiple dyads of the new and old base vector,

$$ R = \vec{e}_{x1} \vec{e}_{x0} + \vec{e}_{y1} \vec{e}_{y0} + \vec{e}_{z1} \vec{e}_{z0}. $$

Because each basis should be orthonormal, then taking the dot product of $R$ with either $\vec{e}_{x0}$, $\vec{e}_{y0}$ or $\vec{e}_{z0}$ will result in the base vectors $\vec{e}_{x1}$, $\vec{e}_{y1}$ or $\vec{e}_{z1}$ respectively. Each dyad can be expressed as a matrix by taking the vector direct product.

Now all you have to do is find an expression for the initial and final orthonormal basis. Since you want the robot to face the center of the sphere, you can define $\vec{e}_{x0}$ by the direction the front of the robot is facing, when no rotation is applied, and $\vec{e}_{x1}$ as the unit vector pointing towards the center of the sphere. By using the addition constraint for the roll you can define $\vec{e}_{y0}$ as the direction the top of the robot is facing, when no rotation is applied, and $\vec{e}_{y1}$ by taking the orthogonal component of $\vec{e}_{y0}$ with respect to $\vec{e}_{x1}$,

$$ \vec{e}_{y1} = \frac{\vec{e}_{y0} - \left(\vec{e}_{y0} \cdot \vec{e}_{x1}\right) \vec{e}_{x1}}{\left\|\vec{e}_{y0} - \left(\vec{e}_{y0} \cdot \vec{e}_{x1}\right) \vec{e}_{x1}\right\|}. $$

The remaining vectors van be found using the fact that each basis should be orthonormal, which can be achieved by simply taking the cross product of the other base vectors. So $\vec{e}_{z0}=\vec{e}_{x0}\times\vec{e}_{y0}$ and $\vec{e}_{z1}=\vec{e}_{x1}\times\vec{e}_{y1}$, you could reverse the order of the vectors in the cross product, but you should do for both the $\vec{e}_{z0}$ and $\vec{e}_{z1}$ because otherwise the resulting "rotation" will also contain a reflection (the robot will be rotated but also a mirror image of itself).

$\endgroup$
0
$\begingroup$
  1. make sure tha your axes are parallel with the coordinate system in which your robots orientations are expressed.

  2. Caclulate the orientation angles geometrically by taking projections of the vector marked with red.

I do not see the need for any rotation matrices.

EDIT:

Here is a 2D example. The camera moves on the circle and the Z orinetation has to be adjusted. The dx and dy can be calculated form the circle equations and the Z rotation is

$Z_{rot}= atan2(dy,dx)$

2D example

$\endgroup$
  • $\begingroup$ Could give an example? $\endgroup$ – test Apr 24 '16 at 7:13
  • $\begingroup$ I need the rotation matrix to perform inverse kinematics, so i have an idea of what the desired transformation is.. $\endgroup$ – test Apr 24 '16 at 7:15
  • $\begingroup$ your inverse kinematics should have angles as input not a rotation matrix. If for some reason it does have a roation matrix as input, you can obtain it by calculating the 3 angels and then multiplying the 3 pure rotation matrces (x, y, z rotations) and obtain yout orientation matrix so $\endgroup$ – 50k4 Apr 24 '16 at 7:27
  • $\begingroup$ but could you give an example... i am having a hard time getting my head around it... $\endgroup$ – test Apr 24 '16 at 7:40
  • $\begingroup$ Thanks for the example.. so given dy,dx you form the rotation along the z-axis. does that mean that given dy,dz will give rotation along x axis, and dz, dx give rotation along the y axis. thus given all angles for making a rotation matrix? $\endgroup$ – test Apr 24 '16 at 9:25

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.