search

orocos / orocos_kinematics_dynamics

Orocos Kinematics and Dynamics C++ library
694 stars 407 forks source link

Fix a bug that treefksolverpos_recursive compute will crash when all joint of a tree is fixed. #388

Closed liufang-robot closed 1 year ago

liufang-robot commented 2 years ago

If all joints of a tree is fixed, the tree.getNrOfJoints() returns 0. in treefksolverpos_recursive.cpp

GetTreeElementSegment(currentElement).pose(q_in(GetTreeElementQNr(currentElement)));

For segment with fixed joint, GetTreeElementQNr(currentElement) always return 0. q_in(0) will cause the eigen vector crash.(remember now the q_in has size 0)

So, just add a robust check and don't useq_in in this case.

MatthijsBurgh commented 2 years ago

  1. It is valid to calculate the joint pose by just giving 0 as argument. As joint.pose() will just return identity for a fixed joint.

  2. When a tree is fully fixed, there is nothing to solve, right? So putting that tree in a FK solver is just useless, right? In that case I don't see why we should be compatible with a fully fixed tree.

liufang-robot commented 2 years ago

I found the problem when I try to load a urdf with full fixed joint, and I want to know the frame of a certain segment in base coordinate. Maybe sometimes, we still want to know the frame of certain segment even it's all fixed.

I try to use this fk computing and it crashes.

In my opinion, at least, this crash should not happend...

MatthijsBurgh commented 2 years ago

I always use the phrase: "If you put shit in, shit will come out". So when the input doesn't meet the requirements, don't expect any good output.

In my opinion the solver doesn't need to accept a full fixed tree.

Though the ChainFkSolverPos_recursive, does seem to handle fixed joints.

So lets see if @smits @meyerj have an opinion.

liufang-robot commented 2 years ago

OK, thanks for your reply.