Cartesian

The following example demonstrates how to compute a simple point to point motion plan using the moveit task constructor. You can take a look at the full source code here: Source

First, lets make sure we specify the planning group and the end effector that we want to use.

group = "panda_arm"

The moveit task constructor provides different planners. We will use the CartesianPath and JointInterpolation planners for this example.

# Cartesian and joint-space interpolation planners
cartesian = core.CartesianPath()
jointspace = core.JointInterpolationPlanner()

Lets start by initializing a task and adding the current planning scene state and robot state to it. This will be the starting state for our motion plan.

task = core.Task()
task.name = "cartesian"

# start from current robot state
task.add(stages.CurrentState("current state"))

To compute a relative motion in cartesian space, we can use the MoveRelative stage. Specify the planning group and frame relative to which you want to carry out the motion. the relative direction can be specified by a Vector3Stamped geometry message.

# move along x
move = stages.MoveRelative("x +0.2", cartesian)
move.group = group
header = Header(frame_id="world")
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2, 0, 0)))
task.add(move)

Similarly we can move along a different axis.

# move along y
move = stages.MoveRelative("y -0.3", cartesian)
move.group = group
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, -0.3, 0)))
task.add(move)

The MoveRelative stage also offers an interface to Twist messages, allowing to specify rotations.

# rotate about z
move = stages.MoveRelative("rz +45°", cartesian)
move.group = group
move.setDirection(TwistStamped(header=header, twist=Twist(angular=Vector3(0, 0, pi / 4.0))))
task.add(move)

Lastly, we can compute linear movements in cartesian space by providing offsets in joint space.

# Cartesian motion, defined as joint-space offset
move = stages.MoveRelative("joint offset", cartesian)
move.group = group
move.setDirection(dict(panda_joint1=pi / 6, panda_joint3=-pi / 6))
task.add(move)

If we want to specify goals instead of directions, we can use the MoveTo stage. In the following example we use simple joint interpolation to move the robot to a named pose. the named pose is defined in the urdf of the robot configuration.

# moveTo named posture, using joint-space interplation
move = stages.MoveTo("moveTo ready", jointspace)
move.group = group
move.setGoal("ready")
task.add(move)

Lastly, we invoke the planning mechanism that traverses the task hierarchy for us and compute a valid motion plan. At this point, when you run this script you are able to inspect the solutions of the individual stages in the rviz mtc panel.