Loosening constraints in Inverse Kinematics

Good afternoon!

We are using the robot to perform acoustic array measurements, so the end-effector is a measurement microphone. Since the microphone is axis-symmetric, it doesn't matter where the x and y directions of the end-effector point as long as the z-axis (direction of the mic) points in the specified direction, i.e. our restrictions are the (x, y, z) position and the direction of the z-axis of the end-effector. Is there any way of implementing that?

I have tried playing with the weights in the inverse kinematics solver, but I found out that since they come from the axis-angle representation, it does not always work because the three values change when we rotate the coordinate system around z. The only parameter I am sure that does not change is that the last column of the rotation matrix should be the same, as in the image (in this case the z-axis of the end-effector points to the global x-axis):

Thanks in advance!

Answer 1

[Karsh Tharyani]

@Augusto César,

The weights vector specifies the weightage of each aspect of pose in the cost function of the Inverse Kinematics problem. The weights vector is of the form [rx,ry,rz,trx,try,trz] where the "r" are the weights on the axis-angle orientation based costs/errors and "t" are the translation based costs/errors. The costs/errors indicate how far is the pose from the desired pose at each step of the iterative optimization which is done in "inverseKinematics".

An axis-angle representation of orientation such that the end-effector's Z-axis aligns with the base frame's Z-axis, as you correctly mention, will have 0 values for the xy components of the axis of rotation. In order to maintain this, then, we should make sure that the optimizer always prioritizes the xy components of this desired rotation (by keeping them zero) but is freely allowed to choose the z value in the axis of rotation. Furthermore, if we want to maintain the position i.e., the translation to some desired value we should make sure these are also specified accordingly. Hence, your desired weight vector should be [1,1,0,1,1,1]. We can illustrate this effect with the help of a sample script shown below.

You should observe that the end-effector is "upright" and also closely follows the desired end-effector positions.

I hope that clarifies your concerns.

Best,

Karsh

%% Load the robot in an initial configuration
% Given the initial joint configuration, find the initial end-effector pose
% and try to maintain an orientation such that the Z-axis is parallel to
% the robot base frame's Z-axis. In the further section we will make the
% end-effector follow a "trajectory" in the X-axis of the base frame,
% hence, also record the Y and Z coordinate that should be maintained.
ur=loadrobot("universalur5",DataFormat="row");
q0=[-1.6007, -1.7271, -2.2030, -0.8080, 1.5951, -0.0310];
eename="tool0";
eepose0=getTransform(ur,q0,eename);
axang0=tform2axang(eepose0);
axang0(2:3)=[0,0];
rottform0=axang2tform(axang0);
show(ur,q0);

trvec0=tform2trvec(eepose0);
y0=trvec0(2);
z0=trvec0(1);

%% Initialize Inverse Kinematics Solver
% Disable random restarts to disallow superbly disparate configurations between two
% consecutive poses along the trajectory. Furthermore, to make sure the
% configurations of two consecutive poses are close, use the previous
% configuration as a guess to find the current one.
ik=inverseKinematics(RigidBodyTree=ur,SolverAlgorithm="bfgs");
ik.SolverParameters.AllowRandomRestart=false;
qsolprev=q0;
qsolall=[];
for x=-0.6:0.05:0.6
    eepose=trvec2tform([x,y0,z0])*rottform0;
    [qsol,solinfo]=ik(eename,eepose,[1,1,0,1,1,1],qsolprev);
    qsolall=[qsolall;qsol];
    qsolprev=qsol;
end

%% Visualize Solved Configurations
figure(Name="Solved Configurations");
for i=1:size(qsolall,1)
    show(ur,qsolall(i,:));
    hold on;
end

[Augusto César]

Thank you for your answer Karsh!

I tested your suggestions and they worked to some extent. I have a few more questions:

1. The alignment by removing the weight of the solution works if I need a direction that is aligned with the global x, y or z. But what if I need an alignment of the z axis of the tool to an axis 45 degrees from the global x axis? Is it possible with this function?
2. Is there a way to detect self collisions or ground collisions?

Once again, thank you for the help!

[Karsh Tharyani]

%% Initialize Inverse Kinematics Solver

% Disable random restarts to disallow superbly disparate configurations between two

% consecutive poses along the trajectory. Furthermore, to make sure the

% configurations of two consecutive poses are close, use the previous

% configuration as a guess to find the current one.

ik=inverseKinematics(RigidBodyTree=ur,SolverAlgorithm="bfgs");

ik.SolverParameters.AllowRandomRestart=false;

qsolprev=q0;

qsolall=[];

for x=-0.6:0.05:0.6

eepose=trvec2tform([x,y0,z0])*rottform0*eul2tform([0,pi/4,0]);%<--

[qsol,solinfo]=ik(eename,eepose,[1,1,1,1,1,1],qsolprev);%<---

tform=getTransform(ur,qsol,'tool0');

acos(dot(tform(1:3,3),[1,0,0]))

qsolall=[qsolall;qsol];

qsolprev=qsol;

end

1) If you change the lines above in the example you should see the results as such. However, for that, you should make sure you that the weights are all ones. I am sure there is some SO3 math that you can also do to relax some orientation weights, but that might require some more investigation on my end.

2) Yes you can! Please see the checkCollision function for that.

Karsh