On Mon, 6 Dec 2010, Ruben Smits wrote: > On Sunday 05 December 2010 17:57:52 Pablo Iņigo Blasco wrote: >> Hi Ruben and ROS community! >> >> Our group is trying to use kdl IKs in a similar way as Herman says. We >> are also interested in relaxing some of the destination frame >> constraints. In our case we're only interested in the location of the >> end-effector, not its orientation. We've been having a look to all >> iksolvers (both position and velocity). As you said it seems easy to >> figure out how solve this problem from the existing code. However we >> are shuffling two alternatives: >> >> 1 - To implement a completely new iksolver_pos inspired in >> ChainIkSolverPos_NR_JL and ChainIkSolverVel_wdls (mainly changing the >> epsilon error check code taking into account the weights in the >> iksolver_pos too) >> >> 2 - To implement the "the mysterious unimplemented and uncommented" >> method: virtual int CartToJnt(const JntArray& q_init, const FrameVel& >> v_in, JntArrayVel& q_out) of the ChainIkSolverVel_wdls. It's curious, >> all of IkSolversVel have this method unimplemented. Why don't remove >> it? :-) >> We wonder if this method is supposed to solve the ikpos and ikvel >> simultaneously. What is the right semantic of this method? > > That's exactly what it is supposed to do (according to the documentation of > the function: Calculate inverse position and velocity kinematics, from > cartesian position and velocity to joint positions and velocities) Well, the naming of the parameters suggests something else: the IVK is computed with the current joint positions as _inputs_, so the IPK need not be solved anymore. > I've introduced the function when I was playing with the idea that this was a > good thing. I'm not so sure anymore. There are use cases where this makes sense, since it is common to be able to read the current joint positions. Herman >> Would such >> implementation be cohesive with the class responsibilities? > > I think that might be the main problem. Because you loose the capability of > mixing different algorithms at the velocity and the position level. > >> This is our theory since the output of this method is a JntArrayVel >> q_out object. We think that it could be implemented in this way: >> q_out = [q_end, delta_q] / q_end = ik(get_pose(v_in)) ^ delta_q = >> q_end - q_init. >> (alternatively, another interpretation for delta_q could be: delta_q= >> ik_vel(get_twist(v_in)) >> >> Suggestions? Corrections? > > After going through this I'm even more convinced the function should be > removed. There is no clean way to implement it generically. > > Ruben > >> Thank you. >> >> >> On Mon, Nov 15, 2010 at 11:34 AM, Ruben Smits >> >> wrote: >>> On Monday 15 November 2010 11:13:51 Ruben Smits wrote: >>>> On Thursday 11 November 2010 11:52:06 Jeroen Willems wrote: >>>>> On 2010-11-09 09:27, Herman Bruyninckx wrote: >>>>>> On Tue, 9 Nov 2010, Jeroen Willems wrote: >>>>>>> I have a question regarding the inverse kinematics using KDL. >>>>>>> >>>>>>> I would like to use carttojnt of the inverse kinematics solver. >>>>>>> The problem is I have to use a destination frame as input. >>>>>>> I'm using something similar to a elbow manipulator so you can >>>>>>> imagine I only want a reference position. The arm only can have >>>>>>> 2 poses to a reference position, to get the rotational part of >>>>>>> the frame I actually have to get the joint positions to >>>>>>> calculate it (correct me if I'm wrong). >>>>>>> >>>>>>> So >>>>>>> 1. Am I right? >>>>>>> 2. Could I use KDL to compute the IK or should I use an analytic >>>>>>> solution? >>>>>> >>>>>> The major issue with your problem is that "IK" does not make too >>>>>> much sense for a 2DOF system: when you don't take precautions the >>>>>> input frame will never lie in the span of your 2DOF, so you will >>>>>> have make a "projection" from the full 6D space onto your 2D >>>>>> subspace. KDL has algorithms to do that, but they make choices >>>>>> about how to project that might not correspond to your intuition. >>>>>> But that's inevitable. >>>>>> >>>>>> So, please try to formulate exactly what kind of input you would >>>>>> like or need to have. >>>>>> >>>>>> Herman >>>>>> _______________________________________________ >>>>>> ros-users mailing list >>>>>> ros-users@code.ros.org >>>>>> https://code.ros.org/mailman/listinfo/ros-users >>>>> >>>>> Thanks for the response. >>>>> >>>>> Okay to be more precise to give a better idea of what I'm doing. >>>>> I have an hexapod, so a torso with 6 legs consisting of 3 links >>>>> (without torso) and 3 joints. I want to be the tip/foot on an >>>>> specific point (x,y,z) in respect to the torso (thorax) so I have to >>>>> know the three joint positions to get there. >>>>> >>>>> So preferable without giving a rotation. So I'm really curious how to >>>>> obtain this or how to use a "projection". >>>> >>>> There is a inverse (CartToJnt) velocity solver in KDL that you can use >>>> to disable (or weigth) some of the degrees of freedom in the >>>> cartesian space: >>>> >>>> http://people.mech.kuleuven.be/~orocos/pub/devel/kdl/latest/api/html/cl >>>> assK DL_1_1ChainIkSolverVel__wdls.html >>>> >>>> You can use the setWeightTS function for this. Setting a diagonal value >>>> of zero disables the joint, every other value neq 0 will be used as a >>>> weighting value. >>> >>> This should have been: disables the degree of freedom (0-5)->(TX-Z,RX-Z) >>> >>> R. >>> >>>> So if you want to disable the rotations, just set the last three >>>> diagonal values to 0. >>>> >>>> You can then use this solver in one of the inverse position solvers, >>>> but the problem is that we do not have a solver yet that can take >>>> these weights into account. But I do not think it should be to hard to >>>> figure that out yourself when starting from the code of the iterative >>>> solver for the inverse position kinematics: >>>> >>>> http://people.mech.kuleuven.be/~orocos/pub/devel/kdl/latest/api/html/cl >>>> assK DL_1_1ChainIkSolverPos__NR.html >>>> >>>> >>>> Ruben