Jacobian Solver/Intermediate Frames

Has anyone developed a version of ChainJntToJacSolver::JntToJac(const JntArray& q_in,Jacobian& jac) that will compute the Jacobian for an intermediate frame in a chain? This would be similar to finding the intermediate frame segmentNr in the forward kinematics solver ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr). Otherwise, you need to create a new chain that terminates at the frame for which you want to find the Jacobian. It seems to me that an option could be added to JntToCart() to accommodate this and would eliminate a lot of unnecessary coding for developing self-motion kinematics. Thanks!

Ruben Smits's picture

Jacobian Solver/Intermediate Frames

On Friday 24 December 2010 17:19:35 CARIGNAN, CRAIG R. (GSFC-540.0)[Bastion
Technologies, Inc.] wrote:
> On Dec 23, 2010, at 3:29 AM, Ruben Smits wrote:
> > On Wednesday 22 December 2010 01:16:48 CARIGNAN, CRAIG R.
> > (GSFC-540.0)[Bastion
> >
> > Technologies, Inc.] wrote:
> >> Has anyone developed a version of ChainJntToJacSolver::JntToJac(const
> >> JntArray& q_in,Jacobian& jac) that will compute the Jacobian for an
> >> intermediate frame in a chain? This would be similar to finding the
> >> intermediate frame segmentNr in the forward kinematics solver
> >> ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame&
> >> p_out, int segmentNr). Otherwise, you need to create a new chain that
> >> terminates at the frame for which you want to find the Jacobian. It
> >> seems to me that an option could be added to JntToCart() to accommodate
> >> this and would eliminate a lot of unnecessary coding for developing
> >> self-motion kinematics.
> >
> > We already have a solver for this for the KDL::Tree class:
> >
> > * int KDL::TreeJntToJacSolver::JntToJac (const JntArray& q_in,
> > Jacobian& jac, const std::string& segmentname)
> >
> > Which will give you the jacobian from the root of the tree up to the
> > segment with the name segmentname. Since a chain is just a special case
> > of a tree and you can easily create a tree from a chain with:
> >
> > * bool addChain (const Chain &chain, const std::string& hook_name)
> >
> > with hook_name the root of the tree (being "root" by default).
> >
> > Would this work for you or would you still like this functionality for
> > the Chain class?
> >
> >> Thanks!
> >
> > -- Ruben
>
> Ruben,
>
> Now that I understand what a tree is (collection of chains), I think it
> would be best to add the functionality of obtaining intermediate Jacobians
> to the chain solver class. The user should not have to create a tree with
> just one chain in order to get the Jacobian of a particular segment. As
> mentioned before, this functionality would be consistent with the ability
> to obtain the intermediate pose of a chain in ChainFkSolverPos_recursive.

Ack.

> Are you the developer of the code to for ChainJntToJacSolver?

Yes I am :)

> If so, would
> this be a difficult option to add?

No, but I need to find the time to implement this. Probably a day or less to
code and a few days to test. You can always take a shot yourself, I'll be
happy to support your efforts with comments.

> This would be very helpful for
> developing self-motion kinematics for a 7-DOF arm which requires the
> calculation of the Jacobians for elbow and wrist translations.
>
> Thanks for your advice and help!
>
> -Craig

Ruben

Ruben Smits's picture

Jacobian Solver/Intermediate Frames

On Wednesday 22 December 2010 01:16:48 CARIGNAN, CRAIG R. (GSFC-540.0)[Bastion
Technologies, Inc.] wrote:
> Has anyone developed a version of ChainJntToJacSolver::JntToJac(const
> JntArray& q_in,Jacobian& jac) that will compute the Jacobian for an
> intermediate frame in a chain? This would be similar to finding the
> intermediate frame segmentNr in the forward kinematics solver
> ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out,
> int segmentNr). Otherwise, you need to create a new chain that terminates
> at the frame for which you want to find the Jacobian. It seems to me that
> an option could be added to JntToCart() to accommodate this and would
> eliminate a lot of unnecessary coding for developing self-motion
> kinematics.

We already have a solver for this for the KDL::Tree class:

* int KDL::TreeJntToJacSolver::JntToJac (const JntArray& q_in, Jacobian& jac,
const std::string& segmentname)

Which will give you the jacobian from the root of the tree up to the segment
with the name segmentname. Since a chain is just a special case of a tree and
you can easily create a tree from a chain with:

* bool addChain (const Chain &chain, const std::string& hook_name)

with hook_name the root of the tree (being "root" by default).

Would this work for you or would you still like this functionality for the
Chain class?

> Thanks!

-- Ruben

Jacobian Solver/Intermediate Frames

On 23/12/2010, at 21:29 , Ruben Smits wrote:

> On Wednesday 22 December 2010 01:16:48 CARIGNAN, CRAIG R. (GSFC-540.0)[Bastion
> Technologies, Inc.] wrote:
>> Has anyone developed a version of ChainJntToJacSolver::JntToJac(const
>> JntArray& q_in,Jacobian& jac) that will compute the Jacobian for an
>> intermediate frame in a chain? This would be similar to finding the
>> intermediate frame segmentNr in the forward kinematics solver
>> ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out,
>> int segmentNr). Otherwise, you need to create a new chain that terminates
>> at the frame for which you want to find the Jacobian. It seems to me that
>> an option could be added to JntToCart() to accommodate this and would
>> eliminate a lot of unnecessary coding for developing self-motion
>> kinematics.
>
> We already have a solver for this for the KDL::Tree class:
>
> * int KDL::TreeJntToJacSolver::JntToJac (const JntArray& q_in, Jacobian& jac,
> const std::string& segmentname)
>
> Which will give you the jacobian from the root of the tree up to the segment
> with the name segmentname. Since a chain is just a special case of a tree and
> you can easily create a tree from a chain with:
>
> * bool addChain (const Chain &chain, const std::string& hook_name)
>
> with hook_name the root of the tree (being "root" by default).
>
> Would this work for you or would you still like this functionality for the
> Chain class?

Why use a (non-real-time) std::string as the identifier? The chain-related solvers use a numeric ID. Is this due to the nature of a tree, vs chain?

Would it be difficult to change the implementation as Craig originally suggested, and like some of the other solvers, just stop at a given segment number?
S

Ruben Smits's picture

Jacobian Solver/Intermediate Frames

On Thursday 23 December 2010 21:35:45 S Roderick wrote:
> On 23/12/2010, at 21:29 , Ruben Smits wrote:
> > On Wednesday 22 December 2010 01:16:48 CARIGNAN, CRAIG R.
> > (GSFC-540.0)[Bastion
> >
> > Technologies, Inc.] wrote:
> >> Has anyone developed a version of ChainJntToJacSolver::JntToJac(const
> >> JntArray& q_in,Jacobian& jac) that will compute the Jacobian for an
> >> intermediate frame in a chain? This would be similar to finding the
> >> intermediate frame segmentNr in the forward kinematics solver
> >> ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame&
> >> p_out, int segmentNr). Otherwise, you need to create a new chain that
> >> terminates at the frame for which you want to find the Jacobian. It
> >> seems to me that an option could be added to JntToCart() to accommodate
> >> this and would eliminate a lot of unnecessary coding for developing
> >> self-motion kinematics.
> >
> > We already have a solver for this for the KDL::Tree class:
> >
> > * int KDL::TreeJntToJacSolver::JntToJac (const JntArray& q_in,
> > Jacobian& jac, const std::string& segmentname)
> >
> > Which will give you the jacobian from the root of the tree up to the
> > segment with the name segmentname. Since a chain is just a special case
> > of a tree and you can easily create a tree from a chain with:
> >
> > * bool addChain (const Chain &chain, const std::string& hook_name)
> >
> > with hook_name the root of the tree (being "root" by default).
> >
> > Would this work for you or would you still like this functionality for
> > the Chain class?
>
> Why use a (non-real-time) std::string as the identifier? The chain-related
> solvers use a numeric ID. Is this due to the nature of a tree, vs chain?

Numbers are not so easy to identify segments in a tree-structure. Names are
also a lot easier from the user pov.

A tree cannot be changed once it ends up in the solver, this means also the
names cannot be changed anymore. So no resizes/allocations happen after
initialisation of the tree/solver.

Internally we use an std::map to store the name-segment pairs. The name is
only used to do the initial lookup (O(log(nr_of_segments))) of the segment.
For the rest of the recursive algorithms pointers are used to traverse through
the tree.

> Would it be difficult to change the implementation as Craig originally
> suggested, and like some of the other solvers, just stop at a given
> segment number?

For most of our solvers this is dead easy ;), The tree algorithms already to
this for segment to root functions.

> S

Ruben