Unable to get current state from MoveGroupInterface

[Ane Fernandez]

Hello,

I'm trying to get the Jacobian of a UR10 on ROS Kinetic using moveit, this is the code I'm using to doingo so:

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
moveit::planning_interface::MoveGroupInterface* group = new moveit::planning_interface::MoveGroupInterface("manipulator");
robot_state::RobotStatePtr kinematic_state = group->getCurrentState();
const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("manipulator");
Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, 6);
kinematic_state->getJacobian(joint_model_group, kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()), reference_point_position, jacobian);

But when I execute the code the application throws the following error everytime I call the getCurrentState() method.

[ INFO] [1499773760.901631600]: Loading robot model 'ur10'...
[ INFO] [1499773760.901835795]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1499773761.006078588]: Loading robot model 'ur10'...
[ INFO] [1499773761.006125946]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1499773761.022290183]: Loading robot model 'ur10'...
[ INFO] [1499773761.022327400]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1499773761.095812404]: Loading robot model 'ur10'...
[ INFO] [1499773761.095845079]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1499773761.897895336, 738.822000000]:
Deprecation warning: Trajectory execution service is deprecated (was replaced by an action).
Replace 'MoveGroupExecuteService' with 'MoveGroupExecuteTrajectoryAction' in move_group.launch
[ INFO] [1499773761.900717546, 738.825000000]: Ready to take commands for planning group manipulator.
[ERROR] [1499773762.903635603, 739.826000000]: Failed to fetch current robot state

I'm using the UR packages from the kinetic-devel branch here: https://github.com/ros-industrial/universal_robot

Thanks in advance

[v4hn]

Hey Ane,

the only output relevant log output w.r.t your problem is

> [ERROR] [1499773762.903635603, 739.826000000]: Failed to fetch current robot state

The method depends on a ROS Subscriber on /joint_states to receive the current state.
If the current state is not received there within a second, the method fails.
The most likely reason for this to fail is that you don't have another spinner thread
to process messages while you wait for the current state.

I attached a small example node that works for me.


v4hn

> I'm using the UR packages from the kinetic-devel branch here:
> https://github.com/ros-industrial/universal_robot
>
> Thanks in advance
>

> --
> You received this message because you are subscribed to the Google Groups "MoveIt! Users" group.
> To unsubscribe from this group and stop receiving emails from it, send an email to moveit-users...@googlegroups.com.
> To view this discussion on the web visit https://groups.google.com/d/msgid/moveit-users/c6405803-02d4-46ff-89f3-ada2826b8d36%40googlegroups.com.


--
Michael Görner, M.Sc. Cognitive Science, PhD Student
Universität Hamburg
Faculty of Mathematics, Informatics and Natural Sciences
Department of Informatics
Group Technical Aspects of Multimodal Systems

Vogt-Kölln-Straße 30
D-22527 Hamburg

Room: F-315
Phone: +49 40 42883-2432
Website: https://tams.informatik.uni-hamburg.de/people/goerner/

[Brett Hemes]

Not directly related but of interest perhaps.... Dave Coleman recently informed me that RobotState's getJacobian is apparently really slow.   If you are making a bunch of calls to this suggested using KDL's equivalent, but I haven't tried fist hand yet.   Good luck!

Brett

--

You received this message because you are subscribed to the Google Groups "MoveIt! Users" group.

To unsubscribe from this group and stop receiving emails from it, send an email to moveit-users+unsubscribe@googlegroups.com.

[Ane Fernandez]

Thank you very much v4hn, I just need to add another spinner thread and it's working properly now.

Do you know if something has changed from Indigo to Kinetic in relation with the way the current state is received? Because in Indigo without anether spinner everything works well. Just to try to understand better!!

Thanks again!!

[Ane Fernandez]

Thank you very much Brett Hemes. We are doing our first test with Moveit Jacobian, and I think it's is enough for us, although it is really slow. In any case, we will take into account your advice for future works!

Thanks again

Ane

[v4hn]

On Tue, Jul 11, 2017 at 11:10:19PM -0700, Ane Fernandez wrote:
> Do you know if something has changed from Indigo to Kinetic in relation
> with the way the current state is received? Because in Indigo without
> anether spinner everything works well. Just to try to understand better!!

Robert Haschke worked on racing conditions last year,
where an outdated state was used for planning.

I believe in the process he discovered that `getCurrentState` did not actually
wait for the current state, but just for a complete state.
I never reviewed this in detail myself, so I'm a bit hazy on the details here.
The relevant patch w.r.t. the MoveGroupInterface should be this:
https://github.com/ros-planning/moveit/pull/350/files#diff-91269c2c54a4fe24e4a4df9d6e8db959


Michael / v4hn