# Some variations of end effector position while using ChainIkSolverVel_pinv::CartToJnt

 Submitted by threelight on Mon, 2008-08-11 10:56

Hello,

currently, I'm using ChainIkSolverVel_pinv::CartToJnt to control a robotc arm with 7 joints. But the arm doesn't reach the desired end effector position exactly. I have variations of some centimetres.
My program calculates the current end effector frame and the end effector frame at the next time stamp of 1 sec. Then I calculate the Twist with KDL::Twist twist = diff(KDL::Frame1, KDL::Frame2). After this, the rotational part of the twist is transformed into the end effector frame. finally the resulting twist builds the input for ChainIkSolverVel_pinv::CartToJnt.
I get the two KDL::Frames from MeMatrix4-matrices which results from
void MeQuaternionToTM ( MeMatrix4 tm, const MeVector4 q )
which is a function of MeMath.h. Maybe the transformation from a quaternion into a matrix is not exactly?

Or do you have an idea where the problem could be?

### Some variations of end effector position while using ChainIkSolv

On Monday 11 August 2008 12:56:22 threelight wrote:
> Hello,
>
> currently, I'm using ChainIkSolverVel_pinv::CartToJnt to control a robotc
> arm with 7 joints. But the arm doesn't reach the desired end effector
> position exactly. I have variations of some centimetres. My program
> calculates the current end effector frame and the end effector frame at the
> next time stamp of 1 sec. Then I calculate the Twist with KDL::Twist twist
> = diff(KDL::Frame1, KDL::Frame2). After this, the rotational part of the
> twist is transformed into the end effector frame. finally the resulting
> twist builds the input for ChainIkSolverVel_pinv::CartToJnt. I get the two
> KDL::Frames from MeMatrix4-matrices which results from void
> MeQuaternionToTM ( MeMatrix4 tm, const MeVector4 q )
> which is a function of MeMath.h. Maybe the transformation from a quaternion
> into a matrix is not exactly?
>
> Or do you have an idea where the problem could be?

You should use the chainiksolvervel in a closed loop.
Each control step you should do the following (lets say at 100Hz or
something):

//Get the current joint positions
KDL::JntArray q_current=....;

//Calculate the forward kinematics to get the current cartesian position
KDL::Frame F_current;
bool succes = KDL::ChainFkSolverPos->JntToCart(q_current,F_current);

//Apply your control-law (this is feedback, Federico uses attraction fields)
KDL::Frame F_desired = ...;
KDL:: Twist V_control=K*diff(F_desired , F_current)

//Calculate the inverse kinematics:
KDL::JntArray q_dot_control(size);
succes = KDL::ChainIkSolverVel->CartToJnt(q_current,V_control,q_dot_control)
//Send the joint velocities to the robot
.....(q_dot_control)

This should do the trick.

Ruben

### Some variations of end effector position while using ChainIkSolv

On Mon, 11 Aug 2008, threelight wrote:

> currently, I'm using ChainIkSolverVel_pinv::CartToJnt to control a robotc arm
> with 7 joints. But the arm doesn't reach the desired end effector position
> exactly. I have variations of some centimetres. My program calculates the
> current end effector frame and the end effector frame at the next time stamp
> of 1 sec. Then I calculate the Twist with KDL::Twist twist =
> diff(KDL::Frame1, KDL::Frame2). After this, the rotational part of the twist
> is transformed into the end effector frame. finally the resulting twist
> builds the input for ChainIkSolverVel_pinv::CartToJnt.
> I get the two KDL::Frames from MeMatrix4-matrices which results from
> void MeQuaternionToTM ( MeMatrix4 tm, const MeVector4 q ) which is a function
> of MeMath.h. Maybe the transformation from a quaternion into a matrix is not
> exactly?
>
> Or do you have an idea where the problem could be?

The best thing is to provide a complete example, that we can reproduce and
look for the bug(s).

But if you interpolate with 1sec intervals, you will make very large errors
at "normal" motion speeds! Typically, 1 milli second is used for decent
accuracy...

Herman

Disclaimer: http://www.kuleuven.be/cwis/email_disclaimer.htm

### providing an example would

providing an example would be difficult because there is a lot of code involved.

I thought that 1 sec is the correct interpolation frequency since Ruben Smits told me that the twist which serves as an input for chainsolvervel_pinv is the velocity of the end effector, from the current end effector frame to the new end effector frame( that I will reach if I apply this twist for 1 sec)???

### providing an example would

On Mon, 11 Aug 2008, threelight wrote:

> providing an example would be difficult because there is a lot of code
> involved.
> I thought that 1 sec is the correct interpolation frequency since Ruben Smits
> told me that the twist which serves as an input for chainsolvervel_pinv is
> the velocity of the end effector, from the current end effector frame to the
> new end effector frame( that I will reach if I apply this twist for 1 sec)???

For "one unit of time"... Whether this is okay or not depends on how much
your desired trajectory deviates from a "constant-twist" trajectory. In
other words: the joint velocities that correspond to a given end-effector
twist will, in general, not make the robot move along the Cartesian twist
path, since the IK is a non-linear transformation from Cartesian to joint
velocities: the jacobian matrix depends on the configuration, and that
configuration changes all the time...

Herman

Disclaimer: http://www.kuleuven.be/cwis/email_disclaimer.htm

### ah, sorry, I already use an

ah, sorry, I already use an interpolation frequency of 1 msec. So this must be correct.