Ruben

On Thu, Mar 22, 2012 at 5:27 AM, Ruben Smits
<ruben [dot] smits [..] ...> wrote:
>
> > A few questions:
> >
> >   * Are new solvers welcome?
>
> New solvers are always welcome, very welcome. And I would love to
> include yours as well.

Great!

>
> >   * What are the requirements (needs unit test? coding standard?
> > mathematical proof? etc)?
>
> * It should at least not break the build of KDL ;)

That's a reasonable requirement : )

> * It should be realtime usable, this means that there shouldn't be any
> memory allocation in the functions that are used in the update cycle
> (CartToJnt), and deterministic in time wrt loops.

I tried to do this, but it's possible I may have missed something
since I've not had much experience worrying about what does and does
not do dynamic allocation internally.

>
> * A unit test is always welcome, but it is not a show-stopper, as long
> as it is missing we can mark the solver as experimental. The unit test
> should test two things: see if we can successfully create the solver
> and call its functions without crashing and secondly prove its
> internal functionality, usually we do this with a forward - inverse
> recursion, which means you start from a given nominal cartesian space
> input, using your solver you calculate the joint space output. Than
> you use the joint space output in a forward solver to verify if it
> results in the desired cartesian space motion. You can check the
> existing unit tests to see how it works.

I'll take a look at the unit tests, I might be able to easily create a
new one with little modification from a copy paste. Otherwise It'll
probably have to wait until we have some down time.

>
>
> > I've received the necessary permission from my company to submit, but
> > I'm
> > not sure how much time I can spend on building unit tests, polishing
> > etc.
>
> The easiest to proceed is that you open a bug report and attach a
> patch against the master branch. Make sure you include your name and
> company in the copyright statement and a license header. Than we can
> start from there to see how much polishing is necessary.

I replied to Herman's comment with code, but I'll put subsequent code
in a bug report.

On 03/22/2012 10:27 AM, Ruben Smits wrote:
> Hi Andy,
>
> On Wed, Mar 21, 2012 at 5:16 PM, Andy Somerville
> <andy [dot] somerville [..] ...> wrote:
>> Hi,
>>
>> I've written a IK position solver in the course of my work and I'm hoping
>> that it might be useful to others.
>>
>> The solver basically combines uses WDLS (Weighted pseudo inverse with Damped
>> Least-Square) velocity solver to do gradient descent while using the joint
>> space weights to respect joint limits. It's worked beautifully to in our IK
>> setup where the NR_JL (Newton-Raphson Joint Limit) had been failing for us.
>>
iTaSC contains also a component doing a similar thing, including priorities next to the WDLS
it isn't integrated in KDL anyway, but it should (?) at some point

nick
>> I'd love to submit it if new solvers are welcome, however as I'm somewhat
>> new to this sub-discipline, I'm not certain it would meet the requirements
>> be considered for inclusion.
>>
>> A few questions:
>>
>> * Are new solvers welcome?
>
> New solvers are always welcome, very welcome. And I would love to
> include yours as well.
>
>> * What are the requirements (needs unit test? coding standard?
>> mathematical proof? etc)?
>
> * It should at least not break the build of KDL ;)
> * It should be realtime usable, this means that there shouldn't be any
> memory allocation in the functions that are used in the update cycle
> (CartToJnt), and deterministic in time wrt loops.
>
> * A unit test is always welcome, but it is not a show-stopper, as long
> as it is missing we can mark the solver as experimental. The unit test
> should test two things: see if we can successfully create the solver
> and call its functions without crashing and secondly prove its
> internal functionality, usually we do this with a forward - inverse
> recursion, which means you start from a given nominal cartesian space
> input, using your solver you calculate the joint space output. Than
> you use the joint space output in a forward solver to verify if it
> results in the desired cartesian space motion. You can check the
> existing unit tests to see how it works.
>
>
>> I've received the necessary permission from my company to submit, but I'm
>> not sure how much time I can spend on building unit tests, polishing etc.
>
> The easiest to proceed is that you open a bug report and attach a
> patch against the master branch. Make sure you include your name and
> company in the copyright statement and a license header. Than we can
> start from there to see how much polishing is necessary.
>
>> Thoughts?
>>
>> Andy
>
> Ruben
>
>>
>>
>> --
>> Orocos-Dev mailing list
>> Orocos-Dev [..] ...
>> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-dev
>>
>
>
>

On Thu, Mar 22, 2012 at 1:12 AM, Herman Bruyninckx <
Herman [dot] Bruyninckx [..] ...> wrote:
>
> On Wed, 21 Mar 2012, Andy Somerville wrote:
>
>> Hi,
>> I've written a IK position solver in the course of my work and I'm hoping
>> that it might be
>> useful to others.
>
> Anyway, I would welcome a discussion about your design on orocos-dev. And
> no decisions have already been taken to indeed proceed in the "new
> direction" mentioned above.
>

The algorithm is probably not particularly novel but I think the two
important
parts are:
* Scale all velocities to be between -1 and 1 by dividing the max Joint
velocity
* Use small steps and cut them in half any time it sees a solution farther
than
the last
* If joint limits are violated, abandon a solution and try again next round
(rather than keeping that solution but pulling in the one joint to be
inside the limits)

rough algorithm:

loop over iterations
check if solution is close enough
if yes
return true

use velocity solver to get joint motion proportions for motion toward goal
find abs-max-joint-velocity and divide velocity solver solution by that
to scale between -1 and 1
multiply by predetermined step size factor (perhaps this is too naive)
add scaled velocity solution to old solution to produce new solution
check to see if new solution is closer than old solution
if not
divide step size factor by 2

set solver weights to 1 for all joints
loop over joints
if joint violates joint limits
set that joint weight to near 0
flag that limits violated

if limts violated throw out new solution and replace with old
(new iteration will attempt to step closer again without moving joint
which violated limits)

return false since all iterations have completed without success

function draft pass at preparing code for KDL (rich text to avoid gmail
line wrapping):

bool ikPosSolve_hillClimb_jointLimit( KDL::ChainIkSolverVel_wdls &
ikVelocitySolver,
KDL::ChainFkSolverPos_recursive &
fkPositionSolver,
const KDL::JntArray &
jointState,
const KDL::Frame &
targetFrame,
KDL::JntArray &
jointSolution,
int
maxIterations,
const KDL::JntArray &
jointMin,
const KDL::JntArray &
jointMax,
float
epsilon )
{
int numJoints = jointState.rows();
jointSolution = jointState;
KDL::JntArray jointPosDiff( numJoints );
KDL::JntArray lastJointSolution( jointSolution );

// magic number for getting initial step size correct FIXME: too naive?
pass in param?
float baseJointMotionFactor = 0.1;

// For weighting joints. Declaring early to move dynamic allocation out
of loop
Eigen::MatrixXd jointSpaceIdenityMatrix = Eigen::MatrixXd::Identity(
numJoints, numJoints );
Eigen::MatrixXd jointSpaceWeightMatrix( jointSpaceIdenityMatrix );

for( int i = 0; i < maxIterations + 1; i++ ) // + 1 time because answers
checks are delayed by a cycle
{
KDL::Twist twistDiff0;

{ // Check for success
KDL::Frame targetAttempt;
fkPositionSolver.JntToCart( jointSolution, targetAttempt );
twistDiff0 = diff( targetAttempt, targetFrame );
if( Equal( twistDiff0, KDL::Twist::Zero(), epsilon ) )
return true; // SUCCESS!
}

{ // do the step toward the solution
int velSolverStatus = ikVelocitySolver.CartToJnt( jointSolution,
twistDiff0, jointPosDiff );
if( velSolverStatus < 0 )
return false;

// Find the biggest joint value so we can scale everything
between -1 and 1
double maxAbsDiffValue = 0.0001;
for( unsigned int jointIndex = 0; jointIndex <
jointSolution.rows(); ++jointIndex )
{
maxAbsDiffValue = std::max( maxAbsDiffValue,
std::abs(jointPosDiff(jointIndex)) );
}

float jointMotionFactor = baseJointMotionFactor;
jointMotionFactor *= 1/maxAbsDiffValue; // scale
everything between -1 and 1 * baseJointMotionFactor

lastJointSolution = jointSolution;
jointSolution.data = jointSolution.data + (jointPosDiff.data *
jointMotionFactor);

// Check out solution and compare to previous
KDL::Frame testAttempt;
fkPositionSolver.JntToCart( jointSolution, testAttempt );
KDL::Twist twistDiff1 = diff( testAttempt, targetFrame );

// If our new solution is further from the target than our new
one, cut our step size in half
if( (twistDiff1.vel.Norm() > twistDiff0.vel.Norm())
|| (twistDiff1.rot.Norm() > twistDiff0.rot.Norm()) )
{
baseJointMotionFactor *= 0.5;
}
}

{ // check for and back off of any joint limit violations
jointSpaceWeightMatrix = jointSpaceIdenityMatrix;

bool jointLimitViolation = false;
for( unsigned int jnt = 0; jnt < jointMin.rows()
&& jnt < jointMax.rows(); ++jnt )
{
if( jointSolution(jnt) < jointMin(jnt)
|| jointSolution(jnt) > jointMax(jnt) )
{
jointSpaceWeightMatrix( jnt, jnt ) = 0.0001; // Not sure
if this needs to be non-zero
jointLimitViolation = true;
}
}

if( jointLimitViolation ) // solution invalidated
by joint limit violation
jointSolution = lastJointSolution; // try step again with
weight of violating joint reduced

ikVelocitySolver.setWeightJS( jointSpaceWeightMatrix );
}
}

// completed max cycles without going below error threshold
return false;
}