#!/usr/b
in/env python
# Copyright (c) 2016 The UUV Simulator Authors.
# All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except
in compliance with the License.
# You may obta
in a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to
in writ
ing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY K
IND, either express or implied.
# See the License for the specific language govern
ing permissions and
# limitations under the License.
# Import the ROS Python library
from __future__ import pr
int_function
import rospy
# Import the NumPy package for numerical computations
import numpy as np
# Import the dynamic position
ing controller base class to
inherit methods such as error computation update,
# publish
ing from some important ROS topics (e.g. trajectory, pose and velocity reference) and access to
# the vehicle model class, that is necessary to explicitly use the vehicle model
from uuv_control_
interfaces import DPControllerBase
class TutorialDPController(DPControllerBase):
# A new controller that is based on the DPControllerBase must at least provide the implementation of
# the method update_controller.
# The _reset_controller method can also be overridden and it will be called every time there is a service call
# <vehicle namespace>/reset_controller. The default implementation sets the reference and error vectors to
# zero.
# The update_controller method must conta
in the implementation of the control algorithm and will be called
# by every update of the vehicle's odometry message. It is therefore not necessary to explicitly call this update
# function
in this controller implementation.
# For the controller to send the control torques to the vehicle's thruster manager, at the end of the
# update_controller function the 6 x 1 control vector (type numpy.ndarray) sent
using the function
# publish_control_wrench from the super class, which will generate a Wrench ROS message and publish it to the
# correspondent thruster manager node.
# For this tutorial, a simple PID controller will be implemented. The controller's control torque output is
# "tau" therefore computed as:
#
# tau = Kp * e + Kd * de/dt + Ki
int_e
#
# where e is the pose error vector,
in this case def
ined as e = (x, y, z, roll, pitch, yaw)^T
def __
init__(self):
# Call
ing the constructor of the super-class DPControllerBase, which has the implementation of the error
# computation update and to publish the result
ing torque control vector.
super(TutorialDPController, self).__
init__(self)
# The controller should read its
parameters from the ROS parameter server for
initial setup
# One way to do this is to read the
parameters from the node's private parameter namespace, which is done by
# read
ing the parameter tag with an "~" at the beg
inn
ing. If this method is used, the
parameters should be
#
initialized accord
ingly
in the controller startup launch file, such as
#
# <launch>
# <node pkg="example_package" type="example_node.py" name="example_node" output="screen">
# <rosparam>
# param_1: 0.0
# param_2: 0.0
# </rosparam>
# </node>
# </launch>
#
# For more
information, see http://wiki.ros.org/
roscpp_tutorials/Tutorials/Access
ingPrivateNamesWithNodeHandle
# Let's
initialize the controller ga
in matrices Kp, Kd and Ki
self._Kp = np.zeros(shape=(6, 6))
self._Kd = np.zeros(shape=(6, 6))
self._Ki = np.zeros(shape=(6, 6))
#
Initialize the
integrator component
self._
int = np.zeros(shape=(6,))
#
Initialize variable that will store the vehicle pose error
self._error_pose = np.zeros(shape=(6,))
# Now the ga
in matrices need to be set accord
ing to the variables stored
in the parameter server
# For simplicity, the ga
in matrices are def
ined as diagonal matrices, so only 6 coefficients are
# needed
if rospy.get_param('~Kp'):
Kp_diag = rospy.get_param('~Kp')
if len(Kp_diag) == 6:
self._Kp = np.diag(Kp_diag)
else:
# If the vector provided has the wrong dimension, raise an exception
raise rospy.ROSException('For the Kp diagonal matrix, 6 coefficients are needed')
# Do the same for the other two matrices
if rospy.get_param('~Kd'):
diag = rospy.get_param('~Kd')
if len(diag) == 6:
self._Kd = np.diag(diag)
pr
int('Kd=\n', self._Kd)
else:
# If the vector provided has the wrong dimension, raise an exception
raise rospy.ROSException('For the Kd diagonal matrix, 6 coefficients are needed')
if rospy.get_param('~Ki'):
diag = rospy.get_param('~Ki')
if len(diag) == 6:
self._Ki = np.diag(diag)
pr
int('Ki=\n', self._Ki)
else:
# If the vector provided has the wrong dimension, raise an exception
raise rospy.ROSException('For the Ki diagonal matrix, 6 coefficients are needed')
self._is_
init = True
def _reset_controller(self):
# The _reset_controller method from the super class DPControllerBase already sets the error
# and reference vectors to zero, but this class has additional attributes that should also
# be taken care of.
# This implementation will, therefore, first call the super class reset method
super(TutorialDPController, self)._reset_controller()
# And then proceed to set the
internal variables back to zero
self._error_pose = np.zeros(shape=(6,))
self._
int = np.zeros(shape=(6,))
def update_controller(self):
if not self._is_
init:
return False
# The controller algorithm must be implemented here, the super class will connect this method
# to the odometry update as a callback function
# First test whether or not the odometry topic subscriber has already been
initialized
if not self.odom_is_
init:
return
# Update the
integrator, read the super class vector for the pose error (orientation is represented
# with Euler angles
in RPY convention) and
integrate to the stored pose error from the last
# iteration
self._
int = self._
int + 0.5 * (self.error_pose_euler + self._error_pose) * self._dt
# Store the current pose error for the next iteration
self._error_pose = self.error_pose_euler
# Compute the control forces and torques
using the current error vectors available
tau = np.dot(self._Kp, self.error_pose_euler) + np.dot(self._Kd, self._errors['vel']) + \
np.dot(self._Ki, self._
int)
# Use the super class method to convert the control force vector
into a ROS message
# and publish it as an
input to the vehicle's thruster manager. The thruster manager module
# will then distribute the efforts amongst the thrusters
using the thruster allocation matrix
self.publish_control_wrench(tau)
if __name__ == '__ma
in__':
# S
ince this is an ROS node, this Python script has to be treated as an executable
# Remember to convert this Python file
into an executable. This can be done with
#
# cd <path_to_ros_package>/scripts
# chmod 777 tutorial_dp_controller.py
#
# This file has also to be
included
in this package's CMakeLists.txt
# After the l
ine catk
in_package()
in the CMakeLists.txt,
include the follow
ing
#
# catk
in_
install_python(PROGRAMS scripts/tutorial_dp_controller.py DEST
INATION ${CATK
IN_PACKAGE_B
IN_DEST
INATION})
pr
int('Tutorial - DP Controller')
rospy.
init_node('tutorial_dp_controller')
try:
node = TutorialDPController()
rospy.sp
in()
except rospy.ROS
InterruptException:
pr
int('caught exception')
pr
int('exit
ing')学习这个代码，编写一个lqr控制器
本文详细介绍了如何在ROS中使用C++进行参数设置与获取，包括参数服务器的基本概念、参数的读写操作以及如何在节点间共享参数，适用于ROS初学者及开发者。
扫一扫
1048
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
