def new_goal(self):
goal = self.moveto_as.accept_new_goal()
self.desired_position = tools.position_from_posetwist(goal.posetwist)
self.desired_orientation = tools.orientation_from_posetwist(
goal.posetwist)
#self.linear_tolerance = goal.linear_tolerance
#self.angular_tolerance = goal.angular_tolerance
rospy.loginfo('Desired position:' + str(self.desired_position))
rospy.loginfo('Current position:' + str(self.current_position))
rospy.loginfo('Desired orientation:' + str(self.desired_orientation))
rospy.loginfo('Current orientation:' + str(self.current_orientation))
rospy.loginfo('linear_tolerance:' + str(self.linear_tolerance))
rospy.loginfo('angular_tolerance:' + str(self.angular_tolerance))
# This controller doesn't handle desired twist
#self.desired_twist = goal.posetwist.twist
if (xyz_array(goal.posetwist.twist.linear).any()
or xyz_array(goal.posetwist.twist.angular).any()):
rospy.logwarn(
'None zero are not handled by the tank steer trajectory generator. Setting twist to 0'
)
if np.linalg.norm(self.current_position -
self.desired_position) > self.orientation_radius:
self.line = line(self.current_position, self.desired_position)
self.redraw_line = True
else:
self.line = line(
self.current_position,
tools.normal_vector_from_rotvec(self.desired_orientation) +
self.current_position)
self.redraw_line = False
def new_goal(self):
goal = self.moveto_as.accept_new_goal()
self.desired_position = tools.position_from_posetwist(goal.posetwist)
self.desired_orientation = tools.orientation_from_posetwist(goal.posetwist)
#self.linear_tolerance = goal.linear_tolerance
#self.angular_tolerance = goal.angular_tolerance
rospy.loginfo('Desired position:' + str(self.desired_position))
rospy.loginfo('Current position:' + str(self.current_position))
rospy.loginfo('Desired orientation:' + str(self.desired_orientation))
rospy.loginfo('Current orientation:' + str(self.current_orientation))
rospy.loginfo('linear_tolerance:' + str(self.linear_tolerance))
rospy.loginfo('angular_tolerance:' + str(self.angular_tolerance))
# This controller doesn't handle desired twist
#self.desired_twist = goal.posetwist.twist
if (xyz_array(goal.posetwist.twist.linear).any() or
xyz_array(goal.posetwist.twist.angular).any() ):
rospy.logwarn('None zero are not handled by the tank steer trajectory generator. Setting twist to 0')
if np.linalg.norm(self.current_position - self.desired_position) > self.orientation_radius:
self.line = line(self.current_position, self.desired_position)
self.redraw_line = True
else:
self.line = line(self.current_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.current_position)
self.redraw_line = False
def update(self):
# Check if in the orientation radius for the first time
if self.redraw_line and self.distance_to_goal < self.orientation_radius:
self.redraw_line = False
rospy.loginfo('Redrawing trajectory line')
self.line = line(
self.desired_position,
tools.normal_vector_from_rotvec(self.desired_orientation) +
self.desired_position)
# Output a posetwist (carrot)
# Project current position onto trajectory line
Bproj = self.line.proj_pt(self.current_position)
parallel_distance = np.linalg.norm(self.desired_position - Bproj)
# Move carrot along line
tracking_step = self.get_tracking_distance()
c_pos = Bproj + self.overshoot * self.line.hat * tracking_step
# If bproj is in threashold just set the carrot to the final position
if parallel_distance < self.orientation_radius:
c_pos = self.desired_position
# Fill up PoseTwist
carrot = PoseTwist(
pose=Pose(position=tools.vector3_from_xyz_array(c_pos),
orientation=tools.quaternion_from_rotvec(
[0, 0, self.line.angle])),
twist=Twist(
linear=Vector3(
tracking_step * self.tracking_to_speed_conv *
self.overshoot, 0,
0), # Wrench Generator handles the sine of the velocity
angular=Vector3()))
return carrot
def update(self, event):
# Publish trajectory
traj = self.get_carrot()
#rospy.loginfo('Trajectory: ' + str(traj))
if not self.killed:
self.traj_pub.publish(traj)
if self.redraw_line and self.distance_to_goal < self.orientation_radius:
self.redraw_line = False
rospy.loginfo('Redrawing trajectory line')
self.line = line(
self.current_position,
tools.normal_vector_from_rotvec(self.desired_orientation) +
self.current_position)
rospy.loginfo('Angle to goal: ' +
str(self.angle_to_goal_orientation[2] * 180 / np.pi) +
'\t\t\tDistance to goal: ' + str(self.distance_to_goal))
# Check if goal is reached
if self.moveto_as.is_active():
if self.distance_to_goal < self.linear_tolerance:
if self.angle_to_goal_orientation[2] < self.angular_tolerance:
rospy.loginfo('succeded')
self.moveto_as.set_succeeded(None)
def update(self):
# Check if in the orientation radius for the first time
if self.redraw_line and self.distance_to_goal < self.orientation_radius:
self.redraw_line = False
rospy.loginfo('Redrawing trajectory line')
self.line = line(self.desired_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.desired_position)
# Output a posetwist (carrot)
# Project current position onto trajectory line
Bproj = self.line.proj_pt(self.current_position)
parallel_distance = np.linalg.norm(self.desired_position - Bproj)
# Move carrot along line
tracking_step = self.get_tracking_distance()
c_pos = Bproj + self.overshoot * self.line.hat * tracking_step
# If bproj is in threashold just set the carrot to the final position
if parallel_distance < self.orientation_radius:
c_pos = self.desired_position
# Fill up PoseTwist
carrot = PoseTwist(
pose = Pose(
position = tools.vector3_from_xyz_array(c_pos),
orientation = tools.quaternion_from_rotvec([0, 0, self.line.angle])),
twist = Twist(
linear = Vector3(tracking_step * self.tracking_to_speed_conv * self.overshoot, 0, 0), # Wrench Generator handles the sine of the velocity
angular = Vector3())
)
return carrot
def __init__(self, name):
# Desired pose
self.desired_position = self.current_position = np.zeros(3)
self.desired_orientation = self.current_orientation = np.zeros(3)
#self.desired_twist = self.current_twist = Twist()
# Goal tolerances before seting succeded
self.linear_tolerance = rospy.get_param('linear_tolerance', 0.5)
self.angular_tolerance = rospy.get_param('angular_tolerance', np.pi / 10)
self.orientation_radius = rospy.get_param('orientation_radius', 0.75)
self.slow_down_radius = rospy.get_param('slow_down_radius', 5)
# Speed parameters
self.max_tracking_distance = rospy.get_param('max_tracking_distance', 5)
self.min_tracking_distance = rospy.get_param('min_tracking_distance', 1)
self.tracking_to_speed_conv = rospy.get_param('tracking_to_speed_conv', 1)
self.tracking_slope = (self.max_tracking_distance - self.min_tracking_distance) / (self.slow_down_radius - self.orientation_radius)
self.tracking_intercept = self.tracking_slope * self.orientation_radius + self.min_tracking_distance
# Publishers
self.traj_pub = rospy.Publisher('/trajectory', PoseTwistStamped, queue_size = 10)
# Set desired twist to 0
#self.desired_twist.linear.x = self.desired_twist.linear.y = self.desired_twist.linear.z = 0
#self.desired_twist.angular.x = self.desired_twist.angular.y = self.desired_twist.angular.z = 0
# Initilize a line
self.line = line(np.zeros(3), np.ones(3))
# Wait for current position and set as desired position
rospy.loginfo('Waiting for /odom')
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_cb, queue_size = 10)
while not self.current_position.any(): # Will be 0 until an odom publishes (if its still 0 it will drift very very soon)
pass
# Initlize Trajectory generator with current position as goal
# Set the line to be along our current orientation
self.desired_position = self.current_position
self.desired_orientation = self.current_orientation
# Make a line along the orientation
self.line = line(self.current_position, tools.normal_vector_from_rotvec(self.current_orientation) + self.current_position)
self.redraw_line = False
rospy.loginfo('Got current pose from /odom')
# Kill handling
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(id=name, description='Tank steer trajectory_generator shutdown')
try:
self.kill_broadcaster.clear() # In case previously killed
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
def start(self, current_pt):
# Set current position and twist
self.current_position = tools.position_from_posetwist(current_pt)
# Zero the Z
self.current_position[2] = 0
self.current_orientation = tools.orientation_from_posetwist(current_pt)
# Initlize Trajectory generator with current position as goal
# Set the line to be along our current orientation
self.desired_position = self.current_position
self.desired_orientation = self.current_orientation
# Make a line along the orientation
self.line = line(self.current_position, tools.normal_vector_from_rotvec(self.current_orientation) + self.current_position)
def feedback(self, pt):
# Update current pt
self.current_position = tools.position_from_posetwist(pt)
# Zero the Z
self.current_position[2] = 0
self.current_orientation = tools.orientation_from_posetwist(pt)
# Get distance to the goal
if np.array_equal(self.current_position, self.desired_position):
self.line = line(self.desired_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.desired_position)
else:
self.line = line(self.current_position, self.desired_position)
self.distance_to_goal = np.linalg.norm(self.line.s)
def start(self, current_pt):
# Set current position and twist
self.current_position = tools.position_from_posetwist(current_pt)
# Zero the Z
self.current_position[2] = 0
self.current_orientation = tools.orientation_from_posetwist(current_pt)
# Initlize Trajectory generator with current position as goal
# Set the line to be along our current orientation
self.desired_position = self.current_position
self.desired_orientation = self.current_orientation
# Make a line along the orientation
self.line = line(self.current_position, tools.normal_vector_from_rotvec(self.current_orientation) + self.current_position)
self.redraw_line = False
def new_goal(self, goal):
self.desired_position = tools.position_from_posetwist(goal)
self.desired_orientation = tools.orientation_from_posetwist(goal)
# Twist not supported
if (xyz_array(goal.twist.linear).any() or
xyz_array(goal.twist.angular).any() ):
rospy.logwarn('None zero twist are not handled by the azi point and shoot trajectory generator. Setting twist to 0')
if np.linalg.norm(self.current_position - self.desired_position) > self.orientation_radius:
#print 'Far out dude'
self.line = line(self.current_position, self.desired_position)
else:
#print 'Pretty close'
self.line = line(self.desired_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.desired_position)
def feedback(self, pt):
# Update current pt
self.current_position = tools.position_from_posetwist(pt)
# Zero the Z
self.current_position[2] = 0
self.current_orientation = tools.orientation_from_posetwist(pt)
# Get distance to the goal
if np.array_equal(self.current_position, self.desired_position):
self.line = line(
self.desired_position,
tools.normal_vector_from_rotvec(self.desired_orientation) +
self.desired_position)
else:
self.line = line(self.current_position, self.desired_position)
self.distance_to_goal = np.linalg.norm(self.line.s)
def update(self, event):
# Publish trajectory
traj = self.get_carrot()
#rospy.loginfo('Trajectory: ' + str(traj))
if not self.killed:
self.traj_pub.publish(traj)
if self.redraw_line and self.distance_to_goal < self.orientation_radius:
self.redraw_line = False
rospy.loginfo('Redrawing trajectory line')
self.line = line(self.current_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.current_position)
rospy.loginfo('Angle to goal: ' + str(self.angle_to_goal_orientation[2] * 180 / np.pi) + '\t\t\tDistance to goal: ' + str(self.distance_to_goal))
# Check if goal is reached
if self.moveto_as.is_active():
if self.distance_to_goal < self.linear_tolerance:
if self.angle_to_goal_orientation[2] < self.angular_tolerance:
rospy.loginfo('succeded')
self.moveto_as.set_succeeded(None)
def new_goal(self, goal):
self.desired_position = tools.position_from_posetwist(goal)
self.desired_orientation = tools.orientation_from_posetwist(goal)
# Twist not supported
if (xyz_array(goal.twist.linear).any()
or xyz_array(goal.twist.angular).any()):
rospy.logwarn(
'None zero twist are not handled by the azi point and shoot trajectory generator. Setting twist to 0'
)
if np.linalg.norm(self.current_position -
self.desired_position) > self.orientation_radius:
#print 'Far out dude'
self.line = line(self.current_position, self.desired_position)
else:
#print 'Pretty close'
self.line = line(
self.desired_position,
tools.normal_vector_from_rotvec(self.desired_orientation) +
self.desired_position)
def traj_cb(self, traj):
# Timing
now = rospy.get_rostime()
dt = (now - self.last_time).to_sec()
self.last_time = now
# Get vectors related to the orientation of the trajectory
o = tools.normal_vector_from_posetwist(traj.posetwist)
#rospy.loginfo('o : ' + str(o))
o_hat = o / np.linalg.norm(o)
#rospy.loginfo('o hat: ' + str(o_hat))
o_norm = np.cross([0, 0, -1], o_hat)
#rospy.loginfo('o norm: ' + str(o_norm))
# Overshoot = 1 if the boat is behind a line drawn perpendicular to the trajectory orientation
# and through the trajectories position, it is -1 if it is on the other side of the before mentioned line
traj_position = tools.position_from_posetwist(traj.posetwist)
boat_proj = line(traj_position, traj_position + o_hat).proj_pt(self.current_position)
#rospy.loginfo('Boat_proj: ' + str(boat_proj))
#rospy.loginfo('traj: ' + str(traj_position))
#rospy.loginfo('traj - Boat_proj: ' + str(traj_position - boat_proj))
boat_to_traj = traj_position - self.current_position
boat_dist_to_traj = np.linalg.norm(boat_to_traj)
boat_to_traj_hat = boat_to_traj / boat_dist_to_traj
boat_to_traj_norm = np.cross([0,0,-1], boat_to_traj_hat)
overshoot = np.dot(boat_to_traj_hat, o_hat)
overshoot = overshoot / abs(overshoot)
rospy.loginfo('overshoot: ' + str(overshoot))
if math.isnan(overshoot):
return
# Method 2:
#
## P error = - angle(between boat and trajectory)
#
#enu_angle_to_traj = np.arccos(boat_to_traj_hat, np.array([1, 0, 0])))
#enu_angle_to_traj = math.copysign(enu_angle_to_traj, np.dot(boat_to_traj_norm, self.current_position))
#boat_angle_to_traj = (self.current_orientation[2] - enu_angle_to_traj) % (np.pi)
boat_orientation_vec = tools.normal_vector_from_rotvec(self.current_orientation)
#angle_error = 0
#torque_dir = 1
if boat_dist_to_traj > self.orientation_radius:
rospy.loginfo('Location: Outside orientation radius')
# Is the angle to trajectory positive or negative ( dot product of two unit vectors is negative when the
# angle between them is greater than 90, by doting the orientation of the boat with a vector that is 90 degrees
# to the trajectory and pointed to the right we get positive numbers if we are pointed to the right and negative
# if we are on the left side)
torque_dir = math.copysign(1, np.dot(boat_to_traj_norm, boat_orientation_vec))
# angle between path to traj and boat orientation
angle_error = np.arccos(np.dot(boat_orientation_vec, boat_to_traj_hat))
else:
rospy.loginfo('Location: Inside orientation radius')
# Same as above but now we are orienting to the desired final orientation instead of towards the trajectory
torque_dir = math.copysign(1, np.dot(o_norm, boat_orientation_vec))
# Angle between traj_orientation and boat orientation
angle_error = np.arccos(np.dot(boat_orientation_vec, o_hat))
#rospy.loginfo('enu_angle_to_traj:\t' + str(enu_angle_to_traj * 180 / np.pi))
#rospy.loginfo('Boat orientation:\t' + str(self.current_orientation[2] * 180 / np.pi))
#rospy.loginfo('Boat angle to traj\t' + str((boat_angle_to_traj) * 180 / np.pi ))
torque = angle_error * torque_dir * self.p
force = traj.posetwist.twist.linear.x
if boat_dist_to_traj < self.orientation_radius and overshoot == -1:
rospy.loginfo('Status: Overshoot in orientation radius')
force = force * -1
elif abs(angle_error) > np.pi / 2:
rospy.loginfo('Status: angle error > 90')
force = 0
else:
rospy.loginfo('Status: Normal')
rospy.loginfo('Angle error: ' + str(angle_error * torque_dir * 180 / np.pi))
rospy.loginfo('torque: ' + str(torque))
rospy.loginfo('Force: ' + str(force))
""" Method 1:
#
## P error = - perpendicular_velocity * p_gain * overshoot
#
# Velocity error = velocity perpendicular to trajectory orientation
velocity_error = self.current_velocity - (np.dot(self.current_velocity, o_hat) * o_hat)
v_dir = np.dot(velocity_error, o_norm)
velocity_error = math.copysign(np.linalg.norm(velocity_error), v_dir)
rospy.loginfo('V_perp = ' + str(velocity_error))
velocity_error = -1 * velocity_error * self.p * overshoot
rospy.loginfo('P error = ' + str(velocity_error))
#
## I error = - position * i_gain * overshoot * ???
#
# Positional error
# boat_position parralel portion of boat position
pos_error = self.current_position - boat_proj
p_dir = np.dot(pos_error, o_norm)
pos_error = math.copysign(np.linalg.norm(pos_error), p_dir)
rospy.loginfo('Perp_position = ' + str(pos_error))
pos_error = -1 * pos_error * self.i * overshoot
rospy.loginfo('I error = ' + str(pos_error))
#
## D error
#
# Acceleration error
acceleration_error = self.d * (velocity_error - self.last_perp_velocity) / dt
self.last_perp_velocity = velocity_error
torque = velocity_error + pos_error + acceleration_error
# Reverse force if overshoot
force = traj.posetwist.twist.linear.x * overshoot
rospy.loginfo('torque: ' + str(torque))
"""
# Output a wrench!
if not self.killed:
self.wrench_pub.publish(
WrenchStamped(
header = Header(
frame_id = '/base_link',
stamp = now),
wrench = Wrench(
force = Vector3(force, 0, 0),
torque = Vector3(0, 0, torque))))
else:
# Publish zero wrench
self.wrench_pub.publish(WrenchStamped())
def __init__(self, name):
# Desired pose
self.desired_position = self.current_position = np.zeros(3)
self.desired_orientation = self.current_orientation = np.zeros(3)
#self.desired_twist = self.current_twist = Twist()
# Goal tolerances before seting succeded
self.linear_tolerance = rospy.get_param('linear_tolerance', 0.5)
self.angular_tolerance = rospy.get_param('angular_tolerance',
np.pi / 10)
self.orientation_radius = rospy.get_param('orientation_radius', 0.75)
self.slow_down_radius = rospy.get_param('slow_down_radius', 5)
# Speed parameters
self.max_tracking_distance = rospy.get_param('max_tracking_distance',
5)
self.min_tracking_distance = rospy.get_param('min_tracking_distance',
1)
self.tracking_to_speed_conv = rospy.get_param('tracking_to_speed_conv',
1)
self.tracking_slope = (
self.max_tracking_distance - self.min_tracking_distance) / (
self.slow_down_radius - self.orientation_radius)
self.tracking_intercept = self.tracking_slope * self.orientation_radius + self.min_tracking_distance
# Publishers
self.traj_pub = rospy.Publisher('/trajectory',
PoseTwistStamped,
queue_size=10)
# Set desired twist to 0
#self.desired_twist.linear.x = self.desired_twist.linear.y = self.desired_twist.linear.z = 0
#self.desired_twist.angular.x = self.desired_twist.angular.y = self.desired_twist.angular.z = 0
# Initilize a line
self.line = line(np.zeros(3), np.ones(3))
# Wait for current position and set as desired position
rospy.loginfo('Waiting for /odom')
self.odom_sub = rospy.Subscriber('/odom',
Odometry,
self.odom_cb,
queue_size=10)
while not self.current_position.any(
): # Will be 0 until an odom publishes (if its still 0 it will drift very very soon)
pass
# Initlize Trajectory generator with current position as goal
# Set the line to be along our current orientation
self.desired_position = self.current_position
self.desired_orientation = self.current_orientation
# Make a line along the orientation
self.line = line(
self.current_position,
tools.normal_vector_from_rotvec(self.current_orientation) +
self.current_position)
self.redraw_line = False
rospy.loginfo('Got current pose from /odom')
# Kill handling
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(
id=name, description='Tank steer trajectory_generator shutdown')
try:
self.kill_broadcaster.clear() # In case previously killed
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
def traj_cb(self, traj):
# Timing
now = rospy.get_rostime()
dt = (now - self.last_time).to_sec()
self.last_time = now
# Get vectors related to the orientation of the trajectory
o = tools.normal_vector_from_posetwist(traj.posetwist)
#rospy.loginfo('o : ' + str(o))
o_hat = o / np.linalg.norm(o)
#rospy.loginfo('o hat: ' + str(o_hat))
o_norm = np.cross([0, 0, -1], o_hat)
#rospy.loginfo('o norm: ' + str(o_norm))
# Overshoot = 1 if the boat is behind a line drawn perpendicular to the trajectory orientation
# and through the trajectories position, it is -1 if it is on the other side of the before mentioned line
traj_position = tools.position_from_posetwist(traj.posetwist)
boat_proj = line(traj_position,
traj_position + o_hat).proj_pt(self.current_position)
#rospy.loginfo('Boat_proj: ' + str(boat_proj))
#rospy.loginfo('traj: ' + str(traj_position))
#rospy.loginfo('traj - Boat_proj: ' + str(traj_position - boat_proj))
boat_to_traj = traj_position - self.current_position
boat_dist_to_traj = np.linalg.norm(boat_to_traj)
boat_to_traj_hat = boat_to_traj / boat_dist_to_traj
boat_to_traj_norm = np.cross([0, 0, -1], boat_to_traj_hat)
overshoot = np.dot(boat_to_traj_hat, o_hat)
overshoot = overshoot / abs(overshoot)
rospy.loginfo('overshoot: ' + str(overshoot))
if math.isnan(overshoot):
return
# Method 2:
#
## P error = - angle(between boat and trajectory)
#
#enu_angle_to_traj = np.arccos(boat_to_traj_hat, np.array([1, 0, 0])))
#enu_angle_to_traj = math.copysign(enu_angle_to_traj, np.dot(boat_to_traj_norm, self.current_position))
#boat_angle_to_traj = (self.current_orientation[2] - enu_angle_to_traj) % (np.pi)
boat_orientation_vec = tools.normal_vector_from_rotvec(
self.current_orientation)
#angle_error = 0
#torque_dir = 1
if boat_dist_to_traj > self.orientation_radius:
rospy.loginfo('Location: Outside orientation radius')
# Is the angle to trajectory positive or negative ( dot product of two unit vectors is negative when the
# angle between them is greater than 90, by doting the orientation of the boat with a vector that is 90 degrees
# to the trajectory and pointed to the right we get positive numbers if we are pointed to the right and negative
# if we are on the left side)
torque_dir = math.copysign(
1, np.dot(boat_to_traj_norm, boat_orientation_vec))
# angle between path to traj and boat orientation
angle_error = np.arccos(
np.dot(boat_orientation_vec, boat_to_traj_hat))
else:
rospy.loginfo('Location: Inside orientation radius')
# Same as above but now we are orienting to the desired final orientation instead of towards the trajectory
torque_dir = math.copysign(1, np.dot(o_norm, boat_orientation_vec))
# Angle between traj_orientation and boat orientation
angle_error = np.arccos(np.dot(boat_orientation_vec, o_hat))
#rospy.loginfo('enu_angle_to_traj:\t' + str(enu_angle_to_traj * 180 / np.pi))
#rospy.loginfo('Boat orientation:\t' + str(self.current_orientation[2] * 180 / np.pi))
#rospy.loginfo('Boat angle to traj\t' + str((boat_angle_to_traj) * 180 / np.pi ))
torque = angle_error * torque_dir * self.p
force = traj.posetwist.twist.linear.x
if boat_dist_to_traj < self.orientation_radius and overshoot == -1:
rospy.loginfo('Status: Overshoot in orientation radius')
force = force * -1
elif abs(angle_error) > np.pi / 2:
rospy.loginfo('Status: angle error > 90')
force = 0
else:
rospy.loginfo('Status: Normal')
rospy.loginfo('Angle error: ' +
str(angle_error * torque_dir * 180 / np.pi))
rospy.loginfo('torque: ' + str(torque))
rospy.loginfo('Force: ' + str(force))
""" Method 1:
#
## P error = - perpendicular_velocity * p_gain * overshoot
#
# Velocity error = velocity perpendicular to trajectory orientation
velocity_error = self.current_velocity - (np.dot(self.current_velocity, o_hat) * o_hat)
v_dir = np.dot(velocity_error, o_norm)
velocity_error = math.copysign(np.linalg.norm(velocity_error), v_dir)
rospy.loginfo('V_perp = ' + str(velocity_error))
velocity_error = -1 * velocity_error * self.p * overshoot
rospy.loginfo('P error = ' + str(velocity_error))
#
## I error = - position * i_gain * overshoot * ???
#
# Positional error
# boat_position parralel portion of boat position
pos_error = self.current_position - boat_proj
p_dir = np.dot(pos_error, o_norm)
pos_error = math.copysign(np.linalg.norm(pos_error), p_dir)
rospy.loginfo('Perp_position = ' + str(pos_error))
pos_error = -1 * pos_error * self.i * overshoot
rospy.loginfo('I error = ' + str(pos_error))
#
## D error
#
# Acceleration error
acceleration_error = self.d * (velocity_error - self.last_perp_velocity) / dt
self.last_perp_velocity = velocity_error
torque = velocity_error + pos_error + acceleration_error
# Reverse force if overshoot
force = traj.posetwist.twist.linear.x * overshoot
rospy.loginfo('torque: ' + str(torque))
"""
# Output a wrench!
if not self.killed:
self.wrench_pub.publish(
WrenchStamped(header=Header(frame_id='/base_link', stamp=now),
wrench=Wrench(force=Vector3(force, 0, 0),
torque=Vector3(0, 0, torque))))
else:
# Publish zero wrench
self.wrench_pub.publish(WrenchStamped())
