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: 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 ...
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).Btw: a
mass
of 118,270 Kg? That is one large/heavy construction.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 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.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
andtrack3_joint
to the existing controller leaves me with the same error as before.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 hereAt 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).Right now you just don't have a controller that can execute the trajectory that MoveIt came up with.