On Fri, 4 Jan 2013, Aaron O'Toole <airuno2L [..] ...> wrote:

> Err. I think I might have hinted at the correct solution when I said:
>
> The only time my end
> effector frame will be oriented right is when my virtual joint's axis
> of rotation happens to be parellel to the axis in which my arm is
> lacking the 6th degree of freedom.
>
> I was assigning the 6th DOF willy nilly before. But I think if I put
> more thought into it this method should work as desired, it will just
> take a little geometry to figure out. I'll give it another try and
> report back. Thanks for the inspiration guys.

You're welcome!

I am looking forward to getting some some case-specific insights from your
efforts :-)

> Aaron.

Herman

>
> On 1/4/13, Aaron O'Toole <airuno2L [..] ...> <airuno2l [..] ...> wrote:
>> Lol hey Erwin, thanks for the reply. I read someone mention this in
>> the mailing list when I was working on this problem and gave it a try.
>>
>> If I was only worried about getting the end effector at a certain
>> position, this method worked well for me. However, I needed the end
>> effector at a certain orientation as well (I needed to solve for 5
>> dof...leaving out one angle). And since my actual end effector ended
>> up being the second to last link in the chain, I had no way of making
>> sure it was angled right. Does that make sense? I'll put it another
>> way just in case:
>>
>> If the virtual frame is in the right position, then my end effector
>> frame has to be in the right position because the virtual link's
>> length is zero. However, if the virtual frame is in the right
>> orientation, my end effector frame will almost never be in the right
>> orientation because the virtual joint had to rotate to get to the
>> corrent orientation and it's axis of rotation can be any combination
>> of roll pitch or yaw depending on the arm's pose. The only time my end
>> effector frame will be oriented right is when my virtual joint's axis
>> of rotation happens to be parellel to the axis in which my arm is
>> lacking the 6th degree of freedom.
>>
>> Let me know if I'm overlooking something here. I'm currently solving
>> IK by hand for this particular arm, while I'm using KDL for all my
>> other arms (which are 6 DOF). I'd love to go back and change it so
>> everything is more similar. It does seem that IKFast has the
>> capability I'm looking for, but I haven't wrapped my head around it
>> yet, I'm not sure if it can be used independant of OpenRave easily.
>>
>> Thanks,
>> Aaron
>>
>>
>> On 1/4/13, Erwin Aertbelien <Erwin [dot] Aertbelien [..] ...> wrote:
>>> It suddenly occurred to me that the solution to your problem
>>> is easily solved with KDL, and I remembered your e-mail of 30 nov. 2012.
>>> (it's a pity I did not think of it sooner, because it is a commonly used
>>> method
>>> to deal with e.g. contact situations).
>>>
>>> The trick is to introduce a 6th _virtual_ joint. In this way an exact
>>> solution
>>> can be found. The solving of this new kinematic
>>> chain can then be done with KDL. After solving, you just ignore the
>>> virtual
>>> joint.
>>>
>>> In this way, you will not need any task space weights.
>>>
>>> Best regards,
>>>
>>> Erwin.
>>>
>>> On 11/30/2012 08:55 PM, Aaron O'Toole <airuno2L [..] ...> wrote:
>>>> Hello all,
>>>>
>>>> Quick question: I have a 5 dof arm I need to do inverse position
>>>> kinematics with (it doens't have a yaw joint). I've read through
>>>> messages in the mailing list about this but am still having some
>>>> trouble.
>>>>
>>>> This conversation has been of help:
>>>> http://lists.mech.kuleuven.be/pipermail/orocos-users/2011-March/003255.html
>>>>
>>>> But I'm not sure how to follow through. I have:
>>>>
>>>> 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]=weight_ts[3]=weight_ts[4]=1.0;//X,Y,Z,roll
>>>> pitch
>>>> weight_ts[5]=0.0;//yaw
>>>> iksolver1v.setWeightTS (weight_ts.asDiagonal());
>>>>
>>>> ChainIkSolverPos_NR iksolver1(chain,fksolver,iksolver1v,1000,1e-3);
>>>> JntArray q(chain.getNrOfJoints());
>>>> JntArray q_init(chain.getNrOfJoints());
>>>>
>>>> Frame F_dest=Frame(Rotation::RPY(-1.07, 0.94, 1.0), Vector(0.58,
>>>> -0.38, -0.85));
>>>> int ret = iksolver1.CartToJnt(q_init,F_dest,q);
>>>>
>>>>
>>>> If I plug in a frame that I know my robot can acheive (including the
>>>> yaw) the ik solver works, but if I just change the yaw value in F_dest
>>>> it will fail and return -3, which tells me its not ignoring the yaw.
>>>> Is there something else I need to do? Ruben mentions "adapt the
>>>> ChainFkSolverPos_recursive to also take a TaskSpace Weight into
>>>> account". Is this needed? Is it already implemented somewhere and I'm
>>>> missing it? If I need to do implement it myself, can someone point me
>>>> in the right direction?
>>>>
>>>> thanks,
>>>> Aaron
>>>>
>>>>
>>>
>>>
>>
>>
>> --
>> - Aaron O'Toole
>>
>
>

On Fri, 4 Jan 2013, Erwin Aertbelien wrote:

> On 01/04/2013 01:07 PM, Herman Bruyninckx wrote:
>> On Fri, 4 Jan 2013, Erwin Aertbelien wrote:
>>
>>> It suddenly occurred to me that the solution to your problem
>>> is easily solved with KDL, and I remembered your e-mail of 30 nov. 2012.
>>> (it's a pity I did not think of it sooner, because it is a commonly used
>>> method
>>> to deal with e.g. contact situations).
>>>
>>> The trick is to introduce a 6th _virtual_ joint. In this way an exact
>>> solution
>>> can be found. The solving of this new kinematic
>>> chain can then be done with KDL. After solving, you just ignore the
>>> virtual
>>> joint.
>>>
>>> In this way, you will not need any task space weights.
>>
>> But I guess the _choice_ of the 6th virtual joint introduces some sort of
>> "weight" behind the screens, isn't it?
> Yes, with this method, you indicate a direction (not necessarily rotational).
> You do not care about deviations around this direction.
> This is equivalent to specifying a weight matrix (in twist/displacement
> space) that has this direction in its null-space.

This idea was (I think) originally introduced in the robotics domain by
John Baillieul, at the ICRA 1985 (Kinematic programming alternatives for redundant
manipulators, 722-728). He extends the 6xn (n<6) Jacobian with 6-n extra
rows; hence, the name "extended Jacoboan" method.

> The point is, that the proposed way is very easy to implement.
>
> But, you're right, you'll have to think about exactly which direction you
> want to set free.
>
> Note: sometimes the specification suggests a way to specify this 6th
> virtual joint.

Agreed.

> E.g. in 5-axis milling machines, the robot pose is often
> specified using a tool tip position and a normalized orientation vector.
> This specification implicitly specifies that the orientation around this
> normalized orientation vector does not matter.
>
> Also the kinematics of the device can suggest a 6th virtual joint, e.g. with
> a 5-axis paletizing robot.
>
> Erwin.

Herman

>>> On 11/30/2012 08:55 PM, Aaron O'Toole <airuno2L [..] ...> wrote:
>>>> Hello all,
>>>>
>>>> Quick question: I have a 5 dof arm I need to do inverse position
>>>> kinematics with (it doens't have a yaw joint). I've read through
>>>> messages in the mailing list about this but am still having some
>>>> trouble.
>>>>
>>>> This conversation has been of help:
>>>> http://lists.mech.kuleuven.be/pipermail/orocos-users/2011-March/003255.html
>>>>
>>>> But I'm not sure how to follow through. I have:
>>>>
>>>> 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]=weight_ts[3]=weight_ts[4]=1.0;//X,Y,Z,roll
>>>> pitch
>>>> weight_ts[5]=0.0;//yaw
>>>> iksolver1v.setWeightTS (weight_ts.asDiagonal());
>>>>
>>>> ChainIkSolverPos_NR iksolver1(chain,fksolver,iksolver1v,1000,1e-3);
>>>> JntArray q(chain.getNrOfJoints());
>>>> JntArray q_init(chain.getNrOfJoints());
>>>>
>>>> Frame F_dest=Frame(Rotation::RPY(-1.07, 0.94, 1.0), Vector(0.58,
>>>> -0.38, -0.85));
>>>> int ret = iksolver1.CartToJnt(q_init,F_dest,q);
>>>>
>>>>
>>>> If I plug in a frame that I know my robot can acheive (including the
>>>> yaw) the ik solver works, but if I just change the yaw value in F_dest
>>>> it will fail and return -3, which tells me its not ignoring the yaw.
>>>> Is there something else I need to do? Ruben mentions "adapt the
>>>> ChainFkSolverPos_recursive to also take a TaskSpace Weight into
>>>> account". Is this needed? Is it already implemented somewhere and I'm
>>>> missing it? If I need to do implement it myself, can someone point me
>>>> in the right direction?
>>>>
>>>> thanks,
>>>> Aaron
>>>>
>>>>
>>>
>>> --
>>> Orocos-Users mailing list
>>> Orocos-Users [..] ...
>>> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>>>
>>
>> --
>> KU Leuven, Mechanical Engineering, Robotics Research Group
>> <http://people.mech.kuleuven.be/~bruyninc> Tel: +32 16 328056
>> Vice-President Research euRobotics <http://www.eu-robotics.net>
>> Open RObot COntrol Software <http://www.orocos.org>
>> Associate Editor JOSER <http://www.joser.org>, IJRR <http://www.ijrr.org>
>
>

--
KU Leuven, Mechanical Engineering, Robotics Research Group
<http://people.mech.kuleuven.be/~bruyninc> Tel: +32 16 328056
Vice-President Research euRobotics <http://www.eu-robotics.net>
Open RObot COntrol Software <http://www.orocos.org>
Associate Editor JOSER <http://www.joser.org>, IJRR <http://www.ijrr.org>

On 12/02/2012 02:26 PM, jelle feringa wrote:
> Dear Erwin,
>
> A pos solver that integrates weightings would be a wonderful contribution.
Yes it would be interesting, an extension of the solver that solves
towards more
general constraints would even be more interesting, but then were
getting into iTaSC territory...

> I recall this [1] thread on a solver based on ChainIkSolverPos_NR_JL
> that takes weighting into account, source code is posted [2].
Yes it is relevant. Strange thing is that the code does not match the
explanations in the mail.
pibgeustalks about weights on xyz RPY but in the code the weights are on
the displacement twist (i.e. the result of KDL's diff(..) )

You should also be sure to use a weighted inv. vel. kin.

Has anyone tried out this solver ?

However, this would not completely solved the problems with the 5 dof
robots, since the weights are still
a diagonal matrix. With these type of weights you cannot specify that
the rotation along a given axes is irrelevant.
You'll need a full symmetric matrix ( or its square root) to specify
such a thing. And even with a full symmetric matrix
it will be annoying to use (because you have to construct the weighing
matrix and that requires some insight).
Better would be in this case to be able to specify the weighing matrix
relative to the end effector.

But you see, there are a lot of use cases, that require different
variants of solvers, ...

Interested people can submit a feature request in our bug tracking
system (bugs.orocos.org), together with
the information below. Then, at least, we are not forgetting about
these type of issues, when we are considering
changes to KDL.

Best regards,
Erwin.
> Possible this is relevant to this discussion?
>
> -jelle
>
> [1] http://ros-users.122217.n3.nabble.com/IK-with-KDL-td1867716.html
> [2]
> http://ros-users.122217.n3.nabble.com/attachment/2055076/0/iksolvers_wei...
>
> But the above is _velocity_ kinematics, if you want inverse pos.
> kinematics, also the convergence criteria inside
> the pos. solver should be weighted. This is currently not the case.
>
> The recent levenberg-marquardt inv. pos. kinematics solver
> ChainIkSolverPos_LMA has
> weights, but it has two problems for this use case:
> - only a diagonal weight matrix can be specified.
> - you cannot adapt the weight matrix between calls, it is
> initialized in the constructor.
>
> The above points are problematic, since in the given case the
> orientation of the "missing" yaw axis
> is changing each time, and the weight matrix has to be adapted to
> exactly the directions of this axis.
> Also, ChainIkSolverPos_LMA does not take into account joint limits.
>
> It is time to refactor ChainIkSolverPos_LMA...
>
>
>

On Sun, Dec 2, 2012 at 5:59 AM, Herman Bruyninckx <
Herman [dot] Bruyninckx [..] ...> wrote:

> Because real robotics systems do a lot
> more than just computing inverse/forward position kinematics
>

I'm working with "real robotics systems" and I only need inverse kinematics

control - is taken care of per joint motor
optimization - doesn't matter in my application
dynamics - also doesn't matter
estimation - not sure what you mean there
joint limit avoidance - I consider this part of the kinematics problem
actuator limit monitoring - is taken care of per joint motor.

On 12/02/2012 11:59 AM, Herman Bruyninckx wrote:
> On Sat, 1 Dec 2012, Konrad Banachowicz wrote:
>
>> I would suggest using http://openrave.org/docs/latest_stable/openravepy/ikfast/ to generate
>> closed-form ik and using numerical ik only if ikfast can't solve your type of kinematic chain.
> Which is almost always the case. Because real robotics systems do a lot
> more than just computing inverse/forward position kinematics: they add
> control, optimization, dynamics, estimation, joint limit avoidance,
> actuator limit monitoring,... into the computations of the kinematics. And
> there are no closed form solutions for those.
>
> However, KDL has very efficient closed form solutions too, for the
> traditional 6-DOF industrial robot kinematics.
Not any more,
several years ago, Ruben removed these routines from KDL.

>
>> Pozdrawiam
>> Konrad Banachowicz
> Herman
>
>> 2012/11/30 Erwin Aertbelien <Erwin [dot] Aertbelien [..] ...>
>> What type of robot are you using? A typical 5-dof welding robot ?
>>
>> If it is a robot with its last 2 axes intersecting in 1 point, an
>> analytical solution to
>> the inverse position kinematics exists:
>>
>> * Such a solution is prefered above numerical solution.
>> * "Pieper's solution" to the inverse kinematics of a 3-2-1 robot can be
>> an inspiration (cfr. any textbook on robotics):
>> you split up the inverse kinematics problem into a part involving the 3
>> first joints and a part involving the
>> last 3 joints. For that last parts the KDL routines for euler angles or
>> rpy angles could be useful.
>>
>> Best regards,
>> Erwin.
>>
>>
>> On 11/30/2012 08:55 PM, Aaron O'Toole <airuno2L [..] ...> wrote:
>> > Hello all,
>> >
>> > Quick question: I have a 5 dof arm I need to do inverse position
>> > kinematics with (it doens't have a yaw joint). I've read through
>> > messages in the mailing list about this but am still having some
>> > trouble.
>> >
>> > This conversation has been of help:
>> > http://lists.mech.kuleuven.be/pipermail/orocos-users/2011-March/003255.html
>> >
>> > But I'm not sure how to follow through. I have:
>> >
>> > 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]=weight_ts[3]=weight_ts[4]=1.0;//X,Y,Z,roll
>> > pitch
>> > weight_ts[5]=0.0;//yaw
>> > iksolver1v.setWeightTS (weight_ts.asDiagonal());
>> >
>> > ChainIkSolverPos_NR iksolver1(chain,fksolver,iksolver1v,1000,1e-3);
>> > JntArray q(chain.getNrOfJoints());
>> > JntArray q_init(chain.getNrOfJoints());
>> >
>> > Frame F_dest=Frame(Rotation::RPY(-1.07, 0.94, 1.0), Vector(0.58,
>> > -0.38, -0.85));
>> > int ret = iksolver1.CartToJnt(q_init,F_dest,q);
>> >
>> >
>> > If I plug in a frame that I know my robot can acheive (including the
>> > yaw) the ik solver works, but if I just change the yaw value in F_dest
>> > it will fail and return -3, which tells me its not ignoring the yaw.
>> > Is there something else I need to do? Ruben mentions "adapt the
>> > ChainFkSolverPos_recursive to also take a TaskSpace Weight into
>> > account". Is this needed? Is it already implemented somewhere and I'm
>> > missing it? If I need to do implement it myself, can someone point me
>> > in the right direction?
>> >
>> > thanks,
>> > Aaron
>> >
>> >
>>
>> --
>> Orocos-Users mailing list
>> Orocos-Users [..] ...
>> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>>
>>
>>
>>
> --
> KU Leuven, Mechanical Engineering, Robotics Research Group
> <http://people.mech.kuleuven.be/~bruyninc> Tel: +32 16 328056
> Vice-President Research euRobotics <http://www.eu-robotics.net>
> Open RObot COntrol Software <http://www.orocos.org>
> Associate Editor JOSER <http://www.joser.org>, IJRR <http://www.ijrr.org>