# exploding rotational values of KDL::Twist for ChainIkSolverVel_pinv::CartToJnt

 Submitted by threelight on Tue, 2008-09-02 14:33

hello,

the twists which I use as input twists for ChainIkSolverVel_pinv::CartToJnt are composed of a translational difference vector and a difference quaternion which contain the rotational part. This builds the end effector velocity during one time step. But unfortunatly the rotational twist values are exploding during a trajectory and this happens abrupt at the end of the trajectory.
Here is an example:

translation vector 1;
translation vector 2;

quaternion 1;
quaternion 2;

diff_vec = (vector 2 - vector 1)/timeStep
diff_quat = (quaternion 2 - quaternion 1)/timeStep

I build the input twist as follows:

KDL::Vector vec1(diff_vec[0],diff_vec[1],diff_vec[2]);

MeVector4 quat = {diff_quat[0],diff_quat[1],diff_quat[2],diff_quat[3]};
MeMatrix4 TM;
MeQuaternionToTM (TM,quat );
KDL::Rotation R(TM[0][0],TM[1][0],TM[2][0],
TM[0][1],TM[1][1],TM[2][1],
TM[0][2],TM[1][2],TM[2][2]);

KDL::Rotation RN = KDL::Rotation::Identity();
KDL::Vector TWISTR = diff(R,RN);
KDL::Twist twist(vec1,TWISTR);

For short trajectories I get quite exact positions and rotations of the end effector, but during larger trajectories the rotational part of the twist explods suddenly.

Here is an exapmple of a time step which occurs during a pure translational trajectory:

translation vector 1; (120.736, 77.7407 35.2536)
translation vector 2; (120.734, 77.77, 35.2535)

quaternion 1; ( 0.615321, 0.599335, -0.343991, -0.379273)
quaternion 2; (0.617887, 0.765928, 0.0133693, -0.177176)

and the resulting twists is:

twist(0) -0.168307
twist(1) -0.0665565
twist(2) -0.0062569
twist(3) -2301.81
twist(4) -4937.65
twist(5) -2792.37

The rotational part is extremly too high! and I don't know why??

### exploding rotational values of KDL::Twist for ChainIkSolverVel_p

On Tue, 2 Sep 2008, manuelbirlo [..] ... wrote:

> the twists which I use as input twists for ChainIkSolverVel_pinv::CartToJnt
> are composed of a translational difference vector and a difference quaternion
> which contain the rotational part. This builds the end effector velocity
> during one time step. But unfortunatly the rotational twist values are
> exploding during a trajectory and this happens abrupt at the end of the
> trajectory. Here is an example:
>
> translation vector 1; translation vector 2;
> quaternion 1; quaternion 2;
> diff_vec = (vector 2 - vector 1)/timeStep
> diff_quat = (quaternion 2 - quaternion 1)/timeStep

The difference of two orientation representations is _meaningless_!!
It isn't a quaternion in itself... So, any function that uses this
difference as input argument as if it were a quaternion will fail, or at
least give bogus results in general...

So, my guess is that that is the cause of your exploding results :-)

Herman
> I build the input twist as follows:
>
> KDL::Vector vec1(diff_vec[0],diff_vec[1],diff_vec[2]);
>
> MeVector4 quat = {diff_quat[0],diff_quat[1],diff_quat[2],diff_quat[3]};
> MeMatrix4 TM;
> MeQuaternionToTM (TM,quat );
> KDL::Rotation R(TM[0][0],TM[1][0],TM[2][0],
> TM[0][1],TM[1][1],TM[2][1],
> TM[0][2],TM[1][2],TM[2][2]);
>
> KDL::Rotation RN = KDL::Rotation::Identity();
> KDL::Vector TWISTR = diff(R,RN);
> KDL::Twist twist(vec1,TWISTR);
>
> For short trajectories I get quite exact positions and rotations of the end
> effector, but during larger trajectories the rotational part of the twist
> explods suddenly. Here is an exapmple of a time step which occurs during
> a pure translational trajectory:
>
> translation vector 1; (120.736, 77.7407 35.2536)
> translation vector 2; (120.734, 77.77, 35.2535)
>
> quaternion 1; ( 0.615321, 0.599335, -0.343991, -0.379273)
> quaternion 2; (0.617887, 0.765928, 0.0133693, -0.177176)
>
> and the resulting twists is:
>
> twist(0) -0.168307
> twist(1) -0.0665565
> twist(2) -0.0062569
> twist(3) -2301.81
> twist(4) -4937.65
> twist(5) -2792.37
>
> The rotational part is extremly too high! and I don't know why??
>
>
>
>

### exploding rotational values of KDL::Twist for ChainIkSolverVel_p

really? But for small end effector rotations I get corecct RPY angles.

### exploding rotational values of KDL::Twist for ChainIkSolverVel_p

On Tue, 2 Sep 2008, manuelbirlo [..] ... wrote:

> really? But for small end effector rotations I get corecct RPY angles.

(_Please_ do not cut away the context of the message to which you reply,
sigh...)

You should read up some more about the fundamentals of 3D motion :-) Small
orientation changes look like velocities (i.e., they behave like vectors
and can be added or subtracted), but large changes do not!

Herman

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

### exploding rotational values of KDL::Twist for ChainIkSolverVel_p

>You should read up some more about the fundamentals of 3D >motion :-) Small
>orientation changes look like velocities (i.e., they behave >like vectors
>and can be added or subtracted), but large changes do not!

Oh, I really should ;-) Thanks!