commandrobotmoveit
MoveIt! is a nice library for kinematics, control, navigation, motion planning and so.
Here we will show some of its very basics, just to achieve a simple control of our robot.
For more on MoveIt!, please refer to its documentation.
The basic idea that we will show here is that one can let MoveIt! compute a joint trajectory to achieve a specific Cartesian pose.
Then, the robot will be commanded in Joint space with a series of joint commands produced directly by the library.
A full example is available HERE.
Here we will just focus on the main aspects. Again, this is a very trivial example.
WARNING : If you execute that code and start ROSSmartServo the robot WILL MOVE. MoveIt! will execute the motion only if a valid plan is found, but still if you are not sure about where the robot will move DON'T MAKE IT MOVE.
First, we need to define a MoveGroup.
// Create MoveGroup
std::string movegroup_name = "manipulator"
move_group_interface::MoveGroup group(movegroup_name);
moveit::planning_interface::MoveGroup::Plan myplan;
// Configure planner
group.setPlanningTime(0.5);
group.setPlannerId(movegroup_name+"[RRTConnectkConfigDefault]");
group.setEndEffectorLink("tool_link_ee");We call an existing MoveGroup called "manipulator", if you remember we defined it with this name HERE.
We allow the planner 0.5 seconds to find a valid path, we specify that the endeffector link is "*tool_link_ee(" (by now you should know that you need to modify this accordingly to the right tool name), and
we choose the RRTConnectkConfigDefault planner, that we suggest to use.
The default planner (LBKPIECE) gives some horrible results when a tool is attached to the robot.
Next, we can plan and execute a trajectory like this :
command_cartesian_position = current_cartesian_position;
command_cartesian_position.pose.position.z -= direction * 0.10;
group.setStartStateToCurrentState();
group.setPoseTarget(command_cartesian_position);
bool success = group.plan(myplan);
if (success)
group.execute(myplan); We define a Cartesian pose, we set as our target pose, we search for a valid plan with group.plan(myplan) and, if found, we execute it.
The line group.setStartStateToCurrentState(); is quite important, it update the planner with the current position of the robot, so that the planning with have the right starting point.
If you forget to call it, the planner will think the robot to be in another position and compute its plan from that one. The result is that the robot will move extremely fast to what it thinks it should be its initial position and then perform the planned trajectory, as you can imagine THIS IS BAD AND YOU DON'T WANT IT.
REMINDER : always call group.setStartStateToCurrentState() before planning.
We have yet another ROS Node, as described before add it to the CMakeLists file.
By now, you should know it is better to have a launch file to call ROS Nodes.
HERE is a launch file that calls MoveIt! within the RViZ visualization tool and runs the ROS Node created above.
In brief :
<include file="$(find iiwa_tool_moveit)/launch/moveit_planning_execution.launch">
<arg name="sim" value="false"/>
</include>this will call the robot description, will create the MoveGroup named manipulator and load the visualization tool, all at once. Then we run our ROS Node as for the previous ones.
The launch file can be called with :
roslaunch iiwa_tool_examples iiwa_tool_command_moveit.launch
Within RViZ you have directly access to the MoveGroup functionalities that we just used in the code.
In the following image you can see the "Planning Library" drop-down menu that allows to select the planner to use, as we said before : always select RRTConnectkConfigDefault.
You might have unpleasant surprises with the default one. You can also search online on how to set the default one to be RRTConnectkConfigDefault.
Next, in the 3D view you can drag the endeffector frame and move/rotate it around the robot workspace.
In the "Planning" tab you can to the following :
- Select Start State : current -> Press Update;
- Press Plan;
- Check if the planned trajectory is good;
- Press Execute to execute the trajectory.
This is exactly the same we did in the code above.
REMEMBER : Select Start State : current -> Press Update is the same as group.setStartStateToCurrentState() in code, as it has the SAME IMPORTANCE