## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links
## note.0=
## descriptive title for the tutorial
## title = Moveit Tutorial
## multi-line description to be displayed in search
## description = In this tutorial we will explain about our moveit package, and how to work with it.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for
## level= IntermediateCategory
## keywords = ric_moveit,komodo moveit
####################################
!<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

== Getting started ==
First of all you need to install the moveit itself, installing moveit is easy just open a new terminal and type the following command

{{{
$ sudo apt-get install ros-indigo-moveit-full
}}}
== Moveit ==
NOTE: Right now there only two komodo model which support moveit functionality

 * Komodo with arm.
 * Komodo with elevator and arm.

== Moveit api ==
=== About the API ===
In MoveIt!, the primary user interface is through the RobotCommander class. It provides functionality for most operations that a user may want to carry out, specifically setting joint or pose goals, creating motion plans, moving the robot, adding objects into the environment and attaching/detaching objects from the robot.
=== Setup ===
To use the python interface to move_group, import the moveit_commander module. We also import rospy and some messages that we will use.
{{{
#!python 
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
}}}

First initialize moveit_commander and rospy.
{{{
#!python
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
}}}

Instantiate a RobotCommander object. This object is an interface to the robot as a whole.
{{{
#!python
robot = moveit_commander.RobotCommander()
}}}

Instantiate a PlanningSceneInterface object. This object is an interface to the world surrounding the robot.
{{{
#!python
scene = moveit_commander.PlanningSceneInterface()
}}}

Instantiate a MoveGroupCommander object. This object is an interface to one group of joints. In this case the group is the joints in the arm. This interface can be used to plan and execute motions on the arm.
{{{
#!python
group = moveit_commander.MoveGroupCommander("arm")
}}}

We create this DisplayTrajectory publisher which is used below to publish trajectories for RVIZ to visualize.
{{{
#!python
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',                                      moveit_msgs.msg.DisplayTrajectory)
}}}

Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
{{{
#!python
print "============ Waiting for RVIZ..."
rospy.sleep(10)
print "============ Starting tutorial "
}}}

=== Getting Basic Information ===
We can get the name of the reference frame for this robot
{{{
#!python
print "============ Reference frame: %s" % group.get_planning_frame()
}}}

We can also print the name of the end-effector link for this group
{{{
#!python
print "============ Reference frame: %s" % group.get_end_effector_link()
}}}

We can get a list of all the groups in the robot
{{{
#!python
print "============ Robot Groups:"
print robot.get_group_names()
}}}

Sometimes for debugging it is useful to print the entire state of the robot.
{{{
#!python
print "============ Printing robot state"
print robot.get_current_state()
print "============"
}}}

=== Planning to a Pose goal ===
We can plan a motion for this group to a desired pose for the end-effector
{{{
#!python
print "============ Generating plan 1"
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.7
pose_target.position.y = -0.05
pose_target.position.z = 1.1
group.set_pose_target(pose_target)
}}}

Now, we call the planner to compute the plan and visualize it if successful Note that we are just planning, not asking move_group to actually move the robot
{{{
#!python
plan1 = group.plan()

print "============ Waiting while RVIZ displays plan1..."
rospy.sleep(5)
}}}

You can ask RVIZ to visualize a plan (aka trajectory) for you. But the group.plan() method does this automatically so this is not that useful here (it just displays the same trajectory again).
{{{
#!python
print "============ Visualizing plan1"
display_trajectory = moveit_msgs.msg.DisplayTrajectory()

display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan1)
display_trajectory_publisher.publish(display_trajectory);

print "============ Waiting while plan1 is visualized (again)..."
rospy.sleep(5)
}}}

=== Moving to a pose goal ===
Moving to a pose goal is similar to the step above except we now use the go() function. Note that the pose goal we had set earlier is still active and so the robot will try to move to that goal. We will not use that function in this tutorial since it is a blocking function and requires a controller to be active and report success on execution of a trajectory.
{{{
#!python
# Uncomment below line when working with a real robot
# group.go(wait=True)
}}}

=== Planning to a joint-space goal ===
Let’s set a joint space goal and move towards it. First, we will clear the pose target we had just set.
{{{
#!python
group.clear_pose_targets()
}}}

Then, we will get the current set of joint values for the group
{{{
#!python
group_variable_values = group.get_current_joint_values()
print "============ Joint values: ", group_variable_values
}}}

Now, let’s modify one of the joints, plan to the new joint space goal and visualize the plan
{{{
#!python
group_variable_values[0] = 1.0
group.set_joint_value_target(group_variable_values)

plan2 = group.plan()

print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(5)
}}}

=== Cartesian Paths ===
You can plan a cartesian path directly by specifying a list of waypoints for the end-effector to go through.
{{{
#!python
waypoints = []

# start with the current pose
waypoints.append(group.get_current_pose().pose)

# first orient gripper and move forward (+x)
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x + 0.1
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z
waypoints.append(copy.deepcopy(wpose))

# second move down
wpose.position.z -= 0.10
waypoints.append(copy.deepcopy(wpose))

# third move to the side
wpose.position.y += 0.05
waypoints.append(copy.deepcopy(wpose))
}}}

We want the cartesian path to be interpolated at a resolution of 1 cm which is why we will specify 0.01 as the eef_step in cartesian translation. We will specify the jump threshold as 0.0, effectively disabling it.
{{{
#!python
(plan3, fraction) = group.compute_cartesian_path(
                             waypoints,   # waypoints to follow
                             0.01,        # eef_step
                             0.0)         # jump_threshold

print "============ Waiting while RVIZ displays plan3..."
rospy.sleep(5)
}}}

=== Adding/Removing Objects and Attaching/Detaching Objects ===
First, we will define the collision object message
{{{
#!python
collision_object = moveit_msgs.msg.CollisionObject()
}}}

When finished shut down moveit_commander.
{{{
#!python
moveit_commander.roscpp_shutdown()
}}}

## AUTOGENERATED DO NOT DELETE
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE