程序代写代做代考 Kinematics Turtlebot

Kinematics Turtlebot

Forward and inverse Kinematics for Turtlebot
We define two functions forward_kinemantics and inverse_kinematics respectively. They allow to
convert from wheel speeds to robot motion and from desired robot motion to wheel speeds.

v denotes the linear velocity of the robot
a denotes the angular velocity of the robot
w_l is the angular velocity of the left wheel
w_r is the angular velocity of the right wheel

unit are m for geometry, m/s for linear velocity and rad/s for angular velocity

In [17]:

In [18]:

v = 0.314159, a = 0.000000

import math
#from geometry_msgs.msg import Twist

# some estimates for the robot gemorty
wheel_radius = 0.05 # 5 cm radius of wheel
robot_radius = 0.25 # 25 cm radio of base

# computing the forward kinematics for a differential drive
def forward_kinematics(w_l, w_r):
c_l = wheel_radius * w_l
c_r = wheel_radius * w_r
v = (c_l + c_r) / 2
a = (c_l – c_r) / robot_radius
return (v, a)

# computing the inverse kinematics for a differential drive
def inverse_kinematics(v, a):
c_l = v + (robot_radius * a) / 2
c_r = v – (robot_radius * a) / 2
w_l = c_l / wheel_radius
w_r = c_r / wheel_radius
return (w_l, w_r)

# try out forward kinematics, both wheels turning at `2pi rad/s` (one full turn per second)

(v, a) = forward_kinematics(2*math.pi, 2*math.pi)
print “v = %f, a = %f” % (v, a)

In [19]:

In [20]:

In [ ]:

v = 0.000000, a = 2.513274

w_l = 20.000000, w_r = 20.000000
v = 1.000000, a = 0.000000

# try out forward kinematics, one wheel turning at `2pi rad/s` (one full turn per second), the other `-2pi rad/s`

(v, a) = forward_kinematics(2*math.pi, -2*math.pi)
print “v = %f, a = %f” % (v, a)

# inverse kinematics:

(w_l, w_r) = inverse_kinematics(1.0, 0.0)
print “w_l = %f, w_r = %f” % (w_l, w_r)

# this should give us again the desired values:
(v, a) = forward_kinematics(w_l, w_r)
print “v = %f, a = %f” % (v, a)