PDA

View Full Version : [Contest Entry] Archer- Zenta's first biped



Zenta
05-26-2010, 04:52 AM
Hi,

I'll edit and update this first post due to the contest entry. (still in progress)

My main goal with this project was to make an 6 DOF biped fully controlled using Inverse Kinematics instead of using a set of sequences.

I've spent an hour of trying to make a "workshop video" :lol: My first take was just to long. But this one is about 10:23. At the end I'm demonstrating my work on my very first SES based biped. I'm using a new Atom Pro board from Basic Micro called ARC-32, you can find more information about it here (http://www.basicmicro.com/ARC32_p_141.html).

Videos:

Workshop video part 1:

http://www.youtube.com/watch?v=9H7Hf5L9fu0

Introduction video:

http://www.youtube.com/watch?v=zDgUqonI0vs

Workshop video part 2:

http://www.youtube.com/watch?v=o2MCLYBa3Vk

Some pictures of the biped, codename "ICS" (Internal Cog Shifter). It doesn't look very good or impressive at the moment. Simply because I want the kinematics and walking to work before I start on the design.

http://forums.trossenrobotics.com/gallery/files/1/5/3/5/6dof_biped_05_800.jpg

The ARC-32 board are mounted on a slider. The main idea was to make it easier to shift the COG from one side to another without needing to translate the body to much. A closer picture of the slider part:

http://forums.trossenrobotics.com/gallery/files/1/5/3/5/6dof_biped_01_800.jpg

There are also some more pictures in the TRC gallery.

I'll update this thread when I've done some more progress.

-Zenta

Adam
05-26-2010, 09:11 AM
Great video. Thanks for sharing.

I've never seen biped IK like that before. Awesome work. I knew it was only a matter of time before you would work this out. I may actually build a biped now....

darkback2
05-26-2010, 09:42 AM
Great work. I love how much life you are getting out of your bots. Now I see how you do it. I don't understand it mind you...but I see it. :)

I really like how you are using one controller for multiple robots, and ohiow much you are getting out of the controller.

great work.

DB

Chench
05-26-2010, 10:10 AM
A fine brood of robots you have Zenta. I've always wondered how you managed to do so many simultaneous movements eg. in your A-pod videos. Now I know, that controller is ingenious. I'm looking forward to seeing how you apply the techniques from your other 'bots to the biped

Robonaut
05-26-2010, 02:51 PM
Zenta, you continue to amaze me. Your robots are so inspiring. I did quickly realize that your remote was the other haft of what makes your robots so gracefully and look so alive. In fact your work has inspired me to focus more on a controller/editing system more than on my robot. I read the threads about your remote and how it was developed and built. I'm just plan impress with it and all the people that contributed to the current state of it. Kudo's to all of you.

I still have to read those threads more because some of it's content was over my head. I have gotten lost several times while trying to read them. My brain just goes numb when I try to get though them. :happy:

Mainly I do not have the in-depth understanding of engineering that you guys do. But I have done a lot of electronic assembly and some coding. Enough to make me think on a good day that I can actually make the remote/editing system that I envision.

One of the things I'm in the process of working out is using 2 potentiometers. I love how you added yours to joy sticks. That is just way cool. There just seems to be nothing about your work that isn't totally awesome. You only have haft a bi-ped and I'm already blown away by it. :eek:

Thanks for the video. It was good to see and hear you speak about your work.

Zenta
05-27-2010, 03:16 AM
Thanks for your comments! I'm glad you found the video interesting despite for my bad english.:(


I did quickly realize that your remote was the other haft of what makes your robots so gracefully and look so alive. In fact your work has inspired me to focus more on a controller/editing system more than on my robot. I read the threads about your remote and how it was developed and built. I'm just plan impress with it and all the people that contributed to the current state of it. Kudo's to all of you.
Yes, the remote controller is a very important part for making it easier to get better control of the robots. I would recommend that you just take your time trying to understand the work Jim, Kurt and Xan has done with the remote combined with the basic Phoenix code.




One of the things I'm in the process of working out is using 2 potentiometers. I love how you added yours to joy sticks. That is just way cool. There just seems to be nothing about your work that isn't totally awesome. You only have haft a bi-ped and I'm already blown away by it. :eek:

Thanks for the video. It was good to see and hear you speak about your work.

Thanks! I find the extra potentiometer very useful for analog functions you need fast/easy access to. I believe there are still some improvements that can be done when it comes to the controller part, like having several function buttons closer to the joysticks for easier access (similar to what you have on the PS2 controller). I'll probably try to make a new version of the DIY remote, some day.. ;)

-Zenta

Quantum
05-28-2010, 01:05 PM
Really Nice. I cant wait to see some walking gaits.

Zenta
07-21-2010, 04:22 AM
Hi,

Here is a little video of Archer just doing some simple walking. I've not implemented turning yet. This video is about a week old and I've done several improvements to both code and hardware since that. I'll post more info later.

http://www.youtube.com/watch?v=zDgUqonI0vs

Quantum
07-21-2010, 11:18 AM
The counter weight system is brilliant.

Psykoman
07-21-2010, 03:01 PM
Awesome work.

Zenta
07-21-2010, 03:26 PM
Thanks guys!

I've still not decided to just settle with 5 DOF or not. There are some stuff that bugs me a bit:

1. The gear backlash on the 5645's is a real pain.. Do some of you know if the titanium or steal gear's are better. Or a better question, what standard sized servos has no or very little gear backlash (with enough torque of course)? I'm probably the only one here not using the AX-12/18.. But I do hope to get time for that too though.

2. The hub used in the LPA's are still to loose and wobble a bit. I did thighten them really hard and used locktite. Going for only 5 DOF would solve that problem though.

I made a main body/head with cockpit for Archer. I'm pretty satisfied with the result. But its a bit heavy (195 gram), making one of plastic (or vacuum forming) would work I think. With the increased weight the gear backlash gets even worse and gets more unstable. Again, going for 5 DOF would make the result better. The problem with only 5 DOF is mainly turning, but I've seen many bipeds doing that fine though. Anyone with some tip about the 5 DOF turning while walking part?

Here are some pictures:
http://forums.trossenrobotics.com/gallery/files/1/5/3/5/p7202560.jpg

Sideview:
http://forums.trossenrobotics.com/gallery/files/1/5/3/5/p7202561.jpg

Posing:
http://forums.trossenrobotics.com/gallery/files/1/5/3/5/p7202579.jpg

Together with Phoenix and T-Hex:
http://forums.trossenrobotics.com/gallery/files/1/5/3/5/archerthexphoenix.jpg

A picture while walking:
http://forums.trossenrobotics.com/gallery/files/1/5/3/5/p7202588.jpg

Posing again.. :wink:
http://forums.trossenrobotics.com/gallery/files/1/5/3/5/p7202578.jpg

Any comments are appreciated!

-Zenta

Adam
07-21-2010, 05:40 PM
In general I love your work, but the chassis/head looks oversized to me.

Upgrayd
07-21-2010, 05:46 PM
Great work here. I really like the weight shifting mechanism. Very clever design.

lnxfergy
07-21-2010, 06:14 PM
The problem with only 5 DOF is mainly turning, but I've seen many bipeds doing that fine though. Anyone with some tip about the 5 DOF turning while walking part?

I think the issue here is that most bots do turning with 5DOF are fairly light, and also not very precise. You have to slide on your feet to turn with a typical 5DOF biped -- this gets messy if the ground isn't too smooth, or the bot is so heavy that friction really builds up.

-Fergs

Zenta
07-22-2010, 05:29 AM
In general I love your work, but the chassis/head looks oversized to me.

I totally agree about that, after assembling the chassis/head I realized it was a bit to big. FYI I've already removed the chassis, and after my vacation I'll make a new one.

Thanks for your honest comment, highly appreciated!

-Zenta

Zenta
07-22-2010, 05:45 AM
I think the issue here is that most bots do turning with 5DOF are fairly light, and also not very precise. You have to slide on your feet to turn with a typical 5DOF biped -- this gets messy if the ground isn't too smooth, or the bot is so heavy that friction really builds up.

-Fergs
Thanks for your input. I'll probably keep the 6. DOF.

darkback2
07-22-2010, 09:17 AM
If the chassis is too big, its only by a little itsy bitsy bit. I think for a chicken walker it is just about spot on. Are you planning to add arms...a camera...

(I know...don't play with other peoples toys!)

Also from the short video, I like how well you have it walking. Are you maybe picking the feet up too far?

Can't wait to see more video.

DB

DresnerRobotics
07-22-2010, 12:38 PM
Fantastic work as always Kare!

Nice new forum title btw, well deserved!

jes1510
07-22-2010, 12:49 PM
Me Likey! Is this just an experiment or are you building for Mech Warfare? I can't wait to see video of this thing.

Zenta
07-22-2010, 03:27 PM
Fantastic work as always Kare!

Nice new forum title btw, well deserved!
LOL ! Thanks for your comment and the cool title. :D

SteamAutomaton
07-22-2010, 07:52 PM
Thanks for your input. I'll probably keep the 6. DOF.
Would giving the foot yaw (7 DOF) give you a better turning ability?

Yours,
SA;)

Zenta
08-17-2010, 01:05 PM
Would giving the foot yaw (7 DOF) give you a better turning ability?

Yours,
SA;)
It might do that but the 6.DOF will do the job though, the problem with rotation are when using only 5 DOF.

---

I've decided to keep the 6.DOF. During my vacation I didn't get much time for robotics but I got some time for tinkering about the kinematics. Archer can now do 3 DOF successive body rotation, body translation, walk forward/backward, walk sideway, turn and walk in an arc. All walking features can be combined and all variables can be changed real-time (legliftheight, walking length, speed++). There are some minor bugs on the rotation kinematics and when doing some combinations, but overall it works very good. I hope to post a new video in the near future.

Here is a picture of Archer doing Yaw body rotation:
http://robot-kits.org/wp-content/blogs.dir/3/files/2010/08/P8163274_crop.jpg

Btw, I've started a blog.. The blog are still under construction and its not updated to my current work. My main goal with the blog is to unify all robotic projects I've been working on, so thats its easier for everyone to follow my work instead of scrolling through endless amount of forum posts. A link to the blog: http://robot-kits.org/
Hopefully someone find it interesting.

parallax
08-17-2010, 04:40 PM
I actually stumbled upon your blog the other day via a google search. I was like, "well well, what do we have here?" :)

It's really nice to be able to see all of your creations in one place, and it shows a great progression of how far your skill level has come when it comes to creating robots. Each successive generation is more clean and polished than the last. This is shown well in this (http://robot-kits.org/projects/) page where you can simply scroll down to see most of them. Most importantly, I think it shows the dedication and time it takes to reach the skill level you have attained (although I believe were gifted from the start). Creations such as A-Pod and ARCher exemplify that effort and are the product of countless hours of tinkering, learning, and working I imagine. Anyone that looks at that blog will be able to see the path you took to get to where you are today, and be inspired.

I guess what I've been trying to say with all of that is: great start on the blog! I look forward to seeing it fleshed out with more information and content.

As for Archer, I am curious: The system of payload shifting you have in place is fantastic. Have you played around at all with the rate, magnitude, acceleration and deceleration of the shift to try for an optimal combination? I noticed in the video the speed of the shift definitely varies- do you have a particular system in place to control that? Have you ever had the payload shift too fast and knock Archer over? Or, have you had it not shift enough?

Sorry for the excessive questions; I'm just really curious :o

-parallax

Zenta
08-19-2010, 02:35 AM
As for Archer, I am curious: The system of payload shifting you have in place is fantastic. Have you played around at all with the rate, magnitude, acceleration and deceleration of the shift to try for an optimal combination? I noticed in the video the speed of the shift definitely varies- do you have a particular system in place to control that? Have you ever had the payload shift too fast and knock Archer over? Or, have you had it not shift enough?


Hi, thanks for your comment about the blog.
The ICS (Internal Cog Shifter) function is pretty simple at the moment. The speed varies accordingly to the walking speed. I do plan to try out some other methods later. Maybe adding an IMU and use the ICS for minor adjustments. The ICS system do have a weakness under fast walking, I believe that when the speed comes to a certain point other walking methods must be used, more dynamic gait I think.. I'll probably need to study more about that part. And I know very little about how to work with IMU's. Does anyone know about writings about IMU's and how to interpret the accelerometer and gyro data altogether into useful data for balancing?

I guess I've to "google" it..

Slugman
08-19-2010, 05:22 PM
Have a look through Aeroquad.com - I don't know much about IMUs, but they do, & I am sure that you can extrapolate from their programming code so you can use it in a robot. They also use Arduinos, so there has to be something useable in there for an Arbotix controlled robot. Yes, I have investigated the combining of what is on that site with the info that is on this site . . . . . :wink: Damn power to weight ratios arn't good enough! :sad: . . . . . . Unless I had RX 64's & money to burn . . . . . . :cool:
With the ICS - Try reducing its effect once you exceed a certain speed, but at a much faster/steeper rate than you were increasing it relative to the walking speed. I.E. Make the ICS dynamic, as well as the gait.

mannyr7
08-19-2010, 06:01 PM
He's using Hitec hobby servos and a Basic Micro ARC-32 microcontroller.

SK.
08-20-2010, 01:35 AM
And I know very little about how to work with IMU's. Does anyone know about writings about IMU's and how to interpret the accelerometer and gyro data altogether into useful data for balancing?

I guess I've to "google" it..
There's some RC autopilots and AHRS out there that might serve as inspiration e.g. :
http://en.wikipedia.org/wiki/OpenPilot
http://diydrones.com/
http://www.mikrokopter.de/ucwiki/en/MikroKopter

In general, using Kalman Filtering for fusing information of the different sensors is used in nearly all of these systems, so "Kalman Filter, EKF, IMU, INS, attitude estimation" are probably good for googling.
Note that attitude estimation is possible with cheap MEMS sensors, but it's basically impossible to get a good translation estimate purely from these cheap, noisy inertial sensors. In UAVs, GPS generally serves as a reference here.

Zenta
08-25-2010, 12:49 AM
Thanks for your tip! It looks like using the Kalman filter might be the way to go. I do hope to find some code about it somewhere, or else I've to write my own.

tom_chang79
08-25-2010, 06:45 PM
Zenta! I love your COG-shifting scheme. This is the same scheme I've seen on many bipeds during my Lego Mindstorm RIS 2.0 days...

Are you having any torque issues using the 5645MG servos? When I used to have the Biped Scout, I found that anything less than a 5990TG-ish servo would not cut it...

Zenta
08-26-2010, 07:53 AM
Are you having any torque issues using the 5645MG servos? When I used to have the Biped Scout, I found that anything less than a 5990TG-ish servo would not cut it...
Hi,

The 5645 seems to have ok torque for the task, they don't even get hot under normal walking. The leg segments are pretty short though and the total weight is about 1,55 kg. But the part that irritate me most is the gear backlash/slope in the metal gears. Whats your experience with the 5990's, do they have much "play" in the gears?

Zenta
09-25-2010, 03:56 PM
Since I'm entering Archer to the contest I'll update the first post with much more information (all videos, material list, pictures and code) about this project. (Work in progress)

Yesterday evening I made a new workshop video for demonstrating some progress on Archer. Body Yaw Rotation, single leg mode and more walking. Also a short demo of a new gait pattern for Phoenix (Triple gait).


http://www.youtube.com/watch?v=o2MCLYBa3Vk

Inspired by Jonny's work on his latest biped (http://lynxmotion.net/viewtopic.php?f=7&t=6724) I decided to replace the short and simple femur on Archer with a ASB-10 (long C) (http://www.lynxmotion.com/p-401-aluminum-long-c-servo-bracket-with-ball-bearings-two-pack.aspx) + ASB-11 (offset) (http://www.lynxmotion.com/p-402-aluminum-offset-servo-bracket-with-ball-bearings-two-pack.aspx) brackets. I do like this combination of brackets as a femur part for a hexapod (like T-hex) and here as the femur part for a biped. By using these brackets the legs got much more freedom, the ankle and the hip joint can almost touch each-other:

http://robot-kits.org/wp-content/blogs.dir/3/files/2010/09/P9233291.jpg

Another picture while Archer do a "single-leg" movement and holding the position, you can clearly see the good ground clearance:

http://robot-kits.org/wp-content/blogs.dir/3/files/2010/09/P9233288.jpg

Archer standing much taler than before:

http://robot-kits.org/wp-content/blogs.dir/3/files/2010/09/P9233292.jpg

So far I'm very impressed by the servos, after about 8 minutes of walking and body movement the servos doesn't even get hot! :shock: They are not cold of course, but they seem to handle the load pretty well.

darkback2
09-26-2010, 12:55 AM
When I see all of the incredible things you have done and are doing I am awed. You do amazing things. Keep up the great and inspiring work.

DB

Zenta
09-26-2010, 01:05 AM
Thanks DB!

I'm glad you liked it. :D

elaughlin
09-26-2010, 07:43 AM
Your projects really are amazing. I need you to do my coding, haha.

Zenta
11-11-2010, 10:29 AM
Hi,

I've not had time to work more with the Archer code. There is still some "cleaning" and tuning to do but the code work pretty good though. So I've decided to share the code. The code is based on Xan's V2.0 code and Kurt's ARC-32 code version.

The two parts I did spent some time to write and modify is the IK and the gait part.

This is the gait code:

GaitSelect
;Gait selector
IF (GaitType = 0) THEN ;9 step Biped gait, Note: Half Walking cycle!
'GaitLegNr(cRightLeg) = 1
'GaitLegNr(cLeftLeg) = 8

'NrLiftedPos = 1
HalfLiftHeigth = 1
LiftedMiddlePos = 5
TLDivFactor = 13
StepsInGait = 9
NomGaitSpeed = 60
ENDIF

return
;--------------------------------------------------------------------
;[GAIT Sequence]
GaitSeq
;Check IF the Gait is in motion
GaitInMotion = ((ABS(TravelLengthX)>cTravelDeadZone) | (ABS(TravelLengthZ)>cTravelDeadZone) | (ABS(TravelRotationY)>cTravelDeadZone) )

;Don't cycle the gait while not walking:
IF NOT Walking Then
GaitStep = 1 'Keep it to the first step until leg start walking again
;Decide what leg to set active before start walking after standing still:
IF TravelRotationY > cTravelDeadZone THEN ;The robot are commanded to walk and turn left
ActiveLeg = cRightLeg ; This means that the left leg are going to be lifted first
ELSEIF TravelRotationY < -cTravelDeadZone
ActiveLeg = cLeftLeg ; This means that the right leg are going to be lifted first
ENDIF
;Same method as for the TravelRotationY, but making sure that sidewalking has higher priority then TravelRotation by placing the code after:
IF TravelLengthX > cTravelDeadZone THEN ;The robot are commanded to sidewalk to the left
ActiveLeg = cRightLeg ; This means that the left leg are going to be lifted first
ELSEIF TravelLengthX < -cTravelDeadZone
ActiveLeg = cLeftLeg
ENDIF
ENDIF

;Calculate Gait sequence
LastLeg = 0
for LegIndex = 0 to 1 ; for all legs

if LegIndex = 1 then ; last leg
LastLeg = 1
endif

GOSUB BipedGAIT [LegIndex]
next ; next leg
return
;--------------------------------------------------------------------
;[BipedGAIT], Half walk Cycle !
; Note: The first and the last GaitStep are used for shifting COG (SC), both leg (feet) are also on ground and walking while SC
BipedGAIT [GaitCurrentLegNr]

;Clear values under the cTravelDeadZone
IF (GaitInMotion=0) THEN
TravelLengthX=0
TravelLengthZ=0
TravelRotationY=0
ENDIF

;Prevent that the feet crashes into eachother while doing sidewalking:
;The robot are commanded to sidewalk to the right and the opposite leg (now the left leg) are the active leg:
IF (TravelLengthX > cMaxOppositeTravel) AND (ActiveLeg = cLeftLeg) THEN
TravelLengthX = cMaxOppositeTravel

ELSEIF (TravelLengthX < -cMaxOppositeTravel) AND (ActiveLeg = cRightLeg)
TravelLengthX = -cMaxOppositeTravel
ENDIF

IF ActiveLeg = GaitCurrentLegNr THEN 'Is this leg the active walking leg?
IF ((GaitStep = 1) or (GaitStep = 2)) THEN 'SC towards the active leg + Walk
'Shift COG routine here:
IF ActiveLeg = cRightLeg THEN
'Shift COG over to the right leg:
CogShifterAngle1 = -cSliderHalfMaxMinDeg * GaitStep
SCBodyPosX = -CogShifterAngle1/cSCBPX 'just for testing..
ELSE
'Shift COG over to the left leg:
CogShifterAngle1 = cSliderHalfMaxMinDeg * GaitStep
SCBodyPosX = -CogShifterAngle1/cSCBPX 'just for testing..
ENDIF
'Call walk sub
GOSUB WalkInDirection [GaitCurrentLegNr]
ELSEIF ((GaitStep = StepsInGait) or (GaitStep = (StepsInGait-1))) 'SC towards the passive leg + Walk
'Shift COG routine here:
IF ActiveLeg = cRightLeg THEN
'Shift COG over to the left leg:
CogShifterAngle1 = -cSliderHalfMaxMinDeg * (StepsInGait - GaitStep + 1) ;GaitStep = (StepsInGait-1) => cSliderHalfMaxMinDeg*2
SCBodyPosX = -CogShifterAngle1/cSCBPX 'just for testing..
ELSE
'Shift COG over to the right leg:
CogShifterAngle1 = cSliderHalfMaxMinDeg * (StepsInGait - GaitStep + 1)
SCBodyPosX = -CogShifterAngle1/cSCBPX 'just for testing..
ENDIF
'Call walk sub
GOSUB WalkInDirection [GaitCurrentLegNr]
ELSE ' Walking only:
'Call walk sub
GOSUB WalkInDirection [GaitCurrentLegNr]
ENDIF

ELSE 'The current leg is not the active, its moving into the lifting state:
CompLiftToe = FALSE ; Reset
IF ((GaitStep = 1) or (GaitStep = 2) or (GaitStep = StepsInGait) or (GaitStep = (StepsInGait-1))) THEN
'Call walk sub only
GOSUB WalkInDirection [GaitCurrentLegNr]
GearSlopON = False 'don't compensate
ELSE 'Lifting state, lift leg and move it towards the walking direction:
;At this moment the gait is not very universal...
GearSlopON = True 'compensate for gear slop / gear backlash
IF (GaitStep = (LiftedMiddlePos-2)) & GaitInMotion THEN
GaitPosX(GaitCurrentLegNr) = -TravelLengthX/2
GaitPosY(GaitCurrentLegNr) = -LegLiftHeight/(HalfLiftHeigth+1)
GaitPosZ(GaitCurrentLegNr) = -TravelLengthZ/2
GaitRotY(GaitCurrentLegNr) = -TravelRotationY/2

ELSEIF (GaitStep = (LiftedMiddlePos-1)) & GaitInMotion
GaitPosX(GaitCurrentLegNr) = -TravelLengthX/4
GaitPosY(GaitCurrentLegNr) = -LegLiftHeight
GaitPosZ(GaitCurrentLegNr) = -TravelLengthZ/4
GaitRotY(GaitCurrentLegNr) = -TravelRotationY/4

;Leg middle up position:
;Gait in motion | Gait NOT in motion, return to home position:
ELSEIF ((GaitInMotion & (GaitStep = LiftedMiddlePos)) | (NOT GaitInMotion & GaitStep=LiftedMiddlePos & ((ABS(GaitPosX(GaitCurrentLegNr))>2) | (ABS(GaitPosZ(GaitCurrentLegNr))>2) | (ABS(GaitRotY(GaitCurrentLegNr))>2))))
GaitPosX(GaitCurrentLegNr) = 0
GaitPosY(GaitCurrentLegNr) = -LegLiftHeight
GaitPosZ(GaitCurrentLegNr) = 0
GaitRotY(GaitCurrentLegNr) = 0

ELSEIF (GaitStep = (LiftedMiddlePos+1)) & GaitInMotion
GaitPosX(GaitCurrentLegNr) = TravelLengthX/4
GaitPosY(GaitCurrentLegNr) = -LegLiftHeight
GaitPosZ(GaitCurrentLegNr) = TravelLengthZ/4
GaitRotY(GaitCurrentLegNr) = TravelRotationY/4
IF TravelLengthZ <-cTravelDeadZone THEN 'Only comp while walking forward
CompLiftToe = TRUE
ENDIF
;Leg front down position:
ELSEIF (GaitStep = (LiftedMiddlePos+2))& (GaitPosY(GaitCurrentLegNr)<0)
GaitPosX(GaitCurrentLegNr) = TravelLengthX/2
GaitPosY(GaitCurrentLegNr) = -LegLiftHeight/(HalfLiftHeigth+1)
GaitPosZ(GaitCurrentLegNr) = TravelLengthZ/2
GaitRotY(GaitCurrentLegNr) = TravelRotationY/2
IF TravelLengthZ <-cTravelDeadZone THEN
CompLiftToe = TRUE
ENDIF
ENDIF
ENDIF
ENDIF

IF NOT Walking THEN 'this must be improved and integrated to the gait cycle better
CogShifterAngle1 = 0
SCBodyPosX = 0
ENDIF

;Advance to the next step
IF LastLeg THEN ;The last leg in this step
GaitStep = GaitStep+1
IF GaitStep>StepsInGait THEN
GaitStep = 1
IF ActiveLeg = cRightLeg THEN 'At the end of a half walkcycle toogle active leg
ActiveLeg = cLeftLeg
ELSE
ActiveLeg = cRightLeg
ENDIF
ENDIF
ENDIF
#ifdef debugGait
if GaitCurrentLegNr = cLeftLeg then
hserout ["Step#:",dec GaitStep," WalkingState:", dec Walking," GaitInMotion:", dec GaitInMotion, " ActiveLeg (0=R):", dec ActiveLeg, " TLX:", sdec TravelLengthX,|
" GaitPosX:", sdec GaitPosX(GaitCurrentLegNr) , 13]
endif

#endif

return
;--------------------------------------------------------------------
;[WalkInDirection] a part of the gait routine
WalkInDirection [GaitCurrentLegNr]
GaitPosX(GaitCurrentLegNr) = GaitPosX(GaitCurrentLegNr) - (TravelLengthX/TLDivFactor)
GaitPosY(GaitCurrentLegNr) = 0
GaitPosZ(GaitCurrentLegNr) = GaitPosZ(GaitCurrentLegNr) - (TravelLengthZ/TLDivFactor)
GaitRotY(GaitCurrentLegNr) = GaitRotY(GaitCurrentLegNr) - (TravelRotationY/TLDivFactor)
return


---

For adding the 6.DOF (HipYaw rotation) I'm doing a local rotation that compensate for the HipYaw servo. In the main loop I'm also calling the local Yaw rotation (GOSUB RotateLeg()):

;Do IK for all Right legs
LegIndex = cRightLeg
GOSUB BodyIK [-LegPosX(LegIndex)+BodyPosX+SCBodyPosX+GaitPosX(Leg Index) - TotalTransX, |
LegPosZ(LegIndex)+BodyPosZ+GaitPosZ(LegIndex) - TotalTransZ, |
LegPosY(LegIndex)+BodyPosY+GaitPosY(LegIndex) - TotalTransY, |
GaitRotY(LegIndex), LegIndex]

GOSUB RotateLeg [-LegPosX(LegIndex)+BodyPosX+SCBodyPosX+GaitPosX(Leg Index) - TotalTransX, |
LegPosZ(LegIndex)+BodyPosZ+GaitPosZ(LegIndex) - TotalTransZ, GaitRotY(LegIndex)]

GOSUB LegIK [LegPosX(LegIndex)-BodyPosX-SCBodyPosX+LegRotPosX+BodyIKPosX-(GaitPosX(LegIndex) - TotalTransX), |
LegPosY(LegIndex)+BodyPosY-BodyIKPosY+GaitPosY(LegIndex) - TotalTransY, |
LegPosZ(LegIndex)+BodyPosZ-LegRotPosZ-BodyIKPosZ+GaitPosZ(LegIndex) - TotalTransZ, LegIndex]


;Do IK for all Left legs
LegIndex = cLeftLeg
GOSUB BodyIK [LegPosX(LegIndex)-BodyPosX-SCBodyPosX+GaitPosX(LegIndex) - TotalTransX, |
LegPosZ(LegIndex)+BodyPosZ+GaitPosZ(LegIndex) - TotalTransZ, |
LegPosY(LegIndex)+BodyPosY+GaitPosY(LegIndex) - TotalTransY, |
GaitRotY(LegIndex), LegIndex]

GOSUB RotateLeg [LegPosX(LegIndex)+BodyPosX+SCBodyPosX+GaitPosX(Leg Index) - TotalTransX, |
LegPosZ(LegIndex)+BodyPosZ+GaitPosZ(LegIndex) - TotalTransZ, GaitRotY(LegIndex)]

GOSUB LegIK [LegPosX(LegIndex)+BodyPosX+SCBodyPosX-LegRotPosX-BodyIKPosX+GaitPosX(LegIndex) - TotalTransX, |
LegPosY(LegIndex)+BodyPosY-BodyIKPosY+GaitPosY(LegIndex) - TotalTransY, |
LegPosZ(LegIndex)+BodyPosZ-LegRotPosZ-BodyIKPosZ+GaitPosZ(LegIndex) - TotalTransZ, LegIndex]


This is the local rotation and the modified IK part:

;[Local Leg Rotation] YAW rotate only, compensate for hip rotation
RotateLeg [PosX, PosZ, RotationY]

;Calculating totals from center of the body to the feet
CPR_X = PosX
CPR_Z = PosZ

;Successive global rotation matrix:
;Math shorts for rotation: Alfa (A) = Xrotate, Beta (B) = Zrotate, Gamma (G) = Yrotate
;Sinus Alfa = sinA, cosinus Alfa = cosA. and so on...

;First calculate sinus and cosinus for Y rotation:

GOSUB GetSinCos [-BodyRotY1-(RotationY*c1DEC)]
SinA4 = Sin4
CosA4 = Cos4

;Calcualtion of rotation matrix:
;Y rotation only (Yaw)
;LegRotPosX = (CPR_X- (CPR_X*CosA - CPR_Z*SinA))
;LegRotPosZ = (CPR_Z- (CPR_X*SinA + CPR_Z*CosA))

LegRotPosX = (CPR_X*c2DEC - ( CPR_X*c2DEC*CosA4/c4DEC - CPR_Z*c2DEC*SinA4/c4DEC))/c2DEC
LegRotPosZ = (CPR_Z*c2DEC - ( CPR_X*c2DEC*SinA4/c4DEC + CPR_Z*c2DEC*CosA4/c4DEC))/c2DEC

return
;--------------------------------------------------------------------
;[LEG INVERSE KINEMATICS] Calculates the angles of the coxa, Femur and tibia for the given position of the feet
;IKFeetPosX - Input position of the Feet X
;IKFeetPosY - Input position of the Feet Y
;IKFeetPosZ - Input Position of the Feet Z
;IKSolution - Output true IF the solution is possible
;IKSolutionWarning - Output true IF the solution is NEARLY possible
;IKSolutionError - Output true IF the solution is NOT possible
;FemurAngle1 - Output Angle of Femur in degrees
;TibiaAngle1 - Output Angle of Tibia in degrees
;CoxaAngle1 - Output Angle of Coxa in degrees
LegIKLegNr var nib
cGearSlop con 50 'test value
cToeComp con 150
LegIK [IKFeetPosX, IKFeetPosY, IKFeetPosZ, LegIKLegNr]

;Calculate IKCoxaAngle and IKFeetPosXZ
GOSUB GetATan2 [IKFeetPosX, IKFeetPosY]
IF GearSlopON AND (LegIKLegNr = ActiveLeg) THEN
HipRollAngle1(LegIKLegNr) = -((ATan4*180) / 3141) +900 + cGearSlop
ELSE
HipRollAngle1(LegIKLegNr) = -((ATan4*180) / 3141) +900
ENDIF
IKFeetLocalPosY = (XYHyp2/c2DEC)-cHipVertLength

HipYawAngle1(LegIKLegNr) = BodyRotY1+(GaitRotY(LegIKLegNr)*c1DEC) ;

;Length between the Coxa and tars (foot)
;IKFeetPosXZ = XYhyp2/c2DEC

;Using GetAtan2 for solving IKA1 and IKSW
;IKA14 - Angle between SW line and the ground in radians
'GOSUB GetATan2 [IKFeetPosY, IKFeetPosXZ-cCoxaLength], IKA14
GOSUB GetArcTan2 [IKFeetPosZ, IKFeetLocalPosY], IKA14 'from felix float
'GOSUB GetATan2 [IKFeetPosZ, IKFeetLocalPosY], IKA14
;IKSW2 - Length between Femur axis and tars
'IKSW2 = XYhyp2
IKSW2 = SQR((((IKFeetPosZ)*(IKFeetPosZ))+(IKFeetLocalPosY* IKFeetLocalPosY))*c4DEC)


;IKA2 - Angle of the line S>W with respect to the Femur in radians
Temp1 = (((cFemurLength*cFemurLength) - (cTibiaLength*cTibiaLength))*c4DEC + (IKSW2*IKSW2))
Temp2 = ((2*cFemurlength)*c2DEC * IKSW2)
GOSUB GetArcCos [Temp1 / (Temp2/c4DEC) ], IKA24



;IKFemurAngle
FemurAngle1(LegIKLegNr) = -(IKA14 + IKA24) * 180 / 3141 + 900 -cFemurOffset

;IKTibiaAngle
Temp1 = (((cFemurLength*cFemurLength) + (cTibiaLength*cTibiaLength))*c4DEC - (IKSW2*IKSW2))
Temp2 = (2*cFemurlength*cTibiaLength)
GOSUB GetArcCos [Temp1 / Temp2]

TibiaAngle1(LegIKLegNr) = -(900-AngleRad4*180/3141)

;AnklePitchAngle
IF (CompLiftToe AND NOT(LegIKLegNr = ActiveLeg)) THEN
AnklePitchAngle1(LegIKLegNr) = FemurAngle1(LegIKLegNr) - TibiaAngle1(LegIKLegNr) -cAnklePitchOffset + cFemurOffset - BodyRotX1 - cToeComp/(1+SingelLegModeOn) ;Do half ToeComp in single leg mode
'high cBattLED' visual debugging
ELSE
AnklePitchAngle1(LegIKLegNr) = FemurAngle1(LegIKLegNr) - TibiaAngle1(LegIKLegNr) -cAnklePitchOffset + cFemurOffset - BodyRotX1
'low cBattLED
ENDIF
;AnkleRollAngle
IF GearSlopON AND (LegIKLegNr = ActiveLeg) THEN
IF LegIKLegNr = cRightLeg then
AnkleRollAngle1(LegIKLegNr) = -HipRollAngle1(LegIKLegNr)+ 2*cGearSlop - BodyRotZ1
ELSE
AnkleRollAngle1(LegIKLegNr) = -HipRollAngle1(LegIKLegNr)+ 2*cGearSlop + BodyRotZ1
ENDIF
ELSE
IF LegIKLegNr = cRightLeg then
AnkleRollAngle1(LegIKLegNr) = -HipRollAngle1(LegIKLegNr) - BodyRotZ1
ELSE
AnkleRollAngle1(LegIKLegNr) = -HipRollAngle1(LegIKLegNr) + BodyRotZ1
ENDIF
ENDIF
;Set the Solution quality
IF(IKSW2 < (cFemurLength+cTibiaLength-30)*c2DEC) THEN
IKSolution = 1
ELSE
IF(IKSW2 < (cFemurLength+cTibiaLength)*c2DEC) THEN
IKSolutionWarning = 1
ELSE
IKSolutionError = 1
ENDIF
ENDIF


return

I'm sorry for the lack of comments and explanations. Its just that my brain is occupied with a lot of family stuff (we are waiting our 3. son in the beginning of December) and my current secret project involving 31 servos (12x5990 + 19x5645). But I do hope some of you find this stuff useful. ;)

4mem8
04-17-2011, 05:37 AM
Zenta, i am always in aww at what you do, omg i wish i could code like you lol, just amazing what you do. love following your projects. just getting back into it after 14 months off. :-)

Zenta
04-19-2011, 11:36 AM
Zenta, i am always in aww at what you do, omg i wish i could code like you lol, just amazing what you do. love following your projects. just getting back into it after 14 months off. :-)

Thanks for your comment! Good to see your back again!

4mem8
04-19-2011, 01:20 PM
yes thank you Zenta, looking fwd to getting into the building again. unfortunately i am using the sc32 and atom pro28 in the 6dof scout so looks like your code is only for the ARC32 am i correct? and your secret next project omg 31 servos knowing you it will be awesome Zenta:-) lol. the coding for ll those servos omg .

SteamAutomaton
05-09-2011, 06:32 PM
I'm sorry for the lack of comments and explanations. Its just that my brain is occupied with a lot of family stuff (we are waiting our 3. son in the beginning of December) and my current secret project involving 31 servos (12x5990 + 19x5645). But I do hope some of you find this stuff useful. ;)
Hmmm, 31 servos. I am thinking full humanoid?:eek::p

Zenta
05-10-2011, 12:35 PM
Hmmm, 31 servos. I am thinking full humanoid?:eek::p
No, that's the MorpHex project (http://forums.trossenrobotics.com/showthread.php?t=4480).
But, yes I would really like to try a full humanoid one day.;)

Zenta
09-15-2011, 04:15 PM
Just wanted to let you know I NEVER got the first prize from Trossen Robotics.:robotindifferent:

Anyway, I found this video on youtube. The robot I never got to have.

One thing is for sure, this bastard is really fast.


http://www.youtube.com/watch?v=-0E0yzTLMtk

DresnerRobotics
09-20-2011, 02:50 PM
We had some manufacturing issues with them (massive backorder on the physical components).

I just built up 20 kits, so yours will be the first to go out. We've had a LOT of people waiting on these to come back in stock, so you weren't alone. :P

Zenta
09-21-2011, 02:51 AM
Ok, thanks for finally letting me know.