dynamic_positioning.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 '''
00003 Created on Feb 04, 2013
00004 
00005 @author: dnad
00006 '''
00007 import roslib; roslib.load_manifest("cart_sim");
00008 import rospy;
00009 from auv_msgs.msg import BodyVelocityReq;
00010 from auv_msgs.msg import BodyForceReq;
00011 from auv_msgs.msg import NavSts;
00012 from std_msgs.msg import Byte
00013 import numpy
00014 import math
00015       
00016 class DynamicPositioning:
00017     def __init__(self):
00018         #rospy.Subscriber("desiredPosition", VehiclePose, self.onRef)
00019         rospy.Subscriber("stateHat", NavSts, self.onStateHat);
00020         rospy.Subscriber("trackedNav", NavSts, self.onTrackedNav);
00021         rospy.Subscriber("tauAch", BodyForceReq, self.onWindupFlag);
00022                
00023         self.stateHat = NavSts();
00024         
00025         self.out = rospy.Publisher("nuRef", BodyVelocityReq);
00026          
00027         w = numpy.array(rospy.get_param(
00028                             "dynamic_positioning/closed_loop_freq"));
00029         
00030         self.Kp = numpy.array([[2*w[0], 0],[0, 2*w[1]]],
00031                               dtype=numpy.float32);
00032         self.Ki = numpy.array([[w[0]*w[0], 0], [0, w[1]*w[1]]], 
00033                                dtype=numpy.float32);
00034                                
00035         self.Kt= 2*self.Ki;
00036         
00037         #self.Kp = numpy.reshape(
00038         #            numpy.array(rospy.get_param("dynamic_positioning/Kp"),
00039         #                        numpy.float32), [2,2]);                
00040         #self.Ki = numpy.reshape(
00041         #            numpy.array(rospy.get_param("dynamic_positioning/Ki"),
00042         #                        numpy.float32), [2,2]);
00043         self.Ts = rospy.get_param("dynamic_positioning/Ts");
00044         
00045         self.mode = rospy.get_param("dynamic_positioning/mode");
00046         
00047         self.max = numpy.array(rospy.get_param("dynamic_positioning/max"), dtype=numpy.float32);
00048                
00049         self.internalState = numpy.array([0,0], numpy.float32);
00050         self.state = numpy.array([0,0], numpy.float32);
00051         self.desired = numpy.array([0,0], numpy.float32);
00052         self.ff = numpy.array([0,0], numpy.float32);
00053         self.windup = numpy.array([0,0], dtype=numpy.int8);
00054         self.lastW = numpy.array([0,0], dtype=numpy.int8);
00055         self.lastI = numpy.array([0,0], numpy.float32);
00056         
00057         
00058         self.R = numpy.identity(2, numpy.float32);
00059         self.uk_1 = 0;
00060         self.lastTime = rospy.Time.now();
00061         
00062         '''Backward suff'''
00063         self.lastP = numpy.array([0,0]);
00064         self.lastFF = numpy.array([0,0]);
00065         
00066         model_name = rospy.get_param("model_name");
00067         b = rospy.get_param(model_name+"/dynamics/damping");
00068         Betainv = numpy.array([[1.0/b[0],0],[0,1.0/b[1]]], dtype=numpy.float32);
00069         cp = numpy.cos(numpy.pi/4); sp = numpy.sin(numpy.pi/4);
00070         allocM = numpy.array([[cp,cp,-cp,-cp],
00071                               [sp,-sp,sp,-sp]]);
00072                                                            
00073         self.Bstar = 0.9*numpy.dot(Betainv,allocM);       
00074         self.tmax = 13/(2*cp);           
00075         
00076     def onWindupFlag(self,data):
00077         self.windup[0] = data.disable_axis.x;
00078         self.windup[1] = data.disable_axis.y;
00079         print "Got windup", data.disable_axis.x, ",", data.disable_axis.y;
00080         
00081     def onStateHat(self,data):
00082         self.stateHat = data;
00083         self.state = numpy.array([data.position.north, data.position.east],
00084                                  dtype=numpy.float32);
00085         yaw = data.orientation.yaw;
00086         self.R = numpy.array([[math.cos(yaw),-math.sin(yaw)],
00087                               [math.sin(yaw),math.cos(yaw)]],numpy.float32);
00088                               
00089         self.step();
00090         
00091     def onRef(self,data):
00092         self.desired = numpy.array([data.position.north,data.position.east], 
00093                                    dtype=numpy.float32);                           
00094         self.step();                           
00095     
00096     def onTrackedNav(self,data):       
00097         self.ff[0] = data.body_velocity.x*math.cos(data.orientation.yaw);
00098         self.ff[1] = data.body_velocity.x*math.sin(data.orientation.yaw);
00099         self.onRef(data);
00100         
00101     def sat(self,data,minVal,maxVal):
00102         if data>maxVal: return maxVal;
00103         if data<minVal: return minVal;
00104         return data;
00105     
00106     def min(self,data1,data2):
00107         if data1<=data2: return data1;
00108         else: return data2;
00109         
00110     def max(self,data1,data2):
00111         if data1>=data2: return data1;
00112         else: return data2;
00113                 
00114     def stepSSbackward(self):
00115         d = self.desired - self.state;
00116         
00117         #ti = numpy.dot(numpy.linalg.pinv(numpy.dot(numpy.linalg.inv(self.Kp),self.Bstar)),d);
00118         #print "Desired error:",d
00119         #print "Desired forces:",ti      
00120         #scale = numpy.max(numpy.array([numpy.abs(ti[0])/self.tmax,numpy.abs(ti[1])/self.tmax,1]));
00121         #ti = ti/scale;
00122         #print "Scaled forces:",ti
00123         #dd = numpy.dot(numpy.dot(numpy.linalg.inv(self.Kp),self.Bstar),ti);
00124         #print "Scaled error:",dd
00125         
00126         #d=dd;
00127         
00128         self.internalState += numpy.dot(self.Kp,d) - self.lastP;
00129         self.lastP = numpy.dot(self.Kp,d);
00130         
00131         self.internalState += numpy.dot(self.R,self.ff) - self.lastFF;
00132         self.lastFF = numpy.dot(self.R,self.ff);
00133         
00134         print "Feed forward:",self.lastFF;
00135         
00136         if (numpy.linalg.norm(self.lastW, 2) == 0) and (numpy.linalg.norm(self.windup, 2) != 0):
00137             print "Oduzimanje."
00138             self.internalState -= self.lastI;
00139             self.lastI = 0;
00140         
00141         if numpy.linalg.norm(self.windup, 2) == 0:
00142             print "Dodavanje"
00143             self.internalState += numpy.dot(self.Ki,d)*self.Ts;          
00144             self.lastI += numpy.dot(self.Ki,d)*self.Ts;
00145         
00146         self.lastW = 1*self.windup;
00147         
00148         u = numpy.dot(numpy.transpose(self.R),self.internalState);
00149         
00150         du = numpy.array([self.stateHat.body_velocity.x, self.stateHat.body_velocity.y], dtype=numpy.float32);
00151         #du = numpy.dot(self.R,du);
00152         
00153         #ti = numpy.dot(numpy.linalg.pinv(self.Bstar),u);
00154         #print "Desired speed:",u
00155         #print "Desired forces:",ti      
00156         #scale = numpy.max(numpy.array([numpy.abs(ti[0])/self.tmax,numpy.abs(ti[1])/self.tmax,1]));
00157         #ti = ti/scale;
00158         #print "Scaled forces:",ti
00159         #ddu = numpy.dot(self.Bstar,ti);
00160         #print "Scaled speed:",ddu
00161         
00162         #if (ddu[0] != u[0]) or (ddu[1] != u[1]):
00163                 #if numpy.linalg.norm(self.windup, 2):
00164                 #    self.internalState = numpy.dot(numpy.linalg.inv(numpy.identity(2, dtype=numpy.float32)+self.Kt*self.Ts),(self.internalState + 
00165                 #                  numpy.dot(self.R,numpy.dot(self.Kt,ddu)*self.Ts) + numpy.dot(self.R,numpy.dot(self.Kt,du)*self.Ts)));
00166         #self.internalState = numpy.dot(numpy.linalg.inv(numpy.identity(2, dtype=numpy.float32)+self.Kt*self.Ts),(self.internalState + 
00167         #                          numpy.dot(self.R,numpy.dot(self.Kt,ddu)*self.Ts)));        
00168         
00169         print "Windup:",self.windup, u;
00170          
00171         '''The velocity only stuff'''                         
00172         #if numpy.linalg.norm(self.windup, 2):
00173         #    self.internalState = numpy.dot(numpy.linalg.inv(numpy.identity(2, dtype=numpy.float32)+self.Kt*self.Ts),(self.internalState + 
00174         #                          numpy.dot(self.Kt,du)*self.Ts));
00175         
00176         u = numpy.dot(numpy.transpose(self.R),self.internalState);
00177               
00178         #u = numpy.dot(numpy.transpose(self.R),ddu);                           
00179         pub = BodyVelocityReq();
00180         pub.twist.linear.x = u[0];
00181         pub.twist.linear.y = u[1];
00182         pub.goal.priority = pub.goal.PRIORITY_NORMAL;
00183         pub.disable_axis.yaw = pub.disable_axis.pitch = 0;
00184         pub.disable_axis.roll = pub.disable_axis.z = 0;
00185         self.out.publish(pub); 
00186         
00187         
00188     def stepSS(self):
00189         d = self.desired - self.state;
00190         u = numpy.dot(numpy.transpose(self.R),(numpy.dot(self.Kp,d) +
00191                                      self.internalState + self.ff));
00192                                   
00193         ti = numpy.dot(numpy.linalg.pinv(self.Bstar),u);
00194         print "Desired speed:",u
00195         print "Desired forces:",ti      
00196         scale = numpy.max(numpy.array([numpy.abs(ti[0])/self.tmax,numpy.abs(ti[1])/self.tmax,1]));
00197         ti = ti/scale;
00198         print "Scaled forces:",ti
00199         ddu = numpy.dot(self.Bstar,ti);
00200         print "Scaled speed:",ddu
00201         
00202         '''Ignore scaling'''
00203         ddu = u;
00204                                              
00205         #ddu = numpy.dot(numpy.transpose(self.R),numpy.array([1.0,1.0], dtype=numpy.float32));
00206         #ddu[0] = u[0] / scale;
00207         #ddu[1] = u[1] / scale;
00208         
00209         du = numpy.array([self.stateHat.body_velocity.x, self.stateHat.body_velocity.y], dtype=numpy.float32);
00210         #ddu = u;
00211                                                                                                   
00212         '''Propagate integration after the output calculation'''
00213         #if numpy.linalg.norm(du,2) == 0 :
00214         if numpy.linalg.norm(self.windup, 2) == 0:
00215             #self.internalState += self.windup*numpy.dot(self.R,numpy.dot(self.Kt,du-u)*self.Ts);
00216             #self.internalState += numpy.dot(self.R,numpy.dot(self.Kt,du-u)*self.Ts);
00217             #if self.windup[0]: u[0] = du[0];
00218             #if self.windup[1]: u[1] = du[1];     
00219             self.internalState += numpy.dot(self.Ki,d)*self.Ts + 0*numpy.dot(self.R,numpy.dot(self.Kt,ddu-u)*self.Ts);
00220         
00221         '''Ignore scaling'''
00222         #self.internalState += numpy.dot(self.Ki,d)*self.Ts + numpy.dot(self.R,numpy.dot(self.Kt,ddu-u)*self.Ts);
00223         
00224         
00225         #     print "No windup"
00226         #else:    
00227         self.uk_1 = u;
00228         
00229         print "Windup:",self.windup, u;
00230         #u=ddu;
00231         
00232         pub = BodyVelocityReq();
00233         pub.twist.linear.x = u[0];
00234         pub.twist.linear.y = u[1];
00235         pub.goal.priority = pub.goal.PRIORITY_NORMAL;
00236         pub.disable_axis.yaw = pub.disable_axis.pitch = 0;
00237         pub.disable_axis.roll = pub.disable_axis.z = 0;
00238         self.out.publish(pub);
00239         
00240     def stepSH(self):
00241         d =self.desired - self.state;
00242         
00243         u = 0.1*numpy.dot(self.Kp,d);
00244         yaw_ref = math.atan2(d[1],d[0]);       
00245                 
00246         yaw_error = (yaw_ref - self.stateHat.orientation.yaw)%(2*math.pi);
00247         
00248         if yaw_error >= math.pi:
00249             yaw_error += -2*math.pi;
00250         elif yaw_error <= -math.pi:
00251             yaw_error += 2*math.pi;           
00252 
00253         pub = BodyVelocityReq();
00254         pub.twist.linear.x = u[0]*math.cos(yaw_ref) + u[1]*math.sin(yaw_ref);
00255         pub.twist.angular.z = 0.3*self.Kp[1][1]*yaw_error;
00256                 
00257         '''Propagate integration after the output calculation'''
00258         #self.internalState[0] += numpy.dot(self.Ki,d)*self.Ts + self.windup[0]*self.Kt(self.stateHat.body_velocity.x - pub.twist.linear.x)*self.Ts;
00259         #self.internalState[1] += self.Ki[1][1]*yaw_error*self.Ts + self.windup[1]*self.Kt[1][1]*(self.stateHat.orientation_rate.yaw - pub.twist.angular.z)*self.Ts;
00260         
00261         print "Windup:",u, pub.twist.linear.x, pub.twist.angular.z
00262         
00263         pub.goal.priority = pub.goal.PRIORITY_NORMAL;
00264         pub.disable_axis.y = pub.disable_axis.pitch = 0;
00265         pub.disable_axis.roll = pub.disable_axis.z = 0;
00266         self.out.publish(pub);
00267         
00268     def step(self):
00269         self.Ts = (rospy.Time.now() - self.lastTime).to_sec();
00270         self.lastTime = rospy.Time.now();
00271         
00272         if self.Ts > 0.2: self.Ts = 0.1;
00273         self.Ts = 0.1;
00274         
00275         #rospy.loginfo("DynamicPositioning: sampling time %f",self.Ts);
00276         
00277         if self.mode == 0:
00278             self.stepSSbackward();
00279         else:
00280             self.stepSH();
00281          
00282         
00283 if __name__ == "__main__":
00284     rospy.init_node("dpcontrol");
00285     dp = DynamicPositioning();
00286     
00287     synced = False;
00288         
00289     rate = rospy.Rate(10.0);
00290         
00291     while not rospy.is_shutdown():
00292         if synced:
00293             dp.step();
00294             rate.sleep();
00295         else:
00296             rospy.spin();
00297         
00298         
00299         


labust_uvapp
Author(s): Dula Nad
autogenerated on Fri Feb 7 2014 11:36:37