#!/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 w
ith 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 wr
iting, software
# distributed under the License is distributed on an "
AS IS" B
ASIS,
# W
ITHOUT WARRANTIES OR COND
ITIONS OF ANY K
IND, e
ither express or implied
.
# See the License for the specific language govern
ing permissions and
# lim
itations 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 pos
ition
ing controller b
ase cl
ass to
inher
it methods such
as error computation update,
# publish
ing from some important ROS topics
(e
.g
. trajectory, pose and veloc
ity reference
) and access to
# the vehicle model cl
ass, that is necessary to explic
itly use the vehicle model
from uuv_control_
interfaces import DPControllerB
ase
cl
ass TutorialDPController
(DPControllerB
ase
):
# A new controller that is b
ased on the DPControllerB
ase must at le
ast 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 algor
ithm and
will be called
# by every update of the vehicle's odometry message
. It is therefore not necessary to explic
itly 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 us
ing the function
# publish_control_wrench from the super cl
ass, 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 c
ase def
ined
as e =
(x, y, z, roll, p
itch, yaw
)^T
def __
init__
(self
):
# Call
ing the constructor of the super-cl
ass DPControllerB
ase, which h
as 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 w
ith 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
ingPrivateNamesW
ithNodeHandle
# 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 simplic
ity, 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 h
as 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 h
as 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 h
as 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 cl
ass DPControllerB
ase already sets the error
# and reference vectors to zero, but this cl
ass h
as add
itional attributes that should also
#
be taken care of
.
# This implementation
will, therefore, first call the super cl
ass 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 algor
ithm must
be implemented here, the super cl
ass
will connect this method
# to the odometry update
as a callback function
# First test whether or not the odometry topic subscri
ber h
as already
been
initialized
if not self
.odom_is_
init:
return
# Update the
integrator, read the super cl
ass vector for the pose error
(orientation is represented
# w
ith Euler angles
in RPY convention
) and
integrate to the stored pose error from the l
ast
#
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 us
ing 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 cl
ass 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 us
ing the thruster allocation matrix
self
.publish_control_wrench
(tau
)
if __name__
== '__ma
in__':
# S
ince this is an ROS node, this Python script h
as to
be treated as an executable
# Remem
ber to convert this Python file
into an executable
. This can
be done w
ith
#
# cd <path_to_ros_package>/scripts
# chmod 777 tutorial_dp_controller
.py
#
# This file h
as 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
('ex
iting'
)学习这个代码，编写一个lqr控制器
本文解决了在使用h5py库时出现的FutureWarning警告问题,该警告涉及到数据类型转换的弃用行为,推荐安装指定版本的h5py来解决此问题。
扫一扫
3938
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
