def update(self):
ref=np.zeros(3)
u=np.zeros(5)
kp=1
ki=1
kd=0.1
t=0.01
pid=PIDRegulator(kp,ki,kd)
sub_pose=rospy.Subscriber('g500/pose',Pose,self.pose_callback)
sub_vel=rospy.Subscriber('g500/dvl',DVL,self.vel_callback)
pub_acc=rospy.Publisher('g500/thrusters_input', Float64MultiArray, queue_size=10)
e_pos=self.ref-self.cur_pos
cmd_vel=pid.regulate(e_pos,t)
e_vel=cmd_vel-self.cur_vel
cmd_acc=pid.regulate(e_vel,t)
u[0]=-cmd_acc[1]
u[1]=-cmd_acc[1]
u[2]=-cmd_acc[2]
u[3]=-cmd_acc[2]
u[4]=-cmd_acc[0]
msg=Float64MultiArray()
msg.data=u
pub_acc.publish(msg)
print self.cur_pos
class AutopilotPID:
def __init__(self):
# PIDRegulator(p, i, d, sat)
self.pid = PIDRegulator(25, 0.024, 3.5, 5.0)
def updateGains(self, p, i, d, sat):
self.pid.p = p
self.pid.i = i
self.pid.d = d
self.pid.sat = sat
def headingController(self, psi_d, psi, t):
# error ENU
e_rot = psi_d - psi
# regulate(err, t)
tau = self.pid.regulate(e_rot, t)
return tau
def depthController(self, z_d, z, t):
e = z_d - z
tau = self.pid.regulate(e, t)
return tau
class CameraPID:
"""
(0,0) increase->
----------------> X
|
|
| increase
| |
| v
v
Y
"""
def __init__(self):
self.sway = PIDRegulator(0.01, 0.0001, 0.0, 7.5)
#self.heading = PIDRegulator(0.02, 0.0, 0.0, 0.15)
self.heading = PIDRegulator(0.15, 0.002, 0.0, 0.25)
self.depth = PIDRegulator(25, 0.024, 3.5, 5.0)
self.speed = PIDRegulator(25, 0.024, 3.5, 5.0)
def swayController(self, px_d, px, t):
# error
e = px_d - px
tau = self.sway.regulate(e, t)
return tau
def depthController(self, z_d, z, t):
e = z_d - z
tau = self.depth.regulate(e, t)
return tau
def speedController(self, u_d, u, t):
e = u_d - u
tau = self.speed.regulate(e, t)
return tau
def headingController(self, psi_d, psi, t):
# error ENU
e_rot = psi_d - psi
# regulate(err, t)
tau = self.heading.regulate(e_rot, t)
return tau
class VelocityControllerNode:
def __init__(self):
print('VelocityControllerNode: initializing node')
self.config = {}
self.v_linear_des = numpy.zeros(3)
self.v_angular_des = numpy.zeros(3)
# Initialize pids with default parameters
self.pid_angular = PIDRegulator(1, 0, 0, 1)
self.pid_linear = PIDRegulator(1, 0, 0, 1)
# ROS infrastructure
self.listener = tf.TransformListener()
self.sub_cmd_vel = rospy.Subscriber('cmd_vel',
numpy_msg(geometry_msgs.Twist),
self.cmd_vel_callback)
self.sub_odometry = rospy.Subscriber('odom', numpy_msg(Odometry),
self.odometry_callback)
self.pub_cmd_accel = rospy.Publisher('cmd_accel',
geometry_msgs.Accel,
queue_size=10)
self.srv_reconfigure = Server(VelocityControlConfig,
self.config_callback)
def cmd_vel_callback(self, msg):
"""Handle updated set velocity callback."""
# Just store the desired velocity. The actual control runs on odometry callbacks
v_l = msg.linear
v_a = msg.angular
self.v_linear_des = numpy.array([v_l.x, v_l.y, v_l.z])
self.v_angular_des = numpy.array([v_a.x, v_a.y, v_a.z])
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
linear = msg.twist.twist.linear
angular = msg.twist.twist.angular
v_linear = numpy.array([linear.x, linear.y, linear.z])
v_angular = numpy.array([angular.x, angular.y, angular.z])
if self.config['odom_vel_in_world']:
# This is a temp. workaround for gazebo's pos3d plugin not behaving properly:
# Twist should be provided wrt child_frame, gazebo provides it wrt world frame
# see http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html
xyzw_array = lambda o: numpy.array([o.x, o.y, o.z, o.w])
q_wb = xyzw_array(msg.pose.pose.orientation)
R_bw = trans.quaternion_matrix(q_wb)[0:3, 0:3].transpose()
v_linear = R_bw.dot(v_linear)
v_angular = R_bw.dot(v_angular)
# Compute compute control output:
t = msg.header.stamp.to_sec()
e_v_linear = (self.v_linear_des - v_linear)
e_v_angular = (self.v_angular_des - v_angular)
a_linear = self.pid_linear.regulate(e_v_linear, t)
a_angular = self.pid_angular.regulate(e_v_angular, t)
# Convert and publish accel. command:
cmd_accel = geometry_msgs.Accel()
cmd_accel.linear = geometry_msgs.Vector3(*a_linear)
cmd_accel.angular = geometry_msgs.Vector3(*a_angular)
self.pub_cmd_accel.publish(cmd_accel)
def config_callback(self, config, level):
"""Handle updated configuration values."""
# config has changed, reset PID controllers
self.pid_linear = PIDRegulator(config['linear_p'], config['linear_i'],
config['linear_d'],
config['linear_sat'])
self.pid_angular = PIDRegulator(config['angular_p'],
config['angular_i'],
config['angular_d'],
config['angular_sat'])
self.config = config
return config
class VelocityControllerNode:
def __init__(self):
print('VelocityControllerNode: initializing node')
self.config = {}
self.v_linear_des = numpy.zeros(3)
self.v_angular_des = numpy.zeros(3)
# Initialize pids with default parameters
self.pid_angular = PIDRegulator(1, 0, 0, 1)
self.pid_linear = PIDRegulator(1, 0, 0, 1)
# ROS infrastructure
self.sub_cmd_vel = rospy.Subscriber('cmd_vel', numpy_msg(geometry_msgs.Twist), self.cmd_vel_callback)
self.sub_odometry = rospy.Subscriber('odom', numpy_msg(Odometry), self.odometry_callback)
self.pub_cmd_accel = rospy.Publisher('cmd_accel', geometry_msgs.Accel, queue_size=10)
self.srv_reconfigure = Server(VelocityControlConfig, self.config_callback)
def cmd_vel_callback(self, msg):
"""Handle updated set velocity callback."""
# Just store the desired velocity. The actual control runs on odometry callbacks
v_l = msg.linear
v_a = msg.angular
self.v_linear_des = numpy.array([v_l.x, v_l.y, v_l.z])
self.v_angular_des = numpy.array([v_a.x, v_a.y, v_a.z])
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
linear = msg.twist.twist.linear
angular = msg.twist.twist.angular
v_linear = numpy.array([linear.x, linear.y, linear.z])
v_angular = numpy.array([angular.x, angular.y, angular.z])
if self.config['odom_vel_in_world']:
# This is a temp. workaround for gazebo's pos3d plugin not behaving properly:
# Twist should be provided wrt child_frame, gazebo provides it wrt world frame
# see http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html
xyzw_array = lambda o: numpy.array([o.x, o.y, o.z, o.w])
q_wb = xyzw_array(msg.pose.pose.orientation)
R_bw = trans.quaternion_matrix(q_wb)[0:3, 0:3].transpose()
v_linear = R_bw.dot(v_linear)
v_angular = R_bw.dot(v_angular)
# Compute compute control output:
t = msg.header.stamp.to_sec()
e_v_linear = (self.v_linear_des - v_linear)
e_v_angular = (self.v_angular_des - v_angular)
a_linear = self.pid_linear.regulate(e_v_linear, t)
a_angular = self.pid_angular.regulate(e_v_angular, t)
# Convert and publish accel. command:
cmd_accel = geometry_msgs.Accel()
cmd_accel.linear = geometry_msgs.Vector3(*a_linear)
cmd_accel.angular = geometry_msgs.Vector3(*a_angular)
self.pub_cmd_accel.publish(cmd_accel)
def config_callback(self, config, level):
"""Handle updated configuration values."""
# config has changed, reset PID controllers
self.pid_linear = PIDRegulator(config['linear_p'], config['linear_i'], config['linear_d'], config['linear_sat'])
self.pid_angular = PIDRegulator(config['angular_p'], config['angular_i'], config['angular_d'], config['angular_sat'])
self.config = config
return config
class PositionControllerNode:
def __init__(self):
print('PositionControllerNode: initializing node')
self.config = {}
self.pos_des = numpy.zeros(3)
self.quat_des = numpy.array([0, 0, 0, 1])
self.initialized = False
# Initialize pids with default parameters
self.pid_rot = PIDRegulator(1, 0, 0, 1)
self.pid_pos = PIDRegulator(1, 0, 0, 1)
# ROS infrastructure
self.sub_cmd_pose = rospy.Subscriber('cmd_pose', numpy_msg(geometry_msgs.PoseStamped), self.cmd_pose_callback)
self.sub_odometry = rospy.Subscriber('odom', numpy_msg(Odometry), self.odometry_callback)
self.pub_cmd_vel = rospy.Publisher('cmd_vel', geometry_msgs.Twist, queue_size=10)
self.srv_reconfigure = Server(PositionControlConfig, self.config_callback)
def cmd_pose_callback(self, msg):
"""Handle updated set pose callback."""
# Just store the desired pose. The actual control runs on odometry callbacks
p = msg.pose.position
q = msg.pose.orientation
self.pos_des = numpy.array([p.x, p.y, p.z])
self.quat_des = numpy.array([q.x, q.y, q.z, q.w])
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
p = msg.pose.pose.position
q = msg.pose.pose.orientation
p = numpy.array([p.x, p.y, p.z])
q = numpy.array([q.x, q.y, q.z, q.w])
if not self.initialized:
# If this is the first callback: Store and hold latest pose.
self.pos_des = p
self.quat_des = q
self.initialized = True
# Compute control output:
t = msg.header.stamp.to_sec()
# Position error
e_pos_world = self.pos_des - p
e_pos_body = trans.quaternion_matrix(q).transpose()[0:3,0:3].dot(e_pos_world)
# Error quaternion wrt body frame
e_rot_quat = trans.quaternion_multiply(trans.quaternion_conjugate(q), self.quat_des)
if numpy.linalg.norm(e_pos_world[0:2]) > 5.0:
# special case if we are far away from goal:
# ignore desired heading, look towards goal position
heading = math.atan2(e_pos_world[1],e_pos_world[0])
quat_des = numpy.array([0, 0, math.sin(0.5*heading), math.cos(0.5*heading)])
e_rot_quat = trans.quaternion_multiply(trans.quaternion_conjugate(q), quat_des)
# Error angles
e_rot = numpy.array(trans.euler_from_quaternion(e_rot_quat))
v_linear = self.pid_pos.regulate(e_pos_body, t)
v_angular = self.pid_rot.regulate(e_rot, t)
# Convert and publish vel. command:
cmd_vel = geometry_msgs.Twist()
cmd_vel.linear = geometry_msgs.Vector3(*v_linear)
cmd_vel.angular = geometry_msgs.Vector3(*v_angular)
self.pub_cmd_vel.publish(cmd_vel)
def config_callback(self, config, level):
"""Handle updated configuration values."""
# Config has changed, reset PID controllers
self.pid_pos = PIDRegulator(config['pos_p'], config['pos_i'], config['pos_d'], config['pos_sat'])
self.pid_rot = PIDRegulator(config['rot_p'], config['rot_i'], config['rot_d'], config['rot_sat'])
self.config = config
return config
class VelocityControllerNode:
def __init__(self):
print('VelocityControllerNode: initializing node')
self.config = {}
self.v_linear_des = numpy.zeros(3)
self.v_angular_des = numpy.zeros(3)
# Initialize pids with default parameters
self.pid_angular = PIDRegulator(1, 0, 0, 1)
self.pid_linear = PIDRegulator(1, 0, 0, 1)
# ROS infrastructure
self.sub_cmd_vel = rospy.Subscriber('/wamv/cmd_vel',
numpy_msg(geometry_msgs.Twist),
self.cmd_vel_callback)
self.sub_odometry = rospy.Subscriber('/wamv/pose_gt',
numpy_msg(Odometry),
self.odometry_callback)
self.pub_cmd_accel = rospy.Publisher('/wamv/cmd_accel',
geometry_msgs.Accel,
queue_size=10)
self.srv_reconfigure = Server(VelocityControlConfig,
self.config_callback)
def cmd_vel_callback(self, msg):
"""Handle updated set velocity callback."""
# Just store the desired velocity. The actual control runs on odometry callbacks
v_l = msg.linear
v_a = msg.angular
self.v_linear_des = numpy.array([v_l.x, v_l.y, v_l.z])
self.v_angular_des = numpy.array([v_a.x, v_a.y, v_a.z])
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
linear = msg.twist.twist.linear
angular = msg.twist.twist.angular
v_linear = numpy.array([linear.x, linear.y, linear.z])
v_angular = numpy.array([angular.x, angular.y, angular.z])
# Compute compute control output:
t = msg.header.stamp.to_sec()
e_v_linear = (self.v_linear_des - v_linear)
e_v_angular = (self.v_angular_des - v_angular)
a_linear = self.pid_linear.regulate(e_v_linear, t)
a_angular = self.pid_angular.regulate(e_v_angular, t)
# Convert and publish accel. command:
cmd_accel = geometry_msgs.Accel()
cmd_accel.linear = geometry_msgs.Vector3(*a_linear)
cmd_accel.angular = geometry_msgs.Vector3(*a_angular)
self.pub_cmd_accel.publish(cmd_accel)
def config_callback(self, config, level):
"""Handle updated configuration values."""
# config has changed, reset PID controllers
self.pid_linear = PIDRegulator(config['linear_p'], config['linear_i'],
config['linear_d'],
config['linear_sat'])
self.pid_angular = PIDRegulator(config['angular_p'],
config['angular_i'],
config['angular_d'],
config['angular_sat'])
self.config = config
return config
class PositionControllerNode:
def __init__(self):
print('PositionControllerNode: initializing node')
self.config = {}
self.pos_des = numpy.zeros(3)
self.quat_des = numpy.array([0, 0, 0, 1])
self.initialized = False
# Initialize pids with default parameters
self.pid_rot = PIDRegulator(1, 0, 0, 1)
self.pid_pos = PIDRegulator(1, 0, 0, 1)
# ROS infrastructure
self.listener = tf.TransformListener()
self.sub_cmd_pose = rospy.Subscriber(
'cmd_pose', numpy_msg(geometry_msgs.PoseStamped),
self.cmd_pose_callback)
self.sub_odometry = rospy.Subscriber('odom', numpy_msg(Odometry),
self.odometry_callback)
self.pub_cmd_vel = rospy.Publisher('cmd_vel',
geometry_msgs.Twist,
queue_size=10)
self.srv_reconfigure = Server(PositionControlConfig,
self.config_callback)
def cmd_pose_callback(self, msg):
"""Handle updated set pose callback."""
# Just store the desired pose. The actual control runs on odometry callbacks
p = msg.pose.position
q = msg.pose.orientation
self.pos_des = numpy.array([p.x, p.y, p.z])
self.quat_des = numpy.array([q.x, q.y, q.z, q.w])
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
p = msg.pose.pose.position
q = msg.pose.pose.orientation
p = numpy.array([p.x, p.y, p.z])
q = numpy.array([q.x, q.y, q.z, q.w])
if not self.initialized:
# If this is the first callback: Store and hold latest pose.
self.pos_des = p
self.quat_des = q
self.initialized = True
# Compute control output:
t = msg.header.stamp.to_sec()
# Position error wrt. body frame
e_pos = trans.quaternion_matrix(q).transpose()[0:3,
0:3].dot(self.pos_des -
p)
# Error quaternion wrt body frame
e_rot_quat = trans.quaternion_multiply(trans.quaternion_conjugate(q),
self.quat_des)
# Error angles
e_rot = numpy.array(trans.euler_from_quaternion(e_rot_quat))
v_linear = self.pid_pos.regulate(e_pos, t)
v_angular = self.pid_rot.regulate(e_rot, t)
# Convert and publish vel. command:
cmd_vel = geometry_msgs.Twist()
cmd_vel.linear = geometry_msgs.Vector3(*v_linear)
cmd_vel.angular = geometry_msgs.Vector3(*v_angular)
self.pub_cmd_vel.publish(cmd_vel)
def config_callback(self, config, level):
"""Handle updated configuration values."""
# Config has changed, reset PID controllers
self.pid_pos = PIDRegulator(config['pos_p'], config['pos_i'],
config['pos_d'], config['pos_sat'])
self.pid_rot = PIDRegulator(config['rot_p'], config['rot_i'],
config['rot_d'], config['rot_sat'])
self.config = config
return config
class ROV_CascadedController(DPControllerBase):
_LABEL = 'Cascaded PID dynamic position controller'
def __init__(self):
DPControllerBase.__init__(self,
is_model_based=False,
planner_full_dof=False)
self._logger.info('Initializing: ' + self._LABEL)
self._force_torque = np.zeros(6)
self.odom_pos = np.zeros(3)
self.odom_quat = np.array([0, 0, 0, 1])
self.odom_vel_ang = np.array(3)
self.odom_vel_lin = np.array(3)
self._logger.info(self._LABEL + ' ready')
self._mass = rospy.get_param("pid/mass")
print self._mass
self._inertial = rospy.get_param("pid/inertial")
print self._inertial
self._use_cascaded_pid = rospy.get_param("use_cascaded_pid", True)
self._last_vel = np.zeros(6)
# update mass, moments of inertia
self._inertial_tensor = np.array([[
self._inertial['ixx'], self._inertial['ixy'], self._inertial['ixz']
], [
self._inertial['ixy'], self._inertial['iyy'], self._inertial['iyz']
], [
self._inertial['ixz'], self._inertial['iyz'], self._inertial['izz']
]])
self._mass_inertial_matrix = np.vstack((np.hstack(
(self._mass * np.identity(3), np.zeros(
(3, 3)))), np.hstack((np.zeros(
(3, 3)), self._inertial_tensor))))
self._velocity_pid = rospy.get_param('velocity_control')
print self._velocity_pid
self._position_pid = rospy.get_param('position_control')
print self._position_pid
self._lin_vel_pid_reg = PIDRegulator(self._velocity_pid['linear_p'], \
self._velocity_pid['linear_i'], \
self._velocity_pid['linear_d'], \
self._velocity_pid['linear_sat'])
self._ang_vel_pid_reg = PIDRegulator(self._velocity_pid['angular_p'], \
self._velocity_pid['angular_i'], \
self._velocity_pid['angular_d'], \
self._velocity_pid['angular_sat'])
self._lin_pos_pid_reg = PIDRegulator(self._position_pid['pos_p'], \
self._position_pid['pos_i'], \
self._position_pid['pos_d'], \
self._position_pid['pos_sat'])
self._ang_pos_pid_reg = PIDRegulator(self._position_pid['rot_p'], \
self._position_pid['rot_i'], \
self._position_pid['rot_d'], \
self._position_pid['rot_sat'])
self.sub_odometry = rospy.Subscriber('/anahita/pose_gt',
numpy_msg(Odometry),
self.o_callback)
self.cmd_pose_pub = rospy.Publisher('/anahita/cmd_pose',
Pose,
queue_size=10)
self._logger.info('Cascaded PID controller ready!')
self._last_t = None
self._is_init = True
def _reset_controller(self):
super(ROV_CascadedController, self).reset_controller()
self._force_torque = np.zeros(6)
def update_controller(self):
if not self._is_init:
return False
t = rospy.get_time()
if self._last_t is None:
self._last_t = t
self._last_vel = self._vehicle_model._vel
return False
dt = t - self._last_t
if dt <= 0:
self._last_t = t
self._last_vel = self._vehicle_model._vel
return False
vel = np.zeros(6)
vel = self._vehicle_model._vel
acc = np.zeros(6)
acc = (vel - self._last_vel) / dt
p = self._vehicle_model._pose['pos']
q = self._vehicle_model._pose['rot']
# Compute control output:
t = rospy.get_rostime().to_sec()
# Position error
e_pos_world = self._reference['pos'] - p
e_pos_body = trans.quaternion_matrix(q).transpose()[0:3, 0:3].dot(
e_pos_world)
# Error quaternion wrt body frame
e_rot_quat = trans.quaternion_multiply(trans.quaternion_conjugate(q),
self._reference['rot'])
if np.linalg.norm(e_pos_world[0:2]) > 5.0:
# special case if we are far away from goal:
# ignore desired heading, look towards goal position
heading = math.atan2(e_pos_world[1], e_pos_world[0])
quat_des = np.array(
[0, 0, math.sin(0.5 * heading),
math.cos(0.5 * heading)])
e_rot_quat = trans.quaternion_multiply(
trans.quaternion_conjugate(q), quat_des)
# Error angles
e_rot = np.array(trans.euler_from_quaternion(e_rot_quat))
v_linear = self._lin_pos_pid_reg.regulate(e_pos_body, t)
v_angular = self._ang_pos_pid_reg.regulate(e_rot, t)
e_linear_vel = v_linear - np.array([vel[0], vel[1], vel[2]])
e_angular_vel = v_angular - np.array([vel[3], vel[4], vel[5]])
a_linear = self._lin_vel_pid_reg.regulate(e_linear_vel, t)
a_angular = self._ang_vel_pid_reg.regulate(e_angular_vel, t)
accel = np.hstack((a_linear, a_angular)).transpose()
if self._use_cascaded_pid:
self._force_torque = self._mass_inertial_matrix.dot(accel)
else:
self._force_torque = self._vehicle_model.compute_force(acc, vel)
cmd_pose = Pose()
cmd_pose.position.x = self._reference['pos'][0]
cmd_pose.position.y = self._reference['pos'][1]
cmd_pose.position.z = self._reference['pos'][2]
cmd_pose.orientation.x = self._reference['rot'][0]
cmd_pose.orientation.y = self._reference['rot'][1]
cmd_pose.orientation.z = self._reference['rot'][2]
cmd_pose.orientation.w = self._reference['rot'][3]
self.cmd_pose_pub.publish(cmd_pose)
return True
# Publish control forces and torques
self.publish_control_wrench(self._force_torque)
self._last_t = t
self._last_vel = self._vehicle_model._vel
return True
def saturate(self):
for i in range(0, 6):
if (self._force_torque[i] > 1500):
self._force_torque[i] = 1500
if (self._force_torque[i] < -1500):
self._force_torque[i] = -1500
def o_callback(self, msg):
p = msg.pose.pose.position
q = msg.pose.pose.orientation
u = msg.twist.twist.linear
w = msg.twist.twist.angular
self.odom_pos = np.array([p.x, p.y, p.z])
self.odom_quat = np.array([q.x, q.y, q.z, q.w])
self.odom_vel_lin = np.array([u.x, u.y, u.z])
self.odom_vel_ang = np.array([w.x, u.y, u.z])
class PositionControllerNode:
def __init__(self):
print('PositionControllerNode: initializing node')
self.config = {}
self.pos_des = numpy.zeros(3)
self.quat_des = numpy.array([0, 0, 0, 1])
self.initialized = False
# Initialize pids with default parameters
self.pid_rot = PIDRegulator(1, 0, 0, 1)
self.pid_pos = PIDRegulator(1, 0, 0, 1)
# ROS infrastructure
self.sub_cmd_pose = rospy.Subscriber('/wamv/cmd_pose',
numpy_msg(geometry_msgs.Pose),
self.cmd_pose_callback)
self.sub_odometry = rospy.Subscriber('/wamv/pose_gt',
numpy_msg(Odometry),
self.odometry_callback)
self.pub_cmd_vel = rospy.Publisher('/wamv/cmd_vel',
geometry_msgs.Twist,
queue_size=10)
self.srv_reconfigure = Server(PositionControlConfig,
self.config_callback)
def cmd_pose_callback(self, msg):
"""Handle updated set pose callback."""
# Just store the desired pose. The actual control runs on odometry callbacks
p = msg.position
q = msg.orientation
self.pos_des = numpy.array([p.x, p.y, p.z])
self.quat_des = numpy.array([q.x, q.y, q.z, q.w])
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
p = msg.pose.pose.position
q = msg.pose.pose.orientation
p = numpy.array([p.x, p.y, p.z])
q = numpy.array([q.x, q.y, q.z, q.w])
if not self.initialized:
# If this is the first callback: Store and hold latest pose.
self.pos_des = p
self.quat_des = q
self.initialized = True
# Compute control output:
t = msg.header.stamp.to_sec()
# Position error
e_pos_world = self.pos_des - p
e_pos_body = trans.quaternion_matrix(q).transpose()[0:3, 0:3].dot(
e_pos_world)
# Error quaternion wrt body frame
e_rot_quat = trans.quaternion_multiply(trans.quaternion_conjugate(q),
self.quat_des)
if numpy.linalg.norm(e_pos_world[0:2]) > 5.0:
# special case if we are far away from goal:
# ignore desired heading, look towards goal position
heading = math.atan2(e_pos_world[1], e_pos_world[0])
quat_des = numpy.array(
[0, 0, math.sin(0.5 * heading),
math.cos(0.5 * heading)])
e_rot_quat = trans.quaternion_multiply(
trans.quaternion_conjugate(q), quat_des)
# Error angles
e_rot = numpy.array(trans.euler_from_quaternion(e_rot_quat))
# print ('error pos: {}, error quat: {}'.format(e_pos_body, e_rot))
v_linear = self.pid_pos.regulate(e_pos_body, t)
v_angular = self.pid_rot.regulate(e_rot, t)
# Convert and publish vel. command:
cmd_vel = geometry_msgs.Twist()
cmd_vel.linear = geometry_msgs.Vector3(*v_linear)
cmd_vel.angular = geometry_msgs.Vector3(*v_angular)
self.pub_cmd_vel.publish(cmd_vel)
def config_callback(self, config, level):
"""Handle updated configuration values."""
# Config has changed, reset PID controllers
self.pid_pos = PIDRegulator(config['pos_p'], config['pos_i'],
config['pos_d'], config['pos_sat'])
self.pid_rot = PIDRegulator(config['rot_p'], config['rot_i'],
config['rot_d'], config['rot_sat'])
self.config = config
return config
class controlPID:
def __init__(self, x, y, z):
self.cur_pos = np.array([0, 0, 0])
self.cur_vel = np.array([0, 0, 0])
self.ref = np.array([x, y, z])
self.pidp = PIDRegulator(0.75, 0, 0)
self.pidv = PIDRegulator(1, 0, 0)
self.u = np.zeros(5)
## This function publish acceleration for each thruster on /g500/thrusters_input.
# It subscribes from /g500/dvl to get velocity and /g500/pose to get veloctiy
# and use cascaded PID controller to move the bot to desired co-ordinates
#
def update(self):
sub_pose = rospy.Subscriber('g500/pose',
Pose,
self.pose_callback,
queue_size=1)
sub_vel = rospy.Subscriber('g500/dvl', DVL, self.vel_callback)
pub_acc = rospy.Publisher('g500/thrusters_input',
Float64MultiArray,
queue_size=1000)
e_pos = np.zeros(3)
e_vel = np.zeros(3)
cmd_vel = np.zeros(3)
cmd_acc = np.zeros(3)
dt = 0.01
#cascaded PID-controller
for i in range(3):
e_pos[i] = self.ref[i] - self.cur_pos[i]
cmd_vel[i] = self.pidp.regulate(e_pos[i], dt)
e_vel[i] = cmd_vel[i] - self.cur_vel[i]
cmd_acc[i] = self.pidv.regulate(e_vel[i], dt)
self.u[0] = -cmd_acc[1]
self.u[1] = -cmd_acc[1]
self.u[2] = -cmd_acc[2]
self.u[3] = -cmd_acc[2]
self.u[4] = -cmd_acc[0]
msg = Float64MultiArray()
msg.data = self.u
pub_acc.publish(msg)
print cmd_acc
#velocity subscriber callback
def vel_callback(self, msg):
self.cur_vel = np.array([msg.bi_x_axis, msg.bi_y_axis, msg.bi_z_axis])
#rounding of velocity to one decimal place to neglect disturbances
for i in range(3):
self.cur_vel[i] = round(self.cur_vel[i], 1)
#postion subscriber callback
def pose_callback(self, msg):
self.cur_pos = np.array(
[msg.position.x, msg.position.y, msg.position.z])
#rounding of velocity to one decimal place to neglect disturbances
for i in range(3):
self.cur_pos[i] = round(self.cur_pos[i], 1)
