Controller failed with error code INVALID_JOINTS

asked 2019-03-11 05:00:14 -0600

mvish7
104 ●19 ●23 ●28

updated 2019-03-11 16:46:45 -0600

jayess
6155 ●26 ●85 ●92

Hello, I’m trying to achieve pick and place in gazebo for panda arm by using moveit+rviz to plan motion. I’m getting below error on the terminal from which i launch moveit_planning _execution.launch file.

Controller panda/panda_hand_controller failed with error code INVALID_JOINTS

here is my config/controllers.yaml file from moveit side

controller_manager_ns: controller_manager
controller_list:
  - name: panda/panda_arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints: 
        - panda_joint1
        - panda_joint2
        - panda_joint3
        - panda_joint4
        - panda_joint5
        - panda_joint6
        - panda_joint7

  - name: panda/panda_hand_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
        - panda_finger_joint1
        - panda_finger_joint2

Below is my joint_names.yaml file

controller_joint_names: ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7', ’panda_finger_joint1’, ’panda_finger_joint2’]

About moveit planning groups from setup assistant tool, below are the groups i have created. I’m using panda_arm as planning group in my code

image description

Can someone tell me how to solve or debug this issue?

Update 1

On the terminal from which i spawn gazebo and controllers i get below error

Joints on incoming goal don't match the controller joints.

My suspect is that these errors are occurring because of following reason

I’m using "panda_arm " as planning group which doesn’t include finger_joints (in this case i’m doing as suggested in moveit_tutorial. ) Here can anyone suggest me how planning group should be in moveit setup_assistant?

Update 2

In gazebo, robot approaches the object and when it should open the finger joints( comparing to fake trajectory execution in rviz) robot stops the trajectory execution.

image description

edit retag flag offensive close merge delete

Comments

Hey - I think we have the same problem, Running moveit+rviz with the Pandas current ros-melodic-franka-description. When you run the standard demo.launch do you get a Clump of Panda joints in the center of your workspace? Screenshot of MoveIt

MoveIt! Quickstart in RViz

I've followed the setup assistant and tried making a couple other joint configurations - but nothing yet. : Setup_assistant_tutorial

Could it be from the latest melodic-franka-description?

lab.engineering ( 2019-03-11 12:40:44 -0600 )edit

@lab.engineering: it's much more likely you're running into #q272692.

gvdhoorn ( 2019-03-11 13:10:46 -0600 )edit

@lab.engineering: For me everything works fine in RVIZ. Even with demo.launch i'm able to see pick and place happening in RVIZ. i have problem in gazebo that panda arm reaches till the pose where it should open up the finger joints but finger_joints are not moving.

mvish7 ( 2019-03-11 13:54:19 -0600 )edit

@gvdhoorn: any suggestions on this issue by looking at update 2??

mvish7 ( 2019-03-11 14:22:57 -0600 )edit

add a comment

0

answered 2019-03-17 06:08:07 -0600

mvish7
104 ●19 ●23 ●28

I was able to solve this issue i would like to note what worked for me.

note: I feel this answer is very specific to panda arm.

At first why this error comes up?

If you look into lines 541 to 552 of joint_trajectory_controller_impl.h, they are as follows

00541   // Goal should specify all controller joints (they can be ordered differently). Reject if this is not the case
00542   using internal::permutation;
00543   std::vector<unsigned int> permutation_vector = permutation(joint_names_, gh.getGoal()->trajectory.joint_names);
00544 
00545   if (permutation_vector.empty())
00546   {
00547     ROS_ERROR_NAMED(name_, "Joints on incoming goal don't match the controller joints.");
00548     control_msgs::FollowJointTrajectoryResult result;
00549     result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
00550     gh.setRejected(result);
00551     return;
00552   }

So error comes up because there is a mismatch between controller joints and joints on the trajectory goal which is sent by moveit.

Second point how did i found it which joint should be removed?

Upon observing the hand.xacro file i noticed that panda_finger_joint2 is set to mimic panda_finger_joint1. (If someone wants to read about mimic joint then its here) so i suspected that it panda_finger_joint2 is to mimic panda_finger_joint1 then trajectory message sent by moveit should only have entry for panda_finger_joint1 .

Anyone correct me on the reasoning i have given.

edit flag offensive delete link

Comments

Can you explain specifically what you did to fix this? I am having the exact same issue. I do not understand what you mean when you said that you removed the joint. It seems like removing it in the URDF file would cause many more issues.

Edit: https://github.com/erdalpekel/panda_s... has more details on how to fix this. I used solution 2.

JGioia ( 2021-02-13 19:00:25 -0600 )edit

add a comment

0

answered 2019-04-05 03:37:17 -0600

liamtht
1 ●1 ●2 ●1

Hi, I would like to ask that if it is possible for me to have your github about this pandar arm on gazebo please? Thank you

edit flag offensive delete link

Comments

Hello, I can't upload this work on github as it concerns my university research obligations. I do temporarily host some part of work on git for asking questions and then take it down. I'm very sorry for this. I would encourage you to ask your question on community and i'll try my best to answer the question.

mvish7 ( 2019-04-05 03:44:12 -0600 )edit

Hi, thank you for your answer. I will try to do the panda arm and may ask you later. Br, Tran

liamtht ( 2019-04-05 03:48:26 -0600 )edit

add a comment

Controller failed with error code INVALID_JOINTS

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Controller failed with error code INVALID_JOINTS edit

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Controller failed with error code INVALID_JOINTS