Constant TCP speed parameterization task seemingly broken for robots with external positioner

[peik-etzell]

I'm working with a robot with an external positioner, where the working frame is not the root link. The KDL parser sets the base link to the root node, which means the following line uses the wrong working frame:

tesseract_planning/tesseract_time_parameterization/kdl/src/constant_tcp_speed_parameterization.cpp

Line 174 in 75c32e1

if (!tesseract_kinematics::parseSceneGraph(data, *env.getSceneGraph(), jg->getBaseLinkName(), manip_info.tcp_frame))

Changing jg->getBaseLinkName() to manip_info.working_frame has worked for me. Is there a reason it's like it is right now?

[Levi-Armstrong]

I assume an oversight but @marip8 may know more.

[marip8]

The reason we use jg->getBaseLinkName() is because we are (implicitly) extracting the Jacobian relative to the joint group's base frame (see snippet below). As far as I know, tesseract currently doesn't provide a way to get the Jacobian relative to an arbitrary reference frame.

tesseract_planning/tesseract_time_parameterization/kdl/src/constant_tcp_speed_parameterization.cpp

Line 259 in 75c32e1

const Eigen::MatrixXd jac = jg->calcJacobian(joints, manip_info.tcp_frame, tcp_offset.translation());

Changing jg->getBaseLinkName() to manip_info.working_frame has worked for me.

I think that might have been somewhat coincidental in your case (maybe jg->getBaseLinkName() and manip_info.working_frame` are close to one another and have the same orientation?). This is not generally true across all applications, so I would guess this change wouldn't work correctly for other setups.

[Levi-Armstrong]

There is a few different ways to support this. Currently we have support for this trajopt for both conditions where the source frame is static and the target is dynamic and when both source and target are dynamic. There is also the geometric jacobian provided by the KinematicGroup or the JointGroup found here but it does not look like we have any unit tests that cover the section of code when the both links are active so use with caution.

• CartPoseJacCalculator
• DynamicCartPoseJacCalculator

[peik-etzell]

... are close to one another and have the same orientation?

Yes that's correct, they are mostly close enough.

There is also the geometric jacobian provided by the KinematicGroup or the JointGroup found here

Using this seems to work, but it's again hard to tell if the output is correct.

[Levi-Armstrong]

I am working on a unit test for the method in KinematicGroup/JointGroup to make sure it is being calculated correctly.