class send_action:
def __init__(self, name):
self.action = actionlib.SimpleActionServer('moveto', MoveToAction, self.new_goal, False)
self.trajectory_pub = rospy.Publisher('/trajectory', PoseTwistStamped, queue_size=10)
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.action.start()
self.to_pose = PoseTwistStamped()
self.temp_pose = PoseTwistStamped()
self.killed = False
self.waypoint = False
def callback(self, msg):
self.waypoint = msg.data
def set_kill(self):
self.to_pose = PoseTwistStamped()
# TODO --> Verify that all values are at 0
#members = [attr for attr in dir(self.to_pose.posetwist.pose) if attr.startswith("position") or attr.startswith("orientation")]
#getattr(self.to_pose.posetwist.pose.position, "x") == 0
rospy.logwarn('Azi_Drive waypoint kill flag off -- All trajectories at 0: %s' % self.kill_listener.get_kills())
self.killed = True
def clear_kill(self):
self.killed = False
rospy.logwarn('Azi_Drive waypoint kill flag off -- Waypoints enabled: %s' % self.kill_listener.get_kills())
def new_goal(self, goal):
self.waypoint = False
self.temp_pose = PoseTwistStamped()
self.temp_pose.posetwist = goal.posetwist
self.temp_pose.header = goal.header
time.sleep(5)
self.waypoint_progress = rospy.Subscriber('/waypoint_progress', Bool, self.callback)
while self.waypoint == False:
None
self.action.set_succeeded()
self.waypoint = True
def over_and_over(self):
r = rospy.Rate(1)
if self.killed == True:
rospy.logwarn('Azi_Drive waypoint kill flag on -- Waypoints disabled: %s' % self.kill_listener.get_kills())
if self.killed == False:
self.to_pose = self.temp_pose
self.trajectory_pub.publish(self.to_pose)
print self.to_pose.posetwist.pose
self.to_pose = PoseTwistStamped()
r.sleep()
class PID_controller:
def __init__(self):
'''
Structure gain array for flexible use
This allows the gain function to access all gains by simple indexing scheme
Place x and y gains in the first row of the 6x3
Place x gains in the bottom row
the i_history array uses this scheme as well
Gain array layout:
[ p_x. i_x. d_x.]
[ p_y. i_y. d_y.]
[ 0. 0. 0.]
[ 0. 0. 0.]
[ 0. 0. 0.]
[ p_z. i_z. d_z.]
'''
self.K = numpy.zeros((6,3))
self.K[0:6, 0:6] = [
[p_x, i_x, d_x], # pid_x
[p_y, i_y, d_y], # pid_y
[0, 0, 0],
[0, 0, 0],
[0, 0, 0],
[p_z, i_z, d_z], # pid_z
]
# Kill functions
self.odom_active = False
self.killed = False
# Create arrays to be used
self.desired_state = numpy.ones(6)
self.desired_velocity = numpy.ones(6)
self.current_state = numpy.zeros(6)
self.current_velocity = numpy.zeros(6)
self.current_error = numpy.ones(6)
self.i_history = [[0 for x in range(1)] for x in range(6)]
self.i_history[0] = deque()
self.i_history[1] = deque()
self.i_history[5] = deque()
self.integrator = numpy.zeros(6)
self.Derivator= 0
self.lock = threading.Lock()
# Used to reset the desired state on startup
self.desired_state_set = False
# ROS components
self.controller_wrench = rospy.Publisher('wrench', WrenchStamped, queue_size = 1)
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
rospy.Subscriber('/trajectory', PoseTwistStamped, self.trajectory_callback)
rospy.Subscriber('/odom', Odometry, self.odom_callback)
rospy.Subscriber("pid_d_gain", Point, self.d_gain_callback)
rospy.Subscriber("pid_p_gain", Point, self.p_gain_callback)
rospy.Subscriber("pid_i_gain", Point, self.i_gain_callback)
def p_gain_callback(self, msg):
x = msg.x
y = msg.y
z = msg.z
self.K[0:2, 0] = [x,y]
self.K[5, 0] = z
def i_gain_callback(self, msg):
x = msg.x
y = msg.y
z = msg.z
self.K[0:2, 1] = [x,y]
self.K[5, 1] = z
def d_gain_callback(self, msg):
x = msg.x
y = msg.y
z = msg.z
self.K[0:2, 2] = [x,y]
self.K[5, 2] = z
def set_kill(self):
self.killed = True
rospy.logdebug('PD_Controller KILLED: %s' % self.kill_listener.get_kills())
def clear_kill(self):
self.killed = False
rospy.logdebug('PD_Controller ACTIVE: %s' % self.kill_listener.get_kills())
def trajectory_callback(self, desired_trajectory):
self.lock.acquire()
self.desired_state_set = True
# Get desired pose and orientation
desired_pose = xyz_array(desired_trajectory.posetwist.pose.position)
desired_orientation = transformations.euler_from_quaternion(xyzw_array(desired_trajectory.posetwist.pose.orientation))
# Get desired linear and angular velocities
desired_lin_vel = xyz_array(desired_trajectory.posetwist.twist.linear)
desired_ang_vel = xyz_array(desired_trajectory.posetwist.twist.angular)
# Add desired position to desired state i_historys
self.desired_state = numpy.concatenate([desired_pose, desired_orientation])
# Add desired velocities to velocity i_history
self.desired_velocity = numpy.concatenate([desired_lin_vel, desired_ang_vel])
self.lock.release()
def odom_callback(self, current_pos):
self.lock.acquire()
self.odom_active = True
# Grab current position and orientation and 0 linear Z and angluar X and Y
# Get current position
current_position = xyz_array(current_pos.pose.pose.position)
current_orientation = numpy.array(transformations.euler_from_quaternion(xyzw_array(current_pos.pose.pose.orientation)))
# Zero unneccesary elements
current_position[2] = 0
current_orientation[0:2] = 0
# Add current position to state i_history
self.current_state = numpy.concatenate([current_position, current_orientation])
# Get current velocities
current_lin_vel = xyz_array(current_pos.twist.twist.linear)
current_ang_vel = xyz_array(current_pos.twist.twist.angular)
# Add current velocities to velocity i_history
self.current_velocity = numpy.concatenate([current_lin_vel, current_ang_vel])
# If the desired state has not yet been set, set desired and current as the same
# Resets the controller to current position on bootup
if (not self.desired_state_set):
self.desired_state = self.current_state
self.desired_state_set = True
self.lock.release()
def PID(self, variable):
# Index in state number we want to access
state_number = 0
if variable == 'x': state_number = 0
if variable == 'y': state_number = 1
if variable == 'z': state_number = 5
#self.current_error = self.desired_state[state_number] - self.current_state[state_number]
#rospy.logdebug(variable + ": " + str(self.current_error[state_number]))
p = self.K[state_number, 0] * self.current_error[state_number]
i = (self.integrator[state_number] + self.current_error[state_number]) * self.K[state_number, 1]
d = self.K[state_number, 2] * (self.current_error[state_number] - self.Derivator)
# This section will be the FOPID implimentation, but I am still working on it
if abs(self.current_error[state_number]) > 0: pass
#i = i * (1 + abs(d))
rospy.logdebug(self.current_error[state_number])
rospy.logwarn('P' + variable + ": " + str(p))
rospy.logwarn('I' + variable + ": " + str(i))
rospy.logwarn('D' + variable + ": " + str(d))
# Set temporary variable for use in integrator sliding window
sliding_window = self.i_history[state_number]
# append to integrator array
sliding_window.append(i)
# If array is larger than 5 items, remove item
if len(sliding_window) > 5:
sliding_window.pop()
# Set up variables for next iteration
# Sum only last 5 numbers of intergration
self.Derivator = self.current_error[state_number]
self.integrator[state_number] = sum(sliding_window)
PID = p + i + d
return PID
def timeout_callback(self, event):
self.odom_active = False
def jacobian(self, x):
# maps global linear velocity/euler rates -> body linear+angular velocities
sphi, cphi = math.sin(x[3]), math.cos(x[3])
stheta, ctheta = math.sin(x[4]), math.cos(x[4])
spsi, cpsi = math.sin(x[5]), math.cos(x[5])
J_inv = numpy.zeros((6, 6))
J_inv[0:3, 0:3] = [
[ ctheta * cpsi , ctheta * spsi , -stheta],
[sphi * stheta * cpsi - cphi * spsi, sphi * stheta * spsi + cphi * cpsi, sphi * ctheta],
[cphi * stheta * cpsi + sphi * spsi, cphi * stheta * spsi - sphi * cpsi, cphi * ctheta],]
J_inv[3:6, 3:6] = [
[1, 0, -stheta],
[0, cphi, sphi * ctheta],
[0, -sphi, cphi * ctheta],]
return J_inv
def main_loop(self, event):
def smallest_coterminal_angle(x):
return (x + math.pi) % (2*math.pi) - math.pi
self.lock.acquire()
# Get linear and angular error between desired and current pose - /enu
linear_error = self.desired_state[0:3] - self.current_state[0:3]
angular_error = map(smallest_coterminal_angle, self.desired_state[3:6] - self.current_state[3:6])
# Combine errors into one array
error_enu = numpy.concatenate([linear_error, angular_error])
#rospy.logdebug(error_enu)
# Translate /enu errors into /base_link errors
error_base = self.jacobian(self.current_state).dot(error_enu)
# Take away velocity from error to avoid overshoot
final_error = error_base - self.current_velocity
# Place errors to be sent into main error array
self.current_error = [final_error[0], final_error[1], 0, 0, 0, final_error[5]]
self.lock.release()
# Send error values through PID controller
x = self.PID('x')
y = self.PID('y')
z = self.PID('z')
# Combine into final wrenches
wrench = [x,y,0,0,0,z]
# If odometry has not been aquired, set wrench to 0
if (not(self.odom_active)):
wrench = [0,0,0,0,0,0]
# If ready to go...
if (self.killed == False):
self.controller_wrench.publish(WrenchStamped(
header = Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force = Vector3(x= wrench[0],y= wrench[1],z= 0),
torque = Vector3(x=0,y= 0,z= wrench[5]),
))
)
# If not ready to go...
if (self.killed == True):
rospy.logdebug('PD_Controller KILLED: %s' % self.kill_listener.get_kills())
self.controller_wrench.publish(WrenchStamped(
header = Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force = Vector3(x= 0,y= 0,z= 0),
torque = Vector3(x=0,y= 0,z= 0),
))
)
class Controller(object):
def __init__(self):
# Old gains
self.d_x = 175
self.d_y = 90
self.d_z = 60
self.p_x = 30
self.p_y = 30
self.p_z = 40
#self.d_x = 175
#self.d_y = 90
#self.d_z = 90
#self.p_x = 40
#self.p_y = 40
#self.p_z = 100
self.killed = False
self.enable = True
self.odom_active = False
self.K_d = numpy.array([[self.d_x, 0, 0, 0, 0, 0],
[0, self.d_y, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, self.d_z]])
self.K_p = numpy.array([[self.p_x, 0, 0, 0, 0, 0],
[0, self.p_y, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, self.p_z]])
# Averaging parameters
self.last_average = numpy.zeros(6)
self.num_to_avg = 75 # At 50 hz this is 50 samples is one second of data
self.outputs = deque(
[numpy.zeros(6) for i in range(0, self.num_to_avg)])
self.desired_state_set = False
self.desired_state = numpy.ones(6)
self.desired_state_dot = numpy.ones(6)
self.state = numpy.ones(6)
self.previous_error = numpy.ones(6)
self.state_dot = numpy.ones(6)
self.state_dot_body = numpy.ones(6)
self.lock = threading.Lock()
# Get tf listener
self.tf_listener = tf.TransformListener()
rospy.Subscriber("pd_d_gain", Point, self.d_gain_callback)
rospy.Subscriber("pd_p_gain", Point, self.p_gain_callback)
rospy.Subscriber('/trajectory', PoseTwistStamped,
self.desired_state_callback)
rospy.Subscriber('/odom', Odometry, self.odom_callback)
self.controller_wrench = rospy.Publisher('wrench',
WrenchStamped,
queue_size=1)
self.waypoint_progress = rospy.Publisher('waypoint_progress',
Bool,
queue_size=1)
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.z_error = 0
self.x_error = 0
self.y_error = 0
self.dz_error = 0
self.dx_error = 0
self.dy_error = 0
def set_kill(self):
self.killed = True
rospy.logwarn('PD_Controller KILLED: %s' %
self.kill_listener.get_kills())
def clear_kill(self):
self.killed = False
rospy.logwarn('PD_Controller ACTIVE: %s' %
self.kill_listener.get_kills())
def d_gain_callback(self, msg):
self.d_x = msg.x
self.d_y = msg.y
self.d_z = msg.z
self.K_d = numpy.array([[self.d_x, 0, 0, 0, 0, 0],
[0, self.d_y, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, self.d_z]])
def p_gain_callback(self, msg):
self.p_x = msg.x
self.p_y = msg.y
self.p_z = msg.z
self.K_p = numpy.array([[self.p_x, 0, 0, 0, 0, 0],
[0, self.p_y, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, self.p_z]])
def desired_state_callback(self, desired_posetwist):
self.lock.acquire()
self.desired_state_set = True
self.desired_state = numpy.concatenate([
xyz_array(desired_posetwist.posetwist.pose.position),
transformations.euler_from_quaternion(
xyzw_array(desired_posetwist.posetwist.pose.orientation))
])
self.desired_state_dot = numpy.concatenate([
xyz_array(desired_posetwist.posetwist.twist.linear),
xyz_array(desired_posetwist.posetwist.twist.angular)
])
self.lock.release()
def odom_callback(self, current_posetwist):
self.lock.acquire()
self.odom_active = True
# Grab position; 0 Z
pose = xyz_array(current_posetwist.pose.pose.position)
pose[2] = 0
# Grab orientation; 0 pitch and roll
orientation = numpy.array(
transformations.euler_from_quaternion(
xyzw_array(current_posetwist.pose.pose.orientation)))
orientation[0:2] = 0
self.state = numpy.concatenate([
xyz_array(current_posetwist.pose.pose.position),
transformations.euler_from_quaternion(
xyzw_array(current_posetwist.pose.pose.orientation))
])
self.state_dot = numpy.concatenate([
xyz_array(current_posetwist.twist.twist.linear),
xyz_array(current_posetwist.twist.twist.angular)
])
if (not self.desired_state_set):
self.desired_state = self.state
self.desired_state_set = True
self.lock.release()
def main_loop(self, event):
self.lock.acquire()
#print 'desired state', desired_state
#print 'current_state', state
def smallest_coterminal_angle(x):
return (x + math.pi) % (2 * math.pi) - math.pi
# sub pd-controller sans rise
rot = None
# Get tf from /enu to /base_link
# Since we are dealing with differences and not absolute positions not translation is required
try:
(_,
rot) = self.tf_listener.lookupTransform('/base_link', '/enu',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn('Tf exception: ' + str(e))
if rot is not None:
# Convert to euler angle ( we only care about rotation about z)
theta = quat_to_rotvec(numpy.array(rot))[2]
# Difference in /enu frame
to_desired_state = self.desired_state[0:2] - self.state[0:2]
# Convert to base_link
s = numpy.sin(theta)
c = numpy.cos(theta)
rot_matrix_enu_base = numpy.array([[c, -s], [s, c]])
to_desired_state = rot_matrix_enu_base.dot(to_desired_state)
to_desired_state = numpy.insert(to_desired_state, 2,
0) # Append a zero for z
# Note: Angular differences are the same in base_link as enu so no tf required
e = numpy.concatenate([
to_desired_state,
map(smallest_coterminal_angle,
self.desired_state[3:6] - self.state[3:6])
]) # e_1 in paper
# Convert desired state dot into base_link
# angular velocities do not rquire transformation as they are differences (rendering them frameless)
# To convert velocites from enu + desired_orientation to base link:
# transform to enu: rotate the velocity vector by desired_orientation
# transform to base link: rotate the velocity vector by the angle between enu and base_link
vels = self.desired_state_dot[0:2]
theta2 = self.desired_state[5]
s = numpy.sin(theta2)
c = numpy.cos(theta2)
rot_matrix_desired_enu = numpy.array([[c, -s], [s, c]])
vels = rot_matrix_desired_enu.dot(vels)
vels = rot_matrix_enu_base.dot(vels)
vels = numpy.insert(vels, 2, 0)
desired_state_dot = numpy.concatenate(
[vels, self.desired_state_dot[3:6]])
#print 'Desired_state tf: ', desired_state_dot
e_dot = desired_state_dot - self.state_dot
output = self.K_p.dot(e) + self.K_d.dot(e_dot)
# Apply simple moving average filter
new = output / self.num_to_avg
old = (self.outputs[0] / self.num_to_avg)
self.last_average = self.last_average - old + new
self.outputs.popleft()
self.outputs.append(output)
# Resuse output var
#print 'Outputs: ', self.outputs
output = self.last_average
#print 'Last Average: ', output
self.lock.release()
self.x_error = e[0]
self.y_error = e[1]
self.z_error = e[5]
self.dx_error = e_dot[0]
self.dy_error = e_dot[1]
self.dz_error = e_dot[5]
#self.to_terminal()
if (not (self.odom_active)):
output = [0, 0, 0, 0, 0, 0]
if (self.enable & self.killed == False):
self.controller_wrench.publish(
WrenchStamped(header=Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force=Vector3(x=output[0],
y=output[1],
z=0),
torque=Vector3(x=0, y=0, z=output[5]),
)))
if ((self.x_error < 1) & (self.y_error < 1) &
(self.z_error < 1)):
self.waypoint_progress.publish(True)
if (self.killed == True):
rospy.logwarn('PD_Controller KILLED: %s' %
self.kill_listener.get_kills())
self.controller_wrench.publish(
WrenchStamped(header=Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force=Vector3(x=0, y=0, z=0),
torque=Vector3(x=0, y=0, z=0),
)))
else:
self.lock.release()
def timeout_callback(self, event):
self.odom_active = False
def to_terminal(self):
print "X: ", self.x_error
print "Y: ", self.y_error
print "Z: ", self.z_error
print "dX: ", self.dx_error
print "dY: ", self.dy_error
print "dZ: ", self.dz_error
class Controller(object):
def __init__(self):
# Old gains
self.d_x = 175
self.d_y = 90
self.d_z = 60
self.p_x = 30
self.p_y = 30
self.p_z = 40
#self.d_x = 175
#self.d_y = 90
#self.d_z = 90
#self.p_x = 40
#self.p_y = 40
#self.p_z = 100
self.killed = False
self.enable = True
self.odom_active = False
self.K_d = numpy.array([
[self.d_x,0,0,0,0,0],
[0,self.d_y,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,self.d_z]])
self.K_p = numpy.array([
[self.p_x,0,0,0,0,0],
[0,self.p_y,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,self.p_z]])
# Averaging parameters
self.last_average = numpy.zeros(6)
self.num_to_avg = 75 # At 50 hz this is 50 samples is one second of data
self.outputs = deque([numpy.zeros(6) for i in range(0, self.num_to_avg)])
self.desired_state_set = False
self.desired_state = numpy.ones(6)
self.desired_state_dot = numpy.ones(6)
self.state = numpy.ones(6)
self.previous_error = numpy.ones(6)
self.state_dot = numpy.ones(6)
self.state_dot_body = numpy.ones(6)
self.lock = threading.Lock()
# Get tf listener
self.tf_listener = tf.TransformListener()
rospy.Subscriber("pd_d_gain", Point, self.d_gain_callback)
rospy.Subscriber("pd_p_gain", Point, self.p_gain_callback)
rospy.Subscriber('/trajectory', PoseTwistStamped, self.desired_state_callback)
rospy.Subscriber('/odom', Odometry, self.odom_callback)
self.controller_wrench = rospy.Publisher('wrench', WrenchStamped, queue_size = 1)
self.waypoint_progress = rospy.Publisher('waypoint_progress', Bool, queue_size = 1)
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.z_error = 0
self.x_error = 0
self.y_error = 0
self.dz_error = 0
self.dx_error = 0
self.dy_error = 0
def set_kill(self):
self.killed = True
rospy.logwarn('PD_Controller KILLED: %s' % self.kill_listener.get_kills())
def clear_kill(self):
self.killed = False
rospy.logwarn('PD_Controller ACTIVE: %s' % self.kill_listener.get_kills())
def d_gain_callback(self, msg):
self.d_x = msg.x
self.d_y = msg.y
self.d_z = msg.z
self.K_d = numpy.array([
[self.d_x,0,0,0,0,0],
[0,self.d_y,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,self.d_z]])
def p_gain_callback(self, msg):
self.p_x = msg.x
self.p_y = msg.y
self.p_z = msg.z
self.K_p = numpy.array([
[self.p_x,0,0,0,0,0],
[0,self.p_y,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,self.p_z]])
def desired_state_callback(self,desired_posetwist):
self.lock.acquire()
self.desired_state_set = True
self.desired_state = numpy.concatenate([xyz_array(desired_posetwist.posetwist.pose.position), transformations.euler_from_quaternion(xyzw_array(desired_posetwist.posetwist.pose.orientation))])
self.desired_state_dot = numpy.concatenate([xyz_array(desired_posetwist.posetwist.twist.linear), xyz_array(desired_posetwist.posetwist.twist.angular)])
self.lock.release()
def odom_callback(self, current_posetwist):
self.lock.acquire()
self.odom_active = True
# Grab position; 0 Z
pose = xyz_array(current_posetwist.pose.pose.position)
pose[2] = 0
# Grab orientation; 0 pitch and roll
orientation = numpy.array(transformations.euler_from_quaternion(xyzw_array(current_posetwist.pose.pose.orientation)))
orientation[0:2] = 0
self.state = numpy.concatenate([xyz_array(current_posetwist.pose.pose.position), transformations.euler_from_quaternion(xyzw_array(current_posetwist.pose.pose.orientation))])
self.state_dot = numpy.concatenate([xyz_array(current_posetwist.twist.twist.linear), xyz_array(current_posetwist.twist.twist.angular)])
if (not self.desired_state_set):
self.desired_state = self.state
self.desired_state_set = True
self.lock.release()
def main_loop(self, event):
self.lock.acquire()
#print 'desired state', desired_state
#print 'current_state', state
def smallest_coterminal_angle(x):
return (x + math.pi) % (2*math.pi) - math.pi
# sub pd-controller sans rise
rot = None
# Get tf from /enu to /base_link
# Since we are dealing with differences and not absolute positions not translation is required
try:
(_, rot) = self.tf_listener.lookupTransform('/base_link', '/enu', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
rospy.logwarn('Tf exception: ' + str(e))
if rot is not None:
# Convert to euler angle ( we only care about rotation about z)
theta = quat_to_rotvec(numpy.array(rot))[2]
# Difference in /enu frame
to_desired_state = self.desired_state[0:2] - self.state[0:2]
# Convert to base_link
s = numpy.sin(theta)
c = numpy.cos(theta)
rot_matrix_enu_base = numpy.array([[c, -s],
[s, c]])
to_desired_state = rot_matrix_enu_base.dot(to_desired_state)
to_desired_state = numpy.insert(to_desired_state, 2, 0) # Append a zero for z
# Note: Angular differences are the same in base_link as enu so no tf required
e = numpy.concatenate([to_desired_state, map(smallest_coterminal_angle, self.desired_state[3:6] - self.state[3:6])]) # e_1 in paper
# Convert desired state dot into base_link
# angular velocities do not rquire transformation as they are differences (rendering them frameless)
# To convert velocites from enu + desired_orientation to base link:
# transform to enu: rotate the velocity vector by desired_orientation
# transform to base link: rotate the velocity vector by the angle between enu and base_link
vels = self.desired_state_dot[0:2]
theta2 = self.desired_state[5]
s = numpy.sin(theta2)
c = numpy.cos(theta2)
rot_matrix_desired_enu = numpy.array([[c, -s],
[s, c]])
vels = rot_matrix_desired_enu.dot(vels)
vels = rot_matrix_enu_base.dot(vels)
vels = numpy.insert(vels, 2, 0)
desired_state_dot = numpy.concatenate([vels, self.desired_state_dot[3:6]])
#print 'Desired_state tf: ', desired_state_dot
e_dot = desired_state_dot - self.state_dot
output = self.K_p.dot(e) + self.K_d.dot(e_dot)
# Apply simple moving average filter
new = output / self.num_to_avg
old = (self.outputs[0] / self.num_to_avg)
self.last_average = self.last_average - old + new
self.outputs.popleft()
self.outputs.append(output)
# Resuse output var
#print 'Outputs: ', self.outputs
output = self.last_average
#print 'Last Average: ', output
self.lock.release()
self.x_error = e[0]
self.y_error = e[1]
self.z_error = e[5]
self.dx_error = e_dot[0]
self.dy_error = e_dot[1]
self.dz_error = e_dot[5]
#self.to_terminal()
if (not(self.odom_active)):
output = [0,0,0,0,0,0]
if (self.enable & self.killed==False):
self.controller_wrench.publish(WrenchStamped(
header = Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force = Vector3(x= output[0],y= output[1],z= 0),
torque = Vector3(x=0,y= 0,z= output[5]),
))
)
if((self.x_error < 1) & (self.y_error < 1) & (self.z_error < 1)):
self.waypoint_progress.publish(True)
if (self.killed == True):
rospy.logwarn('PD_Controller KILLED: %s' % self.kill_listener.get_kills())
self.controller_wrench.publish(WrenchStamped(
header = Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force = Vector3(x= 0,y= 0,z= 0),
torque = Vector3(x=0,y= 0,z= 0),
))
)
else:
self.lock.release()
def timeout_callback(self, event):
self.odom_active = False
def to_terminal(self):
print "X: ", self.x_error
print "Y: ", self.y_error
print "Z: ", self.z_error
print "dX: ", self.dx_error
print "dY: ", self.dy_error
print "dZ: ", self.dz_error
