Using the RoboMoco (Robosapien Motion Compiler)
RoboMoco is a Python program that allows you to control your Robosapien from a computer via a serial port. I created it to see what kinds of complex moves I can get my Robosapien V2 to do. I’ll share my thoughts on this in a future post. In the meantime, here’s a quick guide on how to use RoboMoco:
- z = stop (whatever the robot is doing)
- x = attention (stand up straight)
- q = quit (exit this command-line program)
- To execute any built-in command, type the bytecode in hex format and hit ENTER. For example, to make the RS V2 laugh, enter: 6c. You can view the source for a list of commands.
- To execute a macro, type the macro name and hit ENTER. For example, enter: lhand.grab
The neat thing about the macro feature is that you can built macros on top of other macros, so you can make your Robosapien do pretty sophisticated actions. Robodance seems to do something similar but I have not tried it and it is not open-source.
I am using this program to communicate with an Arduino hooked inside my RS V2.
To get RoboMoco working, you need to install Python (I use 2.6) and download getch.py and robomoco.py. After that, just open a console terminal and type:
python robomoco.py
Here’s the source for RoboMoco:
# robomoco.py -- Robosapien motion compiler
# Modified: 9/19/10
# Author: Ben Bongalon (ben@borglabs.com)
# This program controls your Robosapien from a computer via serial port.
# You can send the raw bytecodes or better yet, define macros to make your bot
# perform complex moves. For more details, see
# http://borglabs.com/projects/projects/robosapien-v2/robomoco
#
# NOTE: I changed Left/Right naming convention so that it now is based on the
# robot's perspective. That is, 'left' refers to the robot's left side.
import sys
import time
import serial
import re
import getch # read keystrokes
# Global variables... yeah, I'm a bad boy
lastCmdToken = None
#############################################################################
# Robosapien V2 high-level macro commands... to make him do complex moves
#############################################################################
Rsv2Macros = {
'wait' : 'DL1S', # do nothing for 1s
'reset' : 'RST', # reset body pose
'stop' : 'STP', # stop
'sen' : 'VIS DL2S SON', # vision and sound sensors on/off
'arms.up' : 'BAU REP9X',
'arms.down' : 'BAD REP9X',
'bend.down' : 'BBF DL200 BBF DL200 BBF',
'bend.up' : 'BBB REP4X',
'look.right' : 'HBR REP7X',
'look.left' : 'HBL REP7X',
'look.around' : 'look.left REP3X DL500 look.right REP3X DL500 ' +
'look.left DL200 look.left',
'larm.out' : 'LAO DL200 LAO DL200 LAU REP4X DL200 LAU REP8X',
'rarm.out' : 'RAO DL200 RAO DL200 RAU REP4X DL200 RAU REP8X',
'rhand.open' : 'look.right DL200 look.right DL200 look.right',
'lhand.open' : 'look.left DL200 look.left DL200 look.left',
'lhand.close' : 'rhand.open',
'rhand.close' : 'lhand.open',
'lhand.grab' : 'atn DL1S DL1S larm.out DL1S lhand.open DL1S ' +
'LAI DL200 LAI DL200 ' +
'BRORD DL200 BROR DL200 BROR DL200 lhand.close',
'rhand.grab' : 'atn DL1S DL1S rarm.out DL1S rhand.open DL1S ' +
'RAI DL500 RAI DL500 ' +
'BROLD DL200 BROL DL200 BROL DL200 rhand.close',
'lhand.pickup' : 'LPKL DL1S REP9X stop DL1S atn',
'rhand.pickup' : 'RPKL DL1S REP9X stop DL1S atn',
'bow' : 'bf DL200 HEU DL50 HEU DL200 bb HED DL100 bb HED',
'dblchop' : 'LCHOP DL2S DL2S RCHOP', # double karate chop
# Drill sergeant commands
'atn' : 'QRS', # Attention!
'lface' : 'WAL', # Left face (doesn't work yet)
'rface' : 'WAR', # Right face (doesn't work yet)
'aface' : 'NOP', # About face (doesn't work yet)
'fmarch' : 'WAF', # Forward march
'bmarch' : 'WAB', # Backward march
}
#############################################################################
# Robosapien V2 commands
#############################################################################
Rsv2Commands = {
# The inject and repeat are new pseudo-commands I created to extend
# Robosapien's capability. I hijacked unused bytecodes of the RSV2.
# Inject a time delay between commands
'DL10' : 0xC1, # Delay 10ms
'DL20' : 0xC2, # Delay 20ms
'DL50' : 0xC3, # Delay 50ms
'DL100' : 0xC4, # Delay 100ms
'DL200' : 0xC5, # Delay 200ms
'DL500' : 0xC6, # Delay 500ms
'DL1S' : 0xC7, # Delay 1s
'DL2S' : 0xC8, # Delay 2s
'DL5S' : 0xC9, # Delay 5s
# Repeat the last command one or more times
'NOREP' : 0xD0, # No repeat .. not needed??
'REP' : 0xD1, # Repeat last command
'REP2X' : 0xD2, # Repeat last command 2x
'REP3X' : 0xD3, # Repeat last command 3x
'REP4X' : 0xD4, # Repeat last command 4x
'REP5X' : 0xD5, # Repeat last command 5x
'REP6X' : 0xD6, # Repeat last command 6x
'REP7X' : 0xD7, # Repeat last command 7x
'REP8X' : 0xD8, # Repeat last command 8x
'REP9X' : 0xD9, # Repeat last command 9x
# All opcodes below are mnemonics for built-in RSV2 commands
'NOP' : 0xC0, # No op ($3C1-$3EF are also NOP... reserved??)
# Sensor control
'SON' : 0x83, # Sonic Sensors On/Off
'VIS' : 0x80, # Vision On/Off
'COD' : 0x89, # Manual Color Mode: Daylight
'COY' : 0x8A, # Manual Color Mode: Indoor Yellow
'COW' : 0x8B, # Manual Color Mode: Indoor White
# Programming Modes
'PPE' : 0x81, # Positional Program Entry
'PPP' : 0x52, # Positional Program Play
'PRVI' : 0xF0, # Vision Program
'PRSO' : 0xF1, # Sound Program
'PRGU' : 0xF2, # Guard Mode
'PRCE' : 0xF3, # Clear Entry
'PRCP' : 0xF4, # Clear Program
'PRMA' : 0xF5, # Main Program
'PRP' : 0xF6, # Perform Program
# Reset and Power modes
'QRS' : 0x90, # Gait Change/Quick Reset
'RST' : 0x53, # Reset
'STP' : 0xAA, # Stop
'OFF' : 0x84, # Power Down
'SLP' : 0x86, # Sleep / WakeUp
# Motion commands
'WAF' : 0x00, # Walk Forward
'WAB' : 0x01, # Walk Backward
'WAL' : 0x02, # Walk Left
'WAR' : 0x03, # Walk Right
'BDZF' : 0x6A, # Bulldoze forward
'BDZB' : 0x72, # Bulldoze backward
'LIE' : 0x51, # Lie Down/Sit up/Lie Down/Stand up
'GUP' : 0x64, # Get up
'DAN' : 0x50, # Dance Demo
'MOV' : 0x54, # Movement Demo
'ROA' : 0x82, # Free Roam
'PLV2' : 0x85, # Play with another Robosapien V2
'PLRA' : 0x87, # Play with a Roboraptor
'PLPE' : 0x88, # Play with a Robopet
# Head and torso
'BBF' : 0x19, # bend forward
'BBB' : 0x18, # bend backward
# ... Is a this dup of HBU,etc. ???
'HB2U' : 0x10, # Head and Upper Body up
'HB2D' : 0x11, # Head and Upper Body down
'HB2R' : 0x12, # Head and Upper Body right (also controls hand open/close)
'HB2L' : 0x13, # Head and Upper Body left (also controls hand open/close)
'HBU' : 0x20, # Head and Upper Body up
'HBD' : 0x21, # Head and Upper Body down
'HBR' : 0x22, # Head and Upper Body right (also controls hand open/close)
'HBL' : 0x23, # Head and Upper Body left (also controls hand open/close)
'HEU' : 0x48, # Head up
'HED' : 0x49, # Head down
'HER' : 0x4A, # Head right
'HEL' : 0x4B, # Head left
'HIF' : 0x40, # Hip thrust forward
'HIB' : 0x41, # Hip thrust backward
'HIR' : 0x42, # Hip bend right
'HIL' : 0x43, # Hip bend left
'BROR' : 0x1A, # Body rotate right
'BROL' : 0x1B, # Body rotate left
'BRORU' : 0x1C, # Body rotate right & up
'BROLU' : 0x1D, # Body rotate left & up
'BRORD' : 0x1E, # Body rotate right & down
'BROLD' : 0x1F, # Body rotate left & down
# Arm movements
# @TODO: check if 4 more dirs are supported eg, 3C/3D/3E/3F
'BAU' : 0x38, # Both arms up
'BAD' : 0x39, # Both arms down
'BAI' : 0x3A, # Both arms in
'BAO' : 0x3B, # Both arms out
'LAU' : 0x30, # Left arm up
'LAD' : 0x31, # Left arm down
'LAI' : 0x32, # Left arm in
'LAO' : 0x33, # Left arm out
'RAU' : 0x28, # Right arm up
'RAD' : 0x29, # Right arm down
'RAI' : 0x2B, # Right arm in
'RAO' : 0x2A, # Right arm out
'RTHW' : 0x55, # Right Arm Throw
'RPKL' : 0x56, # Right Arm Low Pickup
'RPKH' : 0x57, # Right Arm High Pickup
'RGRB' : 0x58, # Right Arm Grab
'RGIV' : 0x59, # Right Arm Give
'RROL' : 0x5A, # Right Arm Roll
'RDRP' : 0x6E, # Right Arm Drop
'LTHW' : 0x5C, # Left Arm Throw
'LPKL' : 0x5D, # Left Arm Low Pickup
'LPKH' : 0x5E, # Left Arm High Pickup
'LGRB' : 0x5F, # Left Arm Grab
'LGIV' : 0x60, # Left Arm Give
'LROL' : 0x61, # Left Arm Roll
'LDRP' : 0x6F, # Left Arm Drop
# Complex Moves
'RKICK' : 0x62, # Right Kick
'RPUSH' : 0x65, # Right Push
'RCHOP' : 0x66, # Right Chopr
'LKICK' : 0x63, # Left Kick
'LPUSH' : 0x68, # Left Push
'LCHOP' : 0x67, # Left Chop
'HI5' : 0x69, # High Five
# Canned speech responses
'OOPS' : 0x5B, # Oops
'BABY' : 0x6B, # Hey baby
'HAHA' : 0x6C, # Laugh
'INSL' : 0x6D, # Insult
'PLAN' : 0x70, # Plan
'SPAR' : 0x71, # Spare Change
'BURP' : 0x73, # Burp
'ROAR' : 0x74, # Roar
'DIOD' : 0x75, # Diode
'FTCH' : 0x76, # Fetch
'DNGR' : 0x77, # Danger
'CALM' : 0x78, # Calm down
'HUG' : 0x79, # Group Hug
'NOPRS' : 0x7A, # Don't Press
'LIFE' : 0x7B # At last, intelligent life
}
# Send a bytecode command to the Robosapien
def send_command(port, cmdToken):
global lastCmdToken
print ' => Exec opcode %s' % cmdToken
if cmdToken == 'NOREP':
pass
elif cmdToken == 'REP': # 'repeat' opcode
if lastCmdToken == None:
raise RuntimeError('Repeat opcode cannot be the first opcode')
send_command(port, lastCmdToken)
elif cmdToken.startswith('REP'): # 'repeat 2x to 9x' opcodes
if lastCmdToken == None:
raise RuntimeError('Repeat opcode cannot be the first opcode')
repcnt = int(cmdToken[3:4])
for i in range(repcnt):
send_command(port, lastCmdToken)
elif cmdToken.startswith('DL'): # 'delay' opcodes
if cmdToken == 'DL10':
time.sleep(0.01)
elif cmdToken == 'DL20':
time.sleep(0.02)
elif cmdToken == 'DL50':
time.sleep(0.05)
elif cmdToken == 'DL100':
time.sleep(0.1)
elif cmdToken == 'DL200':
time.sleep(0.2)
elif cmdToken == 'DL500':
time.sleep(0.5)
elif cmdToken == 'DL2S':
time.sleep(2)
elif cmdToken == 'DL5S':
time.sleep(5)
else:
time.sleep(1)
lastCmdToken = cmdToken
elif cmdToken in Rsv2Commands: # handle mnemonics here
opcode = chr(Rsv2Commands[cmdToken])
try:
port.write(opcode)
except:
print 'WARNING: Failed to send command'
lastCmdToken = cmdToken
else:
raise RuntimeError('Unknown byte command %s' % (cmdToken))
# Evaluate and execute the robot command.
# @TODO: make the 'repeat' command work recusively
def exec_command(port, cmdstr):
if cmdstr in Rsv2Commands: # a mnemonic?
send_command(port, cmdstr)
elif cmdstr in Rsv2Macros: # a macro? so break it down
print ' => Expanding macro %s' % cmdstr
tokens = Rsv2Macros[cmdstr].split()
for tok in tokens:
exec_command(port, tok)
elif (len(cmdstr)==2 and
re.match('[0-9A-F][0-9A-F]', cmdstr.upper()) != None):
print 'Send hex command 0x%s' % cmdstr[0:2]
opcode = int(cmdstr[0:2], 16)
try:
port.write(chr(opcode))
except:
print 'WARNING: Failed to send command'
else:
print 'WARNING: Skipping unknown command %s' % cmdstr
# Configure the serial port connection
ser = serial.Serial(
port='/dev/ttyUSB0', # use '/dev/ttyUSB0' or '/dev/ttyS0'
baudrate=9600,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
bytesize=serial.EIGHTBITS
)
# Main program
inkey = getch._Getch()
cmdstr = ''
ser.open()
ser.isOpen()
print 'Type your commands below (q to quit).'
while True:
# get keyboard input
input = inkey()
if input == '':
continue
elif input == 'z' or input == 'Z':
exec_command(ser, 'stop')
cmdstr=''
elif input == 'x' or input == 'X':
exec_command(ser, 'atn')
cmdstr=''
elif input == 'q' or input == 'Q':
ser.close()
exit()
elif input == '\n' or input == '\r':
exec_command(ser, cmdstr)
cmdstr = ''
else:
cmdstr += input
sys.stdout.write(input)
# Echo the bytes received from the serial port
out = ''
try:
while ser.inWaiting() > 0:
ch = ser.read(1)
out += hex(ord(ch)) + ' '
if out != '':
print '\nrcvd: ' + out + '\n'
except:
pass
Links
Part 1 – Command serial interface for the Robosapien V2
Part 2 – Connecting the Arduino to the Robosapien V2
Part 3 – Using RoboMoco to teach your Robosapien new tricks (this page)