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
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
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