TF_NAN_INPUT error created by moveit_commander

asked 2017-10-30 09:33:12 -0600

cgnarendiran gravatar image

Hi everyone, I run ROS Indigo on Ubuntu 14.04 machine. I spawned the Pioneer P3dX robot as a mobile base and Cool1000 7DOF arm as the manipulator mounted on the mobile base. I use a Xbox kinect for perception. The goal is to use the arm for pick and place tasks using perception. I have used the moveit stack and Shadow robot's smart grasping sandbox (parts of it) for achieving the same. But when I try to plan and execute a named target (example "open_gripper" or "home") using the MoveGroupCommander interface (part of the code):

class SmartGrasper(object):
    __last_joint_state = None
    __current_model_name = "cube"
    __path_to_models = "/root/.gazebo/models/"         
    def __init__(self):
        self.arm_commander = MoveGroupCommander("arm")
        self.gripper_commander = MoveGroupCommander("gripper")
    def open_gripper(self):
        """
        Opens the gripper.         
        @return True on success
        """
        self.gripper_commander.set_named_target("open_gripper") 
        # 'open_gripper' pre-defined in the robot poses in setup_assistant
        rospy.loginfo("Set the named target")
        plan = self.gripper_commander.plan()
        rospy.loginfo("Got the plan from Gripper's MoveGroupCommander")
        if not self.gripper_commander.execute(plan, wait=True):
            return False
        return True
if __name__ == "__main__":
    # Create a node
    rospy.init_node("smart_grasper")
    # Make sure sim time is working
    while not rospy.Time.now():
        pass
    sgs = SmartGrasper()
    sgs.open_gripper()

I get the following repeated TF_INPUT_NAN errors:

[ERROR] [1509340788.274942445, 1691.595000000]: Ignoring transform for child_frame_id "gripper_link1" from authority "unknown_publisher" because of a nan value in the transform (-nan -nan -nan) (-0.000003 0.707426 -0.706787 0.000003)
[ERROR] [1509340788.274984739, 1691.595000000]: Ignoring transform for child_frame_id "gripper_link2" from authority "unknown_publisher" because of a nan value in the transform (-nan -nan -nan) (-0.000003 0.707426 -0.706787 0.000003)
Error:   TF_NAN_INPUT: Ignoring transform for child_frame_id "gripper_link1" from authority "unknown_publisher" because of a nan value in the transform (-nan -nan -nan) (-0.000003 0.707426 -0.706787 0.000003)
         at line 244 in /tmp/binarydeb/ros-indigo-tf2-0.5.16/src/buffer_core.cpp
Error:   TF_NAN_INPUT: Ignoring transform for child_frame_id "gripper_link2" from authority "unknown_publisher" because of a nan value in the transform (-`enter code here`nan -nan -nan) (-0.000003 0.707426 -0.706787 0.000003)
         at line 244 in /tmp/binarydeb/ros-indigo-tf2-0.5.16/src/buffer_core.cpp

I think that the goal pose created by the moveit commander has nan transforms which create these errors, but I'm not so sure. Strange thing is that the tf tree is complete when spawned as it is and creates these errors only when I try to execute a trajectory using moveit commander. When I try to directly send commands to the arm using JointTrajectoryGoal, it works like a charm without throwing any of these horrible NAN errors, why do you think that is? Can you guys please help me find, from where these TF_NAN_INPUT errors are creeping in? Am I missing something very simple here?

This is my project (workspace) for your kind reference: https://gitlab.com/cgnarendiran/rlbot...

edit retag flag offensive close merge delete