Page 1 of 2
How to add the user-defined control algorithm for the IK?
Posted: 22 Jan 2022, 12:20
by zhangm365
Hello, admin
Now, I am trying to use the customed algorithm for IK control problem to replace with the build-in IK method DLS.
So, what are the key points of the process? i.e, the whole framework of the program, parameter setting, etc
Can you give me a hand?
Best Wishes.
Re: How to use the user-defined control algorithm for the IK?
Posted: 25 Jan 2022, 10:03
by coppelia
Hello,
the best would be to fork and modify the
Coppelia kinematics routines, and its related CoppeliaSim
IK plugin.
Cheers
Re: How to add the user-defined control algorithm for the IK?
Posted: 26 Jan 2022, 12:49
by zhangm365
Are there any guidance documents? I am trying to add a user-defined IK method to imitate the build-in method, i.e, DLS.
Best Wishes.
Re: How to use the user-defined control algorithm for the IK?
Posted: 26 Jan 2022, 16:30
by coppelia
Have a look at the bottom part of the
section on plugins: there are several example projects, the most straightforward is probably
this one, which basically just adds a new script function.
Cheers
Re: How to add the user-defined control algorithm for the IK?
Posted: 07 Feb 2022, 09:10
by zhangm365
ok, Thanks your reply.
Now, I still have a lot doubt that I don't understand the process you have given above.
---------------------------------
coppelia wrote: ↑26 Jan 2022, 16:30
Have a look at the bottom part of the
section on plugins: there are several example projects, the most straightforward is probably
this one, which basically just adds a new script function.
Cheers
Please allow me to reformulate my problem as follow:
I am trying to use a customed inverse-kinematic algorithm instead of the built-in algorithm (
DLS) to calculate the inverse kinematics. Of course, I need to imatate the original algorithm(
DLS) so as to add the new calculate method.
How can I do and what is the guiding route of this process?
Best wishes.
Re: How to add the user-defined control algorithm for the IK?
Posted: 08 Feb 2022, 08:50
by coppelia
clone
coppeliaKinematicsRoutines to your programming folder. then clone
simExtIK to your programming folder. Then try to build simExtIK. That's the first step.
The second step would be to modify those two items above.
Then add a new define for your new calculation method in
coppeliaKineamticsRoutines/ik.h, near
ik_method_undamped_pseudo_inverse. In the whole source code search for appearances of
ik_method_undamped_pseudo_inverse and insert your new method here and there.
Then in simExtIK/simExtIK.cpp, add a corresponding entry near:
Code: Select all
simRegisterScriptVariable("simIK.method_undamped_pseudo_inverse@simExtIK",std::to_string(ik_method_undamped_pseudo_inverse).c_str(),0);
Cheers
Re: How to add the user-defined control algorithm for the IK?
Posted: 08 Feb 2022, 15:28
by zhangm365
Thanks a lot for your guidance. I try it now away.
Best wishes.
Re: How to add the user-defined control algorithm for the IK?
Posted: 10 Feb 2022, 10:15
by zhangm365
Dear admin:
Now, I want to express the formula of the joint angle \(\boldsymbol \theta\) as follow:
\(\dot{\boldsymbol \theta}(t)=\boldsymbol J^\dagger (\dot{\boldsymbol p}_d(t)+\gamma (\boldsymbol p_d(t)-\boldsymbol p_a(t))) \label{tracking}\).
Unlike the DLS method form, \(||\boldsymbol J\bigtriangleup {\boldsymbol \theta} - \boldsymbol e||^2 + \lambda^2||\bigtriangleup {\boldsymbol \theta}||^2\), its final expression only include the Jacobian matrix and can be easily written.
The \(\dot{\boldsymbol p}_d(t)\) stands for the velocity of the desired path, how should it be represented in source code in my case?
Best Wishes.
Re: How to add the user-defined control algorithm for the IK?
Posted: 10 Feb 2022, 14:21
by coppelia
You will have to look at the source code around
this line, and figure out where to plug in your part.
Cheers
Re: How to add the user-defined control algorithm for the IK?
Posted: 10 Feb 2022, 15:41
by zhangm365
Well, I noticed that it is where the code should be inserted.
However, I mean the formula in my case as follow:
\(\dot{\boldsymbol \theta}=\boldsymbol J^\dagger (\dot{\boldsymbol p}_d+\gamma (\boldsymbol p_d-\boldsymbol p_a)) \) .
i.e, the variable \(\dot{\boldsymbol p}_d\) ( the velocity of the target position ) is how to be represented?