Ask Your Question
2

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

asked 2018-01-09 13:27:41 -0500

BvOBart gravatar image

updated 2018-01-09 16:27:45 -0500

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: overview

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:   Starting goal sampling thread
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:   Beginning sampling thread computation
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 ...
(more)
edit retag flag offensive close merge delete

Comments

2

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).

gvdhoorn gravatar imagegvdhoorn ( 2018-01-09 15:21:38 -0500 )edit

Btw: a mass of 118,270 Kg? That is one large/heavy construction.

gvdhoorn gravatar imagegvdhoorn ( 2018-01-09 15:22:39 -0500 )edit
1

Also: please attach your image directly to your question. I've given you enough karma for that.

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.

gvdhoorn gravatar imagegvdhoorn ( 2018-01-09 15:24:32 -0500 )edit

@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.

BvOBart gravatar imageBvOBart ( 2018-01-09 16:31:26 -0500 )edit

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.

BvOBart gravatar imageBvOBart ( 2018-01-09 16:36:41 -0500 )edit

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

BvOBart gravatar imageBvOBart ( 2018-01-09 16:45:47 -0500 )edit
3

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).

gvdhoorn gravatar imagegvdhoorn ( 2018-01-10 02:28:54 -0500 )edit
1

Right now you just don't have a controller that can execute the trajectory that MoveIt came up with.

gvdhoorn gravatar imagegvdhoorn ( 2018-01-10 02:29:24 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
3

answered 2018-01-10 08:58:50 -0500

BvOBart gravatar image

updated 2018-01-10 10:30:19 -0500

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)" >
  <include file="$(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" >
  <include file="$(find industrial_robot_simulator)/launch/robot_interface_simulator.launch" />
</group>
<group if="$(arg sim)" ns="track2" >
  <include file="$(find industrial_robot_simulator)/launch/robot_interface_simulator.launch" />
</group>
<group if="$(arg sim)" ns="track3" >
  <include file="$(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... :)

edit flag offensive delete link more

Comments

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 ..

gvdhoorn gravatar imagegvdhoorn ( 2018-01-10 09:25:01 -0500 )edit
1

.. aggregates JointState msgs from the different drivers, coalesces them and then republishes on /joint_state.

See this example for that.

gvdhoorn gravatar imagegvdhoorn ( 2018-01-10 09:25:52 -0500 )edit
1

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.

gvdhoorn gravatar imagegvdhoorn ( 2018-01-10 09:27:12 -0500 )edit

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.

BvOBart gravatar imageBvOBart ( 2018-01-10 09:56:51 -0500 )edit

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.

gvdhoorn gravatar imagegvdhoorn ( 2018-01-10 09:58:45 -0500 )edit

Yeah, I figured that, hahaha :) I've updated my answer and marked it as correct. Thanks again!

BvOBart gravatar imageBvOBart ( 2018-01-10 10:46:15 -0500 )edit
1

Good to hear that you got it to work.

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

gvdhoorn gravatar imagegvdhoorn ( 2018-01-10 10:47:22 -0500 )edit
1

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.

gvdhoorn gravatar imagegvdhoorn ( 2018-01-10 10:54:31 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

4 followers

Stats

Asked: 2018-01-09 13:27:41 -0500

Seen: 557 times

Last updated: Jan 10 '18