dynamics.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Basic ROS imports
00004 import roslib 
00005 roslib.load_manifest('underwater_vehicle_dynamics')
00006 import rospy
00007 import PyKDL
00008 import sys
00009 
00010 # import msgs
00011 from std_msgs.msg import Float64MultiArray 
00012 from geometry_msgs.msg import Pose 
00013 from geometry_msgs.msg import Quaternion
00014 from geometry_msgs.msg import WrenchStamped
00015 
00016 # More imports
00017 from numpy import *
00018 import tf
00019 
00020 class Dynamics :
00021 
00022     
00023     def getConfig(self) :
00024         """ Load parameters from the rosparam server """
00025         self.num_actuators = rospy.get_param(self.vehicle_name+"/num_actuators")
00026         
00027         self.period = rospy.get_param(self.vehicle_name + "/dynamics" + "/period")
00028         self.mass = rospy.get_param(self.vehicle_name + "/dynamics" + "/mass")
00029         self.gravity_center = array(rospy.get_param(self.vehicle_name + "/dynamics" + "/gravity_center"))
00030         self.g = rospy.get_param(self.vehicle_name + "/dynamics" + "/g")
00031         self.radius = rospy.get_param(self.vehicle_name + "/dynamics" + "/radius")
00032         self.ctf = rospy.get_param(self.vehicle_name + "/dynamics" + "/ctf")
00033         self.ctb = rospy.get_param(self.vehicle_name + "/dynamics" + "/ctb")
00034         self.actuators_tau = rospy.get_param(self.vehicle_name + "/dynamics" + "/actuators_tau")
00035         self.actuators_maxsat= rospy.get_param(self.vehicle_name + "/dynamics" + "/actuators_maxsat")
00036         self.actuators_minsat = rospy.get_param(self.vehicle_name + "/dynamics" + "/actuators_minsat")
00037         self.actuators_gain = rospy.get_param(self.vehicle_name + "/dynamics" + "/actuators_gain")
00038         self.dzv = rospy.get_param(self.vehicle_name + "/dynamics" + "/dzv")
00039         self.dv = rospy.get_param(self.vehicle_name + "/dynamics" + "/dv")
00040         self.dh = rospy.get_param(self.vehicle_name + "/dynamics" + "/dh")
00041         self.density = rospy.get_param(self.vehicle_name + "/dynamics" + "/density")
00042         self.tensor = array(rospy.get_param(self.vehicle_name + "/dynamics" + "/tensor"))
00043         self.damping = array(rospy.get_param(self.vehicle_name + "/dynamics" + "/damping"))
00044         self.quadratic_damping = array(rospy.get_param(self.vehicle_name + "/dynamics" + "/quadratic_damping"))
00045       
00046         self.am=rospy.get_param(self.vehicle_name + "/dynamics"+"/allocation_matrix")
00047   
00048         self.p_0 = array(rospy.get_param(self.vehicle_name + "/dynamics" + "/initial_pose"))
00049         self.v_0 = array(rospy.get_param(self.vehicle_name + "/dynamics" + "/initial_velocity"))
00050         self.frame_id = rospy.get_param(self.vehicle_name + "/dynamics" + "/frame_id")
00051     
00052 #       Currents data
00053         self.current_mean = array( rospy.get_param("dynamics/current_mean") )
00054         self.current_sigma = array( rospy.get_param("dynamics/current_sigma") )
00055         self.current_min = array( rospy.get_param("dynamics/current_min") )
00056         self.current_max = array( rospy.get_param("dynamics/current_max") )
00057         
00058         self.uwsim_period = rospy.get_param(self.vehicle_name + "/dynamics/uwsim_period")
00059 
00060         
00061     def s(self, x) :
00062         """ Given a 3D vector computes the 3x3 antisymetric matrix """
00063 #        rospy.loginfo("s(): \n %s", x)
00064         ret = array([0.0, -x[2], x[1], x[2], 0.0, -x[0], -x[1], x[0], 0.0 ])
00065         return ret.reshape(3,3)
00066 
00067 
00068     def generalizedForce(self, du):
00069         """ Computes the generalized force as B*u, being B the allocation matrix and u the control input """
00070         ct = zeros(len(du))
00071         i1 = nonzero(du >= 0.0)
00072         i2 = nonzero(du <= 0.0)
00073         ct[i1] = self.ctf
00074         ct[i2] = self.ctb
00075         
00076         #Evaluates allocation matrix loaded as parameter
00077         b=eval(self.am)
00078         b=array(b).reshape(6,size(b)/6)
00079       
00080         # t = generalized force
00081         t = dot(b, du)
00082         t = squeeze(asarray(t)) #Transforms a matrix into an array
00083         return t
00084 
00085 
00086     def coriolisMatrix(self):
00087         s1 = self.s(dot(self.M[0:3,0:3], self.v[0:3]) + dot(self.M[0:3,3:6], self.v[3:6]))
00088         s2 = self.s(dot(self.M[3:6,0:3], self.v[0:3]) + dot(self.M[3:6,3:6], self.v[3:6])) 
00089         c = zeros((6, 6))
00090         c[0:3,3:6] = -s1
00091         c[3:6,0:3] = -s1
00092         c[3:6,3:6] = -s2
00093         return c
00094     
00095     def dumpingMatrix(self):
00096         # lineal hydrodynamic damping coeficients  
00097         Xu = self.damping[0]
00098         Yv = self.damping[1]
00099         Zw = self.damping[2]
00100         Kp = self.damping[3]
00101         Mq = self.damping[4]
00102         Nr = self.damping[5]
00103         
00104         # quadratic hydrodynamic damping coeficients
00105         Xuu = self.quadratic_damping[0]    #[Kg/m]
00106         Yvv = self.quadratic_damping[1]    #[Kg/m]
00107         Zww = self.quadratic_damping[2]    #[Kg/m]
00108         Kpp = self.quadratic_damping[3]    #[Kg*m*m]
00109         Mqq = self.quadratic_damping[4]    #[Kg*m*m]
00110         Nrr = self.quadratic_damping[5]    #[Kg*m*m]
00111     
00112         d = diag([Xu + Xuu*abs(self.v[0]), 
00113                   Yv + Yvv*abs(self.v[1]),
00114                   Zw + Zww*abs(self.v[2]),
00115                   Kp + Kpp*abs(self.v[3]),
00116                   Mq + Mqq*abs(self.v[4]),
00117                   Nr + Nrr*abs(self.v[5])])
00118         return d
00119 
00120     def gravity(self):
00121         """ Computes the gravity and buoyancy forces. Assumes a sphere model for now """
00122         #Weight and Flotability
00123         W = self.mass * self.g # [Kg]
00124         
00125         #If the vehicle moves out of the water the flotability decreases
00126         #FIXME: Assumes water surface at 0.0. Get this value from uwsim.
00127         if self.p[2] < 0.0: 
00128             r = self.radius + self.p[2]
00129             if r < 0.0:
00130                 r = 0.0
00131         else :
00132             r = self.radius
00133 
00134         #TODO: either set as parameter, since different functions may be desired for different vehicles             
00135         #      or define common models and let the user choose one by the name
00136         #      Eventually let this part to bullet inside uwsim (HfFluid)
00137         F = ((4 * math.pi * pow(r,3))/3)*self.density*self.g 
00138   
00139         # gravity center position in the robot fixed frame (x',y',z') [m]
00140         zg = self.gravity_center[2]
00141         
00142         g = array([(W - F) * sin(self.p[4]),
00143                    -(W - F) * cos(self.p[4]) * sin(self.p[3]),
00144                    -(W - F) * cos(self.p[4]) * cos(self.p[3]),
00145                    zg*W*cos(self.p[4])*sin(self.p[3]),
00146                    zg*W*sin(self.p[4]),
00147                    0.0])
00148         
00149         return g
00150         
00151         
00152     def inverseDynamic(self) :
00153         """ Given the setpoint for each thruster, the previous velocity and the 
00154             previous position computes the v_dot """
00155         du = self.thrustersDynamics(self.u)
00156         t = self.generalizedForce(du)
00157         c = self.coriolisMatrix()
00158         d = self.dumpingMatrix()
00159         g = self.gravity()
00160         c_v = dot((c-d), self.v)
00161         v_dot = dot(self.IM, (t-c_v-g+self.collisionForce)) #t-c_v-g+collisionForce
00162         v_dot = squeeze(asarray(v_dot)) #Transforms a matrix into an array
00163         return v_dot
00164         
00165 #
00166     
00167     def integral(self, x_dot, x, t) :
00168         """ Computes the integral o x dt """
00169         return (x_dot * t) + x
00170     
00171     
00172     def kinematics(self) :
00173         """ Given the current velocity and the previous position computes the p_dot """
00174         roll = self.p[3]
00175         pitch = self.p[4]
00176         yaw = self.p[5]
00177         
00178         rec = [cos(yaw)*cos(pitch), -sin(yaw)*cos(roll)+cos(yaw)*sin(pitch)*sin(roll), sin(yaw)*sin(roll)+cos(yaw)*cos(roll)*sin(pitch),
00179                sin(yaw)*cos(pitch), cos(yaw)*cos(roll)+sin(roll)*sin(pitch)*sin(yaw), -cos(yaw)*sin(roll)+sin(pitch)*sin(yaw)*cos(roll),
00180                -sin(pitch), cos(pitch)*sin(roll), cos(pitch)*cos(roll)]
00181         rec = array(rec).reshape(3,3)
00182         
00183         to = [1.0, sin(roll)*tan(pitch), cos(roll)*tan(pitch),
00184               0.0, cos(roll), -sin(roll),
00185               0.0, sin(roll)/cos(pitch), cos(roll)/cos(pitch)]
00186         to = array(to).reshape(3,3)
00187         
00188         p_dot = zeros(6)
00189         p_dot[0:3] = dot(rec, self.v[0:3])
00190         p_dot[3:6] = dot(to, self.v[3:6])
00191         return p_dot
00192 
00193     def updateThrusters(self, thrusters) :
00194         """Receives the control input, saturates each component to maxsat or minsat, and multiplies each component by the actuator gain"""
00195         #TODO: Check the size of thrusters.data
00196         t = array(thrusters.data)
00197         for i in range(size(t)):
00198            if t[i]>self.actuators_maxsat[i]:
00199                 t[i]=self.actuators_maxsat[i]
00200            elif t[i]<self.actuators_minsat[i]:
00201                 t[i]=self.actuators_minsat[i]
00202         self.u=t
00203         for i in range(size(t)):
00204            self.u[i] = self.u[i]*self.actuators_gain[i]
00205         
00206         
00207     def thrustersDynamics(self, u):
00208         y = zeros(size(u))
00209         for i in range(size(u)):
00210             y[i] = (self.period * u[i] + self.actuators_tau[i] * self.y_1[i]) / (self.period + self.actuators_tau[i])
00211             
00212         self.y_1 = y
00213         return y
00214   
00215     def updateCollision(self, force) :
00216         self.collisionForce=[force.wrench.force.x,force.wrench.force.y,force.wrench.force.z,force.wrench.torque.x,force.wrench.torque.y,force.wrench.torque.z]        
00217     
00218     def pubPose(self, event):
00219         pose = Pose()
00220         
00221         pose.position.x = self.p[0]
00222         pose.position.y = self.p[1]
00223         pose.position.z = self.p[2]
00224         
00225         orientation = tf.transformations.quaternion_from_euler(self.p[3], self.p[4], self.p[5], 'sxyz')
00226         pose.orientation.x = orientation[0]
00227         pose.orientation.y = orientation[1]
00228         pose.orientation.z = orientation[2]
00229         pose.orientation.w = orientation[3]
00230             
00231         self.pub_pose.publish(pose)
00232      
00233         # Broadcast transform
00234         br = tf.TransformBroadcaster()
00235         br.sendTransform((self.p[0], self.p[1], self.p[2]), orientation, 
00236         rospy.Time.now(), "world", str(self.frame_id))
00237     
00238     def computeTf(self, tf):
00239         r = PyKDL.Rotation.RPY(math.radians(tf[3]), math.radians(tf[4]), math.radians(tf[5]))
00240         v = PyKDL.Vector(tf[0], tf[1], tf[2])
00241         frame = PyKDL.Frame(r, v)
00242         return frame
00243     
00244     def __init__(self):
00245         """ Simulates the dynamics of an AUV """
00246 
00247         if len(sys.argv) != 6: 
00248           sys.exit("Usage: "+sys.argv[0]+" <namespace> <input_topic> <output_topic>")
00249  
00250         self.namespace=sys.argv[1]
00251         self.vehicle_name=self.namespace
00252         self.input_topic=sys.argv[2]
00253         self.output_topic=sys.argv[3]
00254 
00255     #  Collision parameters
00256         self.collisionForce = [0,0,0,0,0,0]
00257 
00258     #   Load dynamic parameters
00259         self.getConfig()
00260         #self.altitude = -1.0 
00261         self.y_1 = zeros(5)
00262         
00263     #   Create publisher
00264         self.pub_pose= rospy.Publisher(self.output_topic, Pose)
00265         rospy.init_node("dynamics_"+self.vehicle_name)
00266                 
00267     #   Init pose and velocity and period
00268         self.v = self.v_0
00269         self.p = self.p_0
00270         
00271         # Inertia Tensor. Principal moments of inertia, and products of inertia [kg*m*m]
00272         Ixx = self.tensor[0]
00273         Ixy = self.tensor[1] 
00274         Ixz = self.tensor[2]
00275         Iyx = self.tensor[3]
00276         Iyy = self.tensor[4]
00277         Iyz = self.tensor[5]
00278         Izx = self.tensor[6]
00279         Izy = self.tensor[7]
00280         Izz = self.tensor[8] 
00281         m = self.mass
00282         xg = self.gravity_center[0]
00283         yg = self.gravity_center[1]
00284         zg = self.gravity_center[2]
00285         
00286         Mrb = rospy.get_param(self.vehicle_name + "/dynamics" + "/Mrb")
00287         Mrb = array(Mrb).reshape(6, 6)
00288              
00289         # Inertia matrix of the rigid body
00290         # Added Mass derivative
00291         Ma = rospy.get_param(self.vehicle_name + "/dynamics" + "/Ma")
00292         Ma = array(Ma).reshape(6, 6) 
00293         
00294         self.M = Mrb + Ma    # mass matrix: Mrb + Ma
00295         self.IM = matrix(self.M).I
00296 #        rospy.loginfo("Inverse Mass Matrix: \n%s", str(self.IM))
00297               
00298         #Init currents
00299         random.seed()
00300         self.e_vc = self.current_mean 
00301         #The number of zeros will depend on the number of actuators
00302         self.u = array(zeros(self.num_actuators)) # Initial thrusters setpoint
00303 
00304         #Publish pose to UWSim
00305         rospy.Timer(rospy.Duration(self.uwsim_period), self.pubPose)
00306         
00307     #   Create Subscribers for thrusters and collisions
00308         #TODO: set the topic names as parameters
00309         rospy.Subscriber(self.input_topic, Float64MultiArray, self.updateThrusters)
00310         rospy.Subscriber("/g500/ForceSensor", WrenchStamped, self.updateCollision)
00311         
00312     def iterate(self):
00313         t1 = rospy.Time.now()
00314 
00315         # Main loop operations
00316         self.v_dot = self.inverseDynamic()
00317         self.v = self.integral(self.v_dot, self.v, self.period)
00318         self.p_dot = self.kinematics()
00319         self.p = self.integral(self.p_dot, self.p, self.period)
00320 
00321         t2 = rospy.Time.now()
00322         p = self.period - (t2-t1).to_sec()
00323         if p < 0.0 : p = 0.0
00324         rospy.sleep(p)
00325         
00326 
00327 if __name__ == '__main__':
00328     try:
00329         dynamics = Dynamics() 
00330         while not rospy.is_shutdown():
00331             dynamics.iterate()
00332 
00333     except rospy.ROSInterruptException: pass
00334     


underwater_vehicle_dynamics
Author(s): Arnau Carrera, Narcis Palomeras, Mario Prats
autogenerated on Mon Oct 6 2014 08:24:31