MoveIt: how to actuate individual joints that do not form a chain?

Hi,

I'm working on a project at my university in which we are designing and simulating an industrial robot system. We have created a URDF file that contains the robot system, as well as the platform it sits on. There are also two movable caterpillar tracks on this platform, which we need to be able to control programmatically. We've created a MoveIt configuration for the robotic arm that the robot system consists of, which works properly.

To give you a bit of an image (literally) of how the system and the tracks look like:

Question:

My problem arises when I want to control the two movable tracks, which each consist of just a single joint. The relevant parts of the robot's URDF are included below; for brevity, I left out the rest. I want to be able to control the individual joints track2_joint and track3_joint from Python, but I have not yet been able to find out how...

My attempts

I've tried to make a planning group with just track2_joint in it without selecting a kinematics solver, hoping that I would be able to use group.set_joint_value_target in order to get easy control over the track. The MoveIt Setup Assistant allowed me to do so, so I tried it out in some code, here's the relevant snippet:

robot = moveit_commander.RobotCommander()
joint = robot.get_joint('track2_joint')
moveGroup = moveit_commander.MoveGroupCommander('Track2')
currentJointValues = moveGroup.get_current_joint_values()
print("Min bound: " + str(joint.min_bound()))
print("Value: " + str(currentJointValues))
print("Max bound: " + str(joint.max_bound()))

currentJointValues[0] += pi * 1/3
moveGroup.set_joint_value_target(currentJointValues)
moveGroup.plan()


This seemed to work fine. The print statements printed the correct values and plan caused the correct rotation to be shown in RViz. But when I replaced plan() with go(), I got the following message in the console in which I launched the test:

[ INFO] [1515523071.981027951]: ABORTED: Solution found but controller failed during execution

The following appeared in the console in which I launched the planning execution launch file:

[ INFO] [1515523071.563018404]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1515523071.563289980]: Planning attempt 1 of at most 1
Debug:   Waiting for space information to be set up before the sampling thread can begin computation...
[ INFO] [1515523071.564900737]: Planner configuration 'Track2' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
Debug:   RRTConnect: Planner range detected to be 1.256640
Info:    RRTConnect: Starting planning with 1 states already in datastructure
Debug:   RRTConnect: Waiting for goal region samples ...
Debug:   Stopped goal sampling thread after 10 sampling attempts
Debug:   RRTConnect: Waited 0.010084 seconds for the first goal sample.
Info:    RRTConnect: Created 5 states (2 start + 3 goal)
Info:    Solution found in 0.012115 seconds
Info:    SimpleSetup: Path simplification took 0.001880 seconds and changed from 4 to 17 states
[ERROR] [1515523071.662697635]: Joint trajectory action failing on invalid joints
[ WARN] [1515523071.663024659]: Controller  failed with error code INVALID_JOINTS
[ WARN] [1515523071.663147109]: Controller handle  reports status FAILED


I suspected it was most likely due to the fact that I had not specified a kinematic solver, so I thought I'd just select the default KDLKinematicsPlugin, but that gave me the following error while launching the planning execution file:

[ERROR] [1515523523.939177828]: Group 'Track2' is not a chain
[ERROR] [1515523523.939227171]: Kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for group 'Track2'
[ERROR] [1515523523.940096000]: Kinematics solver could not be instantiated for joint group Track2.


Defining the planning group 'Track2' as a chain from base_link to track2_link was not able to solve that either.

I have been scouring the internet to find another way in which I can actuate a single joint, but I cannot seem to find any good way. This question suggests setting up a "JointModelGroup" in the SRDF for each single joint, which seems to correspond with just creating a planning group in the setup assistant like I tried earlier. The answer to another question I found suggested the same thing.

I've also seen several suggestions towards the RobotState class, but that does not exist in Python. I believe its Python equivalent is RobotCommander though, which is where I can get the joint from as shown in my earlier snippet. The joint object that is returned apparently also has a move method, but that just tries to retrieve the group it is in, set a joint value target on that and then call its go() method, which is the very thing that failed earlier.

Relevant parts of the URDF:

<robot
name="pacbot">

<joint name="fixed_base" type="fixed">
</joint>

<inertial>
<origin
xyz="-1.0287 1.7496 2.683"
rpy="0 0 0" />
<mass
value="118270" />
<inertia
ixx="686480"
ixy="-23450"
ixz="-529.82"
iyy="646590"
iyz="-6038.5"
izz="1177900" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
</geometry>
<material
name="">
<color
rgba="0.75294 0.75294 0.75294 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
</geometry>
</collision>

<inertial>
<origin
xyz="0.467 0.059929 0.10079"
rpy="0 0 0" />
<mass
value="1993" />
<inertia
ixx="741.31"
ixy="6.7487E-05"
ixz="8.7846E-05"
iyy="335.06"
iyz="70.438"
izz="411.08" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
</geometry>
<material
name="">
<color
rgba="0.75294 0.75294 0.75294 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
</geometry>
</collision>
<joint
name="track2_joint"
type="revolute">
<origin
xyz="-0.020167 4.4565 3.216"
rpy="7.015E-17 -3.7018E-16 -2.3562" />
<parent
<child
<axis
xyz="0 1 0" />
<limit
lower="-3.1416"
upper="3.1416"
effort="1000"
velocity="3.1416" />
</joint>
<inertial>
<origin
xyz="0.467 0.059929 0.10079"
rpy="0 0 0" />
<mass
value="1993" />
<inertia
ixx="741.31"
ixy="6.749E-05"
ixz="8.7848E-05"
iyy="335.06"
iyz="70.438"
izz="411.08" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
</geometry>
<material
name="">
<color
rgba="0.75294 0.75294 0.75294 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
</geometry>
</collision>
<joint
name="track3_joint"
type="revolute">
<origin
xyz="0.64027 1.9109 3.216"
rpy="4.4247E-16 3.7928E-16 2.3562" />
<parent
<child
<axis
xyz="0 1 0" />
<limit
lower="-3.1416"
upper="3.1416"
effort="1000"
velocity="3.1416" />
</joint>
</robot>


My controllers.yaml file:

controller_list:
- name: ""
action_ns: joint_trajectory_action
type: FollowJointTrajectory
joints: [cart1_joint, main1_joint, elbow1_joint, forearm1_joint, wrist1_joint, gripper1_joint]


So, how do I get this done? Is it still possible with MoveIt planning groups, or is there another way in which this should be done? I also need this in order to be able to control the different parts of the gripper at the end of the arm.

Originally posted by BvOBart on ROS Answers with karma: 61 on 2018-01-09

Post score: 2

Comment by gvdhoorn on 2018-01-09:
Most likely your MoveIt configuration is incomplete. The error message seems to indicate that either MoveIt has no ctrlrs for the joints that it has a plan for, or the it couldn't execute the plan with the ctrlrs it does have.

You'll have to show us your controllers.yaml (or similar file).

Comment by gvdhoorn on 2018-01-09:
Btw: a mass of 118,270 Kg? That is one large/heavy construction.

Comment by gvdhoorn on 2018-01-09:

And: it's perfectly possible to set joint value targets for groups that don't have an IK solver, so that is not the issue here.

Comment by BvOBart on 2018-01-09:
@gvdhoorn Thank you for your comment and for the karma, I've attached the image directly to my question and I've also included the controllers.yaml file.

Comment by BvOBart on 2018-01-09:
As you can see, it currently contains only one controller. However, if I give it a name, then the robotic arm no longer works. Adding track2_joint and track3_joint to the existing controller leaves me with the same error as before.

Comment by BvOBart on 2018-01-09:
As for the mass, it is indeed one heck of a large construction. The platform that base_link represents is the top part of a tensioner used for laying pipes in the ocean. A horizontal version of the system is shown here

Comment by gvdhoorn on 2018-01-10:
At the very least I'd add controllers for each of the different groups, that are responsible for just the joints in that group.

Then make sure you have FollowJointTrajectory action servers for each of those controllers (or have a single server that accepts goals for all those joints).

Comment by gvdhoorn on 2018-01-10:
Right now you just don't have a controller that can execute the trajectory that MoveIt came up with.

Comment by BvOBart on 2018-01-10:
Alright, I'll add the controllers. However, when I give them a name, e.g. arm1, then MoveIt will look for /arm1/joint_trajectory_action, which it cannot find, as only /joint_trajectory_action exists. How do I set things up so that joint_trajectory_action lauches in the correct namespace?

Comment by BvOBart on 2018-01-10:
I had set up the rest of my MoveIt configuration based on the ROS-I tutorial. They did not use a name in the controller there, and I can't seem to find anything on the matter.

Comment by gvdhoorn on 2018-01-10:
That tutorial only targets relatively simple setups (ie: single arm, single controller). You essentially have (at least) three robots in there. For each robot individually it would make sense, but if you need/want a single setup with everything integrated that needs a different configuration.

Comment by gvdhoorn on 2018-01-10:
You'll probably have to start 3 instances of your joint_trajectory_action server in their respective namespaces.

I don't know which drivers you are using, but make sure your JTA server can still communicate with them.

With the help of @gvdhoorn I have been able to fix the problem. Here's how..

First of all, I noticed that the joint_trajectory_action was launched by the industrial_robot_simulator in the pacbot_moveit_planning_execution.launch, so in order to get those topics in the right namespace, I changed this in that file:

<!-- run the robot simulator and action interface nodes -->
<group if="$$(arg sim)" > (find industrial_robot_simulator)/launch/robot_interface_simulator.launch" />
</group>


to this:

<!-- run the robot simulator and action interface nodes -->
<group if="$$(arg sim)" ns="arm1" > (find industrial_robot_simulator)/launch/robot_interface_simulator.launch" />
</group>
<group if="$$(arg sim)" ns="track2" > (find industrial_robot_simulator)/launch/robot_interface_simulator.launch" />
</group>
<group if="$$(arg sim)" ns="track3" > (find industrial_robot_simulator)/launch/robot_interface_simulator.launch" />
</group>


and added the following to aggregate the joint_states topics of each controller into the general /joint_states topic.

<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="/use_gui" value="false" />
<rosparam param="/source_list">[/arm1/joint_states, /track2/joint_states, /track3/joint_states]</rosparam>
</node>


The rest of that file is still the same as the template given in the tutorial. As for the real robot's drivers, they do not exist yet, nor do we even have access to such a robotic arm. The aim of our project is mainly to provide a simulation as a proof of concept of the system.

As for controllers.yaml file, I've added the controllers of the two tracks and gave them names:

controller_list:
- name: arm1
action_ns: joint_trajectory_action
type: FollowJointTrajectory
joints: [cart1_joint, main1_joint, elbow1_joint, forearm1_joint, wrist1_joint, gripper1_joint]
- name: track2
action_ns: joint_trajectory_action
type: FollowJointTrajectory
joints: [track2_joint]
- name: track3
action_ns: joint_trajectory_action
type: FollowJointTrajectory
joints: [track3_joint]


So now the joint_trajectory_action of each is in the right namespace. But I noticed that the messages published to /arm1/joint_states, /track2/joint_states and /track3/joint_states contained the standard joint names (joint_1, joint_2 ..., joint_6) instead of the joints my system has. After some looking around, I found out how to change my joint_names.yaml accordingly:

arm1:
controller_joint_names: [cart1_joint, main1_joint, elbow1_joint, forearm1_joint, wrist1_joint, gripper1_joint]
track2:
controller_joint_names: [track2_joint]
track3:
controller_joint_names: [track3_joint]


The joint_states topics of these three controllers now contain the right joint names and the joint_state_publisher is now correctly able to aggregate all of these to the general /joint_states topic. All of the above combined solved my problem. Now I'll just need to do this for the gripper as well... :)

Originally posted by BvOBart with karma: 61 on 2018-01-10

This answer was ACCEPTED on the original site

Post score: 3

Comment by gvdhoorn on 2018-01-10:
Now that you have more JTA (and drivers, as the industrial_robot_simulator is essentially a driver), you'll need to make sure that consumers of /joint_state (ie: JointState msgs) still see the complete state of your system.

In this case that means that you'll need to add something that ..

Comment by gvdhoorn on 2018-01-10:
.. aggregates JointState msgs from the different drivers, coalesces them and then republishes on /joint_state.

See this example for that.

Comment by gvdhoorn on 2018-01-10:
Btw: if there is no need for the two tracks to ever do synchronous motion, I would actually recommend that you put both of them in their own groups. That will avoid any potential issues with MoveIt deciding that it needs to move the one when you only want to move the other.

Comment by BvOBart on 2018-01-10:
I was literally just reading about the source_list parameter for joint_state_publisher, but your example made its usage a lot more clear, thanks! I'll update the answer with the final solution once I have put the two tracks back into their own planning groups.

Comment by gvdhoorn on 2018-01-10:
Just to be clear: you obviously don't want /move_group/fake_controller_joint_states in there, but the JointState publishers of all your groups. It was really just an example.

Comment by BvOBart on 2018-01-10:
Yeah, I figured that, hahaha :) I've updated my answer and marked it as correct. Thanks again!

Comment by gvdhoorn on 2018-01-10:
Good to hear that you got it to work.

Also +1 on reporting back on how you finally implemented all of this.

Comment by gvdhoorn on 2018-01-10:
And finally: note that your issue was not at all caused by the joints not being "part of a chain".

Obvious now perhaps, but just noting this for future readers.