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]
dphi = math.radians(30)
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)
dphi = math.radians(90)
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.
Your Logic:
motions = [(1,0), (0,1), (-1,0), (0,-1)]
for v in motions:
dx = v[0]
dy = v[1]
dphi = math.radians(30)
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)
dphi = math.radians(90)
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.
In YOUR for loop:
- What is “300” in
range(300)? - Why are you sleeping the loop for
0.01seconds? - 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.
Sorry for the long post. Please help me understand.
Thanks in advance,
Girish