def compute_velocity(self, actual_pose):
# Compute remaining distances, return if close to target
linear_dist = get_distance(self._target_pose, actual_pose)
angular_dist = get_shortest_angle(self._target_pose.theta, actual_pose.theta)
if ( abs(linear_dist) < self._l_tolerance and
abs(angular_dist) < self._a_tolerance ):
self._linear_complete = True
self._angular_complete = True
return Velocity()
# Find transform matrix of world frame in agent frame
# This is needed for perfect straight line travel. Linear velocity
# to target is a constant deceleration movement, which may not be
# apparent in values posted to /cmd_vel
transform_AW = get_inverse_transform2D(actual_pose.theta,
actual_pose.x,
actual_pose.y)
target_coord_A = np.dot(transform_AW, np.array([self._target_pose.x,
self._target_pose.y,
0, 1]))
# Compute linear and angular velocity
velocity_linear = self.compute_linear_velocity(target_coord_A[self.X_INDEX],
target_coord_A[self.Y_INDEX],
linear_dist)
velocity_angular = self.compute_angular_velocity(angular_dist)
#rospy.loginfo("Velocity angular: " + str(velocity_angular))
return Velocity(velocity_linear[self.X_INDEX],
velocity_linear[self.Y_INDEX],
velocity_angular)
def compute_velocity(self, actual_pose):
dx = self._target_pose.x - actual_pose.x
dy = self._target_pose.y - actual_pose.y
linear_dist = get_distance(self._target_pose, actual_pose)
angular_dist = get_shortest_angle(self._target_pose.theta,
actual_pose.theta)
change_taken_unit = (angular_dist) / linear_dist
if (abs(linear_dist) < self._l_tolerance
and abs(angular_dist) < self._a_tolerance):
self._linear_complete = True
self._angular_complete = True
return Velocity()
if (abs(linear_dist) > self._l_tolerance
and abs(angular_dist) > self._a_tolerance):
angular_dist = get_shortest_angle(atan2(dy, dx), actual_pose.theta)
vel_x = cos(angular_dist) * self._l_max_vel
vel_y = sin(angular_dist) * self._l_max_vel
vel_z = self._a_max_vel * change_taken_unit
return Velocity(vel_x, vel_y, vel_z)
else:
self._linear_complete = True
self._angular_complete = True
return Velocity()
def compute_velocity(self, actual_pose):
# Compute remaining distances, return if close to target
linear_dist = get_distance(self._target_pose, actual_pose)
angular_dist = get_shortest_angle(self._target_pose.theta,
actual_pose.theta)
if (abs(linear_dist) < self._l_tolerance
and abs(angular_dist) < self._a_tolerance):
self._linear_complete = True
self._angular_complete = True
return Velocity()
# Find transform matrix of world frame in agent frame
# This is needed for perfect straight line travel. Linear velocity
# to target is a constant deceleration movement, which may not be
# apparent in values posted to /cmd_vel
transform_AW = get_inverse_transform2D(actual_pose.theta,
actual_pose.x, actual_pose.y)
target_coord_A = np.dot(
transform_AW,
np.array([self._target_pose.x, self._target_pose.y, 0, 1]))
# Compute linear and angular velocity
velocity_linear = self.compute_linear_velocity(
target_coord_A[self.X_INDEX], target_coord_A[self.Y_INDEX],
linear_dist)
velocity_angular = self.compute_angular_velocity(angular_dist)
#rospy.loginfo("Velocity angular: " + str(velocity_angular))
return Velocity(velocity_linear[self.X_INDEX],
velocity_linear[self.Y_INDEX], velocity_angular)
def compute_velocity(self, actual_pose):
# calculate the distance between the current pose and the target pose
dx = self._target_pose.x - actual_pose.x
dy = self._target_pose.y - actual_pose.y
# calculating the linear and angular distance
linear_dist = get_distance(self._target_pose, actual_pose)
angular_dist = get_shortest_angle(self._target_pose.theta,
actual_pose.theta)
velocity_angle = get_shortest_angle(atan2(dy, dx), actual_pose.theta)
if (abs(linear_dist) < self._l_tolerance
and abs(angular_dist) < self._a_tolerance):
self._linear_complete = True
self._angular_complete = True
return Velocity()
# Computing remaining distances
vx = self._l_max_vel * math.cos(velocity_angle)
vy = self._l_max_vel * math.sin(velocity_angle)
angular_vel = angular_dist / (linear_dist / self._l_max_vel)
if angular_vel > self._a_max_vel:
angular_vel = self._a_max_vel
linear_vel = linear_dist / (angular_dist / angular_vel)
vx = linear_vel * math.cos(velocity_angle)
vy = linear_vel * math.sin(velocity_angle)
return Velocity(vx, vy, copysign(angular_vel, angular_dist))
def compute_velocity(self, actual_pose):
# compute remaining distances
linear_dist = get_distance(self._target_pose, actual_pose)
angular_dist = get_shortest_angle(self._target_pose.theta,
actual_pose.theta)
if (abs(linear_dist) < self._l_tolerance
and abs(angular_dist) < self._a_tolerance):
self._linear_complete = True
self._angular_complete = True
return Velocity()
# Calculate linear distance from goal where the robot should start to de-accelerate
l_time_for_de_acceleration = self._l_max_vel / self._l_max_acc
l_distance_for_de_acceleration = 0.5 * self._l_max_acc * (
l_time_for_de_acceleration**2)
# Time required based on max velocity and de-acceleration
linear_time = abs(linear_dist) / self._l_max_vel if \
linear_dist > l_distance_for_de_acceleration else l_time_for_de_acceleration
# Get position with respect to the bot. The angle is given with respect to the frame
theta = actual_pose.theta * -1
multiplication_matrix = np.array([[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta),
np.cos(theta), 0], [0, 0, 1]])
target_pos_bot = multiplication_matrix.dot(
np.array([self._target_pose.x, self._target_pose.y, 1]))
target_pos_bot_x, target_pos_bot_y = target_pos_bot[0], target_pos_bot[
1]
current_pos_bot = multiplication_matrix.dot(
np.array([actual_pose.x, actual_pose.y, 1]))
current_pos_bot_x, current_pos_bot_y = current_pos_bot[
0], current_pos_bot[1]
dx_bot = target_pos_bot_x - current_pos_bot_x
dy_bot = target_pos_bot_y - current_pos_bot_y
linear_vel_x = dx_bot / linear_time
linear_vel_y = dy_bot / linear_time
# Calculate angular distance from goal where the robot should start to de-accelerate
a_time_for_de_acceleration = self._a_max_vel / self._a_max_acc
a_distance_for_de_acceleration = 0.5 * self._a_max_acc * (
a_time_for_de_acceleration**2)
angular_time = abs(angular_dist) / self._a_max_vel if \
angular_dist > a_distance_for_de_acceleration else a_time_for_de_acceleration
angular_vel = angular_dist / angular_time
return Velocity(linear_vel_x, linear_vel_y, angular_vel)
def compute_velocity(self, actual_pose):
# Displacement and orientation to the target in world frame:
dx = self._target_pose.x - actual_pose.x
dy = self._target_pose.y - actual_pose.y
dtheta = get_shortest_angle(self._target_pose.theta, actual_pose.theta)
# Step 1: compute remaining distances
linear_dist = get_distance(self._target_pose, actual_pose)
angular_dist = get_shortest_angle(atan2(dy, dx), actual_pose.theta)
#Finish status condition
if (abs(linear_dist) < self._l_tolerance
and abs(dtheta) < self._a_tolerance):
self._linear_complete = True
self._angular_complete = True
rospy.loginfo("Goal reached")
return Velocity()
# Step 2: compute velocities
linear_vel_x = 0.0
linear_vel_y = 0.0
angular_vel = 0.0
#compute velocity while far from goal
if abs(linear_dist) > self._l_tolerance or abs(
dtheta) > self._a_tolerance:
#Desaccelerating before breaking distance
if abs(linear_dist) < self._l_breaking_distance or abs(
dtheta) < self._a_breaking_distance:
linear_vel = min(
sqrt(2 * linear_dist * self._l_max_acc),
sqrt((2 * self._a_max_acc * linear_dist**2) / abs(dtheta)))
else:
linear_vel = self._l_max_vel
linear_vel_x = linear_vel * cos(angular_dist)
linear_vel_y = linear_vel * sin(angular_dist)
#Angular speed relative to linear speed to allow simultaneous behavior
angular_vel = (dtheta * linear_vel) / linear_dist
if abs(angular_vel) > self._a_max_vel:
angular_vel = self._a_max_vel
return Velocity(linear_vel_x, linear_vel_y,
copysign(angular_vel, dtheta))
def compute_velocity(self, actual_pose):
# Displacement and orientation to the target in world frame:
dx = self._target_pose.x - actual_pose.x
dy = self._target_pose.y - actual_pose.y
# Step 1: compute remaining distances
linear_dist = get_distance(self._target_pose, actual_pose)
angular_dist_1 = get_shortest_angle(self._target_pose.theta,
actual_pose.theta)
if (abs(linear_dist) < self._l_tolerance
and abs(angular_dist_1) < self._a_tolerance):
self._linear_complete = True
self._angular_complete = True
return Velocity()
# Step 2: compute velocities
#Initializing the velocities
x_linear_vel, y_linear_vel, angular_vel = 0, 0, 0
lin_time = self._l_max_vel / self._l_max_acc
ang_time = self._a_max_vel / self._a_max_acc
if (abs(linear_dist) > self._l_tolerance
or ((self._target_pose.theta - actual_pose.theta) != 0)):
#Getting the orientation of the goal with respect to the current position of the bot
angular_dist = get_shortest_angle(math.atan2(dy, dx),
actual_pose.theta)
#Resolving the linear velocity vector in the x direction using cosine
x_linear_vel = (self._l_max_vel *
math.cos(angular_dist) if abs(linear_dist) > 5 *
self._l_tolerance else self._l_tolerance)
#Resolving the linear velocity vector in the y direction using sine
y_linear_vel = (self._l_max_vel *
math.sin(angular_dist) if abs(linear_dist) > 5 *
self._l_tolerance else self._l_tolerance)
#Angular velocity is divided into a part for every unit of linear distance so that the angular
#transition is synced with the linear motion
angular_vel = (angular_dist_1 * self._a_max_vel / linear_dist)
#Publishing the calculated linear and angular velocity using Velocity() class in velocity_controller.py
return Velocity(x_linear_vel, y_linear_vel, angular_vel)
else:
#If the bot has reached the target close enough to breach the tolerance, we simply stop it by giving 0 velocity
return Velocity(0, 0, 0)
def compute_velocity(self, actual_pose):
# Displacement and orientation to the target in world frame:
dx = self._target_pose.x - actual_pose.x
dy = self._target_pose.y - actual_pose.y
# Step 1: compute remaining distances
linear_dist = get_distance(self._target_pose, actual_pose)
angular_dist = get_shortest_angle(self._target_pose.theta,
actual_pose.theta)
if (abs(linear_dist) < self._l_tolerance
and abs(angular_dist) < self._a_tolerance):
self._linear_complete = True
self._angular_complete = True
#print "near goal"
return Velocity()
if abs(linear_dist > self._l_tolerance):
angular_dist = get_shortest_angle(atan2(dy, dx), actual_pose.theta)
# We still need to drive to the target, therefore we first need
# to make sure that we are oriented towards it.
# Step 2: compute velocities
#print "at goal" compute turn here
linear_vel, angular_vel = 0, 0
if abs(linear_dist) > self._l_tolerance:
linear_vel = (self._l_max_vel if abs(linear_dist) > 5 *
self._l_tolerance else self._l_tolerance)
if abs(angular_dist) > self._a_tolerance:
angular_vel = (self._a_max_vel if abs(angular_dist) > 5 *
self._a_tolerance else self._a_tolerance)
if abs(angular_dist) > self._a_tolerance * 5:
# We need to rotate a lot, so stand still and rotate with max velocity.
#print "running in straight line"
return Velocity(0, 0, copysign(angular_vel, angular_dist))
else:
# We need to rotate just a bit (or do not need at all),
# so it is fine to combine with linear motion if needed.
#print "in else"
return Velocity(copysign(linear_vel, linear_dist), 0,
copysign(angular_vel, angular_dist))
def compute_velocity(self, actual_pose):
# Displacement and orientation to the target in world frame:
dx = self._target_pose.x - actual_pose.x
dy = self._target_pose.y - actual_pose.y
# Step 1: compute remaining distances
linear_dist = get_distance(self._target_pose, actual_pose)
angular_dist = get_shortest_angle(self._target_pose.theta, actual_pose.theta)
if ( abs(linear_dist)<self._l_tolerance and
abs(angular_dist)<self._a_tolerance ):
self._linear_complete = True
self._angular_complete = True
return Velocity()
if abs(linear_dist>self._l_tolerance):
angular_dist = get_shortest_angle(atan2(dy, dx), actual_pose.theta)
# We still need to drive to the target, therefore we first need
# to make sure that we are oriented towards it.
# Step 2: compute velocities
linear_vel, angular_vel = 0, 0
if abs(linear_dist)>self._l_tolerance:
linear_vel = (self._l_max_vel if abs(linear_dist)>5*self._l_tolerance else
self._l_tolerance)
if abs(angular_dist)>self._a_tolerance:
angular_vel = (self._a_max_vel if abs(angular_dist)>5*self._a_tolerance else
self._a_tolerance)
if abs(angular_dist)>self._a_tolerance*5:
# We need to rotate a lot, so stand still and rotate with max velocity.
return Velocity(0, 0, copysign(angular_vel, angular_dist))
else:
# We need to rotate just a bit (or do not need at all),
# so it is fine to combine with linear motion if needed.
return Velocity(copysign(linear_vel, linear_dist),
0,
copysign(angular_vel, angular_dist))
def compute_velocity(self, actual_pose):
rospy.loginfo(actual_pose)
initial_l_vel = abs(self._l_max_vel)
initial_a_vel = abs(self._a_max_vel)
#Displacement and orientation to the target in world frame:
dx = self._target_pose.x - actual_pose.x
dy = self._target_pose.y - actual_pose.y
pose_theta = get_shortest_angle(atan2(dy, dx), actual_pose.theta)
# Step 1: compute remaining distances
linear_dist = get_distance(self._target_pose, actual_pose)
angular_dist = get_shortest_angle(self._target_pose.theta,
actual_pose.theta)
#Checking if target is reached
if (abs(linear_dist) < self._l_tolerance
and abs(angular_dist) < self._a_tolerance):
rospy.loginfo("Completing.....")
rospy.loginfo(actual_pose)
self._linear_complete = True
self._angular_complete = True
return Velocity()
#calculate distance to stop
l_dist_stop = abs(-(self._l_max_vel**2) / (2 * self._max_l_acc))
a_dist_stop = abs(-(self._a_max_vel**2) / (2 * self._max_a_acc))
#calculate distance for fixed velocity and deaccelerated velocity
l_dist_unacc = abs(linear_dist - l_dist_stop)
a_dist_unacc = abs(angular_dist - a_dist_stop)
# Step 2: compute velocities
linear_vel, linear_vel_x, linear_vel_y, angular_vel = 0, 0, 0, 0
scaled_l_vel, scaled_a_vel = 0, 0
if (linear_dist < l_dist_stop and angular_dist < a_dist_stop):
#Deaccelerate both (A)
rospy.loginfo("Undergoing Linear and angular deacceleration")
linear_vel = abs(sqrt(abs(-(2 * self._max_l_acc * linear_dist))))
angular_vel = abs(sqrt(abs(-(2 * self._max_a_acc * angular_dist))))
l_time = abs((-(linear_vel) / self._max_l_acc))
a_time = abs((-(angular_vel) / self._max_a_acc))
elif (linear_dist > l_dist_stop and angular_dist < a_dist_stop):
#Deaccelerate angular (B)
rospy.loginfo("Undergoing angular deacceleration")
linear_vel = abs(initial_l_vel)
angular_vel = abs(sqrt(abs(-(2 * self._max_a_acc * angular_dist))))
l_time_1 = (l_dist_unacc / linear_vel)
l_time_2 = abs((-(linear_vel) / self._max_l_acc))
l_time = l_time_1 + l_time_2
a_time = abs((-(angular_vel) / self._max_a_acc))
elif (linear_dist < l_dist_stop and angular_dist > a_dist_stop):
#Deaccelerate linear (C)
rospy.loginfo("Undergoing linear deacceleration")
linear_vel = abs(sqrt(abs(-(2 * self._max_l_acc * linear_dist))))
angular_vel = abs(initial_a_vel)
a_time_1 = (a_dist_unacc / angular_vel)
a_time_2 = abs((-(angular_vel) / self._max_a_acc))
a_time = a_time_1 + a_time_2
l_time = abs((-(linear_vel) / self._max_l_acc))
elif (linear_dist > l_dist_stop and angular_dist > a_dist_stop):
#No deacceleration (D)
rospy.loginfo("No deacceleration")
linear_vel = abs(initial_l_vel)
angular_vel = abs(initial_a_vel)
l_time_1 = (l_dist_unacc / linear_vel)
l_time_2 = abs((-(linear_vel) / self._max_l_acc))
l_time = l_time_1 + l_time_2
a_time_1 = (a_dist_unacc / angular_vel)
a_time_2 = abs((-(angular_vel) / self._max_a_acc))
a_time = a_time_1 + a_time_2
#scaling velocities so that both motion ends at same time
if (a_time < l_time):
scaled_a_vel = angular_vel * (a_time / l_time)
scaled_l_vel = linear_vel
else:
scaled_a_vel = angular_vel
scaled_l_vel = linear_vel * (l_time / a_time)
if abs(linear_dist) > self._l_tolerance:
rospy.loginfo("Calculating Linear vel")
linear_vel_x = abs(
(scaled_l_vel * cos(pose_theta) if
abs(linear_dist) > self._l_tolerance else self._l_tolerance *
cos(pose_theta)))
linear_vel_y = abs(
(scaled_l_vel * sin(pose_theta) if
abs(linear_dist) > self._l_tolerance else self._l_tolerance *
sin(pose_theta)))
linear_vel = abs(sqrt(linear_vel_x**2 + linear_vel_y**2))
rospy.loginfo(copysign(linear_vel, linear_dist))
rospy.loginfo(copysign(linear_vel_x, dx))
rospy.loginfo(copysign(linear_vel_y, dy))
if abs(angular_dist) > self._a_tolerance:
rospy.loginfo("Calculating angular vel")
angular_vel = abs(
(scaled_a_vel if abs(angular_dist) > self._a_tolerance else
self._a_tolerance))
rospy.loginfo(copysign(angular_vel, angular_dist))
return Velocity(copysign(linear_vel_x, dx), copysign(linear_vel_y, dy),
copysign(angular_vel, angular_dist))