We are experiencing an issue with the intermediate frames returned by JntToCart() in the ChainFkSolverPos class that appears to be inherent to the way segments are defined. For a given segment i, the joint i rotation happens first and then the origin shifts to get to frame i+1. However, what is usually desired is the frame i pose after joint i rotates, which occurs between these two steps and is therefore not accessible. Therefore, the only way to get the frame i after joint i rotates using JntToCart is by calculating the pose at the intermediate frame and then applying the joint i rotation to that result. I have appended code at the end of this email to illustrate this procedure.
If the segments were defined in reverse order (translate and then joint rotate), there remains a problem. You will still not be able to get the frame pose if you're using modified DH parameters. This is because the joint rotation Rz(theta_i) happens in the middle of the overall transformation from frame T(i) to T(i+1):
{i-1}T_i = Rx(alpha_{i-1}) Dx(a_{i-1}) Rz (theta_i) Dz(d_i)
In other words, the translation can't all be done before the joint rotation. So the correct orientation for frame i will be returned but not the origin for frame i. (Please refer to Fig. 3.5 of J. Craig's "Introduction to Robotics".)
A potential solution might be to create a new segment definition (SegmentDH?) and formulate it to accommodate the above transformation architecture. Another avenue is to manually extract the required intermediate frame as illustrated below within the current function. I would welcome anyone's thoughts who may have encountered this same issue and how they resolved it.
Regards,
Craig
// NOTE: JntToCart(jointPos,cartPose,i) returns pose of frame i prior to joint(i-1) rotation
// Note: segment and joint numbers start at 0
KDL::Chain msia50 = robot::MotomanSIA50() ;
unsigned int nj = msia50.getNrOfJoints();
KDL::ChainFkSolverPos_recursive fksolver(msia50);
KDL::JntArray jointPos(nj);
KDL::Frame cartPose, toolPose, cartPose_rotated ;
KDL::Rotation Rz, Runrotated, Rframe ;
int segment_nr ;
for (int i=0; i<nj; ++i) {
jointPos(i) = 0;
}
std::cout << "Forward Kinematics test each frame:" << std::endl;
for(unsigned int i=1;i<=nj;i++){
rc = fksolver.JntToCart(jointPos,cartPose,i);
std::cout << "pose " << i << std::endl << " = " << std::endl << cartPose << std::endl;
Runrotated = cartPose.M ;
Rz = KDL::Rotation::RPY(0,0,jointPos(i-1)) ;
Rframe = Runrotated * Rz ;
std::cout << "R[" << i << "] = " << std::endl << Rframe << std::endl;
}
rc = fksolver.JntToCart(jointPos,cartPose);
std::cout << "TOOL POSE " << std::endl << cartPose << std::endl;
>
Issues with Intermediate Frames returned by ChainFkSolverPos::Jn
On Monday 28 March 2011 19:46:20 CARIGNAN, GSFC-442.0, Inc.] wrote:
> We are experiencing an issue with the intermediate frames returned by
> JntToCart() in the ChainFkSolverPos class that appears to be inherent to
> the way segments are defined. For a given segment i, the joint i rotation
> happens first and then the origin shifts to get to frame i+1. However,
> what is usually desired is the frame i pose after joint i rotates, which
> occurs between these two steps and is therefore not accessible.
Not directly no, but if you are interested in this you could easily split up
the segments in two parts, one segment containing the joint but not the link
and a second segment that does not have a joint but only contains the link.
> Therefore,
> the only way to get the frame i after joint i rotates using JntToCart is by
> calculating the pose at the intermediate frame and then applying the joint
> i rotation to that result. I have appended code at the end of this email
> to illustrate this procedure.
>
> If the segments were defined in reverse order (translate and then joint
> rotate), there remains a problem. You will still not be able to get the
> frame pose if you're using modified DH parameters. This is because the
> joint rotation Rz(theta_i) happens in the middle of the overall
> transformation from frame T(i) to T(i+1):
>
> {i-1}T_i = Rx(alpha_{i-1}) Dx(a_{i-1}) Rz (theta_i) Dz(d_i)
>
> In other words, the translation can't all be done before the joint rotation.
> So the correct orientation for frame i will be returned but not the origin
> for frame i. (Please refer to Fig. 3.5 of J. Craig's "Introduction to
> Robotics".)
>
> A potential solution might be to create a new segment definition
> (SegmentDH?) and formulate it to accommodate the above transformation
> architecture. Another avenue is to manually extract the required
> intermediate frame as illustrated below within the current function. I
> would welcome anyone's thoughts who may have encountered this same issue
> and how they resolved it.
I would go for the adapted tree with segments cut into two pieces. It's an
equally ugly solution ;), but does not need any extra calculations from your
side and no code changes on KDL's side. Or is this a no-go for you?
> Regards,
> Craig
-- Ruben
>
> // NOTE: JntToCart(jointPos,cartPose,i) returns pose of frame i prior to
> joint(i-1) rotation // Note: segment and joint numbers start at 0
>
> KDL::Chain msia50 = robot::MotomanSIA50() ;
> unsigned int nj = msia50.getNrOfJoints();
> KDL::ChainFkSolverPos_recursive fksolver(msia50);
> KDL::JntArray jointPos(nj);
> KDL::Frame cartPose, toolPose, cartPose_rotated ;
> KDL::Rotation Rz, Runrotated, Rframe ;
> int segment_nr ;
>
> for (int i=0; i<nj; ++i) {
> jointPos(i) = 0;
> }
>
> std::cout << "Forward Kinematics test each frame:" << std::endl;
>
> for(unsigned int i=1;i<=nj;i++){
> rc = fksolver.JntToCart(jointPos,cartPose,i);
> std::cout << "pose " << i << std::endl << " = " << std::endl << cartPose
> << std::endl; Runrotated = cartPose.M ;
> Rz = KDL::Rotation::RPY(0,0,jointPos(i-1)) ;
> Rframe = Runrotated * Rz ;
> std::cout << "R[" << i << "] = " << std::endl << Rframe << std::endl;
> }
> rc = fksolver.JntToCart(jointPos,cartPose);
> std::cout << "TOOL POSE " << std::endl << cartPose << std::endl;
Issues with Intermediate Frames returned by ChainFkSolverPos::Jn
On Mar 31, 2011, at 05:59 , Ruben Smits wrote:
> On Monday 28 March 2011 19:46:20 CARIGNAN, GSFC-442.0, Inc.] wrote:
>> We are experiencing an issue with the intermediate frames returned by
>> JntToCart() in the ChainFkSolverPos class that appears to be inherent to
>> the way segments are defined. For a given segment i, the joint i rotation
>> happens first and then the origin shifts to get to frame i+1. However,
>> what is usually desired is the frame i pose after joint i rotates, which
>> occurs between these two steps and is therefore not accessible.
>
> Not directly no, but if you are interested in this you could easily split up
> the segments in two parts, one segment containing the joint but not the link
> and a second segment that does not have a joint but only contains the link.
>
>> Therefore,
>> the only way to get the frame i after joint i rotates using JntToCart is by
>> calculating the pose at the intermediate frame and then applying the joint
>> i rotation to that result. I have appended code at the end of this email
>> to illustrate this procedure.
>>
>> If the segments were defined in reverse order (translate and then joint
>> rotate), there remains a problem. You will still not be able to get the
>> frame pose if you're using modified DH parameters. This is because the
>> joint rotation Rz(theta_i) happens in the middle of the overall
>> transformation from frame T(i) to T(i+1):
>>
>> {i-1}T_i = Rx(alpha_{i-1}) Dx(a_{i-1}) Rz (theta_i) Dz(d_i)
>>
>> In other words, the translation can't all be done before the joint rotation.
>> So the correct orientation for frame i will be returned but not the origin
>> for frame i. (Please refer to Fig. 3.5 of J. Craig's "Introduction to
>> Robotics".)
>>
>> A potential solution might be to create a new segment definition
>> (SegmentDH?) and formulate it to accommodate the above transformation
>> architecture. Another avenue is to manually extract the required
>> intermediate frame as illustrated below within the current function. I
>> would welcome anyone's thoughts who may have encountered this same issue
>> and how they resolved it.
>
> I would go for the adapted tree with segments cut into two pieces. It's an
> equally ugly solution ;), but does not need any extra calculations from your
> side and no code changes on KDL's side. Or is this a no-go for you?
That's how we are currently beating this problem. Costs you twice as many frame transform computations though ... :-(
Nice and ugly, but workable ...
S
Issues with Intermediate Frames returned by ChainFkSolverPos::Jn
On Mar 31, 2011, at 5:59 AM, Ruben Smits wrote:
> On Monday 28 March 2011 19:46:20 CARIGNAN, GSFC-442.0, Inc.] wrote:
>> We are experiencing an issue with the intermediate frames returned by
>> JntToCart() in the ChainFkSolverPos class that appears to be inherent to
>> the way segments are defined. For a given segment i, the joint i rotation
>> happens first and then the origin shifts to get to frame i+1. However,
>> what is usually desired is the frame i pose after joint i rotates, which
>> occurs between these two steps and is therefore not accessible.
>
> Not directly no, but if you are interested in this you could easily split up
> the segments in two parts, one segment containing the joint but not the link
> and a second segment that does not have a joint but only contains the link.
>
Thanks for the suggestions. Yes, that is possible to do, but means extra complication in keeping track of segments that are now double in number.
>> Therefore,
>> the only way to get the frame i after joint i rotates using JntToCart is by
>> calculating the pose at the intermediate frame and then applying the joint
>> i rotation to that result. I have appended code at the end of this email
>> to illustrate this procedure.
>>
>> If the segments were defined in reverse order (translate and then joint
>> rotate), there remains a problem. You will still not be able to get the
>> frame pose if you're using modified DH parameters. This is because the
>> joint rotation Rz(theta_i) happens in the middle of the overall
>> transformation from frame T(i) to T(i+1):
>>
>> {i-1}T_i = Rx(alpha_{i-1}) Dx(a_{i-1}) Rz (theta_i) Dz(d_i)
>>
>> In other words, the translation can't all be done before the joint rotation.
>> So the correct orientation for frame i will be returned but not the origin
>> for frame i. (Please refer to Fig. 3.5 of J. Craig's "Introduction to
>> Robotics".)
>>
>> A potential solution might be to create a new segment definition
>> (SegmentDH?) and formulate it to accommodate the above transformation
>> architecture. Another avenue is to manually extract the required
>> intermediate frame as illustrated below within the current function. I
>> would welcome anyone's thoughts who may have encountered this same issue
>> and how they resolved it.
>
> I would go for the adapted tree with segments cut into two pieces. It's an
> equally ugly solution ;), but does not need any extra calculations from your
> side and no code changes on KDL's side. Or is this a no-go for you?
>
That's a no-go for me. I would prefer a cleaner solution as per my reply to Herman's emails (either internal chain class conversion or external conversion function).
>> Regards,
>> Craig
>
> -- Ruben
>
>>
>> // NOTE: JntToCart(jointPos,cartPose,i) returns pose of frame i prior to
>> joint(i-1) rotation // Note: segment and joint numbers start at 0
>>
>> KDL::Chain msia50 = robot::MotomanSIA50() ;
>> unsigned int nj = msia50.getNrOfJoints();
>> KDL::ChainFkSolverPos_recursive fksolver(msia50);
>> KDL::JntArray jointPos(nj);
>> KDL::Frame cartPose, toolPose, cartPose_rotated ;
>> KDL::Rotation Rz, Runrotated, Rframe ;
>> int segment_nr ;
>>
>> for (int i=0; i<nj; ++i) {
>> jointPos(i) = 0;
>> }
>>
>> std::cout << "Forward Kinematics test each frame:" << std::endl;
>>
>> for(unsigned int i=1;i<=nj;i++){
>> rc = fksolver.JntToCart(jointPos,cartPose,i);
>> std::cout << "pose " << i << std::endl << " = " << std::endl << cartPose
>> << std::endl; Runrotated = cartPose.M ;
>> Rz = KDL::Rotation::RPY(0,0,jointPos(i-1)) ;
>> Rframe = Runrotated * Rz ;
>> std::cout << "R[" << i << "] = " << std::endl << Rframe << std::endl;
>> }
>> rc = fksolver.JntToCart(jointPos,cartPose);
>> std::cout << "TOOL POSE " << std::endl << cartPose << std::endl;
Issues with Intermediate Frames returned by ChainFkSolverPos::Jn
On Mon, 28 Mar 2011, CARIGNAN, CRAIG R. (GSFC-442.0)[Bastion Technologies, Inc.] wrote:
> We are experiencing an issue with the intermediate frames returned by
> JntToCart() in the ChainFkSolverPos class that appears to be inherent to the way
> segments are defined. For a given segment i, the joint i rotation happens first
> and then the origin shifts to get to frame i+1. However, what is usually
> desired is the frame i pose after joint i rotates, which occurs between these
> two steps and is therefore not accessible. Therefore, the only way to get the
> frame i after joint i rotates using JntToCart is by calculating the pose at the
> intermediate frame and then applying the joint i rotation to that result. I
> have appended code at the end of this email to illustrate this procedure.
>
> If the segments were defined in reverse order (translate and then joint rotate),
> there remains a problem. You will still not be able to get the frame pose if
> you're using modified DH parameters. This is because the joint rotation
> Rz(theta_i) happens in the middle of the overall transformation from frame T(i)
> to T(i+1):
>
> {i-1}T_i = Rx(alpha_{i-1}) Dx(a_{i-1}) Rz (theta_i) Dz(d_i)
>
> In other words, the translation can't all be done before the joint rotation. So
> the correct orientation for frame i will be returned but not the origin for
> frame i. (Please refer to Fig. 3.5 of J. Craig's "Introduction to Robotics".)
>
> A potential solution might be to create a new segment definition (SegmentDH?)
> and formulate it to accommodate the above transformation architecture. Another
> avenue is to manually extract the required intermediate frame as illustrated
> below within the current function. I would welcome anyone's thoughts who may
> have encountered this same issue and how they resolved it.
Thanks for your very interesting mail! I needed some time to think about it :-)
The "result" of this thinking is the following. First, some observations:
- the FK (and IK) solvers in KDL have as major goal to provide general and
efficient solutions.
- many (if not all) applications need "TCP frames" attached to segments of
a kinematic chain, and that do not happen to correspond to the segment
frames.
- the (implicit) KDL policy has always been to separate both
above-mentioned "concerns".
Hence, we (or rather, I :-) prefer to keep the original implementation and
motivate users to add their own "TCP transformations". Because otherwise we
run the risk of getting tons of "new segment definitions" as you suggest.
We see similar questions popping up on the list very regularly about the
IK, more in particular, people suggesting to add new functionality to cope
with joint limits of any kind, etc. Also there, I always suggest to keep
the separate concerns really separate.
So, that was "the bad news"; now for some (potentially) good news :-)
We have a student at Leuven who is currently working on a design of a
"scene graph", which is a tree (or possibly also a graph) with joints and
segments, and (possibly) lots of "TCP frames", where the goal is to have a
highly _user-configurable solver_: adding new TCP frames, removing existing
TCP frames, reconfiguring the computational schedule of the solver
computations (e.g., temporarily _not_ doing some computations), adding
"call back" computations to any joint or segment in the graph, etc. And all
these (re)configurations could be done at run time.
I think this design will come a long way in the direction of what you
(rightfully) desire to see in KDL...
I am open to discuss possible counter-arguments or further suggestions.
> Regards,
> Craig
Herman
>
> // NOTE: JntToCart(jointPos,cartPose,i) returns pose of frame i prior to
> joint(i-1) rotation
> // Note: segment and joint numbers start at 0
>
> KDL::Chain msia50 = robot::MotomanSIA50() ;
> unsigned int nj = msia50.getNrOfJoints();
> KDL::ChainFkSolverPos_recursive fksolver(msia50);
> KDL::JntArray jointPos(nj);
> KDL::Frame cartPose, toolPose, cartPose_rotated ;
> KDL::Rotation Rz, Runrotated, Rframe ;
> int segment_nr ;
>
> for (int i=0; i<nj; ++i) {
> jointPos(i) = 0;
> }
>
> std::cout << "Forward Kinematics test each frame:" << std::endl;
>
> for(unsigned int i=1;i<=nj;i++){
> rc = fksolver.JntToCart(jointPos,cartPose,i);
> std::cout << "pose " << i << std::endl << " = " << std::endl << cartPose <<
> std::endl;
> Runrotated = cartPose.M ;
> Rz = KDL::Rotation::RPY(0,0,jointPos(i-1)) ;
> Rframe = Runrotated * Rz ;
> std::cout << "R[" << i << "] = " << std::endl << Rframe << std::endl;
> }
> rc = fksolver.JntToCart(jointPos,cartPose);
> std::cout << "TOOL POSE " << std::endl << cartPose << std::endl;
>
>
>
--
K.U.Leuven, Mechanical Eng., Mechatronics & Robotics Research Group
<http://people.mech.kuleuven.be/~bruyninc> Tel: +32 16 328056
EURON Coordinator (European Robotics Research Network) <http://www.euron.org>
Open Realtime Control Services <http://www.orocos.org>
Associate Editor JOSER <http://www.joser.org>, IJRR <http://www.ijrr.org>
Issues with Intermediate Frames returned by ChainFkSolverPos::Jn
On Mar 31, 2011, at 3:40 AM, Herman Bruyninckx wrote:
> On Mon, 28 Mar 2011, CARIGNAN, CRAIG R. (GSFC-442.0)[Bastion Technologies, Inc.] wrote:
>
>> We are experiencing an issue with the intermediate frames returned by
>> JntToCart() in the ChainFkSolverPos class that appears to be inherent to the way
>> segments are defined. For a given segment i, the joint i rotation happens first
>> and then the origin shifts to get to frame i+1. However, what is usually
>> desired is the frame i pose after joint i rotates, which occurs between these
>> two steps and is therefore not accessible. Therefore, the only way to get the
>> frame i after joint i rotates using JntToCart is by calculating the pose at the
>> intermediate frame and then applying the joint i rotation to that result. I
>> have appended code at the end of this email to illustrate this procedure.
>>
>> If the segments were defined in reverse order (translate and then joint rotate),
>> there remains a problem. You will still not be able to get the frame pose if
>> you're using modified DH parameters. This is because the joint rotation
>> Rz(theta_i) happens in the middle of the overall transformation from frame T(i)
>> to T(i+1):
>>
>> {i-1}T_i = Rx(alpha_{i-1}) Dx(a_{i-1}) Rz (theta_i) Dz(d_i)
>>
>> In other words, the translation can't all be done before the joint rotation. So
>> the correct orientation for frame i will be returned but not the origin for
>> frame i. (Please refer to Fig. 3.5 of J. Craig's "Introduction to Robotics".)
>>
>> A potential solution might be to create a new segment definition (SegmentDH?)
>> and formulate it to accommodate the above transformation architecture. Another
>> avenue is to manually extract the required intermediate frame as illustrated
>> below within the current function. I would welcome anyone's thoughts who may
>> have encountered this same issue and how they resolved it.
>
> Thanks for your very interesting mail! I needed some time to think about it :-)
>
> The "result" of this thinking is the following. First, some observations:
> - the FK (and IK) solvers in KDL have as major goal to provide general and
> efficient solutions.
> - many (if not all) applications need "TCP frames" attached to segments of
> a kinematic chain, and that do not happen to correspond to the segment
> frames.
> - the (implicit) KDL policy has always been to separate both
> above-mentioned "concerns".
> Hence, we (or rather, I :-) prefer to keep the original implementation and
> motivate users to add their own "TCP transformations". Because otherwise we
> run the risk of getting tons of "new segment definitions" as you suggest.
>
> We see similar questions popping up on the list very regularly about the
> IK, more in particular, people suggesting to add new functionality to cope
> with joint limits of any kind, etc. Also there, I always suggest to keep
> the separate concerns really separate.
>
> So, that was "the bad news"; now for some (potentially) good news :-)
> We have a student at Leuven who is currently working on a design of a
> "scene graph", which is a tree (or possibly also a graph) with joints and
> segments, and (possibly) lots of "TCP frames", where the goal is to have a
> highly _user-configurable solver_: adding new TCP frames, removing existing
> TCP frames, reconfiguring the computational schedule of the solver
> computations (e.g., temporarily _not_ doing some computations), adding
> "call back" computations to any joint or segment in the graph, etc. And all
> these (re)configurations could be done at run time.
> I think this design will come a long way in the direction of what you
> (rightfully) desire to see in KDL...
>
> I am open to discuss possible counter-arguments or further suggestions.
>
Thanks very much for your thoughts regarding the intermediate frames issue. I understand (and agree with) the need to keep the segment frame construction separate from any particular link frame convention in order to avoid a plurality of segment frame definitions. Although the development of a "scene graph" with flexible tree construction is encouraging, I would prefer a solution within the chain class without resorting to the need to use tree class.
With that in mind, I would like to propose two alternatives:
1. Add conversion within the chain solvers to output link frames instead of segment frames. For example, JntToCart(jointPos,cartPose,i,DH) would return the intermediate (modified or traditional) DH link frame {i} instead of the segment frame, and default to the segment frame without the last argument. This solution keeps the segment and link frames separate but adds a functionality that I believe many users of KDL would appreciate.
2. A set of converter functions such as SegmentToDHFrame() could be added as inline chain functions. Although this solution would require an additional call, it would at least avoid everyone from having to develop the same functions repeatedly or explicitly in their code as I have illustrated below.
>> Regards,
>> Craig
>
> Herman
>
>>
>> // NOTE: JntToCart(jointPos,cartPose,i) returns pose of frame i prior to
>> joint(i-1) rotation
>> // Note: segment and joint numbers start at 0
>>
>> KDL::Chain msia50 = robot::MotomanSIA50() ;
>> unsigned int nj = msia50.getNrOfJoints();
>> KDL::ChainFkSolverPos_recursive fksolver(msia50);
>> KDL::JntArray jointPos(nj);
>> KDL::Frame cartPose, toolPose, cartPose_rotated ;
>> KDL::Rotation Rz, Runrotated, Rframe ;
>> int segment_nr ;
>>
>> for (int i=0; i<nj; ++i) {
>> jointPos(i) = 0;
>> }
>>
>> std::cout << "Forward Kinematics test each frame:" << std::endl;
>>
>> for(unsigned int i=1;i<=nj;i++){
>> rc = fksolver.JntToCart(jointPos,cartPose,i);
>> std::cout << "pose " << i << std::endl << " = " << std::endl << cartPose <<
>> std::endl;
>> Runrotated = cartPose.M ;
>> Rz = KDL::Rotation::RPY(0,0,jointPos(i-1)) ;
>> Rframe = Runrotated * Rz ;
>> std::cout << "R[" << i << "] = " << std::endl << Rframe << std::endl;
>> }
>> rc = fksolver.JntToCart(jointPos,cartPose);
>> std::cout << "TOOL POSE " << std::endl << cartPose << std::endl;
>>
>>
>>
>
> --
> K.U.Leuven, Mechanical Eng., Mechatronics & Robotics Research Group
> <http://people.mech.kuleuven.be/~bruyninc> Tel: +32 16 328056
> EURON Coordinator (European Robotics Research Network) <http://www.euron.org>
> Open Realtime Control Services <http://www.orocos.org>
> Associate Editor JOSER <http://www.joser.org>, IJRR <http://www.ijrr.org>
Issues with Intermediate Frames returned by ChainFkSolverPos::Jn
On Thu, 31 Mar 2011, CARIGNAN, CRAIG R. (GSFC-442.0)[Bastion Technologies, Inc.] wrote:
> On Mar 31, 2011, at 3:40 AM, Herman Bruyninckx wrote:
>
>> On Mon, 28 Mar 2011, CARIGNAN, CRAIG R. (GSFC-442.0)[Bastion Technologies, Inc.] wrote:
>>
>>> We are experiencing an issue with the intermediate frames returned by
>>> JntToCart() in the ChainFkSolverPos class that appears to be inherent to the way
>>> segments are defined. For a given segment i, the joint i rotation happens first
>>> and then the origin shifts to get to frame i+1. However, what is usually
>>> desired is the frame i pose after joint i rotates, which occurs between these
>>> two steps and is therefore not accessible. Therefore, the only way to get the
>>> frame i after joint i rotates using JntToCart is by calculating the pose at the
>>> intermediate frame and then applying the joint i rotation to that result. I
>>> have appended code at the end of this email to illustrate this procedure.
>>>
>>> If the segments were defined in reverse order (translate and then joint rotate),
>>> there remains a problem. You will still not be able to get the frame pose if
>>> you're using modified DH parameters. This is because the joint rotation
>>> Rz(theta_i) happens in the middle of the overall transformation from frame T(i)
>>> to T(i+1):
>>>
>>> {i-1}T_i = Rx(alpha_{i-1}) Dx(a_{i-1}) Rz (theta_i) Dz(d_i)
>>>
>>> In other words, the translation can't all be done before the joint rotation. So
>>> the correct orientation for frame i will be returned but not the origin for
>>> frame i. (Please refer to Fig. 3.5 of J. Craig's "Introduction to Robotics".)
>>>
>>> A potential solution might be to create a new segment definition (SegmentDH?)
>>> and formulate it to accommodate the above transformation architecture. Another
>>> avenue is to manually extract the required intermediate frame as illustrated
>>> below within the current function. I would welcome anyone's thoughts who may
>>> have encountered this same issue and how they resolved it.
>>
>> Thanks for your very interesting mail! I needed some time to think about it :-)
>>
>> The "result" of this thinking is the following. First, some observations:
>> - the FK (and IK) solvers in KDL have as major goal to provide general and
>> efficient solutions.
>> - many (if not all) applications need "TCP frames" attached to segments of
>> a kinematic chain, and that do not happen to correspond to the segment
>> frames.
>> - the (implicit) KDL policy has always been to separate both
>> above-mentioned "concerns".
>> Hence, we (or rather, I :-) prefer to keep the original implementation and
>> motivate users to add their own "TCP transformations". Because otherwise we
>> run the risk of getting tons of "new segment definitions" as you suggest.
>>
>> We see similar questions popping up on the list very regularly about the
>> IK, more in particular, people suggesting to add new functionality to cope
>> with joint limits of any kind, etc. Also there, I always suggest to keep
>> the separate concerns really separate.
>>
>> So, that was "the bad news"; now for some (potentially) good news :-)
>> We have a student at Leuven who is currently working on a design of a
>> "scene graph", which is a tree (or possibly also a graph) with joints and
>> segments, and (possibly) lots of "TCP frames", where the goal is to have a
>> highly _user-configurable solver_: adding new TCP frames, removing existing
>> TCP frames, reconfiguring the computational schedule of the solver
>> computations (e.g., temporarily _not_ doing some computations), adding
>> "call back" computations to any joint or segment in the graph, etc. And all
>> these (re)configurations could be done at run time.
>> I think this design will come a long way in the direction of what you
>> (rightfully) desire to see in KDL...
>>
>> I am open to discuss possible counter-arguments or further suggestions.
>>
>
> Thanks very much for your thoughts regarding the intermediate frames
> issue. I understand (and agree with) the need to keep the segment frame
> construction separate from any particular link frame convention in order
> to avoid a plurality of segment frame definitions. Although the
> development of a "scene graph" with flexible tree construction is
> encouraging, I would prefer a solution within the chain class without
> resorting to the need to use tree class.
And why would your personal preference be good for everyone? :-)
> With that in mind, I would like to propose two alternatives:
>
> 1. Add conversion within the chain solvers to output link frames instead
> of segment frames. For example, JntToCart(jointPos,cartPose,i,DH) would
> return the intermediate (modified or traditional) DH link frame {i}
> instead of the segment frame, and default to the segment frame without
> the last argument. This solution keeps the segment and link frames
> separate but adds a functionality that I believe many users of KDL would
> appreciate.
Yes. But there are so many "functionalities that users of KDL would
appreciate", and supporting them all would lead to an unmaintainable set of
slightly different solvers. I have not yet heard any technical rationale
why not to go with the suggested _decoupled_ solutions...
> 2. A set of converter functions such as SegmentToDHFrame() could be
> added as inline chain functions. Although this solution would require an
> additional call, it would at least avoid everyone from having to develop
> the same functions repeatedly or explicitly in their code as I have
> illustrated below.
You are using _your_ definition of a DH frame, but I hope you are aware of
the facts that (i) there are more than one such definitions, (ii) the
DH convention (of whatever type) has representation singularities, and
(iii) they provide only a subset of all possible frame-to-frame
transformations (which is obvious since they have only four parameters).
Again, this suggestion leads directly to a bunch of slightly different
implementations.
Another problem that I have is about the name giving: what
"SegmentToDHFrame" does is nothing else but a transformation between
frames. So, let's not introduce yet another method to do such a
transformation, but use the existing ones; the adaptation to your
particular use case should be done via the name of the _frames_ between
which the transformations are being done, and not of the method.
Again, your suggestion would lead directly to a bunch of slightly different
names with basically the same implementation.
As ever, I remain open for further discussion, but be sure that they are
technically motivated, and do not depend on your personal taste or
tradition. :-)
>>> Regards,
>>> Craig
Herman
Issues with Intermediate Frames returned by ChainFkSolverPos::Jn
On Mar 31, 2011, at 10:58 AM, Herman Bruyninckx wrote:
> On Thu, 31 Mar 2011, CARIGNAN, CRAIG R. (GSFC-442.0)[Bastion Technologies, Inc.] wrote:
>
>> On Mar 31, 2011, at 3:40 AM, Herman Bruyninckx wrote:
>>
>>> On Mon, 28 Mar 2011, CARIGNAN, CRAIG R. (GSFC-442.0)[Bastion Technologies, Inc.] wrote:
>>>
>>>> We are experiencing an issue with the intermediate frames returned by
>>>> JntToCart() in the ChainFkSolverPos class that appears to be inherent to the way
>>>> segments are defined. For a given segment i, the joint i rotation happens first
>>>> and then the origin shifts to get to frame i+1. However, what is usually
>>>> desired is the frame i pose after joint i rotates, which occurs between these
>>>> two steps and is therefore not accessible. Therefore, the only way to get the
>>>> frame i after joint i rotates using JntToCart is by calculating the pose at the
>>>> intermediate frame and then applying the joint i rotation to that result. I
>>>> have appended code at the end of this email to illustrate this procedure.
>>>>
>>>> If the segments were defined in reverse order (translate and then joint rotate),
>>>> there remains a problem. You will still not be able to get the frame pose if
>>>> you're using modified DH parameters. This is because the joint rotation
>>>> Rz(theta_i) happens in the middle of the overall transformation from frame T(i)
>>>> to T(i+1):
>>>>
>>>> {i-1}T_i = Rx(alpha_{i-1}) Dx(a_{i-1}) Rz (theta_i) Dz(d_i)
>>>>
>>>> In other words, the translation can't all be done before the joint rotation. So
>>>> the correct orientation for frame i will be returned but not the origin for
>>>> frame i. (Please refer to Fig. 3.5 of J. Craig's "Introduction to Robotics".)
>>>>
>>>> A potential solution might be to create a new segment definition (SegmentDH?)
>>>> and formulate it to accommodate the above transformation architecture. Another
>>>> avenue is to manually extract the required intermediate frame as illustrated
>>>> below within the current function. I would welcome anyone's thoughts who may
>>>> have encountered this same issue and how they resolved it.
>>>
>>> Thanks for your very interesting mail! I needed some time to think about it :-)
>>>
>>> The "result" of this thinking is the following. First, some observations:
>>> - the FK (and IK) solvers in KDL have as major goal to provide general and
>>> efficient solutions.
>>> - many (if not all) applications need "TCP frames" attached to segments of
>>> a kinematic chain, and that do not happen to correspond to the segment
>>> frames.
>>> - the (implicit) KDL policy has always been to separate both
>>> above-mentioned "concerns".
>>> Hence, we (or rather, I :-) prefer to keep the original implementation and
>>> motivate users to add their own "TCP transformations". Because otherwise we
>>> run the risk of getting tons of "new segment definitions" as you suggest.
>>>
>>> We see similar questions popping up on the list very regularly about the
>>> IK, more in particular, people suggesting to add new functionality to cope
>>> with joint limits of any kind, etc. Also there, I always suggest to keep
>>> the separate concerns really separate.
>>>
>>> So, that was "the bad news"; now for some (potentially) good news :-)
>>> We have a student at Leuven who is currently working on a design of a
>>> "scene graph", which is a tree (or possibly also a graph) with joints and
>>> segments, and (possibly) lots of "TCP frames", where the goal is to have a
>>> highly _user-configurable solver_: adding new TCP frames, removing existing
>>> TCP frames, reconfiguring the computational schedule of the solver
>>> computations (e.g., temporarily _not_ doing some computations), adding
>>> "call back" computations to any joint or segment in the graph, etc. And all
>>> these (re)configurations could be done at run time.
>>> I think this design will come a long way in the direction of what you
>>> (rightfully) desire to see in KDL...
>>>
>>> I am open to discuss possible counter-arguments or further suggestions.
>>>
>>
>> Thanks very much for your thoughts regarding the intermediate frames
>> issue. I understand (and agree with) the need to keep the segment frame
>> construction separate from any particular link frame convention in order
>> to avoid a plurality of segment frame definitions. Although the
>> development of a "scene graph" with flexible tree construction is
>> encouraging, I would prefer a solution within the chain class without
>> resorting to the need to use tree class.
>
> And why would your personal preference be good for everyone? :-)
>
Maybe my comment is misguided, but I thought that trees were constructions of multiple chains. If so, then what is the logic of using a tree when you only have a single chain? I cannot find any documentation online about tree class, so I really don't know. Regardless, it sounds like your student is engaged in an exciting new development, and I'm anxious to see the results.
>> With that in mind, I would like to propose two alternatives:
>>
>> 1. Add conversion within the chain solvers to output link frames instead
>> of segment frames. For example, JntToCart(jointPos,cartPose,i,DH) would
>> return the intermediate (modified or traditional) DH link frame {i}
>> instead of the segment frame, and default to the segment frame without
>> the last argument. This solution keeps the segment and link frames
>> separate but adds a functionality that I believe many users of KDL would
>> appreciate.
>
> Yes. But there are so many "functionalities that users of KDL would
> appreciate", and supporting them all would lead to an unmaintainable set of
> slightly different solvers. I have not yet heard any technical rationale
> why not to go with the suggested _decoupled_ solutions...
>
Nobody wants a solution that leads to an "unmaintainable" set of solvers, but are there so many frame conventions that this would happen? You currently support two link frames in frame class (DH and DH_Craig1989), so at some point, you already made a decision to support these conventions.
If by "suggested decoupled solutions", you mean using tree class, then I cannot evaluate it without seeing it. Ruben's proposed solution of splitting segments into joints and links will work, but it seems to defeat the purpose of segments and will require twice as many frame transformations.
>> 2. A set of converter functions such as SegmentToDHFrame() could be
>> added as inline chain functions. Although this solution would require an
>> additional call, it would at least avoid everyone from having to develop
>> the same functions repeatedly or explicitly in their code as I have
>> illustrated below.
>
> You are using _your_ definition of a DH frame, but I hope you are aware of
> the facts that (i) there are more than one such definitions, (ii) the
> DH convention (of whatever type) has representation singularities, and
> (iii) they provide only a subset of all possible frame-to-frame
> transformations (which is obvious since they have only four parameters).
> Again, this suggestion leads directly to a bunch of slightly different
> implementations.
>
I am aware of the different DH frame conventions and even referred to modified and traditional DH in my proposed solution 1 above. I do not know of any other widely used conventions, although there's always a possibility that another convention will come along that unseats DH. The issue of singularities does not seem relevant to the forward kinematics chains. A given set of DH parameters will produce only one set of link frames. However, the reverse is obviously not true.
> Another problem that I have is about the name giving: what
> "SegmentToDHFrame" does is nothing else but a transformation between
> frames. So, let's not introduce yet another method to do such a
> transformation, but use the existing ones; the adaptation to your
> particular use case should be done via the name of the _frames_ between
> which the transformations are being done, and not of the method.
> Again, your suggestion would lead directly to a bunch of slightly different
> names with basically the same implementation.
>
I agree that a SegmentToDHFrame transformation is probably not a useful asset. It would probably only help if the user does not realize that segment frames differ from the DH link frames by the z-axis rotation for the current joint.
> As ever, I remain open for further discussion, but be sure that they are
> technically motivated, and do not depend on your personal taste or
> tradition. :-)
>
My motivation in initiating this discussion was to achieve more flexibility in KDL for developing advanced kinematics algorithms. If I am having difficulty, then possibly others are running into the same issues. Is that not the purpose of open source development?
>>>> Regards,
>>>> Craig
>
> Herman
Craig
Issues with Intermediate Frames returned by ChainFkSolverPos::Jn
On Fri, 1 Apr 2011, CARIGNAN, CRAIG R. (GSFC-442.0)[Bastion Technologies, Inc.] wrote:
> On Mar 31, 2011, at 10:58 AM, Herman Bruyninckx wrote:
>
>> On Thu, 31 Mar 2011, CARIGNAN, CRAIG R. (GSFC-442.0)[Bastion Technologies, Inc.] wrote:
>>
>>> On Mar 31, 2011, at 3:40 AM, Herman Bruyninckx wrote:
>>>
>>>> On Mon, 28 Mar 2011, CARIGNAN, CRAIG R. (GSFC-442.0)[Bastion Technologies, Inc.] wrote:
>>>>
>>>>> We are experiencing an issue with the intermediate frames returned by
>>>>> JntToCart() in the ChainFkSolverPos class that appears to be inherent to the way
>>>>> segments are defined. For a given segment i, the joint i rotation happens first
>>>>> and then the origin shifts to get to frame i+1. However, what is usually
>>>>> desired is the frame i pose after joint i rotates, which occurs between these
>>>>> two steps and is therefore not accessible. Therefore, the only way to get the
>>>>> frame i after joint i rotates using JntToCart is by calculating the pose at the
>>>>> intermediate frame and then applying the joint i rotation to that result. I
>>>>> have appended code at the end of this email to illustrate this procedure.
>>>>>
>>>>> If the segments were defined in reverse order (translate and then joint rotate),
>>>>> there remains a problem. You will still not be able to get the frame pose if
>>>>> you're using modified DH parameters. This is because the joint rotation
>>>>> Rz(theta_i) happens in the middle of the overall transformation from frame T(i)
>>>>> to T(i+1):
>>>>>
>>>>> {i-1}T_i = Rx(alpha_{i-1}) Dx(a_{i-1}) Rz (theta_i) Dz(d_i)
>>>>>
>>>>> In other words, the translation can't all be done before the joint rotation. So
>>>>> the correct orientation for frame i will be returned but not the origin for
>>>>> frame i. (Please refer to Fig. 3.5 of J. Craig's "Introduction to Robotics".)
>>>>>
>>>>> A potential solution might be to create a new segment definition (SegmentDH?)
>>>>> and formulate it to accommodate the above transformation architecture. Another
>>>>> avenue is to manually extract the required intermediate frame as illustrated
>>>>> below within the current function. I would welcome anyone's thoughts who may
>>>>> have encountered this same issue and how they resolved it.
>>>>
>>>> Thanks for your very interesting mail! I needed some time to think about it :-)
>>>>
>>>> The "result" of this thinking is the following. First, some observations:
>>>> - the FK (and IK) solvers in KDL have as major goal to provide general and
>>>> efficient solutions.
>>>> - many (if not all) applications need "TCP frames" attached to segments of
>>>> a kinematic chain, and that do not happen to correspond to the segment
>>>> frames.
>>>> - the (implicit) KDL policy has always been to separate both
>>>> above-mentioned "concerns".
>>>> Hence, we (or rather, I :-) prefer to keep the original implementation and
>>>> motivate users to add their own "TCP transformations". Because otherwise we
>>>> run the risk of getting tons of "new segment definitions" as you suggest.
>>>>
>>>> We see similar questions popping up on the list very regularly about the
>>>> IK, more in particular, people suggesting to add new functionality to cope
>>>> with joint limits of any kind, etc. Also there, I always suggest to keep
>>>> the separate concerns really separate.
>>>>
>>>> So, that was "the bad news"; now for some (potentially) good news :-)
>>>> We have a student at Leuven who is currently working on a design of a
>>>> "scene graph", which is a tree (or possibly also a graph) with joints and
>>>> segments, and (possibly) lots of "TCP frames", where the goal is to have a
>>>> highly _user-configurable solver_: adding new TCP frames, removing existing
>>>> TCP frames, reconfiguring the computational schedule of the solver
>>>> computations (e.g., temporarily _not_ doing some computations), adding
>>>> "call back" computations to any joint or segment in the graph, etc. And all
>>>> these (re)configurations could be done at run time.
>>>> I think this design will come a long way in the direction of what you
>>>> (rightfully) desire to see in KDL...
>>>>
>>>> I am open to discuss possible counter-arguments or further suggestions.
>>>>
>>>
>>> Thanks very much for your thoughts regarding the intermediate frames
>>> issue. I understand (and agree with) the need to keep the segment frame
>>> construction separate from any particular link frame convention in order
>>> to avoid a plurality of segment frame definitions. Although the
>>> development of a "scene graph" with flexible tree construction is
>>> encouraging, I would prefer a solution within the chain class without
>>> resorting to the need to use tree class.
>>
>> And why would your personal preference be good for everyone? :-)
>>
>
> Maybe my comment is misguided, but I thought that trees were
> constructions of multiple chains. If so, then what is the logic of using
> a tree when you only have a single chain?
A single chain is a simple border case of a tree. So, it makes sense (I
think) to design solvers to be able to work with trees, instead of limiting
themselves to serial chains only.
In addition, even your case turns a serial chain into a tree: you want to
add a "TCP" somewhere halfway through the serial chain, which implicitly
cause you to work with a tree.
> I cannot find any
> documentation online about tree class, so I really don't know.
<http://people.mech.kuleuven.be/~orocos/pub/devel/kdl/latest/api/html/tree_8hpp-source.html>
The clue is that a tree allows a "SegmentMap" and not just one single
segment in each link.
> Regardless, it sounds like your student is engaged in an exciting new
> development, and I'm anxious to see the results.
Me too :-)
>>> With that in mind, I would like to propose two alternatives:
>>>
>>> 1. Add conversion within the chain solvers to output link frames instead
>>> of segment frames. For example, JntToCart(jointPos,cartPose,i,DH) would
>>> return the intermediate (modified or traditional) DH link frame {i}
>>> instead of the segment frame, and default to the segment frame without
>>> the last argument. This solution keeps the segment and link frames
>>> separate but adds a functionality that I believe many users of KDL would
>>> appreciate.
>>
>> Yes. But there are so many "functionalities that users of KDL would
>> appreciate", and supporting them all would lead to an unmaintainable set of
>> slightly different solvers. I have not yet heard any technical rationale
>> why not to go with the suggested _decoupled_ solutions...
>>
>
> Nobody wants a solution that leads to an "unmaintainable" set of solvers,
> but are there so many frame conventions that this would happen? You
> currently support two link frames in frame class (DH and DH_Craig1989),
> so at some point, you already made a decision to support these
> conventions.
We want to support _all_ _frame_ conventions, but your suggestion
("SegmentDH") mixes a _frame_ convention ("DH") with an _application goal"
("Segment"). And that's where the explosion of combinations will slip in :-)
The frame-convention-to-frame-convention utility functions should go in
their own source tree, and not pollute the IK source tree. That's (one of
the things) I understand under the term "decoupling".
So, wouldn't you be able to use the "DH_Craig1989" method to do exactly
what you need? That is, if I understand correctly, to transform from the
standard joint frame in the chain to the "TCP" you are interested in?
> If by "suggested decoupled solutions", you mean using tree class, then I
> cannot evaluate it without seeing it.
See the link above.
> Ruben's proposed solution of
> splitting segments into joints and links will work, but it seems to
> defeat the purpose of segments and will require twice as many frame
> transformations.
My suggested solution does not:
- it keeps the FK/IK efficient, by using only the minimal amount of frames
to solve.
- you pay a price of one extra transformation for each "local TCP" to which
you want to branch.
That's a "price" I want to pay, when nice decoupling in the source code
is what we buy for that :-)
>>> 2. A set of converter functions such as SegmentToDHFrame() could be
>>> added as inline chain functions. Although this solution would require an
>>> additional call, it would at least avoid everyone from having to develop
>>> the same functions repeatedly or explicitly in their code as I have
>>> illustrated below.
>>
>> You are using _your_ definition of a DH frame, but I hope you are aware of
>> the facts that (i) there are more than one such definitions, (ii) the
>> DH convention (of whatever type) has representation singularities, and
>> (iii) they provide only a subset of all possible frame-to-frame
>> transformations (which is obvious since they have only four parameters).
>> Again, this suggestion leads directly to a bunch of slightly different
>> implementations.
>>
>
> I am aware of the different DH frame conventions and even referred to
> modified and traditional DH in my proposed solution 1 above. I do not
> know of any other widely used conventions, although there's always a
> possibility that another convention will come along that unseats DH. The
> issue of singularities does not seem relevant to the forward kinematics
> chains.
It does, since it constrains the developer to only 4DOF to define his
application-specific TCP. (The singularities do not influence the
_computations_ in the FK, that's true.)
> A given set of DH parameters will produce only one set of link
> frames. However, the reverse is obviously not true.
That's why I do not like to introduce DH in the FK/IK source tree:
supporting DH in that source tree will require too many "if_def"s in solvers.
>> Another problem that I have is about the name giving: what
>> "SegmentToDHFrame" does is nothing else but a transformation between
>> frames. So, let's not introduce yet another method to do such a
>> transformation, but use the existing ones; the adaptation to your
>> particular use case should be done via the name of the _frames_ between
>> which the transformations are being done, and not of the method.
>> Again, your suggestion would lead directly to a bunch of slightly different
>> names with basically the same implementation.
>>
>
> I agree that a SegmentToDHFrame transformation is probably not a useful
> asset. It would probably only help if the user does not realize that
> segment frames differ from the DH link frames by the z-axis rotation for
> the current joint.
>
>> As ever, I remain open for further discussion, but be sure that they are
>> technically motivated, and do not depend on your personal taste or
>> tradition. :-)
>
> My motivation in initiating this discussion was to achieve more
> flexibility in KDL for developing advanced kinematics algorithms. If I
> am having difficulty, then possibly others are running into the same
> issues. Is that not the purpose of open source development?
Absolutely!
Just to make sure: I am not objecting against your rightful requirement for
optimal flexibility. I am only trying to keep our code as cleanly decoupled
as possible.
Herman