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 pressureIntersec(
point1, refCurve1, point2
): # refCurve1 is required just to get the direction and material
pres1 = point1[1] + 1.E-20
vel1 = point1[0] + 1.E-20
pres2 = point2[1] + 1.E-20
vel2 = point2[0] + 1.E-20
interEnd1 = refCurve1.interEnd
intersecInterface = setting.interface[interEnd1].position
direction = refCurve1.direction
if (direction == 1):
mat1 = setting.interface[interEnd1].previousMat
mat2 = setting.interface[interEnd1].nextMat
elif (direction == 0):
mat1 = setting.interface[interEnd1].nextMat
mat2 = setting.interface[interEnd1].previousMat
mat1Z = material[mat1].Z
mat2Z = material[mat2].Z
presSign = pres1 / abs(pres1)
p1 = [vel1, pres1]
p2 = [vel1 + 1.E9, abs(pres1) - mat1Z * 1.E9]
p3 = [vel2, pres2]
p4 = [vel2 + 1.E9, abs(pres2) + mat2Z * 1.E9]
l1 = tools.line(p1, p2)
l2 = tools.line(p3, p4)
newPoint = tools.intersection(l1, l2)
return newPoint
def pressureInterface(motherId, motherCurve, calculated):
""" When a single curve crosses at an intersection """
# id of the interface where the mothercurve lands
interEnd = motherCurve.interEnd
intersecInterface = setting.interface[interEnd].position
direction = motherCurve.direction
daughterCurve0Id = curveHeritage[motherId][0][0]
daughterCurve1Id = curveHeritage[motherId][1][0]
daughter0 = curve[daughterCurve0Id]
daughter1 = curve[daughterCurve1Id]
if (direction == 1):
motherMat = setting.interface[interEnd].previousMat
# daughterMat = material next to the mother where one of the shockwave will go
daughterMat = setting.interface[interEnd].nextMat
elif (direction == 0):
motherMat = setting.interface[interEnd].nextMat
daughterMat = setting.interface[interEnd].previousMat
motherMatZ = material[motherMat].Z
daughterMatZ = material[daughterMat].Z
motherVel = motherCurve.velocity
motherPres = motherCurve.pressure
presSign = motherPres / abs(motherPres)
# pressure of the outgoing curve that has the same direction as the mothercurve
p1 = [motherVel, motherPres]
p2 = [motherVel + 1.E9, presSign * (abs(motherPres) - motherMatZ * 1.E9)]
l1 = tools.line(p1, p2)
# next material hugoniot
p3 = [0., 0.]
p4 = [1.E9, presSign * daughterMatZ * 1E9]
l2 = tools.line(p3, p4)
# intersection of both hugoniot
newPoint = tools.intersection(l1, l2)
daughter1.velocity = newPoint[0]
daughter1.pressure = newPoint[1]
calculated[daughterCurve1Id] = newPoint
presSign = daughter1.pressure / abs(daughter1.pressure)
# pressure of the outgoing curve that has the opposite direction than the mothercurve
p1 = [daughter1.velocity, daughter1.pressure]
p2 = [
daughter1.velocity + 1.E9,
daughter1.pressure + presSign * motherMatZ * 1.E9
]
l1 = tools.line(p1, p2)
p3 = [0., 0.]
p4 = [1.E9, presSign * (-motherMatZ) * 1.E9]
l2 = tools.line(p3, p4)
newPoint = tools.intersection(l1, l2)
calculated[daughterCurve0Id] = newPoint
daughter0.velocity = newPoint[0]
daughter0.pressure = newPoint[1]
return
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 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 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 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 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):
# Desired pose
self.desired_position = self.current_position = np.zeros(3)
self.desired_orientation = self.current_orientation = np.zeros(3)
# Region deffinitions
self.orientation_radius = rospy.get_param('orientation_radius', 1)
self.slow_down_radius = rospy.get_param('slow_down_radius', 3.0)
# Speed parameters
self.max_tracking_distance = rospy.get_param('max_tracking_distance',
2.0)
self.min_tracking_distance = rospy.get_param(
'min_tracking_distance', 0.5) # Half orientation radius
self.tracking_to_speed_conv = rospy.get_param('tracking_to_speed_conv',
1.0)
self.tracking_slope = (
self.max_tracking_distance - self.min_tracking_distance) / (
self.slow_down_radius - self.orientation_radius)
self.tracking_intercept = self.min_tracking_distance - self.tracking_slope * self.orientation_radius
# Goal paramiters
self.distance_to_goal = 0.0
# Initilize a line
self.redraw_line = True
self.line = line(np.zeros(3), np.ones(3))
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 pressureCross(point1, refCurve1, point2,
refCurve2): # refCurve1 is required just to get the material
pres1 = point1[1] + 1.E-13
vel1 = point1[0] + 1.E-13
pres2 = point2[1] + 1.E-13
vel2 = point2[0] + 1.E-13
interEnd1 = refCurve1.interEnd
interEnd2 = refCurve2.interEnd
mat1 = setting.interface[interEnd1].previousMat
mat2 = setting.interface[interEnd2].nextMat
mat1Z = material[mat1].Z
mat2Z = material[mat2].Z
presSign = pres1 / abs(pres1)
velSign = vel1 / abs(vel1)
p1 = [vel1, pres1]
p2 = [vel1 + 1.E9, abs(pres1) - mat1Z * 1.E9]
p3 = [vel2, pres2]
p4 = [vel2 + 1.E9, abs(pres2) + mat2Z * 1.E9]
if mat1Z == mat2Z:
if pres1 / pres2 > 0:
if vel1 < vel2:
p2 = [vel1 + 1.E9, abs(pres1) + presSign * mat1Z * 1.E9]
p4 = [vel2 + 1.E9, abs(pres2) - presSign * mat2Z * 1.E9]
else:
p2 = [vel1 + 1.E9, abs(pres1) - presSign * mat1Z * 1.E9]
p4 = [vel2 + 1.E9, abs(pres2) + presSign * mat2Z * 1.E9]
elif vel1 / vel2 > 0:
if pres1 < pres2:
p2 = [vel1 + 1.E9, abs(pres1) + presSign * mat1Z * 1.E9]
p4 = [vel2 + 1.E9, abs(pres2) - presSign * mat2Z * 1.E9]
else:
p2 = [vel1 + 1.E9, abs(pres1) - presSign * mat1Z * 1.E9]
p4 = [vel2 + 1.E9, abs(pres2) + presSign * mat2Z * 1.E9]
l1 = tools.line(p1, p2)
l2 = tools.line(p3, p4)
newPoint = tools.intersection(l1, l2)
return newPoint
def look_for(search, database, text1, text2, text3="(Press 1 to quit this option)"):
name = input(f"Please enter {search} you are looking for:\n").capitalize()
temp = 0
while name not in database:
for data in database:
if name.lower() == data.lower():
return data
if re.search(name.lower(), data.lower()):
temp += 1
if temp == 1:
print_with_nl("Here are some suggestions:")
print(data)
if name == '1':
return
line()
name = input(
f"There is {text1} '{capwords(name)}' in my database."
f"\nPlease enter {text2}\n{text3}\n")
line()
return name
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 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 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 pressureMultRef(point, inc):
refCurve = curve[inc[0]]
interEnd = refCurve.interEnd
if (refCurve.direction == 1):
mat = setting.interface[interEnd].previousMat
else:
mat = setting.interface[interEnd].nextMat
matZ = material[mat].Z
presSign = point[1] / abs(point[1] + 1.e-13)
if (point[0] < 0):
p2 = [point[0] + 1., presSign * (abs(point[1]) + matZ)]
else:
p2 = [point[0] + 1., presSign * (abs(point[1]) - matZ)]
p1 = [point[0], point[1]]
p3 = [0, 0]
p4 = [1, 0]
l1 = tools.line(p1, p2)
l2 = tools.line(p3, p4)
newPoint = tools.intersection(l1, l2)
return newPoint
def __init__(self):
# Desired pose
self.desired_position = self.current_position = np.zeros(3)
self.desired_orientation = self.current_orientation = np.zeros(3)
# Region deffinitions
self.orientation_radius = rospy.get_param('orientation_radius', 2)
self.slow_down_radius = rospy.get_param('slow_down_radius', 3.0)
# Speed parameters
self.max_tracking_distance = rospy.get_param('max_tracking_distance', 2.0)
self.min_tracking_distance = rospy.get_param('min_tracking_distance', 0.5) # Half orientation radius
self.tracking_to_speed_conv = rospy.get_param('tracking_to_speed_conv', 1.0)
self.tracking_slope = (self.max_tracking_distance - self.min_tracking_distance) / (self.slow_down_radius - self.orientation_radius)
self.tracking_intercept = self.min_tracking_distance - self.tracking_slope * self.orientation_radius
# Goal paramiters
self.distance_to_goal = 0.0
# Initilize a line
self.line = line(np.zeros(3), np.ones(3))
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 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 pressureMultInt(
point, inc
): # value of pressure and velocity for incomming curves, list inc curves, list outgoing curves
""" When multiple curve crosses at an intersection """
pres1 = point[1] + 1E-13
vel1 = point[0]
direction = curve[inc[0]].direction
refCurve = curve[inc[0]]
interEnd = refCurve.interEnd
if (direction == 1):
mat1 = setting.interface[interEnd].previousMat
mat2 = setting.interface[interEnd].nextMat
elif (direction == 0):
mat1 = setting.interface[interEnd].nextMat
mat2 = setting.interface[interEnd].previousMat
mat1Z = material[mat1].Z
mat2Z = material[mat2].Z
presSign = pres1 / abs(pres1)
p1 = [vel1, pres1]
if vel1 >= 0:
p2 = [vel1 + 1.E9, presSign * (abs(pres1) - mat1Z * 1.E9)]
else:
p2 = [vel1 + 1.E9, presSign * (abs(pres1) + mat1Z * 1.E9)]
l1 = tools.line(p1, p2)
p3 = [0., 0.]
if vel1 >= 0:
p4 = [1.E9, presSign * mat2Z * 1.E9]
else:
p4 = [-1.E9, presSign * mat2Z * 1.E9]
l2 = tools.line(p3, p4)
temp1 = tools.intersection(l1, l2)
p1 = [temp1[0], temp1[1]]
if vel1 >= 0:
p2 = [temp1[0] + 1.E9, temp1[1] + presSign * mat1Z * 1.E9]
else:
p2 = [temp1[0] + 1.E9, temp1[1] - presSign * mat1Z * 1.E9]
l1 = tools.line(p1, p2)
# next material hugoniot
p3 = [0., 0.]
if vel1 >= 0:
p4 = [1.E9, presSign * (-mat1Z) * 1.E9]
else:
p4 = [1.E9, presSign * (mat1Z) * 1.E9]
l2 = tools.line(p3, p4)
temp2 = tools.intersection(l1, l2)
if (direction == 0):
if refCurve.cType == 0:
if mat1Z < mat2Z:
if abs(temp2[1]) < abs(temp1[1]):
topL = temp1
topR = temp2
else:
topL = temp2
topR = temp1
else:
if temp2[1] / pres1 < 0:
topL = temp1
topR = temp2
else:
topL = temp2
topR = temp1
else:
if mat1Z < mat2Z:
if abs(temp2[1]) < abs(temp1[1]):
topL = temp1
topR = temp2
else:
topL = temp2
topR = temp1
else:
if temp2[1] / pres1 < 0:
topL = temp1
topR = temp2
else:
topL = temp2
topR = temp1
elif (direction == 1):
if refCurve.cType == 0:
if mat1Z < mat2Z:
if abs(temp2[1]) < abs(temp1[1]):
topR = temp1
topL = temp2
else:
topR = temp2
topL = temp1
else:
if temp2[1] / pres1 < 0:
topR = temp1
topL = temp2
else:
topR = temp2
topL = temp1
else:
if mat1Z < mat2Z:
if abs(temp2[1]) < abs(temp1[1]):
topR = temp1
topL = temp2
else:
topR = temp2
topL = temp1
else:
if temp2[1] / pres1 < 0:
topR = temp1
topL = temp2
else:
topR = temp2
topL = temp1
return topL, topR
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())
def event(self):
mp = list(pg.mouse.get_pos())
for event in pg.event.get():
if event.type == pg.QUIT:
self.running = False
quit()
if event.type == pg.MOUSEBUTTONDOWN:
### SCROLLING ###
if event.button == 4:
try:
if self.left_color < len(self.toolbar.cells)-1:
self.left_color += 1
self.toolbar.color_l.color = self.toolbar.palette.colors[self.left_color]
self.toolbar.update()
self.screen.blit(self.toolbar.color_l.image, self.toolbar.color_l)
pg.display.update(self.toolbar.area)
except TypeError:
pass
if event.button == 5:
try:
if self.left_color > 0:
self.right_color -= 1
self.toolbar.color_l.color = self.toolbar.palette.colors[self.left_color]
self.toolbar.update()
self.screen.blit(self.toolbar.color_l.image, self.toolbar.color_l)
pg.display.update(self.toolbar.area)
except TypeError:
pass
if event.type == pg.MOUSEMOTION or event.type == pg.MOUSEBUTTONDOWN:
### TOOLBAR ###
# Data
if self.toolbar.area.rect.collidepoint(mp[0], mp[1]):
if pg.mouse.get_pressed() == (1, 0, 0):
if not self.working_data:
if self.toolbar.button_save.rect.collidepoint(mp[0], mp[1]):
print("Enter name to save")
name = input(": ")
self.working_data = True
self.save(name)
elif self.toolbar.button_load.rect.collidepoint(mp[0], mp[1]):
print("Enter name to load")
name = input(": ")
self.working_data = True
self.load(name)
elif self.toolbar.button_export.rect.collidepoint(mp[0], mp[1]):
print("Enter name to save")
name = input(": ")
self.last_save_dir = name
self.working_data = True
self.port(name, 0)
elif self.toolbar.button_import.rect.collidepoint(mp[0], mp[1]):
print("Enter name to load")
name = input(": ")
self.last_save_dir = name
self.working_data = True
self.port(name, 1)
# Set Tool
for t in self.toolbar.tools:
if t.rect.collidepoint(mp[0], mp[1]):
if t.cursor != None:
self.cursor = t.cursor
cursor = pg.cursors.compile(self.cursor, black='X', white='.', xor='o')
pg.mouse.set_cursor((16, 16), (0, 0), *cursor)
# Change Color
if self.toolbar.palette_area.rect.collidepoint(mp[0], mp[1]):
if pg.mouse.get_pressed() == (1, 0, 0):
if not self.working_data:
for i, c in enumerate(self.toolbar.cells):
if c.rect.collidepoint(mp[0], mp[1]):
self.left_color = i
self.toolbar.color_l.color = self.toolbar.palette.colors[self.left_color]
self.toolbar.update()
self.screen.blit(self.toolbar.color_l.image, self.toolbar.color_l)
pg.display.update(self.toolbar.area)
if pg.mouse.get_pressed() == (0, 0, 1):
if not self.working_data:
for i, c in enumerate(self.toolbar.cells):
if c.rect.collidepoint(mp[0], mp[1]):
self.right_color = i
self.toolbar.color_r.color = self.toolbar.palette.colors[self.right_color]
self.toolbar.update()
self.screen.blit(self.toolbar.color_r.image, self.toolbar.color_r)
pg.display.update(self.toolbar.area)
### CANVAS ###
# Apply Tool
if self.canvas_area.rect.collidepoint(mp[0], mp[1]):
if pg.mouse.get_pressed() == (1, 0, 0):
if not self.working_data:
if self.cursor == cursor_norm:
tools.select(self, mp)
if self.cursor == cursor_draw:
tools.draw(self, mp, self.left_color)
pg.display.update(self.canvas_area)
if self.cursor == cursor_line:
self.working_data = True
tools.line(self, mp, self.left_color)
pg.display.update(self.canvas_area)
if self.cursor == cursor_rect:
self.working_data = True
tools.rect_f(self, mp, self.left_color)
pg.display.update(self.canvas_area)
if self.cursor == cursor_rcte:
self.working_data = True
tools.rect_e(self, mp, self.left_color)
pg.display.update(self.canvas_area)
if self.cursor == cursor_oval:
self.working_data = True
tools.oval_f(self, mp, self.left_color)
pg.display.update(self.canvas_area)
if self.cursor == cursor_ovle:
self.working_data = True
tools.oval_e(self, mp, self.left_color, self.tool_thickness)
pg.display.update(self.canvas_area)
if self.cursor == cursor_crcl:
self.working_data = True
tools.circle(self, mp, self.left_color)
pg.display.update(self.canvas_area)
if self.cursor == cursor_fill:
self.working_data = True
tools.flood_fill(self, mp, self.left_color)
pg.display.update(self.canvas_area)
if self.cursor == cursor_pick:
self.left_color = tools.dropper(self, mp)
self.toolbar.color_l.color = tools.dropper(self, mp)
self.screen.blit(self.toolbar.color_l.image, self.toolbar.color_l)
pg.display.update(self.toolbar.area)
if pg.mouse.get_pressed() == (0, 0, 1):
if not self.working_data:
if self.cursor == cursor_draw:
tools.draw(self, mp, self.right_color)
pg.display.update(self.canvas_area)
if self.cursor == cursor_line:
self.working_data = True
tools.line(self, mp, self.right_color)
pg.display.update(self.canvas_area)
if self.cursor == cursor_rect:
self.working_data = True
tools.rect_f(self, mp, self.right_color)
pg.display.update(self.canvas_area)
if self.cursor == cursor_rcte:
self.working_data = True
tools.rect_e(self, mp, self.right_color)
pg.display.update(self.canvas_area)
if self.cursor == cursor_oval:
self.working_data = True
tools.oval_f(self, mp, self.right_color)
pg.display.update(self.canvas_area)
if self.cursor == cursor_ovle:
self.working_data = True
tools.oval_e(self, mp, self.right_color, self.tool_thickness)
pg.display.update(self.canvas_area)
if self.cursor == cursor_crcl:
self.working_data = True
tools.circle(self, mp, self.right_color)
pg.display.update(self.canvas_area)
if self.cursor == cursor_fill:
self.working_data = True
tools.flood_fill(self, mp, self.right_color)
pg.display.update(self.canvas_area)
if self.cursor == cursor_pick:
self.right_color = tools.dropper(self, mp)
self.toolbar.color_r.color = tools.dropper(self, mp)
self.screen.blit(self.toolbar.color_r.image, self.toolbar.color_r)
pg.display.update(self.toolbar.area)
# Commit Selection
if event.type == pg.MOUSEBUTTONUP:
if self.canvas_area.rect.collidepoint(mp[0], mp[1]):
if self.cursor == cursor_norm:
if not self.working_data:
self.working_data = True
tools.set_select(self, mp)
for c in self.clipboard['matrix']:
c.update()
self.screen.blit(c.image, c)
pg.display.update(self.canvas_area)
else:
for s in self.toolbar.cells:
self.screen.blit(s.image, s)
for b in self.toolbar.buttons:
self.screen.blit(b.image, b)
for t in self.toolbar.tools:
self.screen.blit(t.image, t)
pg.display.update(self.toolbar.palette_area)
pg.display.update(self.toolbar.area)
### KEYS ###
if event.type == pg.KEYDOWN:
# Rotation
if event.key == pg.K_KP2:
self.rotation = 270
if event.key == pg.K_KP4:
self.rotation = 180
if event.key == pg.K_KP8:
self.rotation = 90
if event.key == pg.K_KP6:
self.rotation = 0
# Blur
if event.key == pg.K_b:
if not self.working_data:
tools.blur(self)
pg.display.update(self.canvas_area)
# Quick Save
if (event.mod == pg.KMOD_LCTRL or event.mod == pg.KMOD_RCTRL) and event.key == pg.K_s:
if not self.working_data:
if self.last_save_dir != '':
self.working_data = True
self.port(self.last_save_dir, 0)
# Grid
if (event.mod == pg.KMOD_LCTRL or event.mod == pg.KMOD_RCTRL) and event.key == pg.K_g:
self.grid = False
for c in self.canvas:
self.screen.blit(c.image, c)
pg.display.update(self.canvas_area)
made_changes = True
if (event.mod == pg.KMOD_LSHIFT or event.mod == pg.KMOD_RSHIFT) and event.key == pg.K_g:
self.grid = True
# Pasting
if (event.mod == pg.KMOD_LCTRL or event.mod == pg.KMOD_RCTRL) and event.key == pg.K_v:
tools.paste(self, mp)
pg.display.update(self.canvas_area)
if (event.mod == pg.KMOD_LSHIFT or event.mod == pg.KMOD_RSHIFT) and event.key == pg.K_v:
tools.paste(self, mp, mode=1)
pg.display.update(self.canvas_area)
# Undo
if (event.mod == pg.KMOD_LCTRL or event.mod == pg.KMOD_RCTRL) and event.key == pg.K_z:
self.undo()
# Exiting
if event.key == pg.K_ESCAPE:
self.running = False
# Change Mouse Position
if mp[1] > 0 and mp[1] < list(self.canvas_size)[1] and mp[0] > 0 and mp[0] < list(self.canvas_size)[0]:
if event.key == pg.K_UP:
for c in self.canvas:
if c.rect.collidepoint(mp[0], mp[1]):
pg.mouse.set_pos([c.posx+(CELL_SIZE/2), c.posy+(CELL_SIZE/2)-CELL_SIZE])
if event.key == pg.K_DOWN:
for c in self.canvas:
if c.rect.collidepoint(mp[0], mp[1]):
pg.mouse.set_pos([c.posx+(CELL_SIZE/2), c.posy+(CELL_SIZE/2)+CELL_SIZE])
if event.key == pg.K_LEFT:
for c in self.canvas:
if c.rect.collidepoint(mp[0], mp[1]):
pg.mouse.set_pos([c.posx+(CELL_SIZE/2)-CELL_SIZE, c.posy+(CELL_SIZE/2)])
if event.key == pg.K_RIGHT:
for c in self.canvas:
if c.rect.collidepoint(mp[0], mp[1]):
pg.mouse.set_pos([c.posx+(CELL_SIZE/2)+CELL_SIZE, c.posy+(CELL_SIZE/2)])
