Making a Motion Planner (JPos)

Original Author: John H. Price


This tutorial will show by example how to write a simple motion planner in python to work with controlit. The example we will look at uses feed-forward acceleration control to make a smooth motion from angle A to angle B. The logic is as follows: calculate the change in position as velocity goes to 0 at set rate 'a'; if the desired change in position is greater, accelerate in the positive direction; otherwise, accelerate in the negative direction. In order to apply the acceleration to the controlit command effort, the JPosTaskCompWiseVel.cpp file will accept feed-forward acceleration by subscribing to controller_namespace/task_type/goalAcceleration (i.e. stickbot_lowerleg_3dof_controller/JPosTask/goalAcceleration).

Import Necessary Libraries

#!/usr/bin/env python

import rospy		#for interacting with ROS
import sys, getopt	#for getting parameters and sys.exit()
import math			#for abs()
from std_msgs.msg import Float64MultiArray, MultiArrayDimension 
import time			#for sleep()
import threading	#for locks
import re			#for parsing

Formatting the Data

# define the method for generating a one dimensional MultiArray
def makeMultiArray(iterable, label):
	arrayList = []
	for el in iterable:
	dim = MultiArrayDimension()
	dim.size = len(arrayList)
	dim.label = label
	dim.stride = len(arrayList)

	tempArray = Float64MultiArray() = arrayList
	tempArray.layout.data_offset = 0
	return tempArray

This will allow us to convert an iterable into a Float64MultiArray. This is the data type used by Controlit! topics for arrays of data.

Locks and Variable Initialization

#initialize ROS node
rospy.init_node('listener', anonymous=True)

gLock=threading.Lock()	#goal position lock
pLock=threading.Lock()	#current position lock
vLock=threading.Lock()	#current velocity

dataLen = 0			#number of DOFs

#data lists
latestGoal = None	#input goal position
latestPos = None	#current position
latestVel = None	#current velocity
aAccelMsg = None	#command (output) acceleration
kpMsg = None		#proportional gain values (output for PD control)
kdMsg = None		#derivative gain values (output for PD control)

Initialize the locks to protect shared data. gLock will be used to prevent the goal subscriber's callback from writing to the latestGoal variable while latestGoal is being read. pLock and vLock will do the same for latestPos and latestVel.
the latest- variables will each store a list of data provided by the corresponding subscriber.
Other global variables are also initialized here.


topics = rospy.get_published_topics(namespace = "/")
for topic in topics:
	robotname = re.findall("/(.+?)/JPosTask/+?",topic[0])
	if robotname !=[]:
		publishTopic = "/" + robotname[0] + "/JPosTask/" # set publisher/subscriber namespace

Using Regular Expressions, we sort through a list of active topics to find the model namespace that we will subscribe and publish under.


if (len(sys.argv) < 6): #check to ensure the proper number of parameters has been provided
	print("Error! More parameters needed.\nPlease provide: topic kp kd a rate")
#assign the parameter values to the appropriate variables
kp = float(sys.argv[2])
kd = float(sys.argv[3])
a = float(sys.argv[4])
rate = float(sys.argv[5])

See comments.

ROS Publishers

goalPosPublisher = rospy.Publisher(publishTopic + "goalPosition", Float64MultiArray)
addAccelPublisher = rospy.Publisher(publishTopic + "goalAcceleration", Float64MultiArray)
kpPublisher = rospy.Publisher(publishTopic + "kp", Float64MultiArray, latch=True)
kdPublisher = rospy.Publisher(publishTopic + "kd", Float64MultiArray, latch=True)

Here we initialize the ROS publishers using the namespace retrieved from the ROS topic list above.


Goal Position Callback

def Goalcallback(data):
	if (latestGoal == None): # first time this callback is called
		global latestGoal
		latestGoal = makeMultiArray(, "goalPos")
		global dataLen
		dataLen = len(
		global aAccelMsg
		aAccelMsg = makeMultiArray([0]*dataLen, "addAccel")
		global kpMsg
		kpMsg = makeMultiArray([0]*dataLen, "kp")
		global kdMsg
		kdMsg = makeMultiArray([0]*dataLen, "kd")
	else: =

Acquire gLock so that only this method can proceed past gLock to read or write to the protected data. Other threads must wait for gLock to be released at the end of this method. is the data returned by the ROS subscriber. latestGoal is updated to a Float64MulitArray containing that data.
All of the subscribers will return a Float64MultiArray of the same length because each element represents a DOF of the model. Our published data must have the same length, so we measure this length and store it as dataLen. We use dataLen to make uniform lists.
We will set the gains for PD control (kp and kd) to 0 initially so that the PD control will output no command effort until we give the gains non-zero values again (when the joint is close to its goal and we want the PD control to take over).
Every other time the callback is called, update the contents of latestGoal with the new subscriber data.

Current Position and Velocity Callbacks

def Actualcallback(data):
	if latestPos == None:
		global latestPos
		latestPos = makeMultiArray(,"actualPos")
	else: =

def Velocitycallback(data):
	if latestVel == None:
		global latestVel
		latestVel = makeMultiArray(,"actualVel")
	else: =

Acquire the relevant lock. The first run through, make a Float64MultiArray with the data returned by the ROS subscriber. Every other time, update the existing Float64MultiArray with the current subscriber data. Release the lock.

Setting the Goal

def setPath(r):
	goalRef = = [0]*dataLen = [0]*dataLen
	while goalRef ==
		aArray= []
		for ind in range(dataLen):
			dP = ([ind] -[ind])
			vel =[ind]
			if abs(dP) > .5 or abs(vel) > 1:
				aDeterm = (vel * abs((vel)/(2*a))) 
				if aDeterm < dP:
			else:[ind] = kp[ind] = kd
				aArray.append(0) = aArray


The locks for setPath are acquired in the Main Method.
goalRef is used to keep track of when the goal has been changed. When the goal does change, setPath will exit to be called again by the main method.
setPath contains the logic set forth in the Introduction section. For each joint, if the desired change in position is below a threshold, the gains will be set to their specified non-zero values and aArray will get a zero value for that joint so that the PD control will manage that joint until a new goal is issued.
If the desired change in position is above the threshold, we append a positive value 'a' to list aArray if the change in position as velocity goes to zero at rate a is less than the desired change in position; Append a negative value '-a'to aArray otherwise.

The Main Method

if __name__ == '__main__':
	rospy.Subscriber(sys.argv[1], Float64MultiArray, Goalcallback)
	rospy.Subscriber(publishTopic + "actualPosition", Float64MultiArray, Actualcallback)
	rospy.Subscriber(publishTopic + "actualVelocity", Float64MultiArray, Velocitycallback)
	while latestGoal == None or latestPos == None or latestVel == None:
		print("Waiting for publishers")
		if latestVel == None or latestPos == None:
			print("Please make sure a Controlit! model is running.")
		if latestGoal == None:
			print("Please make sure a goal is being published.")
	print("no longer waiting")
	r = rospy.Rate(rate) # 20hz
	while not rospy.is_shutdown():

Initialize the ROS subscribers and ensure that they are working properly by waiting for their callbacks. Until the command is given to shut down, work towards the goal as described in setPath.

Usage Notes

For best results, try kp=50, kd=15, a = 1, and rate = 20 (or values close to these).

For example, after launching a robot model, you might enter into the console:

$ rosrun controlit_trajectory_generators /plannerTopic 50 15 1 20

Then, follow the model's directions for publishing goals but replace that topic with /plannerTopic.

You can even use the GUI from the "Creating a Graphical User Interface for Controlit! (Jpos)" by passing your publisher topic to the GUI as a parameter.