30
30
31
31
import numpy
32
32
import openravepy
33
+ import warnings
33
34
from manipulator import Manipulator
34
35
from prpy .clone import Clone
35
36
from .. import util
@@ -161,7 +162,7 @@ def ServoTo(manipulator, target, duration, timeStep=0.05, collisionChecking=True
161
162
else :
162
163
return False
163
164
164
- def GetVelocityLimits (self , openrave = True , owd = True ):
165
+ def GetVelocityLimits (self , openrave = None , owd = None ):
165
166
"""Get the OpenRAVE and OWD joint velocity limits.
166
167
This function checks both the OpenRAVE and OWD joint velocity limits.
167
168
If they do not match, a warning is printed and the minimum value is
@@ -170,37 +171,13 @@ def GetVelocityLimits(self, openrave=True, owd=True):
170
171
@param owd flag to set the OWD velocity limits
171
172
@return list of velocity limits, in radians per second
172
173
"""
173
- # Update the OpenRAVE limits.
174
- if openrave :
175
- or_velocity_limits = Manipulator .GetVelocityLimits (self )
176
- if self .simulated or not owd :
177
- return or_velocity_limits
178
-
179
- # Update the OWD limits.
180
- if owd and not self .simulated :
181
- args = [ 'GetSpeed' ]
182
- args_str = ' ' .join (args )
183
- owd_speed_limits_all = self .controller .SendCommand (args_str )
184
- #if we get nothing back, e.g. if the arm isn't running, return openrave lims
185
- if owd_speed_limits_all is None :
186
- return or_velocity_limits
187
-
188
- owd_speed_limits = map (float , owd_speed_limits_all .split (',' ));
189
- #first 7 numbers are velocity limits
190
- owd_velocity_limits = numpy .array (owd_speed_limits [0 :len (self .GetIndices ())])
191
-
192
- diff_arr = numpy .subtract (or_velocity_limits , owd_velocity_limits )
193
- max_diff = max (abs (diff_arr ))
194
-
195
- if max_diff > 0.01 :
196
- # TODO: Change this to use the logging framework.
197
- print ('GetVelocityLimits Error: openrave and owd limits very different' )
198
- print ('\t Openrave limits:\t ' + str (or_velocity_limits ))
199
- print ('\t OWD limits:\t \t ' + str (owd_velocity_limits ))
200
-
201
- return numpy .minimum (or_velocity_limits , owd_velocity_limits )
174
+ if openrave is not None or owd is not None :
175
+ warnings .warn (
176
+ 'The "openrave" and "owd" flags are deprecated in'
177
+ ' GetVelocityLimits and will be removed in a future version.' ,
178
+ DeprecationWarning )
202
179
203
- return or_velocity_limits
180
+ return Manipulator . GetVelocityLimits ( self )
204
181
205
182
def SetVelocityLimits (self , velocity_limits , min_accel_time ,
206
183
openrave = True , owd = True ):
0 commit comments