def servoOff(self, jname='all', wait=True):
'''
@type jname: str
@param jname: The value 'all' works iteratively for all servos.
@type wait: bool
@rtype: int
@return: 1 = all arm servo off. 2 = all servo on arms and hands off.
-1 = Something wrong happened.
'''
# do nothing for simulation
if self.simulation_mode:
print self.configurator_name, 'omit servo off'
return 0
print 'Turn off Hand Servo'
if self.sc_svc:
self.sc_svc.servoOff()
# if the servos aren't on switch power off
if not self.isServoOn(jname):
if jname.lower() == 'all':
self.rh_svc.power('all', SWITCH_OFF)
return 1
# if jname is not set properly set to all -> is this safe?
if jname == '':
jname = 'all'
self.liftRobotUp()
if wait:
waitInputConfirm(
'!! Robot Motion Warning (Servo OFF)!!\n\n'
'Push [OK] to servo OFF (%s).' % (jname)) # :
try:
self.rh_svc.servo('all', SWITCH_OFF)
time.sleep(0.2)
if jname == 'all':
self.rh_svc.power('all', SWITCH_OFF)
# turn off hand motors
print 'Turn off Hand Servo'
if self.sc_svc:
self.sc_svc.servoOff()
return 2
except:
print self.configurator_name, 'servo off: communication error'
return -1
def servoOff(self, jname='all', wait=True):
'''
@type jname: str
@param jname: The value 'all' works iteratively for all servos.
@type wait: bool
@rtype: int
@return: 1 = all arm servo off. 2 = all servo on arms and hands off.
-1 = Something wrong happened.
'''
# do nothing for simulation
if self.simulation_mode:
print self.configurator_name, 'omit servo off'
return 0
print 'Turn off Hand Servo'
if self.sc_svc:
self.sc_svc.servoOff()
# if the servos aren't on switch power off
if not self.isServoOn(jname):
if jname.lower() == 'all':
self.rh_svc.power('all', SWITCH_OFF)
return 1
# if jname is not set properly set to all -> is this safe?
if jname == '':
jname = 'all'
if wait:
waitInputConfirm(
'!! Robot Motion Warning (Servo OFF)!!\n\n'
'Push [OK] to servo OFF (%s).' % (jname)) # :
try:
self.rh_svc.servo('all', SWITCH_OFF)
time.sleep(0.2)
if jname == 'all':
self.rh_svc.power('all', SWITCH_OFF)
# turn off hand motors
print 'Turn off Hand Servo'
if self.sc_svc:
self.sc_svc.servoOff()
return 2
except:
print self.configurator_name, 'servo off: communication error'
return -1
def checkEncoders(self, jname='all', option=''):
'''
Run the encoder checking sequence for specified joints,
run goActual to adjust the direction values, and then turn servos on.
@type jname: str
@param jname: The value 'all' works iteratively for all servos.
@type option: str
@param option: Possible values are follows (w/o double quote):\
"-overwrite": Overwrite calibration value.
'''
if self.isServoOn():
waitInputConfirm('Servo must be off for calibration')
return
# do not check encoders twice
elif self.isCalibDone():
waitInputConfirm('System has been calibrated')
return
self.rh_svc.power('all', SWITCH_ON)
msg = '!! Robot Motion Warning !!\n'\
'Turn Relay ON.\n'\
'Then Push [OK] to '
if option == '-overwrite':
msg = msg + 'calibrate(OVERWRITE MODE) '
else:
msg = msg + 'check '
if jname == 'all':
msg = msg + 'the Encoders of all.'
else:
msg = msg + 'the Encoder of the Joint "' + jname + '".'
try:
waitInputConfirm(msg)
except:
print "If you're connecting to the robot from remote, " + \
"make sure tunnel X (eg. -X option with ssh)."
self.rh_svc.power('all', SWITCH_OFF)
return 0
is_result_hw = True
print self.configurator_name, 'calib-joint ' + jname + ' ' + option
self.rh_svc.initializeJointAngle(jname, option)
print self.configurator_name, 'done'
is_result_hw = is_result_hw and self.rh_svc.power('all', SWITCH_OFF)
self.goActual() # This needs to happen before turning servo on.
time.sleep(0.1)
is_result_hw = is_result_hw and self.rh_svc.servo(jname, SWITCH_ON)
if not is_result_hw:
# The step described in the following msg is confirmed by the manufacturer 12/14/2015
print(
"Turning servos ({}) failed. This is likely because of issues happening in lower level. Please check if the Kawada's proprietary tool NextageOpenSupervisor returns without issue or not. If the issue persists, contact the manufacturer."
.format(jname))
# turn on hand motors
print 'Turn on Hand Servo'
if self.sc_svc:
self.sc_svc.servoOn()
def checkEncoders(self, jname='all', option=''):
'''
Run the encoder checking sequence for specified joints,
run goActual to adjust the direction values, and then turn servos on.
@type jname: str
@param jname: The value 'all' works iteratively for all servos.
@type option: str
@param option: Possible values are follows (w/o double quote):\
"-overwrite": Overwrite calibration value.
'''
if self.isServoOn():
waitInputConfirm('Servo must be off for calibration')
return
# do not check encoders twice
elif self.isCalibDone():
waitInputConfirm('System has been calibrated')
return
self.rh_svc.power('all', SWITCH_ON)
msg = '!! Robot Motion Warning !!\n'\
'Turn Relay ON.\n'\
'Then Push [OK] to '
if option == '-overwrite':
msg = msg + 'calibrate(OVERWRITE MODE) '
else:
msg = msg + 'check '
if jname == 'all':
msg = msg + 'the Encoders of all.'
else:
msg = msg + 'the Encoder of the Joint "' + jname + '".'
try:
waitInputConfirm(msg)
except:
print "If you're connecting to the robot from remote, " + \
"make sure tunnel X (eg. -X option with ssh)."
self.rh_svc.power('all', SWITCH_OFF)
return 0
print self.configurator_name, 'calib-joint ' + jname + ' ' + option
self.rh_svc.initializeJointAngle(jname, option)
print self.configurator_name, 'done'
self.rh_svc.power('all', SWITCH_OFF)
self.goActual() # This needs to happen before turning servo on.
time.sleep(0.1)
self.rh_svc.servo(jname, SWITCH_ON)
# turn on hand motors
print 'Turn on Hand Servo'
if self.sc_svc:
self.sc_svc.servoOn()
def checkEncoders(self, jname='all', option=''):
'''
Run the encoder checking sequence for specified joints,
run goActual and turn on servos.
@type jname: str
@param jname: The value 'all' works iteratively for all servos.
@type option: str
@param option: Possible values are follows (w/o double quote):\
"-overwrite": Overwrite calibration value.
'''
if self.isServoOn():
waitInputConfirm('Servo must be off for calibration')
return
# do not check encoders twice
elif self.isCalibDone():
waitInputConfirm('System has been calibrated')
return
self.rh_svc.power('all', SWITCH_ON)
msg = '!! Robot Motion Warning !!\n'\
'Turn Relay ON.\n'\
'Then Push [OK] to '
if option == '-overwrite':
msg = msg + 'calibrate(OVERWRITE MODE) '
else:
msg = msg + 'check '
if jname == 'all':
msg = msg + 'the Encoders of all.'
else:
msg = msg + 'the Encoder of the Joint "' + jname + '".'
try:
waitInputConfirm(msg)
except:
print "If you're connecting to the robot from remote, " + \
"make sure tunnel X (eg. -X option with ssh)."
self.rh_svc.power('all', SWITCH_OFF)
return 0
print self.configurator_name, 'calib-joint ' + jname + ' ' + option
self.rh_svc.initializeJointAngle(jname, option)
print self.configurator_name, 'done'
self.rh_svc.power('all', SWITCH_OFF)
self.goActual()
time.sleep(0.1)
self.rh_svc.servo(jname, SWITCH_ON)
# turn on hand motors
print 'Turn on Hand Servo'
if self.sc_svc:
self.sc_svc.servoOn()
def servoOn(self, jname='all', destroy=1, tm=3):
'''
Turn on servo motors at joint specified.
Joints need to be calibrated (otherwise error returns).
@type jname: str
@param jname: The value 'all' works iteratively for all servos.
@param destroy: Not used.
@type tm: float
@param tm: Second to complete.
@rtype: int
@return: 1 or -1 indicating success or failure, respectively.
'''
# check joints are calibrated
if not self.isCalibDone():
waitInputConfirm(
'!! Calibrate Encoders with checkEncoders first !!\n\n')
return -1
# check servo state
if self.isServoOn():
return 1
# check jname is acceptable
if jname == '':
jname = 'all'
#self.rh_svc.power('all', SWITCH_ON) #do not switch on before goActual
try:
waitInputConfirm(\
'!! Robot Motion Warning (SERVO_ON) !!\n\n'
'Confirm RELAY switched ON\n'
'Push [OK] to switch servo ON(%s).' % (jname))
except: # ths needs to change
self.rh_svc.power('all', SWITCH_OFF)
raise
# Need to reset JointGroups.
# See https://code.google.com/p/rtm-ros-robotics/issues/detail?id=277
try:
# remove jointGroups
self.seq_svc.removeJointGroup("larm")
self.seq_svc.removeJointGroup("rarm")
self.seq_svc.removeJointGroup("head")
self.seq_svc.removeJointGroup("torso")
except:
print(
self.configurator_name,
'Exception during servoOn while removing JoingGroup. ' +
_MSG_ASK_ISSUEREPORT)
try:
# setup jointGroups
self.setSelfGroups() # restart groups
except:
print(
self.configurator_name,
'Exception during servoOn while removing setSelfGroups. ' +
_MSG_ASK_ISSUEREPORT)
try:
self.goActual() # This needs to happen before turning servo on.
time.sleep(0.1)
self.rh_svc.servo(jname, SWITCH_ON)
time.sleep(2)
# time.sleep(7)
if not self.isServoOn(jname):
print self.configurator_name, 'servo on failed.'
raise
except:
print self.configurator_name, 'exception occured'
try:
angles = self.flat2Groups(
map(numpy.rad2deg,
self.getActualState().angle))
print 'Move to Actual State, Just a minute.'
for i in range(len(self.Groups)):
self.setJointAnglesOfGroup(self.Groups[i][0],
angles[i],
tm,
wait=False)
for i in range(len(self.Groups)):
self.seq_svc.waitInterpolationOfGroup(self.Groups[i][0])
except:
print self.configurator_name, 'post servo on motion trouble'
# turn on hand motors
print 'Turn on Hand Servo'
if self.sc_svc:
self.sc_svc.servoOn()
return 1
def servoOn(self, jname='all', destroy=1, tm=3):
'''
Turn on/off servos.
Joints need to be calibrated (otherwise error returns).
@type jname: str
@param jname: The value 'all' works iteratively for all servos.
@param destroy: Not used.
@type tm: float
@param tm: Second to complete.
@rtype: int
@return: 1 or -1 indicating success or failure, respectively.
'''
# check joints are calibrated
if not self.isCalibDone():
waitInputConfirm('!! Calibrate Encoders with checkEncoders first !!\n\n')
return -1
# check servo state
if self.isServoOn():
return 1
# check jname is acceptable
if jname == '':
jname = 'all'
#self.liftRobotUp() #lift robot up does not need for non-legged robot
#self.rh_svc.power('all', SWITCH_ON) #do not switch on before goActual
try:
waitInputConfirm(\
'!! Robot Motion Warning (SERVO_ON) !!\n\n'
'Confirm RELAY switched ON\n'
'Push [OK] to switch servo ON(%s).' % (jname))
except: # ths needs to change
self.rh_svc.power('all', SWITCH_OFF)
raise
# Need to reset JointGroups.
# See https://code.google.com/p/rtm-ros-robotics/issues/detail?id=277
try:
# remove jointGroups
self.seq_svc.removeJointGroup("larm")
self.seq_svc.removeJointGroup("rarm")
self.seq_svc.removeJointGroup("head")
self.seq_svc.removeJointGroup("torso")
except:
print(self.configurator_name,
'Exception during servoOn while removing JoingGroup. ' +
_MSG_ASK_ISSUEREPORT)
try:
# setup jointGroups
self.setSelfGroups() # restart groups
except:
print(self.configurator_name,
'Exception during servoOn while removing setSelfGroups. ' +
_MSG_ASK_ISSUEREPORT)
try:
self.stOff()
self.goActual()
time.sleep(0.1)
self.rh_svc.servo(jname, SWITCH_ON)
time.sleep(2)
# time.sleep(7)
if not self.isServoOn(jname):
print self.configurator_name, 'servo on failed.'
raise
except:
print self.configurator_name, 'exception occured'
try:
angles = self.flat2Groups(map(numpy.rad2deg,
self.getActualState().angle))
print 'Move to Actual State, Just a minute.'
for i in range(len(self.Groups)):
self.setJointAnglesOfGroup(self.Groups[i][0], angles[i], tm,
wait=False)
for i in range(len(self.Groups)):
self.seq_svc.waitInterpolationOfGroup(self.Groups[i][0])
except:
print self.configurator_name, 'post servo on motion trouble'
# turn on hand motors
print 'Turn on Hand Servo'
if self.sc_svc:
self.sc_svc.servoOn()
return 1
def servoOn(self, jname='all', destroy=1, tm=3):
'''
Turn on servo motors at joint specified.
Joints need to be calibrated (otherwise error returns).
*Troubleshooting*
When this method does not seem to function as expected, try the
following first before you report to the developer's community:
- Manually move the arms to the safe pose where arms do not obstruct
to anything and they can move back to the initial pose by goInitial.
Then run the command again.
- Make sure the emergency switch is toggled back.
- Try running goActual() then servoOn().
If none of the above did not solve your issue, please report with:
- The result of this command (%ROSDISTRO% is "indigo" as of May 2017):
Ubuntu$ rosversion hironx_ros_bridge
Ubuntu$ dpkg -p ros-%ROSDISTRO%-hironx-ros-bridge
@type jname: str
@param jname: The value 'all' works iteratively for all servos.
@param destroy: Not used.
@type tm: float
@param tm: Second to complete.
@rtype: int
@return: 1 or -1 indicating success or failure, respectively.
'''
# check joints are calibrated
if not self.isCalibDone():
waitInputConfirm(
'!! Calibrate Encoders with checkEncoders first !!\n\n')
return -1
# check servo state
if self.isServoOn():
return 1
# check jname is acceptable
if jname == '':
jname = 'all'
#self.rh_svc.power('all', SWITCH_ON) #do not switch on before goActual
try:
waitInputConfirm('!! Robot Motion Warning (SERVO_ON) !!\n\n'
'Confirm RELAY switched ON\n'
'Push [OK] to switch servo ON(%s).' % (jname))
except: # ths needs to change
self.rh_svc.power('all', SWITCH_OFF)
raise
# Need to reset JointGroups.
# See https://code.google.com/p/rtm-ros-robotics/issues/detail?id=277
try:
# remove jointGroups
self.seq_svc.removeJointGroup("larm")
self.seq_svc.removeJointGroup("rarm")
self.seq_svc.removeJointGroup("head")
self.seq_svc.removeJointGroup("torso")
except:
print(
self.configurator_name,
'Exception during servoOn while removing JoingGroup. ' +
_MSG_ASK_ISSUEREPORT)
try:
# setup jointGroups
self.setSelfGroups() # restart groups
except:
print(
self.configurator_name,
'Exception during servoOn while removing setSelfGroups. ' +
_MSG_ASK_ISSUEREPORT)
try:
self.goActual() # This needs to happen before turning servo on.
time.sleep(0.1)
self.rh_svc.servo(jname, SWITCH_ON)
time.sleep(2)
# time.sleep(7)
if not self.isServoOn(jname):
print self.configurator_name, 'servo on failed.'
raise
except:
print self.configurator_name, 'exception occured'
try:
angles = self.flat2Groups(
map(numpy.rad2deg,
self.getActualState().angle))
print 'Move to Actual State, Just a minute.'
for i in range(len(self.Groups)):
self.setJointAnglesOfGroup(self.Groups[i][0],
angles[i],
tm,
wait=False)
for i in range(len(self.Groups)):
self.seq_svc.waitInterpolationOfGroup(self.Groups[i][0])
except:
print self.configurator_name, 'post servo on motion trouble'
# turn on hand motors
print 'Turn on Hand Servo'
if self.sc_svc:
is_servoon = self.sc_svc.servoOn()
print('Hands Servo on: ' + str(is_servoon))
if not is_servoon:
print(
'One or more hand servos failed to turn on. Make sure all hand modules are properly cabled ('
+ _MSG_RESTART_QNX + ') and run the command again.')
return -1
else:
print('hrpsys ServoController not found. Ignore this if you' +
' do not intend to use hand servo (e.g. NEXTAGE Open).' +
' If you do intend, then' + _MSG_RESTART_QNX +
' and run the command again.')
return 1
