PDA

View Full Version : [Question(s)] PhantomX Code 6 legs in to 8 legs

Kar
05-06-2011, 05:13 AM
OK so I'm brand new to the hole robotics and programing thing.
I have been doing a lot of reading on programing and how it works and are slowly starting to
understand it. But it has been prity slow going for me. What you got to under stand is that
I'm not a real computer person unlike a lot of the readers on this forum, but i am trying to be.
My first question of what will probably be many questions to come. Is it really this simple.
Now i have the code for the Phantom X (AKA 6 leged robot) and i am wondering if it is realy as
simple as i think it is to change it to a 8 leg robot code. Or am i just being naive about this
and i need to spend a lot more time reading about things.
OK for my first example i would like to show.
================================================== ==============================================
#define LEG_COUNT 6
/* Body
* We assume 4 legs are on the corners of a box defined by X_COXA x Y_COXA
* Middle legs for a hexapod can be different Y, but should be halfway in X
*/
#define X_COXA 60 // MM between front and back legs /2
#define Y_COXA 60 // MM between front/back legs /2
#define M_COXA 100 // MM between two middle legs /2
/* Legs */
#define L_COXA 52 // MM distance from coxa servo to femur servo
#define L_FEMUR 82 // MM distance from femur servo to tibia servo
#define L_TIBIA 140 // MM distance from tibia servo to foot
================================================== ==============================================
*first thing that comes to attention is the define leg count. simply change that to 8.
*now on to the body. so i now have a new set of legs that need to be plumed in. Now X= front legs
and back legs that making our mid legs Y. Is it as simple as this

x= front legs
xy=mid legs
yx=mid legs
x=back legs

#define X_COXA 60 // MM between front and back legs /2
#define Y_COXA 60 // MM between front/back legs /2
#define XY_COXA 100 // MM between two middle legs /2
#define YX_COXA 100 // MM between two middle legs /2
I hope i have not lost you yet with the questions.

Now the next part of the code seems pretty ezy.
--------------------------------------------------------------------------------------------------
/* Servo IDs */
#define RM_TIBIA 18
#define RF_COXA 2
#define LR_TIBIA 11
#define LF_FEMUR 3
#define RF_TIBIA 6
#define RM_FEMUR 16
#define RM_COXA 14
#define RR_COXA 8
#define LF_TIBIA 5
#define LF_COXA 1
#define LR_FEMUR 9
#define RR_FEMUR 10
#define LM_TIBIA 17
#define RF_FEMUR 4
#define LM_FEMUR 15
#define RR_TIBIA 12
#define LM_COXA 13
#define LR_COXA 7
--------------------------------------------------------------------------------------------------
In this part of the code it shows each of the 18 servos that are used and where they are used on
the hex. Can i just simply duplicate all the mid leg servos and then change the servo number to
the right one. So it would look like this.
24 servos
/* Servo IDs */
#define RM_TIBIA 18
#define RF_COXA 2
#define LR_TIBIA 11
#define LF_FEMUR 3
#define RF_TIBIA 6
#define RM_FEMUR 16
#define RM_COXA 14
#define RR_COXA 8
#define LF_TIBIA 5
#define LF_COXA 1
#define LR_FEMUR 9
#define RR_FEMUR 10
#define LM_TIBIA 17
#define RF_FEMUR 4
#define LM_FEMUR 15
#define RR_TIBIA 12
#define LM_COXA 13
#define LR_COXA 7
#define LM_COXA 19
#define RM_COXA 20
#define LM_FEMUR 21
#define RM_FEMUR 22
#define LM_TIBIA 23
#define RM_TIBIA 24

----------------------------------------------------------------------------------------------
/* Actual positions, and indices of array. */
extern ik_req_t endpoints[LEG_COUNT];
#define RIGHT_FRONT 0
#define RIGHT_REAR 1
#define LEFT_FRONT 2
#define LEFT_REAR 3
#define RIGHT_MIDDLE 4
#define LEFT_MIDDLE 5
----------------------------------------------------------------------------------------------
Above is the original code. Mine would simply be.
/* Actual positions, and indices of array. */
extern ik_req_t endpoints[LEG_COUNT];
#define RIGHT_FRONT 0
#define RIGHT_REAR 1
#define LEFT_FRONT 2
#define LEFT_REAR 3
#define RIGHT_MIDDLE 4
#define LEFT_MIDDLE 5
#define RIGHT_MIDDLE 6
#define LEFT_MIDDLE 7

I gess that's about it for the first part of questions.

Below i pasted the original code that is unchanged

-------------------------------------------------------------------------------------------

/************************************************** ****************************
* Inverse Kinematics for 4/6 legged bots using 3DOF lizard legs
*
* Auto-Generated by NUKE!
*FRONT VIEW ^ ==0 0==
* /\___/\ | | 0==[___]==0 |
* / \ Z | |
*
* TOP VIEW
* \ / ^
* \_____/ |
*___| |___ X
* |_____|
* / \ Y->
* / \
************************************************** ***************************/
#ifndef NUKE
#define NUKE
#define LEG_COUNT 6
/* Body
* We assume 4 legs are on the corners of a box defined by X_COXA x Y_COXA
* Middle legs for a hexapod can be different Y, but should be halfway in X
*/
#define X_COXA 60 // MM between front and back legs /2
#define Y_COXA 60 // MM between front/back legs /2
#define M_COXA 100 // MM between two middle legs /2
/* Legs */
#define L_COXA 52 // MM distance from coxa servo to femur servo
#define L_FEMUR 82 // MM distance from femur servo to tibia servo
#define L_TIBIA 140 // MM distance from tibia servo to foot

/* Servo IDs */
#define RM_TIBIA 18
#define RF_COXA 2
#define LR_TIBIA 11
#define LF_FEMUR 3
#define RF_TIBIA 6
#define RM_FEMUR 16
#define RM_COXA 14
#define RR_COXA 8
#define LF_TIBIA 5
#define LF_COXA 1
#define LR_FEMUR 9
#define RR_FEMUR 10
#define LM_TIBIA 17
#define RF_FEMUR 4
#define LM_FEMUR 15
#define RR_TIBIA 12
#define LM_COXA 13
#define LR_COXA 7

/* A leg position request (output of body calcs, input to simple 3dof solver). */
typedef struct{
int x;
int y;
int z;
float r;
} ik_req_t;

/* Servo ouptut values (output of 3dof leg solver). */
typedef struct{
int coxa;
int femur;
int tibia;
} ik_sol_t;

/* Actual positions, and indices of array. */
extern ik_req_t endpoints[LEG_COUNT];
#define RIGHT_FRONT 0
#define RIGHT_REAR 1
#define LEFT_FRONT 2
#define LEFT_REAR 3
#define RIGHT_MIDDLE 4
#define LEFT_MIDDLE 5
extern BioloidController bioloid;

/* Parameters for manipulating body position */
extern float bodyRotX; // body roll
extern float bodyRotY; // body pitch
extern float bodyRotZ; // body rotation
extern int bodyPosX;
extern int bodyPosY;

/* Parameters for gait manipulation */
extern int Xspeed;
extern int Yspeed;
extern float Rspeed;
extern int tranTime;
extern float cycleTime;
extern int stepsInCycle;
extern int liftHeight;
extern int step;

/* Gait Engine */
extern int gaitLegNo[]; // order to move legs in
extern ik_req_t gaits[]; // gait position

/* convert radians to a dynamixel servo offset */
/* select a gait pattern to use */
void gaitSelect(int GaitType);

#include "gaits.h"
/* find the translation of the coxa point (x,y) in 3-space, given our rotations */
ik_req_t bodyIK(int X, int Y, int Z, int Xdisp, int Ydisp, float Zrot);
/* given our leg offset (x,y,z) from the coxa point, calculate servo values */
ik_sol_t legIK(int X, int Y, int Z);
/* ties all of the above together */
void doIK();
/* setup the starting positions of the legs. */
void setupIK();

#endif

lnxfergy
05-06-2011, 02:58 PM
I think most of the above probably isn't going to work. From what I can recall of the top of my head you'll need to:

Add the servo IDs (but, unlike above, you need new names for servos, you can't reuse RM_TIBIA for instance -- same goes for endpoints definitions, you can't redefine the same name)
The real work will be in modifying the function "doIK" which does the IK computation, you need to notice that it currently does 6 calculations, you'll need it to do 8.
Also, you'll need to update each gait generator function to work on 8 legs
And, the gaitSetup function to match the gait generator needs
And, the setupIK function (to initialize 8 legs, not 6).

What might actually be easier is to take the sketch for a Quadruped, and simply pair each set of legs, so the front and 3rd leg on the left side would each have the same position.... although, I think that only works for forward speeds, you might need to do something funky for turning....

-Fergs

Kar
05-09-2011, 08:44 PM
Ok since I made the above post i have done alot of reading and wached alot of video tutorials. So I now understand my above belfes where very noob.
Moving on i have started to make changes to my NukeEditor.py pile.
I have made as many changes as i think that there needs to be. But some how i got a felling i have missed a few things that i dont know about yet. This is where im gona need some help.

I have compiled the below code and it has retern with no problems that making me think that its all good. But when i past the newly changed file in to the Arbotix folder i get a error when i try to open the nuke page in pypose.

This is my error message that comes up in a dos window.
================================================== ==============
PyPose starting...
Traceback (most recent call last):
self.panel = panelClass(self,self.port)
File "tools\NukeEditor.py", line 252, in __init__
NameError: global name 'servoSize' is not defined
================================================== ==============

below is the changed NukeEditor.py file

================================================== ==============
#!/usr/bin/env python
"""
PyPose: Bioloid pose system for arbotiX robocontroller
Nuke: The Nearly Universal Kinematics Engine
Copyright (c) 2009-2010 Michael E. Ferguson. All right reserved.
This program is free software; you can redistribute it and/or modify
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software Foundation,
Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
"""
import wx, sys, os
import project
from ToolPane import ToolPane
from ax12 import *
import time
from commander import Commander
# Which IK models to load?
from models.manifest import iKmodels
# TODO: encapsulate pose editor into a seperate class, so we can load it for neutral tuning...
# TODO: Release 0013
# Sign test for mammal3, image for neutral
# visual indication that captures are done.
################################################## #############################
# nuke editor window
class NukeEditor(ToolPane):
""" editor for NUKE engine. """
BT_RELAX = wx.NewId()
BT_LIMITS = wx.NewId()
BT_NEUTRAL = wx.NewId()
BT_SIGN = wx.NewId()
BT_TUNER = wx.NewId()
BT_EXPORT = wx.NewId()
BT_DRIVE = wx.NewId()
ID_LEGCOUNT = wx.NewId()
ID_IKTYPE = wx.NewId()
ID_GAIT_BOX = wx.NewId()
# sizes
ID_LEN_COXA = wx.NewId()
ID_LEN_FEMUR = wx.NewId()
ID_LEN_TIBIA = wx.NewId()
ID_BODY_X = wx.NewId()
ID_BODY_Y = wx.NewId()
ID_BODY_MIDY = wx.NewId()
ID_COG_X = wx.NewId()
ID_COG_Y = wx.NewId()
# servo assignments
ID_RF_COXA = wx.NewId()
ID_RF_FEMUR = wx.NewId()
ID_RF_TIBIA = wx.NewId()
ID_RR_COXA = wx.NewId()
ID_RR_FEMUR = wx.NewId()
ID_RR_TIBIA = wx.NewId()
ID_LF_COXA = wx.NewId()
ID_LF_FEMUR = wx.NewId()
ID_LF_TIBIA = wx.NewId()
ID_LR_COXA = wx.NewId()
ID_LR_FEMUR = wx.NewId()
ID_LR_TIBIA = wx.NewId()
ID_ANY = wx.NewId()
def __init__(self, parent, port=None):
ToolPane.__init__(self, parent, port)
# default data
self.signs = "+"*18
self.curpose = ""
self.ikChoice = ""
self.legChoice = ""

sizer = wx.GridBagSizer(10,10)

# Body Dimensions
temp = wx.StaticBox(self, -1, 'Body Dimensions')
temp.SetFont(wx.Font(10, wx.DEFAULT, wx.NORMAL, wx.BOLD))
bodyBox = wx.StaticBoxSizer(temp,orient=wx.VERTICAL)
bodySizer = wx.GridBagSizer(5,5)

bodySizer.Add(wx.StaticText(self, -1, "Leg"), (0,0), wx.GBSpan(1,1), wx.EXPAND | wx.TOP, 10)
bodySizer.Add(wx.StaticText(self, -1, "Offsets"), (4,0), wx.GBSpan(1,1), wx.EXPAND | wx.TOP, 10)
bodySizer.Add(wx.StaticText(self, -1, "COG Offset"), (8,0), wx.GBSpan(1,1), wx.EXPAND | wx.TOP, 10)

# VARS = coxaLen, femurLen, tibiaLen, xBody, yBody, midyBody, xCOG, yCOG
self.vars = list()
self.vars.append(wx.SpinCtrl(self, self.ID_ANY, '50', min=1, max=5000))
self.vars.append(wx.SpinCtrl(self, self.ID_ANY, '50', min=1, max=5000))
self.vars.append(wx.SpinCtrl(self, self.ID_ANY, '50', min=1, max=5000))
self.picture = wx.StaticBitmap(self, bitmap=wx.Bitmap("tools/images/leg.jpg"))
self.vars.append(wx.SpinCtrl(self, self.ID_ANY, '50', min=1, max=5000))
self.vars.append(wx.SpinCtrl(self, self.ID_ANY, '50', min=1, max=5000))
self.vars.append(wx.SpinCtrl(self, self.ID_ANY, '50', min=1, max=5000))
self.picture1 = wx.StaticBitmap(self, bitmap=wx.Bitmap("tools/images/body.jpg"))
self.vars.append(wx.SpinCtrl(self, self.ID_ANY, '0', min=-200, max=200))
self.vars.append(wx.SpinCtrl(self, self.ID_ANY, '0', min=-200, max=200))
# IK styles/Config
temp = wx.StaticBox(self, -1, 'IK Config')
temp.SetFont(wx.Font(10, wx.DEFAULT, wx.NORMAL, wx.BOLD))
configBox = wx.StaticBoxSizer(temp,orient=wx.VERTICAL)
configSizer = wx.GridBagSizer(5,5)
self.ikType = wx.ComboBox(self, self.ID_IKTYPE, choices=iKmodels.keys())

# Number of Legs
hbox = wx.BoxSizer(wx.HORIZONTAL)
configSizer.Add(wx.StaticText(self, -1, "# of Legs:"),(1,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
self.legCount = wx.ComboBox(self, self.ID_LEGCOUNT, choices=["4","6","8"])
# Actions buttons
temp = wx.StaticBox(self, -1, 'Actions')
temp.SetFont(wx.Font(10, wx.DEFAULT, wx.NORMAL, wx.BOLD))
actionBox = wx.StaticBoxSizer(temp,orient=wx.VERTICAL)
actionSizer = wx.GridBagSizer(5,5)
actionSizer.Add(wx.StaticText(self, -1, "Capture Limits:"), (0,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
actionSizer.Add(wx.StaticText(self, -1, "Capture Neutral:"), (1,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
actionSizer.Add(wx.StaticText(self, -1, "Set/Test Signs:"), (2,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
#actionSizer.Add(wx.StaticText(self, -1, "Test Drive:"), (3,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)

self.signButton = wx.Button(self, self.BT_SIGN, 'Go')
#driveButton = wx.Button(self, self.BT_DRIVE, 'Drive')
#driveButton.Disable()

# Servo ID Selections
temp = wx.StaticBox(self, -1, 'Servo Assignments')
temp.SetFont(wx.Font(10, wx.DEFAULT, wx.NORMAL, wx.BOLD))
servoBox = wx.StaticBoxSizer(temp,orient=wx.VERTICAL)
servoSizer = wx.GridBagSizer(5,5)

servoSizer.Add(wx.StaticText(self, -1, "LF Coxa:"), (0,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL| wx.TOP, 10)
servoSizer.Add(wx.StaticText(self, -1, "LF Femur:"), (1,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "LF Tibia:"), (2,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "RF Coxa:"), (0,2), wx.GBSpan(1,1), wx.ALIGN_CENTER_VERTICAL| wx.TOP, 10)
servoSizer.Add(wx.StaticText(self, -1, "RF Femur:"), (1,2), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "RF Tibia:"), (2,2), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "LM Coxa:"), (4,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "LM Femur:"), (5,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "LM Tibia:"), (6,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "RM Coxa:"), (4,2), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "RM Femur:"), (5,2), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "RM Tibia:"), (6,2), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "ML Coxa:"), (8,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "ML Femur:"), (9,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "ML Tibia:"), (10,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "MR Coxa:"), (8,2), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "MR Femur:"), (9,2), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "MR Tibia:"), (10,2), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)

servoSizer.Add(wx.StaticText(self, -1, "LR Coxa:"), (12,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "LR Femur:"), (13,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "LR Tibia:"), (14,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "RR Coxa:"), (12,2), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "RR Femur:"), (13,2), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(wx.StaticText(self, -1, "RR Tibia:"), (14,2), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)

# SERVOS = Coxa, Femur, Tibia (LF, RF, LM, RM, ML, MR, LR, RR)
self.servos = list()
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '2', min=1, max=self.parent.project.count)) # LF
servoSizer.Add(self.servos[0], (0,1), wx.GBSpan(1,1), wx.EXPAND | wx.TOP, 10)
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '4', min=1, max=self.parent.project.count))
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '6', min=1, max=self.parent.project.count))
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '1', min=1, max=self.parent.project.count)) # RF
servoSizer.Add(self.servos[3], (0,3), wx.GBSpan(1,1), wx.EXPAND | wx.TOP, 10)
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '3', min=1, max=self.parent.project.count))
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '5', min=1, max=self.parent.project.count))
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '14', min=1, max=self.parent.project.count)) # LM
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '16', min=1, max=self.parent.project.count))
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '18', min=1, max=self.parent.project.count))
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '13', min=1, max=self.parent.project.count)) # RM
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '15', min=1, max=self.parent.project.count))
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '17', min=1, max=self.parent.project.count))
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '8', min=1, max=self.parent.project.count)) # ML
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '10', min=1, max=self.parent.project.count))
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '12', min=1, max=self.parent.project.count))
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '7', min=1, max=self.parent.project.count)) # MR
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '9', min=1, max=self.parent.project.count))
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '11', min=1, max=self.parent.project.count))
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '20', min=1, max=self.parent.project.count)) # LR
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '22', min=1, max=self.parent.project.count))
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '24', min=1, max=self.parent.project.count))
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '19', min=1, max=self.parent.project.count)) # RR
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '21', min=1, max=self.parent.project.count))
self.servos.append(wx.SpinCtrl(self, self.ID_ANY, '23', min=1, max=self.parent.project.count))
# NUKE Label
nukeIt = wx.StaticText(self, -1, "NUKE: Kinematics Made Easy")
nukeIt.SetFont(wx.Font(15, wx.DEFAULT, wx.NORMAL, wx.BOLD, wx.ITALIC))
# Buttons
hbox = wx.BoxSizer(wx.HORIZONTAL)
gaitBut = wx.Button(self,-1,'Gait Builder')
gaitBut.Disable()
viewBut = wx.Button(self,-1, 'Viewer/Tuner')
viewBut.Disable()
self.SetSizer(sizer)
self.SetAutoLayout(1)
sizer.Fit(self)
# event handling
wx.EVT_BUTTON(self, self.BT_LIMITS, self.doLimits)
wx.EVT_BUTTON(self, self.BT_NEUTRAL, self.doNeutral)
wx.EVT_BUTTON(self, self.BT_SIGN, self.doSignTest)
wx.EVT_BUTTON(self, self.BT_DRIVE, self.doWalkTest)
wx.EVT_BUTTON(self, self.BT_EXPORT, self.doExport)
wx.EVT_COMBOBOX(self, self.ID_IKTYPE, self.doIKType)
wx.EVT_COMBOBOX(self, self.ID_LEGCOUNT, self.doLegCount)
wx.EVT_SPINCTRL(self, self.ID_ANY, self.save)
################################################## #########################
# sitrep checks
def doChecks(self, checks):
for c in checks:
if c == "project":
if self.parent.project.name == "":
self.parent.sb.SetBackgroundColour('RED')
self.parent.timer.Start(30)
return 0
elif c == "port":
if self.port == None:
self.parent.sb.SetBackgroundColour('RED')
self.parent.sb.SetStatusText('No Port Open...')
self.parent.timer.Start(30)
return 0
elif c == "ik":
if self.legCount.GetValue == "":
self.parent.sb.SetBackgroundColour('RED')
self.parent.timer.Start(30)
return 0
else:
print "Unknown System Check:", c
# all systems go!
return 1
################################################## #########################
# export
def doExport(self, e=None):
if self.doChecks(["project","ik"]) == 0:
return
else:
# get directory name to save to
dlg = wx.DirDialog(self,'Directory for Sketch Export' )
if dlg.ShowModal() == wx.ID_OK:
skDir = dlg.GetPath()
else:
return
print "Writing a NUKE sketch to:",skDir
# map servo name to ID
servoMap = dict()
servoMap["LF_COXA"] = int(self.servos[0].GetValue())
servoMap["LF_FEMUR"] = int(self.servos[1].GetValue())
servoMap["LF_TIBIA"] = int(self.servos[2].GetValue())
servoMap["RF_COXA"] = int(self.servos[3].GetValue())
servoMap["RF_FEMUR"] = int(self.servos[4].GetValue())
servoMap["RF_TIBIA"] = int(self.servos[5].GetValue())
if str(self.legCount.GetValue()) == "6":
servoMap["LM_COXA"] = int(self.servos[6].GetValue())
servoMap["LM_FEMUR"] = int(self.servos[7].GetValue())
servoMap["LM_TIBIA"] = int(self.servos[8].GetValue())
servoMap["RM_COXA"] = int(self.servos[9].GetValue())
servoMap["RM_FEMUR"] = int(self.servos[10].GetValue())
servoMap["RM_TIBIA"] = int(self.servos[11].GetValue())
if str(self.legCount.GetValue()) == "8":
servoMap["ML_COXA"] = int(self.servos[12].GetValue())
servoMap["ML_FEMUR"] = int(self.servos[13].GetValue())
servoMap["ML_TIBIA"] = int(self.servos[14].GetValue())
servoMap["MR_COXA"] = int(self.servos[15].GetValue())
servoMap["MR_FEMUR"] = int(self.servos[16].GetValue())
servosMap["MR_TIBIA"] = int(self.servos[17].GetValue())
servoMap["LR_COXA"] = int(self.servos[18].GetValue())
servoMap["LR_FEMUR"] = int(self.servos[19].GetValue())
servoMap["LR_TIBIA"] = int(self.servos[20].GetValue())
servoMap["RR_COXA"] = int(self.servos[21].GetValue())
servoMap["RR_FEMUR"] = int(self.servos[22].GetValue())
servoMap["RR_TIBIA"] = int(self.servos[23].GetValue())

# setup model parameters
params = dict()
params["legs"] = str(self.legCount.GetValue())
params["@VAL_LCOXA"] = str(self.vars[0].GetValue())
params["@VAL_LFEMUR"] = str(self.vars[1].GetValue())
params["@VAL_LTIBIA"] = str(self.vars[2].GetValue())
params["@VAL_XCOXA"] = str(self.vars[3].GetValue())
params["@VAL_YCOXA"] = str(self.vars[4].GetValue())
params["@VAL_MCOXA"] = str(self.vars[5].GetValue())
params["@SERVO_COUNT"] = str(self.parent.project.count)
params["@SERVO_INDEXES"] = ""
for k,v in servoMap.items():
params["@SERVO_INDEXES"] = params["@SERVO_INDEXES"] + "#define " + k + " " + str(v) + "\n"
params["@SERVO_MINS"] = "int mins[] = {"+str(self.parent.project.poses["ik_min"])+"};"
params["@SERVO_MAXS"] = "int maxs[] = {"+str(self.parent.project.poses["ik_max"])+"};"
# Simple Heuristics (TODO: Replace with a gait builder)
params["@X_STANCE"] = str(self.vars[0].GetValue())
params["@Y_STANCE"] = str(self.vars[0].GetValue() + self.vars[1].GetValue())
params["@Z_STANCE"] = str(int(0.75*self.vars[2].GetValue()))
params["@LIFT_HEIGHT"] = str(int(0.2*self.vars[2].GetValue()))
code = dict()
current = ""
for line in template:
if line.find("@") == 0 and current == "":
current = line.strip().rstrip()
elif line.find("@END_SECTION") > -1:
current = ""
else:
try:
code[current] = code[current] + line
except:
code[current] = line
# load the parameters for our particular model
modelDir = iKmodels[self.ikType.GetValue()].folder
current = ""
for line in template:
if line.find("@") == 0 and current == "":
current = line.strip().rstrip()
elif line.find("@END_SECTION") > -1:
current = ""
else:
try:
code[current] = code[current] + line
except:
code[current] = line
code.pop("")
templates = dict()
sketch = os.path.split(skDir)[1]
# for each file
for fileName in templates.keys():
# insert code blocks
for var, val in code.items():
templates[fileName] = templates[fileName].replace(var,val)
# search and replace variables
for var, val in params.items():
if var.find("@") == 0:
templates[fileName] = templates[fileName].replace(var,val)
for k,v in servoMap.items():
templates[fileName] = templates[fileName].replace("@NEUTRAL_"+k, str(self.parent.project.poses["ik_neutral"][v-1]))
templates[fileName] = templates[fileName].replace("@SIGN_"+k, self.signs[v-1:v])
# save and reopen as lines
open(skDir+"/temp","w").write(templates[fileName])
# process IF/ELSE/END
i = 0
out = None
if fileName.endswith(".pde"):
if os.path.exists(skDir+"/"+fileName):
# open a different file, not the actual sketch
out = open(skDir+"/sketch.NEW","w")
else:
out = open(skDir+"/"+fileName,"w")
else:
out = open(skDir+"/"+fileName,"w")
while i < len(template):
if template[i].find("@IF") >= 0:
# do we include this?
line = template[i][template[i].find("@IF")+3:].strip().rstrip()
var = line[0:line.find(" ")].rstrip()
val = line[line.find(" ")+1:].strip().rstrip()
i = i + 1
if params[var] == val:
while template[i].find("@ELSE") < 0 and template[i].find("@END") < 0:
print>>out, template[i].rstrip()
i = i + 1
while template[i].find("@END") < 0:
i = i + 1
else:
while template[i].find("@ELSE") < 0 and template[i].find("@END") < 0:
i = i + 1
if template[i].find("@ELSE") >= 0:
i = i + 1 # don't output @ELSE
while template[i].find("@END") < 0:
print>>out, template[i].rstrip()
i = i + 1
else:
print>>out, template[i].rstrip()
i = i + 1
out.close()
################################################## #########################
# data management
if self.parent.project.name == "":
# Disable it all
self.ikType.Disable()
self.legCount.Disable()
for i in range(8):
self.vars[i].Disable()
for i in range(18):
self.servos[i].Disable()
elif self.parent.project.nuke == "":
# Disable, but save defaults to project
self.legCount.Disable()
for i in range(8):
self.vars[i].Disable()
for i in range(18):
self.servos[i].Disable()
self.save()
else:
nukeStr = self.parent.project.nuke.rstrip()
# primary data
self.ikType.SetValue(nukeStr[0:nukeStr.find(",")])
nukeStr = nukeStr[nukeStr.find(",")+1:]
self.legCount.SetValue(nukeStr[0:nukeStr.find(",")])
nukeStr = nukeStr[nukeStr.find(",")+1:]
self.signs = nukeStr[0:nukeStr.find(",")]
nukeStr = nukeStr[nukeStr.find(",")+1:]
# vars
for i in range(8):
self.vars[i].SetValue(int(nukeStr[0:nukeStr.find(",")]))
nukeStr = nukeStr[nukeStr.find(",")+1:]
# servos
for i in range(17):
self.servos[i].SetValue(int(nukeStr[0:nukeStr.find(",")]))
nukeStr = nukeStr[nukeStr.find(",")+1:]
self.servos[17].SetValue(int(nukeStr))
# Data loaded, now Enable/Disable as needed
if self.ikType.GetValue() == "":
# Disable it all
self.legCount.Disable()
for i in range(8):
self.vars[i].Disable()
for i in range(18):
self.servos[i].Disable()
else:
self.legCount.SetItems([str(c) for c in iKmodels[self.ikType.GetValue()].legoptions])
self.legCount.Enable()
if self.legCount.GetValue() == "":
# Disable all but legCount
for i in range(8):
self.vars[i].Disable()
for i in range(18):
self.servos[i].Disable()
else:
# Enable all
for i in range(8):
self.vars[i].Enable()
for i in range(18):
self.servos[i].Enable()
if self.legCount.GetValue() == "4":
# disable those few
for i in range(6):
self.servos[i+6].Disable()
self.vars[5].Disable()
def save(self, e=None):
""" Export the NUKE string to our project, mark project as needing a save. """
nukeStr = self.ikType.GetValue() + "," + str(self.legCount.GetValue()) + "," + self.signs + ","
# vars
for i in range(8):
nukeStr = nukeStr + str(self.vars[i].GetValue()) + ","
# servos
for i in range(17):
nukeStr = nukeStr + str(self.servos[i].GetValue()) + ","
nukeStr = nukeStr + str(self.servos[17].GetValue())
self.parent.project.nuke = nukeStr
self.parent.project.save = True
################################################## #########################
# Limit & Neutral capture
def doLimits(self, e=None):
if self.doChecks(["project","port"]) == 0:
return
else:
print "Relax servos for capture..."
self.parent.doRelax()
print "Capturing limits..."
self.parent.project.poses["ik_min"] = project.pose("",self.parent.project.count)
self.parent.project.poses["ik_max"] = project.pose("",self.parent.project.count)
self.parent.project.save = True
self.captureLimits(1)
def captureLimits(self, servoID, end=0, prev=0, error=0):
""" Capture the limits of this servo, recursively call next. """
if servoID <= self.parent.project.count:
if end == 0:
result = wx.ID_OK
if error == 0: # first time through, ask user to prepare
#dlg = wx.MessageDialog(self.parent, 'Click OK when you have moved servo ' + str(servoID) + ' to one extreme', 'Capture Limits', wx.OK | wx.CANCEL)
dlg = NukeDialog(self.parent, 'Capture Limits', 'Click OK when you have moved\n servo ' + str(servoID) + ' to one extreme')
result = dlg.ShowModal()
dlg.Destroy()
if result == wx.ID_OK:
try:
a = self.port.getReg(servoID,P_PRESENT_POSITION_L, 2)
a = a[0] + (a[1]<<8)
# get other limit
self.captureLimits(servoID,1,a)
except:
if error < 3: # try again
self.captureLimits(servoID, 0, 0, error+1)
else: # fail and move on
dlg = wx.MessageDialog(self.parent, 'Unable to read servo ' + str(servoID), 'Capture Error', wx.OK)
dlg.ShowModal()
self.captureLimits(servoID+1)
elif result == 42:
self.captureLimits(servoID-1)
else:
result = wx.ID_OK
if error == 0: # first time through, ask user to prepare
dlg = NukeDialog(self.parent, 'Capture Limits', 'Click OK when you have moved\n servo ' + str(servoID) + ' to the other extreme')
result = dlg.ShowModal()
dlg.Destroy()
if result == wx.ID_OK:
try:
a = self.port.getReg(servoID,P_PRESENT_POSITION_L, 2)
a = a[0] + (a[1]<<8)
# do our update to pose
if prev > a:
self.parent.project.poses["ik_max"][servoID-1] = prev
self.parent.project.poses["ik_min"][servoID-1] = a
print "Capture Limits", servoID, ":", a, " to ", prev
else:
self.parent.project.poses["ik_max"][servoID-1] = a
self.parent.project.poses["ik_min"][servoID-1] = prev
print "Capture Limits", servoID, ":", prev, ",", a
self.captureLimits(servoID+1)
except:
if error < 3: # try again
self.captureLimits(servoID, 1, prev, error+1)
else: # fail and move on
dlg = wx.MessageDialog(self.parent, 'Unable to read servo ' + str(servoID), 'Capture Error', wx.OK)
dlg.ShowModal()
self.captureLimits(servoID+1)
elif result == 42:
self.captureLimits(servoID)
def doNeutral(self, e = None):
""" Capture the Neutral Position. """
if self.doChecks(["project","port"]) > 0:
print "Relax servos for capture..."
self.parent.doRelax()
print "Capturing neutral..."
# show dialog with what neutral should like for this bot
modelClassName = iKmodels[self.ikType.GetValue()].folder
dlg = NeutralDialog(self.parent, 'Capture Neutral Position', "tools/models/"+modelClassName+"/neutral.jpg")
#dlg = wx.MessageDialog(self.parent, 'Click OK when ready!', 'Capture Neutral Position', wx.OK | wx.CANCEL)
if dlg.ShowModal() == wx.ID_OK:
self.parent.project.poses["ik_neutral"] = project.pose("",self.parent.project.count)
errors = "could not read servos: "
for servo in range(self.parent.project.count):
pos = self.port.getReg(servo+1,P_PRESENT_POSITION_L, 2)
if pos != -1:
self.parent.project.poses["ik_neutral"][servo] = pos[0] + (pos[1]<<8)
else:
errors = errors + str(servo+1) + ", "
if errors != "could not read servos: ":
self.parent.sb.SetStatusText(errors[0:-2],0)
""" Load the model for our IK solution. """
modelClassName = iKmodels[self.ikType.GetValue()].folder
if "tools/models/"+modelClassName not in sys.path:
sys.path.append("tools/models/"+modelClassName)
modelModule = __import__(modelClassName, globals(), locals(), [modelClassName])
modelClass = getattr(modelModule, modelClassName)
# make instance
self.legChoice = self.legCount.GetValue()
model = modelClass(int(self.legChoice),True) # legs/debug/GaitGen
# VARS = coxaLen, femurLen, tibiaLen, xBody, yBody, midyBody, xCOG, yCOG
model.L_COXA = int(self.vars[0].GetValue())
model.L_FEMUR = int(self.vars[1].GetValue())
model.L_TIBIA = int(self.vars[2].GetValue())
model.X_COXA = int(self.vars[3].GetValue())
model.Y_COXA = int(self.vars[4].GetValue())
model.Y_MID = int(self.vars[5].GetValue())
# SERVOS = Coxa, Femur, Tibia (LF, RF, LM, RM, ML, MR, LR, RR)
model["RF_COXA"] = int(self.servos[3].GetValue())-1
model["RF_FEMUR"] = int(self.servos[4].GetValue())-1
model["RF_TIBIA"] = int(self.servos[5].GetValue())-1
model["LF_COXA"] = int(self.servos[0].GetValue())-1
model["LF_FEMUR"] = int(self.servos[1].GetValue())-1
model["LF_TIBIA"] = int(self.servos[2].GetValue())-1
model["RR_COXA"] = int(self.servos[21].GetValue())-1
model["RR_FEMUR"] = int(self.servos[22].GetValue())-1
model["RR_TIBIA"] = int(self.servos[23].GetValue())-1
model["LR_COXA"] = int(self.servos[18].GetValue())-1
model["LR_FEMUR"] = int(self.servos[19].GetValue())-1
model["LR_TIBIA"] = int(self.servos[20].GetValue())-1
model["RIGHT_FRONT"] = [60,90,100]
model["RIGHT_REAR"] = [-60,90,100]
model["LEFT_FRONT"] = [60,-90,100]
model["LEFT_REAR"] = [-60,-90,100]
model["RM_COXA"] = int(self.servos[9].GetValue())-1
model["RM_FEMUR"] = int(self.servos[10].GetValue())-1
model["RM_TIBIA"] = int(self.servos[11].GetValue())-1
model["LM_COXA"] = int(self.servos[6].GetValue())-1
model["LM_FEMUR"] = int(self.servos[7].GetValue())-1
model["LM_TIBIA"] = int(self.servos[8].GetValue())-1
model["RIGHT_MIDDLE"] = [0,90,100]
model["LEFT_MIDDLE"] = [0,-90,100]
model["MR_COXA"] = int(self.servos[15].GetValue())-1
model["MR_FEMUR"] = int(self.servos[16].GetValue())-1
model["MR_TIBIA"] = int(self.servos[17].GetValue())-1
model["ML_COXA"] = int(self.servos[12].GetValue())-1
model["ML_FEMUR"] = int(self.servos[13].Getvalue())-1
model["ML_TIBIA"] = int(self.servos[14].GetValue())-1
model["RIGHT_MIDDLE"] = [0,90,100]
model["LEFT_MIDDLE"] = [0,-90,100]

model.mins = self.parent.project.poses["ik_min"]
model.maxs = self.parent.project.poses["ik_max"]
model.neutrals = self.parent.project.poses["ik_neutral"]
model.signs = [1+(-2*(t=="-")) for t in self.signs]

self.model = model

################################################## #########################
def writePose(self, pose, dt):
# set pose size -- IMPORTANT!
self.port.execute(253, 7, [self.parent.project.count])
self.port.execute(253, 8, [0] + project.extract(pose))
self.port.execute(253, 9, [0, dt%256, dt>>8,255,0,0])
self.port.execute(253, 10, list())
def doSignTest(self, e=None):
if self.doChecks(["project","port","ik"]) > 0:
self.signs = self.model.doSignTest(self)
self.save()
def doWalkTest(self, e=None):
if self.doChecks(["port"]) > 0:
comm = Commander(self, self.port.ser)
comm.Center()
#if self.doChecks(["project","port","ik"]) > 0:
# self.model.gaitGen = self.model.defaultGait
# for i in range(24):
# self.model.doIK()
# self.writePose(self.model.nextPose, 50)
# #time.sleep(0.001)
# self.model.gaitGen = None

################################################## #########################
# holla back -- the callbacks
def doIKType(self, e=None):
""" Set IKType, make leg box visible """
self.save()
self.legCount.SetItems([str(c) for c in iKmodels[self.ikType.GetValue()].legoptions])
self.legCount.Enable()
def doLegCount(self, e=None):
""" Set the # of legs, make everything else visible. """
self.legChoice = self.legCount.GetValue()
for i in range(8):
self.vars[i].Enable()
for i in range(18):
self.servos[i].Enable()
if str(self.legCount.GetValue()) == "4":
for i in range(6):
self.servos[i+6].Disable()
self.vars[5].Disable()
self.save()

################################################## #########################
# A message box, with backup ability
class NukeDialog(wx.Dialog):
def __init__(self, parent, title, msg):
wx.Dialog.__init__(self, parent, 0, title, size=(390, 120))
vbox = wx.BoxSizer(wx.VERTICAL)
vbox.Add(wx.StaticText(self, -1, msg, (340,100)), 1, wx.ALIGN_CENTER | wx.BOTTOM | wx.TOP, 10)
# Cancel | Backup | OK
hbox = wx.BoxSizer(wx.HORIZONTAL)
closeButton = wx.Button(self, wx.ID_CANCEL, 'Cancel', size=(100, 50))
backupButton = wx.Button(self, 42, 'Oops, Back Up', size=(120, 50))
okButton = wx.Button(self, wx.ID_OK, 'Ok', size=(80, 50))
vbox.Add(hbox, 1, wx.ALIGN_CENTER | wx.BOTTOM, 25)
self.SetSizer(vbox)
wx.EVT_BUTTON(self, 42, self.doBackUp)
def doBackUp(self, e=None):
self.EndModal(42)
################################################## #########################
# A message box, that can display an image
class NeutralDialog(wx.Dialog):
# TODO: This crap was generated by wxGlade, should probably be cleaned up...
def __init__(self, parent, title, image):
#def __init__(self, *args, **kwds):
#kwds["style"] = wx.DEFAULT_DIALOG_STYLE
wx.Dialog.__init__(self, parent, 0, title, style=wx.DEFAULT_DIALOG_STYLE) #*args, **kwds)
self.label_3 = wx.StaticText(self, -1, "Click OK when you've positioned the robot as shown:")
self.bitmap_1 = wx.StaticBitmap(self, -1, wx.Bitmap(image, wx.BITMAP_TYPE_ANY))
self.button_1 = wx.Button(self, wx.ID_CANCEL, "Cancel")
self.button_2 = wx.Button(self, wx.ID_OK, "OK")
#self.SetTitle("dialog_2")
self.label_3.SetFont(wx.Font(11, wx.DEFAULT, wx.NORMAL, wx.BOLD, 0, ""))
sizer_2 = wx.BoxSizer(wx.VERTICAL)
sizer_3 = wx.BoxSizer(wx.HORIZONTAL)