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

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?

Ruben Smits's picture

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.