def __init__(self, sim,
iktype=openravepy.IkParameterization.Type.Transform6D):
Manipulator.__init__(self)
"""
Initialize Mico manipulator
@parameter sim: a boolean variable indicating whether we want to run it in simulation or not
@type sim: bool
@parameter iktype: the type of IK
@type iktype: openravepy.IkParameterization.Type
"""
self.simulated = sim
self.iktype = iktype
robot = self.GetRobot()
env = robot.GetEnv()
with env:
dof_indices = self.GetIndices()
accel_limits = robot.GetDOFAccelerationLimits()
accel_limits[dof_indices[0]] = 1.65
accel_limits[dof_indices[1]] = 1.76
accel_limits[dof_indices[2]] = 1.70
accel_limits[dof_indices[3]] = 1.80
accel_limits[dof_indices[4]] = 1.70
accel_limits[dof_indices[5]] = 1.77
robot.SetDOFAccelerationLimits(accel_limits)
# Load or_nlopt_ik as the IK solver. Unfortunately, IKFast doesn't work
# on the Mico.
if iktype is not None:
with env:
self.iksolver = openravepy.RaveCreateIkSolver(env, 'TracIK')
if self.iksolver is None:
raise Exception('Could not create the ik solver')
set_ik_succeeded = self.SetIKSolver(self.iksolver);
if not set_ik_succeeded:
raise Exception('could not set the ik solver')
# Load simulation controllers.
if sim:
from prpy.simulation import ServoSimulator
self.controller = robot.AttachController(
self.GetName(), '', self.GetArmIndices(), 0, True)
self.servo_simulator = ServoSimulator(self, rate=20,
watchdog_timeout=0.1)
else:
#if not simulation, create publishers for each joint
self.velocity_controller_names = ['vel_j' + str(i) + '_controller' for i in range(1,7)]
self.velocity_topic_names = [controller_name + '/command' for controller_name in self.velocity_controller_names]
self.velocity_publishers = [rospy.Publisher(topic_name, Float64, queue_size=1) for topic_name in self.velocity_topic_names]
self.velocity_publisher_lock = threading.Lock()
#create watchdog to send zero velocity
self.servo_watchdog = Watchdog(timeout_duration=0.25, handler=self.SendVelocitiesToMico, args=[[0.,0.,0.,0.,0.,0.,]])
def __init__(self, sim, namespace='',
iktype=openravepy.IkParameterization.Type.Transform6D):
Manipulator.__init__(self)
self.simulated = sim
self._iktype = iktype
self.namespace = namespace
if iktype is not None:
self._SetupIK(iktype)
if sim:
from prpy.simulation import ServoSimulator
self.servo_simulator = ServoSimulator(
self, rate=20, watchdog_timeout=0.1)
def CloneBindings(self, parent):
Manipulator.CloneBindings(self, parent)
self.simulated = True
self._iktype = parent._iktype
if self._iktype is not None:
self._SetupIK(self._iktype)
def SetVelocityLimits(self,
velocity_limits,
min_accel_time,
openrave=True):
logger.warning(
'Only setting velocity limits of OR model, The function should'
'set the velocity limit of both OR and real robot')
Manipulator.SetVelocityLimits(self, velocity_limits, min_accel_time)
def GetVelocityLimits(self, openrave=None, owd=None):
"""Get the OpenRAVE and OWD joint velocity limits.
This function checks both the OpenRAVE and OWD joint velocity limits.
If they do not match, a warning is printed and the minimum value is
returned.
@param openrave flag to set the OpenRAVE velocity limits
@param owd flag to set the OWD velocity limits
@return list of velocity limits, in radians per second
"""
if openrave is not None or owd is not None:
warnings.warn(
'The "openrave" and "owd" flags are deprecated in'
' GetVelocityLimits and will be removed in a future version.',
DeprecationWarning)
return Manipulator.GetVelocityLimits(self)
def SetVelocityLimits(self, velocity_limits, min_accel_time,
openrave=True, owd=None):
"""Change the OpenRAVE and OWD joint velocity limits.
Joint velocities that exceed these limits will trigger a velocity fault.
@param velocity_limits vector of joint velocity limits in radians per second
@param min_accel_time minimum acceleration time used to compute acceleration limits
@param openrave flag to set the OpenRAVE velocity limits
@param owd flag to set the OWD velocity limits
"""
# TODO implement in ros_control
if owd is not None:
warnings.warn(
'The "owd" flag is deprecated in'
' SetVelocityLimits and will be removed in a future version.',
DeprecationWarning)
# Update the OpenRAVE limits.
if openrave:
Manipulator.SetVelocityLimits(self, velocity_limits, min_accel_time)
def __init__(self):
Manipulator.__init__(self)
self.controller = self.GetRobot().GetController()
def GetVelocityLimits(self, openrave=None):
logger.warning(
'Only getting velocity limits of OR model, The function should'
'print an warning if the or velocity and real robot velocity does not match'
)
return Manipulator.GetVelocityLimits(self)