Hi,

We are looking at implementing serial-chain dynamics using an iterative Newton-Euler method. We are using the KDL data structures and were hoping this could be incorporated into KDL eventually.

I was hoping to get some feedback from people on the method to use (iterative Newton-Euler, Featherstone, etc, etc.) and suggestions for testing our implementation (using standard software, hard-coded models for specific robots, etc.).

Also, I was curious about using trees instead of serial-chains. Does KDL now implement trees or is this still in the works?

Thank you,

Regards,

Sachin

## adding serial-chain dynamics to KDL

On Mon, 14 Jul 2008, sachinc wrote:

> We are looking at implementing serial-chain dynamics using an iterative

> Newton-Euler method. We are using the KDL data structures and were hoping

> this could be incorporated into KDL eventually.

It will! The data structures are designed for this, and can represent

serial as well as tree-structured chains.

> I was hoping to get some feedback from people on the method to use (iterative

> Newton-Euler, Featherstone, etc, etc.) and suggestions for testing our

> implementation (using standard software, hard-coded models for specific

> robots, etc.).

I have been looking for a long time myself to find some code to test

against, but there is not much out there. Maybe Peter Corke's Robotics

Toolbox is the best one....

> Also, I was curious about using trees instead of serial-chains. Does KDL now

> implement trees or is this still in the works?

The recursive algorithms work on a segment-to-segment basis, which requires

only a small extension between serial chains and trees; this extension is

purely bookkeeping. We haven't implemented that yet, but we have concrete

ideas how to tackle that problem.

We have been studying the following things during the last years, and have

come to some design ideas (which we can discuss further on the list, if you

like; I would, for sure :-)):

- KDL is ready to represent kinematic chains that are trees and graphs.

- we still miss "standards" to represent stiffness/compliance (joint space

and Cartesian space), damping/inverse damping, inertia/mobility.

- we think we need two complementary APIs: a semantic API that is

independent of the computational solver, and the mathematical API, one

for each solver. For example: the inverse position kinematics problem has

the same semantic form for all kinematic chains, e.g.

JointVector = InversePositionKinematics(chain, CartesianPose).

But different solvers are possible: analytical (for specific kinematic

design), and numerical (with a variety of approaches).

Going to velocity and acceleration levels, and certainly to dynamics,

makes this splitting between semantics and solvers even more useful.

The semantic APIs are easy to define, and we have to have a good approach

to define concrete solver APIs. This shouldn't be too difficult: each

concrete solver adds solver-specific data structures and properties to the

interface, which could be taken together in one single solver class, which

is added to the semantic API. Also the mathematical representation of the

kinematic and dynamic objects should be specified for each

abstract/semantic "kinematic chain".

- one should have separate objects (both data structures and algorithsm)

for the following (to be strictly decoupled!) components:

- "system" (kinematic chain): representing the connection structure and

the fixed geometric and dynamic paramters;

- "solver": extra parameters for each solver, such as "epsilon" values

for numerical approximation; tree-transversal representations for most

recursive methods; specific numerical algorithms, such as O(N) or

O(N^3) methods, etc.

- "state": the values of joint and Cartesian objects (position, velocity,

acceleration, torque, force) at a specific instant in the solver's

computation; intermediate computational results in recursive solvers;

"distance" to singularities; etc.

- "operators": temporary objects in the solver, such as (SVD

decompositions of) Jacobian matrices; frames at intermediate joints; etc.

- "study": a "trajectory" of inputs (both explicit setpoints and implicit

constraints) and desired outputs, and a time frame over which the

"state" of the "system" must be calculated (and stored for later analysis

or visualisation);

- "control": how to use the output of the forward/inverse

kinematics/dynamics to move the robot to the next time instant.

- 'persistency formats' with which to read from, and write to, file (or

"stream") all of the objects above, with both XML and binary file formats.

- we would like to start with a "damped least-squares" like solver, with

both a recursive O(N) method and a traditional O(N^3) Newton-Euler; and

with an instantaneous solver and a "closed loop inverse kinematics"

"filter".

Relevant literature for the O(N) solvers comes from: Vereshchagin;

Featherstone; Rodrigues and Jain.

- we have simple interfaces to Blender (via Python), in order to visualise

results, and we have ideas about how KDL could replace the current simple

(and erroneous) IK function of Blender.

- in our research of the last two years, we have developed an integrated

task specification approach that is based on kinematic loops, and that

fits to all kinds of controllers and estimators and sensors. KDL will be

extended to support this task specification framework; Ruben has done

already some preliminary (currently too demo-specific :-( ) work on this.

We can do, for example, force control and visual servoing with the same

"system".

- the work by Khatib is impressive, with respect to the control problems

they solve in their approach: internally, the mass matrices and motions

are calculated with recursive O(N) methods, but their control uses

explicit (inertia-weighted) pseudo-inverses. They haven't published any

details about the implementations, but a good overview of what they have

as functionality for humanoids can be found in the recent publications of

Luis Sentis, a PhD student of Khatib:

- I am preparing (since a long time already...) a paper that takes all

these (linear!) constraints into account in the same recursive approach,

including the task priority-based controllers of Khatib. I hope I will

find the time to finish it... But I don't know whether a fully recursive

method will be computationally more efficient than Khatib's approach.

- I want to design a KDL API (semantic and mathematical) that can do all of

Khatib's stuff, since I think they have 'done it all' and certainly ahve

the richest feature set of all kinematic and dynamic libraries...

So, please comment on how well or bad this "roadmap" fits with yours, and

where your priorities lie. Rougly said, we will prefer to cooperate on

contributions that fit into this roadmap :-)

Herman

Disclaimer: http://www.kuleuven.be/cwis/email_disclaimer.htm

## adding serial-chain dynamics to KDL

On Tue, 15 Jul 2008, Herman Bruyninckx wrote:

>> We are looking at implementing serial-chain dynamics using an iterative

>> Newton-Euler method. We are using the KDL data structures and were hoping

>> this could be incorporated into KDL eventually.

>> I was hoping to get some feedback from people on the method to use

>> (iterative Newton-Euler, Featherstone, etc, etc.) and suggestions for

>> testing our implementation (using standard software, hard-coded models for

>> specific robots, etc.).

> I have been looking for a long time myself to find some code to test

> against, but there is not much out there. Maybe Peter Corke's Robotics

> Toolbox is the best one....

Or maybe the ROBOOP code:

...

Herman

Disclaimer: http://www.kuleuven.be/cwis/email_disclaimer.htm

## adding serial-chain dynamics to KDL

FYI we are using both of these at work, RobOOP and Orocos. You have to pay particular attention to get them to agree, at least in terms of forward and inverse kinematics of a serial 7 DOF manipulator.

YMMV

S

On Tuesday, July 15, 2008, at 03:44PM, "Herman Bruyninckx" <Herman [dot] Bruyninckx [..] ...> wrote: ...

>On Tue, 15 Jul 2008, Herman Bruyninckx wrote:

>

>>> We are looking at implementing serial-chain dynamics using an iterative

>>> Newton-Euler method. We are using the KDL data structures and were hoping

>>> this could be incorporated into KDL eventually.

>>> I was hoping to get some feedback from people on the method to use

>>> (iterative Newton-Euler, Featherstone, etc, etc.) and suggestions for

>>> testing our implementation (using standard software, hard-coded models for

>>> specific robots, etc.).

>> I have been looking for a long time myself to find some code to test

>> against, but there is not much out there. Maybe Peter Corke's Robotics

>> Toolbox is the best one....

>

>Or maybe the ROBOOP code:

>

>

>Herman

## First-cut working implementation of serial chain dynamics

We have a first-cut working implementation of inverse dynamics for

a serial chain with revolute joints. We have checked our implementation

against Peter Corke's robotics toolbox.

This required us to make some changes and additions to the existing

KDL data structures. We would like to get feedback about the best

way to make the implementation conform with the KDL's coding style,

have multiple people check it and and then hopefully contribute it

to the KDL repository.

This is the output of svn stat:

M inertia.hpp

M frames_io.hpp

A inertiamatrix.cpp

M segment.cpp

A chainidsolver_newtoneuler.hpp

M inertia.cpp

M frames_io.cpp

A inertiamatrix.hpp

A chainidsolver.hpp

M segment.hpp

A chainidsolver_newtoneuler.cpp

M joint.hpp

I'll briefly explain the changes that I have made:

1. inertia.hpp -

- encapsulated the Inertia properties into a separate class

(InertiaMatrix). This makes it cleaner when we want to

multiply the Inertia matrix with a Vector. (Similar to

multiplication of Rotation with a Vector)

2. frames_io.hpp and cpp-

- function for printing out the InertiaMatrix.

3. segement.hpp and cpp -

- added member variable r_cm which is the location of the

center of mass of the segment in body coordinates.

4. joint.hpp -

- function to return the Vector corresponding to the joint

axis for a revolute joint.

The following are the files that I have added:

1. inertiamatrix.hpp and cpp -

- definition of the class InertiaMatrix.

- defines multiplication between the Inertia Matrix and a

Vector. (Similar to the Rotation class).

2. chainidsolver.hpp -

- defines the abstract class of all Inverse Dynamics solvers

for serial chains.

- Currently only function defined is InverseDynamics (joint

position, velocity, acceln -> joint torques)

- functions to return the mass, coriolis and gravity matrix

should also be added to this class.

3. chainidsolver_newtoneuler.hpp and cpp -

- implementation of the chainidsolver using Newton-Euler

formulation.

- serial chains with revolute joints are supported.

- code has been checked against Peter Corke's robotics toolbox

(python version) for a PUMA560.

All these files can be downloaded as a tarball from this page:

http://pr.willowgarage.com/wiki/KDL

Is there any way of adding attachments to forum posts?

Advait Jain

Willow Garage Inc.

(http://www.willowgarage.com)

## First-cut working implementation of serial chain dynamics

On Tue, Jul 22, 2008 at 12:37 AM, advaitjain <advaitjain [..] ...> wrote:

[...]

> All these files can be downloaded as a tarball from this page:

> http://pr.willowgarage.com/wiki/KDL

>

> Is there any way of adding attachments to forum posts?

Normally there is (at least I can do it), but the preferred way for

such "feature requests" is to use the orocos bugzilla, where you can

easily attach your patches.

All output of this bugzilla ends up on the dev mailinglist (and thus

the forums/website) too.

regards,

Klaas

## First-cut working implementation of serial chain dynamics

On Tue, 21 Jul 2008, advaitjain wrote:

> We have a first-cut working implementation of inverse dynamics for

> a serial chain with revolute joints. We have checked our implementation

> against Peter Corke's robotics toolbox.

Thanks a lot for your contributions!!!

I prefer moving this thread to the developers mailing list, orocos-dev,

which is more appropriate at this stage where code is being produced and

discussed... :-) So, please don't reply on this message anymore on the

orocos-users list.

> This required us to make some changes and additions to the existing

> KDL data structures. We would like to get feedback about the best

> way to make the implementation conform with the KDL's coding style,

> have multiple people check it and and then hopefully contribute it

> to the KDL repository.

I leave it to KDL's maintainer, Ruben, to make the decisions, but I have

at this moment, so he might be a bit

some comments here and there. (Ruben is on the RobotCub coding summerschool

slower than usual in responding.)

>

> This is the output of svn stat:

>

> M inertia.hpp

> M frames_io.hpp

> A inertiamatrix.cpp

> M segment.cpp

> A chainidsolver_newtoneuler.hpp

> M inertia.cpp

> M frames_io.cpp

> A inertiamatrix.hpp

> A chainidsolver.hpp

> M segment.hpp

> A chainidsolver_newtoneuler.cpp

> M joint.hpp

>

>

> I'll briefly explain the changes that I have made:

>

> 1. inertia.hpp -

> - encapsulated the Inertia properties into a separate class

> (InertiaMatrix). This makes it cleaner when we want to

> multiply the Inertia matrix with a Vector. (Similar to

> multiplication of Rotation with a Vector)

>

> 2. frames_io.hpp and cpp-

> - function for printing out the InertiaMatrix.

>

> 3. segement.hpp and cpp -

> - added member variable r_cm which is the location of the

> center of mass of the segment in body coordinates.

>

> 4. joint.hpp -

> - function to return the Vector corresponding to the joint

> axis for a revolute joint.

>

>

> The following are the files that I have added:

>

> 1. inertiamatrix.hpp and cpp -

> - definition of the class InertiaMatrix.

> - defines multiplication between the Inertia Matrix and a

> Vector. (Similar to the Rotation class).

>

> 2. chainidsolver.hpp -

> - defines the abstract class of all Inverse Dynamics solvers

> for serial chains.

Your method signature is a bit too restricted; at least, I had a more

generic API for the abstract ID class in mind, which I will explain in a

follow-up "roadmap" email later today. More in particular: Inverse Dynamics

can be done in more ways that by given the joint positions, velocities and

accelerations, so locking down the method signature to only this case is

too restrictive. Especially for redundant arms, where the joint

accelerations are not (necessarily) inputs.

Basically, my idea of abstract API was as follows:

outState ChainID (Chain, inState, Solver);

where "Chain" is a specific KinematicChain object, "Solver" is a specific

numerical algorithm, and "inState" and "outState" are the (chain and

solver-specific!) state objects (they will in general not be of the same

type, since they contain different sets of joint, Cartesian and chain

variables).

> - Currently only function defined is InverseDynamics (joint

> position, velocity, acceln -> joint torques)

This particular set of inputs belongs to one particular _solver_

implementation. (Or, rather, a _family_ of solvers, with the same input and

output variables.)

> - functions to return the mass, coriolis and gravity matrix

> should also be added to this class.

In my opinion, these are properties of a _Chain_ in a certain state; not of

a solver...

> 3. chainidsolver_newtoneuler.hpp and cpp -

> - implementation of the chainidsolver using Newton-Euler

> formulation.

"Newton-Euler" is a bit too restrictive to identify a solver uniquely. I

think (but please comment on this suggestion!) that solvers should rather

be identified by the specific algorithm that is being used, for example,

Luh, Walker and Paul's (which is what you use (via Spong's book, I guess),

or did you base your code on another publication?).

It also seems that you use dynamic allocations, which makes the solver

dangerous for realtime applications; all temporary variables are best

allocated on the stack; or kept as "state" variables in your solver,

especially if the solver is to be called repeatedly such as in a control

loop.

> - serial chains with revolute joints are supported.

> - code has been checked against Peter Corke's robotics toolbox

> (python version) for a PUMA560.

>

> All these files can be downloaded as a tarball from this page:

> http://pr.willowgarage.com/wiki/KDL

>

> Is there any way of adding attachments to forum posts?

>

> Advait Jain

>

> Willow Garage Inc.

> (http://www.willowgarage.com)

>

## First-cut working implementation of serial chain dynamics

On Tue, 21 Jul 2008, advaitjain wrote:

> We have a first-cut working implementation of inverse dynamics for

> a serial chain with revolute joints. We have checked our implementation

> against Peter Corke's robotics toolbox.

Thanks a lot for your contributions!!!

I prefer moving this thread to the developers mailing list, orocos-dev,

which is more appropriate at this stage where code is being produced and

discussed... :-) So, please don't reply on this message anymore on the

orocos-users list.

> This required us to make some changes and additions to the existing

> KDL data structures. We would like to get feedback about the best

> way to make the implementation conform with the KDL's coding style,

> have multiple people check it and and then hopefully contribute it

> to the KDL repository.

I leave it to KDL's maintainer, Ruben, to make the decisions, but I have

at this moment, so he might be a bit

some comments here and there. (Ruben is on the RobotCub coding summerschool

slower than usual in responding.)

>

> This is the output of svn stat:

>

> M inertia.hpp

> M frames_io.hpp

> A inertiamatrix.cpp

> M segment.cpp

> A chainidsolver_newtoneuler.hpp

> M inertia.cpp

> M frames_io.cpp

> A inertiamatrix.hpp

> A chainidsolver.hpp

> M segment.hpp

> A chainidsolver_newtoneuler.cpp

> M joint.hpp

>

>

> I'll briefly explain the changes that I have made:

>

> 1. inertia.hpp -

> - encapsulated the Inertia properties into a separate class

> (InertiaMatrix). This makes it cleaner when we want to

> multiply the Inertia matrix with a Vector. (Similar to

> multiplication of Rotation with a Vector)

>

> 2. frames_io.hpp and cpp-

> - function for printing out the InertiaMatrix.

>

> 3. segement.hpp and cpp -

> - added member variable r_cm which is the location of the

> center of mass of the segment in body coordinates.

>

> 4. joint.hpp -

> - function to return the Vector corresponding to the joint

> axis for a revolute joint.

>

>

> The following are the files that I have added:

>

> 1. inertiamatrix.hpp and cpp -

> - definition of the class InertiaMatrix.

> - defines multiplication between the Inertia Matrix and a

> Vector. (Similar to the Rotation class).

>

> 2. chainidsolver.hpp -

> - defines the abstract class of all Inverse Dynamics solvers

> for serial chains.

Your method signature is a bit too restricted; at least, I had a more

generic API for the abstract ID class in mind, which I will explain in a

follow-up "roadmap" email later today. More in particular: Inverse Dynamics

can be done in more ways that by given the joint positions, velocities and

accelerations, so locking down the method signature to only this case is

too restrictive. Especially for redundant arms, where the joint

accelerations are not (necessarily) inputs.

Basically, my idea of abstract API was as follows:

outState ChainID (Chain, inState, Solver);

where "Chain" is a specific KinematicChain object, "Solver" is a specific

numerical algorithm, and "inState" and "outState" are the (chain and

solver-specific!) state objects (they will in general not be of the same

type, since they contain different sets of joint, Cartesian and chain

variables).

> - Currently only function defined is InverseDynamics (joint

> position, velocity, acceln -> joint torques)

This particular set of inputs belongs to one particular _solver_

implementation. (Or, rather, a _family_ of solvers, with the same input and

output variables.)

> - functions to return the mass, coriolis and gravity matrix

> should also be added to this class.

In my opinion, these are properties of a _Chain_ in a certain state; not of

a solver...

> 3. chainidsolver_newtoneuler.hpp and cpp -

> - implementation of the chainidsolver using Newton-Euler

> formulation.

"Newton-Euler" is a bit too restrictive to identify a solver uniquely. I

think (but please comment on this suggestion!) that solvers should rather

be identified by the specific algorithm that is being used, for example,

Luh, Walker and Paul's (which is what you use (via Spong's book, I guess),

or did you base your code on another publication?).

It also seems that you use dynamic allocations, which makes the solver

dangerous for realtime applications; all temporary variables are best

allocated on the stack; or kept as "state" variables in your solver,

especially if the solver is to be called repeatedly such as in a control

loop.

> - serial chains with revolute joints are supported.

> - code has been checked against Peter Corke's robotics toolbox

> (python version) for a PUMA560.

>

> All these files can be downloaded as a tarball from this page:

> http://pr.willowgarage.com/wiki/KDL

>

> Is there any way of adding attachments to forum posts?

>

> Advait Jain

>

> Willow Garage Inc.

> (http://www.willowgarage.com)

>

## First-cut working implementation of serial chain dynamics

On Tuesday 22 July 2008 11:35:33 Herman Bruyninckx wrote:

> On Tue, 21 Jul 2008, advaitjain wrote:

> > We have a first-cut working implementation of inverse dynamics for

> > a serial chain with revolute joints. We have checked our implementation

> > against Peter Corke's robotics toolbox.

>

[...]

>

> It also seems that you use dynamic allocations, which makes the solver

> dangerous for realtime applications; all temporary variables are best

> allocated on the stack; or kept as "state" variables in your solver,

> especially if the solver is to be called repeatedly such as in a control

> loop.

As long as you allow different implementations, I see no problem with such

code. Also, 'we' are moving to real-time systems where dynamic allocations

are possible in a real-time context (given some memory limits). This will

make most of this kind of discussions largely irrelevant.

>

> > - serial chains with revolute joints are supported.

> > - code has been checked against Peter Corke's robotics toolbox

> > (python version) for a PUMA560.

> >

> > All these files can be downloaded as a tarball from this page:

> > http://pr.willowgarage.com/wiki/KDL

> >

> > Is there any way of adding attachments to forum posts?

Not yet, you'll need to use the Bugzilla page for posting patches. Also,

attachments of mailinglist posts do not appear on the forum,

so you need to consult the mailinglist archive.

Peter

## First-cut working implementation of serial chain dynamics

On Tue, 12 Aug 2008, Peter Soetens wrote:

> [...]

>>

>> It also seems that you use dynamic allocations, which makes the solver

>> dangerous for realtime applications; all temporary variables are best

>> allocated on the stack; or kept as "state" variables in your solver,

>> especially if the solver is to be called repeatedly such as in a control

>> loop.

>

> As long as you allow different implementations, I see no problem with such

> code.

This is indeed one of the advantages I see of the introduction of the

"Solver" concept: each solver can serve different needs, while still being

compatible at API level. We do need to come up with a good documentation

and "inspection" approach, such that these properties are revealed

unambiguously and completely to the user of the solvers.

> Also, 'we' are moving to real-time systems where dynamic allocations

> are possible in a real-time context (given some memory limits). This will

> make most of this kind of discussions largely irrelevant.

You are already refuting your own answer: "given some memory limits"! If

there are memory limits, you have to take them into account

deterministically, or you don't have realtime... But again: I agree with

your statement that these "problems" can be coped with rather easily once

we support different solvers for the same API.

Herman

## adding serial-chain dynamics to KDL

On Monday 14 July 2008 22:59:08 sachinc wrote:

> Hi,

>

> We are looking at implementing serial-chain dynamics using an iterative

> Newton-Euler method. We are using the KDL data structures and were hoping

> this could be incorporated into KDL eventually.

Any contributions are welcome.

> I was hoping to get some feedback from people on the method to use

> (iterative Newton-Euler, Featherstone, etc, etc.) and suggestions for

> testing our implementation (using standard software, hard-coded models for

> specific robots, etc.).

We are looking into different algorithms right now, the implementation is on my

personal "by the end of the year" list.

> Also, I was curious about using trees instead of serial-chains. Does KDL

> now implement trees or is this still in the works?

I had an implementation for trees and a Forward Position/Velocity kinematics

algorithm for it (which is quite straightforward). But because data got

started to be spread all over the different structures i decided that the

implementation was no good and started again (see tree.cpp/hpp) and never

finished it (lack of time). I think the current code is able to build trees

from single segments, sub-chains and sub-trees, but again, maybe this code is

not the way to go since i started implementing my own graph structure instead

of existing graph code (such as boost::graph) . The solvers have disappeared,

but the idea was to copy the kinematic graph structure put algorithms on the

edges and temporal data in the nodes.

Ruben

## adding serial-chain dynamics to KDL

On Tuesday 15 July 2008 09:19:22 Ruben Smits wrote:

> graph structure instead of existing graph code (such as boost::graph) . The

From my personal experience, boost::graph (or any graph library for that

matter) isn't worth it unless you have a very good reason (read: need for

some boost::graph algorithms which you can't find elsewhere). boost::graph is

hard to setup and use and generally, you'll be writing some wrapper classes

anyway to help your users getting around the difficult API. It's better to

embed your self written tree structure in your own data structures.

Peter