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 __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)
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
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
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 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
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 __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 __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)
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 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
def __init__(self): # Initializing arm interface self._arm_interface = ArmInterface() # PID controllers self._controllers = dict() # Current reference for each joint self._reference_pos = dict() # Output command topics self._command_topics = dict() # Axes mapping self._axes = dict() # Axes gain values self._axes_gain = dict() # Default for the RB button of the XBox 360 controller self._deadman_button = 5 if rospy.has_param('~deadman_button'): self._deadman_button = int(rospy.get_param('~deadman_button')) # If these buttons are pressed, the arm will not move if rospy.has_param('~exclusion_buttons'): self._exclusion_buttons = rospy.get_param('~exclusion_buttons') if type(self._exclusion_buttons) in [float, int]: self._exclusion_buttons = [int(self._exclusion_buttons)] elif type(self._exclusion_buttons) == list: for n in self._exclusion_buttons: if type(n) not in [float, int]: raise rospy.ROSException( 'Exclusion buttons must be an' ' integer index to the joystick button') else: self._exclusion_buttons = list() # Default for the start button of the XBox 360 controller self._home_button = 7 if rospy.has_param('~home_button'): self._home_button = int(rospy.get_param('~home_button')) # Last joystick update timestamp self._last_joy_update = rospy.get_time() # Joystick topic subscriber self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_callback) # Reading the controller configuration controller_config = rospy.get_param('~controller_config') for joint in self._arm_interface.joint_names: for tag in controller_config: if tag in joint: try: # Read the controller parameters self._controllers[joint] = PIDRegulator( controller_config[tag]['controller']['p'], controller_config[tag]['controller']['i'], controller_config[tag]['controller']['d'], 1000) self._command_topics[joint] = rospy.Publisher( controller_config[tag]['topic'], Float64, queue_size=1) self._axes[joint] = controller_config[tag][ 'joint_input_axis'] self._axes_gain[joint] = controller_config[tag][ 'axis_gain'] # Setting the starting reference to the home position # in the robot parameters file self._reference_pos[joint] = deepcopy( self._arm_interface.home[joint]) except: raise rospy.ROSException( 'Error while trying to setup controller for joint <%s>' % joint) rate = rospy.Rate(50) while not rospy.is_shutdown(): pos = self._arm_interface.joint_angles for joint in pos: u = self._controllers[joint].regulate( self._reference_pos[joint] - pos[joint], rospy.get_time()) self._command_topics[joint].publish(Float64(u)) rate.sleep()
def __init__(self): # Timeout for input inactivity self._timeout = 0.01 # Initializing the gripper interface for the current namespace self._gripper = GripperInterface() # Initial values for the gripper joystick buttons # Default - B button from XBox 360 controller self._open_button = 1 if rospy.has_param('~open_button'): self._open_button = rospy.get_param('~open_button') # Default - X button from XBox 360 controller self._close_button = 2 if rospy.has_param('~close_button'): self._close_button = rospy.get_param('~close_button') # Default - RB button from XBox 360 controller self._deadman_button = 5 if rospy.has_param('~deadman_button'): self._deadman_button = rospy.get_param('~deadman_button') if rospy.has_param('~exclusion_buttons'): self._exclusion_buttons = rospy.get_param('~exclusion_buttons') if type(self._exclusion_buttons) in [float, int]: self._exclusion_buttons = [int(self._exclusion_buttons)] elif type(self._exclusion_buttons) == list: for n in self._exclusion_buttons: if type(n) != float: raise rospy.ROSException( 'Exclusion buttons must be an' ' integer index to the joystick button') else: self._exclusion_buttons = list() self._joy_gain = 0.1 if rospy.has_param('~joy_gain'): self._joy_gain = rospy.get_param('~joy_gain') # Retrieve the publish rate self._publish_rate = 100 if rospy.has_param('~publish_rate'): self._publish_rate = rospy.get_param('~publish_rate') if self._publish_rate <= 0: raise rospy.ROSException('Invalid negative publish rate') self._pos_goal = self._gripper.closed_pos self._ratio_goal = 0.0 if rospy.has_param('~kp'): self._kp = rospy.get_param('~kp') else: self._kp = 100.0 if rospy.has_param('~ki'): self._ki = rospy.get_param('~ki') else: self._ki = 0.0 if rospy.has_param('~kd'): self._kd = rospy.get_param('~kd') else: self._kd = 0.0 self._controllers = dict() self._controllers[self._gripper.control_joint] = PIDRegulator( self._kp, self._ki, self._kd, 100) self._controllers[self._gripper.mimic_joint] = PIDRegulator( self._kp, self._ki, self._kd, 100) if self._gripper.control_joint is None: raise rospy.ROSException('Gripper cannot be controlled') self._limits = self._gripper.control_joint_limits self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_callback) rate = rospy.Rate(self._publish_rate) while not rospy.is_shutdown(): self._update() rate.sleep()
def __init__(self): # PIDRegulator(p, i, d, sat) self.pid = PIDRegulator(25, 0.024, 3.5, 5.0)
def __init__(self, *args): self._regulators = [PIDRegulator(*arg) for arg in args]
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
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 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('/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 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 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)