- include <stdlib.h>
- include <kdl/chain.hpp>
- include <kdl/chainiksolver.hpp>
- include <kdl/chainfksolverpos_recursive.hpp>
- include <kdl/chainiksolvervel_pinv.hpp>
- include <kdl/chainiksolverpos_nr.hpp>
using namespace KDL; int main(int argc, char** argv) {
KDL::Chain chain;chain.addSegment(Segment(Joint(Joint::None),Frame(Vector(0.0,0.0,0.26)))); chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.17,0.0)))); chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.20,0.0)))); chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.15,0))));
ChainFkSolverPos_recursive fksolver(chain);//Forward position solver ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver
ChainIkSolverPos_NR iksolver1(chain,fksolver,iksolver1v,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6 JntArray q(chain.getNrOfJoints()); JntArray q_init(chain.getNrOfJoints());
Frame F_dest=Frame(Vector(0.0,0.52,0.26)); int ret = iksolver1.CartToJnt(q_init,F_dest,q);
return (EXIT_SUCCESS); }
I am using this example of inverse kinematics for my intelligent robotic arm.. I want to limit my links rotation in specific anglular rotation i.e 180 degrees only.One links rotation along Z-axis and another two link along X-axis. So. what new changes should i apply in this program.
inverse kinematics
On Sat, 26 Feb 2011, soni07sharma [..] ... wrote:
> #include <stdlib.h>
> #include <kdl/chain.hp
> #include <kdl/chainiksolver.hp
> #include <kdl/chainfksolverpos_recursive.hp
> #include <kdl/chainiksolvervel_pinv.hp
> #include <kdl/chainiksolverpos_nr.hp
>
>
>
> using namespace KDL;
> int main(int argc, char** argv) {
> KDL::Chain chain;
> chain.addSegment(Segment(Joint(Joint::None),Frame(Vector(0.0,0.0,0.26))));
> chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.17,0.0))));
> chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.20,0.0))));
> chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.15,0))));
>
>
> ChainFkSolverPos_recursive fksolver(chain);//Forward position solver
> ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver
>
> ChainIkSolverPos_NR iksolver1(chain,fksolver,iksolver1v,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6
> JntArray q(chain.getNrOfJoints());
> JntArray q_init(chain.getNrOfJoints());
>
> Frame F_dest=Frame(Vector(0.0,0.52,0.26));
> int ret = iksolver1.CartToJnt(q_init,F_dest,q);
>
>
>
> return (EXIT_SUCCESS);
> }
>
>
>
>
> I am using this example of inverse kinematics for my intelligent robotic arm..
> I want to limit my links rotation in specific anglular rotation i.e 180 degrees only.One links rotation along Z-axis and another two link along X-axis. So. what new changes should i apply in this program.
The simplest thing is to monitor the current joint angles in your
controller and act appropriately whenever you come near the limits.
Most people seem to think that joint angle limitation should be the job of
any inverse kinematics algorithm but that's not true: what limits to use,
and how to react to (almost) reaching them is a task-level responsibility.
Herman
inverse kinematics
Thanks for your reply but i am not getting the correct angle of arm when a destination is given to arm by this program. How to solve this problem of finding angle when arm destinations co-ordinate is given.
inverse kinematics
On Sun, 27 Feb 2011, soni07sharma [..] ... wrote:
> Thanks for your reply but i am not getting the correct angle of arm when a destination is given to arm by this program. How to solve this problem of finding angle when arm destinations co-ordinate is given.
I think you will have to provide more details before we can understand the
problem completely... What is, according to you, the "correct angle", and
what angle does the program returns?
Herman
inverse kinematics
> #include <stdlib.h> > #include <kdl/chain.hp
> #include <kdl/chainiksolver.hp
> #include <kdl/chainfksolverpos_recursive.hp
> #include <kdl/chainiksolvervel_pinv.hp
> #include <kdl/chainiksolverpos_nr.hp
> >> > using namespace KDL; > int main(int argc, char** argv) { > KDL::Chain chain; > chain.addSegment(Segment(Joint(Joint::None),Frame(Vector(0.0,0.0,0.26)))); > chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.17,0.0)))); > chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.20,0.0)))); > chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.15,0)))); >> > ChainFkSolverPos_recursive fksolver(chain);//Forward position solver > ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver >> ChainIkSolverPos_NR iksolver1(chain,fksolver,iksolver1v,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6 > JntArray q(chain.getNrOfJoints()); > JntArray q_init(chain.getNrOfJoints()); >> Frame F_dest=Frame(Vector(0.0,0.42,0.43)); > int ret = iksolver1.CartToJnt(q_init,F_dest,q); >> >> return (EXIT_SUCCESS); > } I have attached a file containing my arm description.
Please suggest me where i am wrong in my program.
inverse kinematics
On Tue, 1 Mar 2011, soni07sharma [..] ... wrote:
>> #include <stdlib.h>
>> #include <kdl/chain.hp
>> #include <kdl/chainiksolver.hp
>> #include <kdl/chainfksolverpos_recursive.hp
>> #include <kdl/chainiksolvervel_pinv.hp
>> #include <kdl/chainiksolverpos_nr.hp
>>
>> using namespace KDL;
>> int main(int argc, char** argv) {
>> KDL::Chain chain;
>> chain.addSegment(Segment(Joint(Joint::None),Frame(Vector(0.0,0.0,0.26))));
>> chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.17,0.0))));
>> chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.20,0.0))));
>> chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.15,0))));
>> ChainFkSolverPos_recursive fksolver(chain);//Forward position solver
>> ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver
>>
>> ChainIkSolverPos_NR iksolver1(chain,fksolver,iksolver1v,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6
>> JntArray q(chain.getNrOfJoints());
>> JntArray q_init(chain.getNrOfJoints());
>>
>> Frame F_dest=Frame(Vector(0.0,0.42,0.43));
>> int ret = iksolver1.CartToJnt(q_init,F_dest,q);
>>
>> return (EXIT_SUCCESS);
>> }
> I have attached a file containing my arm description.
Sending MS Office files on a mailing list (especially of a open source
project) is not really "best practice" :-)
> Please suggest me where i am wrong in my program.
Aren't you using a _velocity_ inverse kinematics algorithm while you think
you are doing inverse _position_ kinematics?
Herman
inverse kinematics
2011/3/3 Herman Bruyninckx <Herman [dot] Bruyninckx [..] ...>
> On Tue, 1 Mar 2011, soni07sharma [..] ... wrote:
>
> >> #include <stdlib.h>
> >> #include <kdl/chain.hp
> >> #include <kdl/chainiksolver.hp
> >> #include <kdl/chainfksolverpos_recursive.hp
> >> #include <kdl/chainiksolvervel_pinv.hp
> >> #include <kdl/chainiksolverpos_nr.hp
> >>
> >> using namespace KDL;
> >> int main(int argc, char** argv) {
> >> KDL::Chain chain;
> >>
> chain.addSegment(Segment(Joint(Joint::None),Frame(Vector(0.0,0.0,0.26))));
> >>
> chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.17,0.0))));
> >>
> chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.20,0.0))));
> >> chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.15,0))));
>
> >> ChainFkSolverPos_recursive fksolver(chain);//Forward position solver
> >> ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver
> >>
> >> ChainIkSolverPos_NR
> iksolver1(chain,fksolver,iksolver1v,100,1e-6);//Maximum 100 iterations, stop
> at accuracy 1e-6
> >> JntArray q(chain.getNrOfJoints());
> >> JntArray q_init(chain.getNrOfJoints());
> >>
> >> Frame F_dest=Frame(Vector(0.0,0.42,0.43));
> >> int ret = iksolver1.CartToJnt(q_init,F_dest,q);
> >>
> >> return (EXIT_SUCCESS);
> >> }
> > I have attached a file containing my arm description.
>
> Sending MS Office files on a mailing list (especially of a open source
> project) is not really "best practice" :-)
>
Good thing, Gmail can handle that! :-)
>
> > Please suggest me where i am wrong in my program.
>
> Aren't you using a _velocity_ inverse kinematics algorithm while you think
> you are doing inverse _position_ kinematics?
I would say no, at least in the provided code it's written correctly
"iksolver1" and not "iksolver1v".
Or did I miss something?
> Herman
> --
> Orocos-Users mailing list
> Orocos-Users [..] ...
> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>
Regarding what you wrote in the attached file:
You are writing that the solver is returning a negative number. I guess,
it's "-3" or is it different?
Looking at the source code of the solver, it is either 0 or -3. "-3" means,
that the solver reached it's maximum number of iterations (100 in your case)
and
couldn't reached the desired position with the given accuracy.
Since your number of iterations seems enough for me, you maybe set a desired
position for the endeffector, which the arm can't reach?
I was too lazy to calculate, if you could reach (0.0,0.42, 0.43) with your
robot arm. :-)
Also, I would assign numbers to q_init, not only initialize it. But since
your code works in the other examples, the problem should be something else.
Hope I could help!
:-) Marcus
inverse kinematics
On Thursday 03 March 2011 22:14:41 Marcus Liebhardt wrote:Herman [dot] Bruyninckx [..] ...en.
> 2011/3/3 Herman Bruyninckx
> <Herman [dot] Bruyninckx [..] ...
> be>>
>
> On Tue, 1 Mar 2011, soni07sharma [..] ...<mailto:soni07sharma [..] ...>
wrote:
> >> #include <stdlib.h>
> >> #include <kdl/chain.hp
> >> #include <kdl/chainiksolver.hp
> >> #include <kdl/chainfksolverpos_recursive.hp
> >> #include <kdl/chainiksolvervel_pinv.hp
> >> #include <kdl/chainiksolverpos_nr.hp
> >>
> >> using namespace KDL;
> >> int main(int argc, char** argv) {
> >> KDL::Chain chain;
> >> chain.addSegment(Segment(Joint(Joint::None),Frame(Vector(0.0,0.0,0.26)
> >> )));
> >> chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.17,0.0
> >> ))));
> >> chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.20,0.0
> >> ))));
> >> chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.15,0))
> >> ));
> >>
> >> ChainFkSolverPos_recursive fksolver(chain);//Forward position solver
> >> ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver
> >>
> >> ChainIkSolverPos_NR
> >> iksolver1(chain,fksolver,iksolver1v,100,1e-6);//Maximum 100
> >> iterations, stop at accuracy 1e-6 JntArray q(chain.getNrOfJoints());
> >> JntArray q_init(chain.getNrOfJoints());
> >>
> >> Frame F_dest=Frame(Vector(0.0,0.42,0.43));
> >> int ret = iksolver1.CartToJnt(q_init,F_dest,q);
> >>
> >> return (EXIT_SUCCESS);
> >> }
> >
> > I have attached a file containing my arm description.
>
> Sending MS Office files on a mailing list (especially of a open source
> project) is not really "best practice" :-)
>
> Good thing, Gmail can handle that! :-)
>
> > Please suggest me where i am wrong in my program.
>
> Aren't you using a _velocity_ inverse kinematics algorithm while you think
> you are doing inverse _position_ kinematics?
>
> I would say no, at least in the provided code it's written correctly
> "iksolver1" and not "iksolver1v". Or did I miss something?
>
> Herman
> --
> Orocos-Users mailing list
> Orocos-Users [..] ...<mailto:Orocos-Users [..] ...en.
> be> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>
> Regarding what you wrote in the attached file:
> You are writing that the solver is returning a negative number. I guess,
> it's "-3" or is it different? Looking at the source code of the solver, it
> is either 0 or -3. "-3" means, that the solver reached it's maximum number
> of iterations (100 in your case) and couldn't reached the desired position
> with the given accuracy.
> Since your number of iterations seems enough for me, you maybe set a desired
> position for the endeffector, which the arm can't reach? I was too lazy to
> calculate, if you could reach (0.0,0.42, 0.43) with your robot arm. :-)
> Also, I would assign numbers to q_init, not only initialize it. But since
> your code works in the other examples, the problem should be something
> else.
>
Don't forget that KDL uses 6D solvers, so the solver does not only try to
reach the desired position but also the desired orientation. (Which is
Rotation::Identity(), if you did not specify it during the construction of the
Frame).
Since your robot only has 3DOF it will most probably be the reason why the
solver cannot reach the goal.
If you only need the position, and you don't care about the orientation you
need the use a solver that can disable some of the Cartesian space DOF. Try
the following:
ChainFkSolverPos_recursive fksolver(chain);//Forward position solver
ChainIkSolverVel_wdls iksolver1v(chain);//Inverse velocity solver
Eigen::VectorXd weight_ts(6);
weight_ts[1]=weight_ts[2]=weight_ts[3]=1.0;//X,Y,Z
weight_ts[3]=weight_ts[4]=weight_ts[5]=0.0;//Rx,Ry,Rz
iksolver1v.setWeightTS (weight_ts.asDiagonal());
ChainIkSolverPos_NR iksolver1(chain,fksolver,iksolver1v,100,1e-6);//Maximum
100 iterations, stop at accuracy 1e-6
JntArray q(chain.getNrOfJoints());
JntArray q_init(chain.getNrOfJoints());
Frame F_dest=Frame(Vector(0.0,0.42,0.43));
int ret = iksolver1.CartToJnt(q_init,F_dest,q);
And adapt the ChainFkSolverPos_recursive to also take a TaskSpace Weight into
account, this is fairly simple to add. If you have no idea on how to start on
this or you have some patience I'll try to find some time to add a solver for
this in the coming days.
> Hope I could help!
>
> :-) Marcus
-- Ruben
inverse kinematics
On Saturday 05 March 2011 06:18:00 soni07sharma [..] ... wrote:
> Thanks for all your help.The problem was solved as i increased the number of
> iterations.
>
> Now,in the current program the solver assumes motion of motor from neutral
> position to max clockwise and max anticlockwise,can you help me if i want
> to limit the solver to assume that my motor moves from initial to max
> position and vice versa i.e. my motor should rotate 0 to 180 degree instead
> of -90 to 90 degree.
There is a position solver implemented:
http://people.mech.kuleuven.be/~orocos/pub/devel/kdl/latest/api/html/cla...
This uses a very dumb way of staying away from limits, you can give it a try,
but I wouldn't expect to much from it ;)
-- Ruben
inverse kinematics
Thanks for all your help.The problem was solved as i increased the number of iterations.
Now,in the current program the solver assumes motion of motor from neutral position to max clockwise and max anticlockwise,can you help me if i want to limit the solver to assume that my motor moves from initial to max position and vice versa i.e. my motor should rotate 0 to 180 degree instead of -90 to 90 degree.
inverse kinematics
On Thu, 3 Mar 2011, Marcus Liebhardt wrote:
> 2011/3/3 Herman Bruyninckx <Herman [dot] Bruyninckx [..] ...>
> On Tue, 1 Mar 2011, soni07sharma [..] ... wrote:
>
> >> #include <stdlib.h>
> >> #include <kdl/chain.hp
> >> #include <kdl/chainiksolver.hp
> >> #include <kdl/chainfksolverpos_recursive.hp
> >> #include <kdl/chainiksolvervel_pinv.hp
> >> #include <kdl/chainiksolverpos_nr.hp
> >>
> >> using namespace KDL;
> >> int main(int argc, char** argv) {
> >> KDL::Chain chain;
> >>
> chain.addSegment(Segment(Joint(Joint::None),Frame(Vector(0.0,0.0,0.26))));
> >>
> chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.17,0.0))));
> >>
> chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.20,0.0))));
> >>
> chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.15,0))));
>
> >> ChainFkSolverPos_recursive fksolver(chain);//Forward position
> solver
> >> ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver
> >>
> >> ChainIkSolverPos_NR
> iksolver1(chain,fksolver,iksolver1v,100,1e-6);//Maximum 100
> iterations, stop at accuracy 1e-6
> >> JntArray q(chain.getNrOfJoints());
> >> JntArray q_init(chain.getNrOfJoints());
> >>
> >> Frame F_dest=Frame(Vector(0.0,0.42,0.43));
> >> int ret = iksolver1.CartToJnt(q_init,F_dest,q);
> >>
> >> return (EXIT_SUCCESS);
> >> }
> > I have attached a file containing my arm description.
>
> Sending MS Office files on a mailing list (especially of a open source
> project) is not really "best practice" :-)
>
> Good thing, Gmail can handle that! :-)
That's not really a convincing argument...
> > Please suggest me where i am wrong in my program.
>
> Aren't you using a _velocity_ inverse kinematics algorithm while you
> think you are doing inverse _position_ kinematics?
>
> I would say no, at least in the provided code it's written correctly
> "iksolver1" and not "iksolver1v".
> Or did I miss something?
No, _I_ missed that "v" :-(
> Regarding what you wrote in the attached file:
> You are writing that the solver is returning a negative number. I guess,
> it's "-3" or is it different?
> Looking at the source code of the solver, it is either 0 or -3. "-3" means,
> that the solver reached it's maximum number of iterations (100 in your case)
> and
> couldn't reached the desired position with the given accuracy.
> Since your number of iterations seems enough for me, you maybe set a desired
> position for the endeffector, which the arm can't reach?
> I was too lazy to calculate, if you could reach (0.0,0.42, 0.43) with your
> robot arm. :-)
> Also, I would assign numbers to q_init, not only initialize it. But since
> your code works in the other examples, the problem should be something else.
>
> Hope I could help!
>
> :-) Marcus
Thanks!
Herman