How to set UR Inverse Kinematic Solver?

[bi3ri]

Hallo zusammen,

I want to use the UR Inverse Kinematic Solver, which is provided by tesseract_planning.

I am able to initialize a Solver:

tesseract_scene_graph::SceneGraph::ConstPtr scene_graph = monitor_->getSceneGraph();
fwd_kin.init(scene_graph, "base_link", "tool0", "mi");

auto inv_kin = std::make_shared<tesseract_kinematics::URInvKin>();

bool status = inv_kin->init("manipulator_tcp",
                              tesseract_kinematics::UR5Parameters,
                              fwd_kin.getBaseLinkName(),
                              fwd_kin.getTipLinkName(),
                              fwd_kin.getJointNames(),
                              fwd_kin.getLinkNames(),
                              fwd_kin.getActiveLinkNames(),
                              fwd_kin.getLimits());
//status = true

But how am I able to set it up properly?

bool status = env_->getManipulatorManager()->setDefaultInvKinematicSolver("manipulator_tcp", "URInvKin");
//bool = false

auto solvers= env_->getManipulatorManager()->getAvailableInvKinematicsSolvers();
//solvers = "KDLInvKinChainLMA"

Thanks for your help!
Johannes

[Levi-Armstrong]

Before you set it as the default solver you will need to add it to the manipulator manager. There should be an addInvKinematicsSolver method.

I have an open PR which I am writing more unit tests for before merging, but it will allow you to configure the kinematics using a yaml file.

[bi3ri]

Ah, how could I miss that! Thanks!

tesseract_scene_graph::SceneGraph::ConstPtr scene_graph = monitor_->getSceneGraph();
fwd_kin.init(scene_graph, "base_link", "tool0", "mi");

auto inv_kin = std::make_shared<tesseract_kinematics::URInvKin>();

bool status = inv_kin->init("manipulator_tcp",
                              tesseract_kinematics::UR5Parameters,
                              fwd_kin.getBaseLinkName(),
                              fwd_kin.getTipLinkName(),
                              fwd_kin.getJointNames(),
                              fwd_kin.getLinkNames(),
                              fwd_kin.getActiveLinkNames(),
                              fwd_kin.getLimits());

bool status = env_->getManipulatorManager()->addInvKinematicSolver(inv_kin);
//status = true

bool status1 = env_->getManipulatorManager()->setDefaultInvKinematicSolver("manipulator_tcp", "URInvKin");
// status1 = true

auto solvers= env_->getManipulatorManager()->getAvailableInvKinematicsSolvers();
// solvers = "KDLInvKinChainLMA"

But after adding it still shows me only "KDLInvKinChainLMA" as an available solver.

Checking on getAvailableInvKinematicsManipulators it gives back the names of inv_kin_factories_.

Do I also need to set up a factory and what does the factory do?

cheers,
Johannes

[Levi-Armstrong]

Try calling getAvailableInvKinematicsSolvers() instead of getAvailableInvKinematicsManipulators() to get the list of available solvers. The method you are currently calling returns the list the of manipulators which have an inverse kinematics solver defined.

[bi3ri]

Sorry, I copied the wrong line from my IDE. getAvailableInvKinematicsManipulators() returns "manipulator_tcp" and "manipulator_camera", which is right.

getAvailableInvKinematicsSolvers() returns "KDLInvKinChainLMA".

[Levi-Armstrong]

Does the adding of the kinematics return true? Is so, if you request the inverse solver by name does it return a null pointer?

[bi3ri]

Adding the kinematics returns true and requesting the same inverse solver is not returning a null pointers so it seems to work. Also, the path I am solving is succeeding now every time, which did not before with the default KDLInvKinChainLMA solver. Thank you :)

But still, getAvailableInvKinematicsSolvers only returns "KDLInvKinChainLMA" as the method is returning only the inverse kinematic solvers of "inv_kin_factories_", which is not what one would expect by the name of the method.

[Levi-Armstrong]

The kinematics have been updated so they are loaded as plugins to make it easier for users to use custom IK solvers. See the documentation here.