# Need help with understanding Holonomic movement

Hi The Construct Team,

I just finished Kinematics of Holonomic Robots (Chapter 4) of Basic Kinematics of Mobile Robots Course.
I am unable to understand the concept of holonomic robot movement.

This is the code comparison of `square.py` exercise at the end of chapter 4.

YOUR CODE (copied from notebook solution):

``````#! /usr/bin/env python

import rospy, math, numpy as np
from std_msgs.msg import Float32MultiArray
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion

def odom_callback(msg):
global phi
position = msg.pose.pose.position
(_, _, phi) = euler_from_quaternion([msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w])

rospy.init_node('absolute_motion', anonymous=True)
pub = rospy.Publisher('wheel_speed', Float32MultiArray, queue_size=10)
position_sub = rospy.Subscriber("/odom", Odometry, odom_callback)
rospy.sleep(1)

def velocity2twist(dphi, dx, dy):
R = np.array([[1, 0, 0],
[0,  np.cos(phi), np.sin(phi)],
[0, -np.sin(phi), np.cos(phi)]])
v = np.array([dphi, dx, dy])
v.shape = (3,1)
twist = np.dot(R, v)
wz, vx, vy = twist.flatten().tolist()
return wz, vx, vy

def twist2wheels(wz, vx, vy):
l = 0.500/2
r = 0.254/2
w = 0.548/2
H = np.array([[-l-w, 1, -1],
[ l+w, 1,  1],
[ l+w, 1, -1],
[-l-w, 1,  1]]) / r
twist = np.array([wz, vx, vy])
twist.shape = (3,1)
u = np.dot(H, twist)
return u.flatten().tolist()

motions = [(1,0), (0,1), (-1,0), (0,-1)]
for v in motions:
dx = v[0]
dy = v[1]
for _ in range(300):
wz, vx, vy = velocity2twist(dphi, dx, dy)
u = twist2wheels(wz, vx, vy)
msg = Float32MultiArray(data=u)
pub.publish(msg)
rospy.sleep(0.01)
stop = [0,0,0,0]
msg = Float32MultiArray(data=stop)
pub.publish(msg)
``````

MY CODE (my solution):

``````#! /usr/bin/env python

import rospy
import math
import numpy as np
from std_msgs.msg import Float32MultiArray
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion

def velocity2twist(dphi, dx, dy):
mat = np.asarray([[1, 0, 0],
[0, np.cos(phi), np.sin(phi)],
[0, -np.sin(phi), np.cos(phi)]]).reshape(3, 3)
dq = np.asarray([dphi, dx, dy]).reshape(3, 1)
vb = np.dot(mat, dq)
wz, vx, vy = vb
return (wz, vx, vy)

l = (0.500 / 2)
w = (0.548 / 2)
r = (0.254 / 2)
m = np.asarray([[-l-w, 1, -1],
[+l+w, 1, +1],
[+l+w, 1, -1],
[-l-w, 1, +1]]).reshape(4, 3)

def twist2wheels(wz, vx, vy):
v = np.asarray([wz, vx, vy]).reshape(3, 1)
u = (1 / r) * np.dot(m, v)
return u

def odom_callback(msg):
global phi
position = msg.pose.pose.position
(_, _, phi) = euler_from_quaternion([msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w])
return None

rospy.init_node('holonomic_controller', anonymous=True)
pub = rospy.Publisher('/wheel_speed', Float32MultiArray, queue_size=10)
position_sub = rospy.Subscriber("/odom", Odometry, odom_callback)
rospy.sleep(1.0)

# do a square movement
side = 3
movements = [(1, 0), (0, 1), (-1, 0), (0, -1)]

for (mx, my) in movements:
dx = (side * mx)
dy = (side * my)
for _ in range(100):
wz, vx, vy = velocity2twist(dphi=dphi, dx=dx, dy=dy)
u = twist2wheels(wz, vx, vy)
msg = Float32MultiArray(data=u)
pub.publish(msg)
rospy.sleep(0.01)

stop = [0, 0, 0, 0]
msg = Float32MultiArray(data=stop)
pub.publish(msg)

# End of Code
``````

Both the codes work as expected and give the correct results/output.
But I cannot understand the for loop logic.

``````motions = [(1,0), (0,1), (-1,0), (0,-1)]
for v in motions:
dx = v[0]
dy = v[1]
for _ in range(300):
wz, vx, vy = velocity2twist(dphi, dx, dy)
u = twist2wheels(wz, vx, vy)
msg = Float32MultiArray(data=u)
pub.publish(msg)
rospy.sleep(0.01)
``````

My Logic (using previous exercise example and some hit-and-trial method):

``````# do a square movement
side = 3
movements = [(1, 0), (0, 1), (-1, 0), (0, -1)]

for (mx, my) in movements:
dx = (side * mx)
dy = (side * my)
for _ in range(100):
wz, vx, vy = velocity2twist(dphi=dphi, dx=dx, dy=dy)
u = twist2wheels(wz, vx, vy)
msg = Float32MultiArray(data=u)
pub.publish(msg)
rospy.sleep(0.01)
``````

My Problem:
I cannot understand the for loop. I need more explanation / clarity.

1. What is â€ś300â€ť in `range(300)`?
2. Why are you sleeping the loop for `0.01` seconds?
3. How can I control the speed of movement?

Your logic has `dphi=radians(30)` and `range(300)` for loop
AND
My logic has `dphi=radians(90)` and `range(100)` for loop.
I cannot understand this specific logic. Please explain.

Girish

Hello @girishkumar.kannan,
this is not a direct answer to your questions as I think it is better for learning to find the answer by one self.

I would recommend to try it out, learning by doing.
How does the output change when you use` for _ in range(300):` instead of `for _ in range(100)`? Do you see any difference in the movement of the robot? What if you use ` for _ in range(600):` ?

Same recommendation here. What if you comment out that sleep statement? What if we put a ` rospy.sleep(1.0)` statement? Try it out and find out how the behavior changes? Does the change make sense to you?

In the code above, identify the variables that define the velocities. Try modify them, add a fixed value to the X axis movement of the robot for instance.

Should you still have questions, or your findings donâ€™t make sense, reach out again at any time.

Cheers,

Roberto

@rzegers , Thanks for the reply. It helped me do my research. I am sharing my knowledge here so it would help future students to understand, if they have the same question as me.

@rzegers , I did not want to â€śplay aroundâ€ť with the values but I wanted to know the mathematical relationship.
I did not understand why `100` was chosen as iteration multiple. I had the doubt : what if it was `96` or `107` instead of `100`. And following is my understanding.

@rzegers , please do correct me if I am wrong.

Throughout the square movement / motion, angular velocity is assumed to be constant. Angular velocity units are radians per sec. Important factor to note here - it is PER SECOND.

What is `100` then?
`100` is the count of messages published in `/wheel_speed` PER SECOND. Therefore, `100` can be any value based on the rate of messages published in the publisher per second. It can be 94 or 95 or 96 or 103 or 104 or even 200 or any accurate value like 743.
Here is where the `rospy.sleep(...)` comes into action. Simply put, `0.01` is basically `1 / 100`. Which means, if you are publishing 100 messages per second, you need to wait 0.01 seconds between two consecutive message publications.

But, there is a caveat. You need to account for â€śdriftâ€ť in messages. If you set messages per second as `10`, then you would sleep for `0.1` seconds. For `100` it would be `0.01` and for `1000` it should be `0.001`. But if you try out `1000` you will notice that time taken will be longer and you will get a wrong result position and orientation. So setting messages per second as `750` for a sleep time of `0.001` will fix the issue, if you choose `1000`.

So here is the detailed code for better understanding:

``````#! /usr/bin/env python

import rospy
import math
import numpy as np
from std_msgs.msg import Float32MultiArray
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion

def velocity2twist(dphi, dx, dy):
mat = np.asarray([[1, 0, 0],
[0, np.cos(phi), np.sin(phi)],
[0, -np.sin(phi), np.cos(phi)]]).reshape(3, 3)
dq = np.asarray([dphi, dx, dy]).reshape(3, 1)
vb = np.dot(mat, dq)
wz, vx, vy = vb
return (wz, vx, vy)

l = (0.500 / 2)
w = (0.548 / 2)
r = (0.254 / 2)
m = np.asarray([[-l-w, 1, -1],
[+l+w, 1, +1],
[+l+w, 1, -1],
[-l-w, 1, +1]]).reshape(4, 3)

def twist2wheels(wz, vx, vy):
v = np.asarray([wz, vx, vy]).reshape(3, 1)
u = (1 / r) * np.dot(m, v)
return u

def odom_callback(msg):
global phi
position = msg.pose.pose.position
(_, _, phi) = euler_from_quaternion([msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w])
return None

rospy.init_node('holonomic_controller', anonymous=True)
pub = rospy.Publisher('/wheel_speed', Float32MultiArray, queue_size=10)
position_sub = rospy.Subscriber("/odom", Odometry, odom_callback)
rospy.sleep(1.0)

# do a square movement
side = 5  # distance, that is, side measure of square in meters

# simple fixed motion coordinates for a square path
movements = [(1, 0), (0, 1), (-1, 0), (0, -1)]

for (mx, my) in movements:
dphi = math.radians(10)  # degrees per sec
t90deg = (math.radians(90) / dphi)  # time to turn 90 degrees
dx = (side / t90deg) * mx  # meters per sec
dy = (side / t90deg) * my  # meters per sec
msgs_per_sec = 100
iterations = int(t90deg * msgs_per_sec)
sleep_time = (1.0 / msgs_per_sec)
print(msgs_per_sec, iterations, sleep_time)
ts = rospy.get_time()
for i in range(iterations):
wz, vx, vy = velocity2twist(dphi=dphi, dx=dx, dy=dy)
u = twist2wheels(wz, vx, vy)
msg = Float32MultiArray(data=u)
pub.publish(msg)
rospy.sleep(sleep_time)
te = rospy.get_time()
print(te - ts, ts, te)
stop = [0, 0, 0, 0]
msg = Float32MultiArray(data=stop)
pub.publish(msg)
rospy.sleep(1.0)

# End of Code
``````

Since your resultant angle after each side (of square) travelled should be 90 degrees / 1.5708 radians, AND angular velocity is constant in radians per second, time taken for your robot to turn 90 degrees would be:
`90 degrees / angular_velocity_in degs_per_second ==> radians(90) / radians(ang_vel_degs_per_sec)`
You should also note that distance travelled is the side measure of square. Therefore, when 90 degree completes, you should finish the distance.
Here, the velocity you will need is, simply, `distance / time_taken_to_turn_90_degrees`.
Then you multiply velocities x and y with movement measures through each coordinate [mentioned in `movements`].

Finally, with linear and angular velocities accounted, the iteration count is simply, rate of messages published multiplied by time taken to complete the movement [that is, to turn 90 degrees while covering the distance].
Given as `iterations = int(msgs_per_sec * time_taken_to_turn_90_degs)`

Regards,
Girish

Hello @girishkumar.kannan ,

you really did a deep examination on the exercise code, kudos for your hard work and commitment!

As you have found out by yourself there is a direct relation between the number of iterations:

And the amount of time that we pause the execution:

Using the above values the loop that publishes one cmd_vel for one movement will execute for about 3 seconds (it is just approximately 3 seconds because the execution time of the other lines aside of the sleep command will also take some minimal time to run).

If you change those values to `for _ in range(100):` and `rospy.sleep(0.03)` you will have the same total execution time of 3 seconds for this cmd_vel instruction, just that you will send less messages (lower publishing frequency).

Note that this is an open loop controller for moving the robot in a square.

If you change the total execution time for one cmd_vel value to say 6 seconds, for instance using the values `for _ in range(200):` and `rospy.sleep(0.03)` then the robot will not move in a square, it will just continue turning past the 90 degrees turn position.

I have added additional code comments to the provided code to make this example clearer for further students.

Congrats again for writing your own solution to this problem.

Cheers,

Roberto

1 Like