Inverse Kinematics for a robot. Question with math
I have been working on this project for a while and hit a wall.
Below is my code and some pictures.· I·made the Hexapod and designed the controller board below.·
The·code is·working but I would like to add a ramping feature to allow smoother movement at different speeds.
I have tried to describe the code and give some values that are returned thru TV_text.
The math below solves·IK angles of two servos based·on geometric solutions. (2 Servos at the moment but more eventually)
The code is a work in progress and I plan on making it more advanced where I slowly and the hip servo.· Then make movements from the center of the body.
I have an issue with DS14 and DS15 equations towards the end of the code.· I have written comments into the code and where my problem is aswell.
If something is not that clear I will eloborate.·
Post Edited (QuantumRI) : 11/7/2008 6:02:31 AM GMT
Below is my code and some pictures.· I·made the Hexapod and designed the controller board below.·
The·code is·working but I would like to add a ramping feature to allow smoother movement at different speeds.
I have tried to describe the code and give some values that are returned thru TV_text.
The math below solves·IK angles of two servos based·on geometric solutions. (2 Servos at the moment but more eventually)
The code is a work in progress and I plan on making it more advanced where I slowly and the hip servo.· Then make movements from the center of the body.
I have an issue with DS14 and DS15 equations towards the end of the code.· I have written comments into the code and where my problem is aswell.
''********************************************** ''* QRIK V3.0 * ''* Author: Paul Krasinski * ''* Copyright (c) 2008 Quantum Robotics Inc. * ''* See end of file for terms of use. * ''********************************************** '' CON _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 Degrees = (180.0 / pi) Radians = (pi / 180.0) SD = (180.0/1700.00) L1 = 8.0 '8mm Length Section 2 L2 = 11.0 '11mm Length Section 1 OBJ SERVO : "Servo32v3" 'term : "tv_terminal" Not Used to much space taken term : "tv_text" f : "Float32full" fp : "FloatString" Var Long Time Long Loop PUB start | Temp f.start 'Start Float32Full Object term.start(24) 'Start TV _Terminal on pins 24,25,26 repeat temp from 2 to 21 'Preset ALL servo's to center position SERVO.Set(temp,1500) SERVO.Start 'Start Servo32v3 object Time := 80_000_000 Repeat Repeat Loop from 1 to 1000 ' Leg1(8.0,-11.0) 'Cordinate system for the leg. Based on X Y with Servo 2 as the origin. Below in a pic the front right is Servo 1 Leg2(8.0,-12.0,6.0,-7.0) 'This is the part I am trying to ramp from 8,-12 to 6,-7. Waitcnt (cnt + time) Repeat Loop From 1 to 1000 Leg2(6.0,-7.0,8.0,-12.0) Waitcnt (cnt + time) ***I have just included the code for one of the legs since it's the same for all since. There is only one different between l and r side this is the value is S14 & S15 is 2350. This is to mirror the movements on the other side. PRI Leg2(X,Y,XA,YA)|C, Q1, Q2, T1R, T2R, DS14, DS15, CA, Q1A, Q2A, T1RA, T2RA, S14A, S15A, DS14A, DS15A, S14B, S15B, S14, S15, Change14, Change15 Case Loop 1: C := f.fsqr(f.fadd((f.fmul(X,X)),(f.fmul(Y,Y)))) 'Pythagorean Theorem Q1 := f.fmul(degrees,F.atan2(Y,X)) '(y,x) Q2 := f.fmul(degrees,F.Acos((f.fdiv((f.fadd((f.fsub((f.fmul(L1,L1)),(f.fmul(L2,L2)))),(f.fmul(C,C)))),(f.fmul((f.fmul(2.0,L1)),C)))))) T1R := f.fadd(Q1,Q2) T2R := f.fmul(degrees,F.Acos((f.fdiv((f.fsub((f.fadd((f.fmul(L1,L1)),(f.fmul(L2,L2)))),(f.fmul(C,C)))),(f.fmul((f.fmul(L1,L2)),2.0)))))) T1R := f.fabs((f.fsub(T1R,90.0))) CA := f.fsqr(f.fadd((f.fmul(XA,XA)),(f.fmul(YA,YA)))) 'Pythagorean Theorem Q1A := f.fmul(degrees,F.atan2(YA,XA)) '(y,x) Q2A := f.fmul(degrees,F.Acos((f.fdiv((f.fadd((f.fsub((f.fmul(L1,L1)),(f.fmul(L2,L2)))),(f.fmul(CA,CA)))),(f.fmul((f.fmul(2.0,L1)),CA)))))) T1RA := f.fadd(Q1A,Q2A) T2RA := f.fmul(degrees,F.Acos((f.fdiv((f.fsub((f.fadd((f.fmul(L1,L1)),(f.fmul(L2,L2)))),(f.fmul(CA,CA)))),(f.fmul((f.fmul(L1,L2)),2.0)))))) T1RA := f.fabs((f.fsub(T1RA,90.0))) S14 := f.ftrunc((f.fadd((f.fdiv(T1R,SD)),650.0))) This is the value used for servo movement which is plugged in below. These work and move the servos correctly S15 := f.ftrunc((f.fadd((f.fdiv(T2R,SD)),650.0))) S14A := f.ftrunc((f.fadd((f.fdiv(T1RA,SD)),650.0))) S15A := f.ftrunc((f.fadd((f.fdiv(T2RA,SD)),650.0))) DS14 := f.fadd((f.fdiv(T1R,SD)),650.0) This value actually shows a number that is understandable EX DS14= 1500 which is center of a servo DS15 := f.fadd((f.fdiv(T2R,SD)),650.0) These 4 lines are the same as above with out be trunc. I dont understand why they need to be trunc DS14A := f.fadd((f.fdiv(T1RA,SD)),650.0) in order for 23v3 to work correctly. Currently DS15A := f.fadd((f.fdiv(T2RA,SD)),650.0) Change14 := f.fdiv((f.fsub(DS14,DS14A)),1000.0) 'I am taking the first X value and sub from XA then div by 1000 for resolution. This gives me 1000 Change15 := f.fdiv((f.fsub(DS15,DS15A)),1000.0) different x value steps. So I have a nice ramping feature term.str(string("DS14 = ")) 'I use these few lines to see actual values on a screen. I change these back forth saves time from rewriting them term.str(fp.floattostring(DS14)) term.out(13) term.str(string("DS15 = ")) term.str(fp.floattostring(DS15)) term.out(13) term.str(string("Change 14 = ")) term.str(fp.floattostring(change14)) term.out(13) term.str(string("Change 15 = ")) term.str(fp.floattostring(Change15)) term.out(13) DS14 := f.fadd(DS14,Change14) 'The difference/steps between the cordiates added to the orginal to the movement. DS15 := f.fadd(DS15,Change15) I need to trunc this in order for servo set to move otherwise the output is 0. I cant figure this part out. I am taking the change14 adding to the start X value and I want to ramp it to the XA value by increasing this every time the loop increases from 1 to 1000. This works and I have seen it increment on tv out but SERVO set wants a trunc value. When I trunc this it turns to 0 since the value is a decimal. ***I think my probably is to use another method that works with decimals but I have tried some many different things that at this point I am totaly lost. Servo.set(14,S14) 'These two lines control the pulse to the servo Servo.Set(15,S15) term.str(string("S14 = ")) 'Next few lines are just to visually see thre results term.str(fp.floattostring(DS14)) term.out(13) term.str(string("S15 = ")) term.str(fp.floattostring(DS15)) term.out(13) Waitcnt (cnt + time) {{ ┌──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┐ │ TERMS OF USE: MIT License │ ├──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┤ │Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation │ │files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, │ │modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software│ │is furnished to do so, subject to the following conditions: │ │ │ │The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.│ │ │ │THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE │ │WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR │ │COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, │ │ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. │ └──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┘ }}
If something is not that clear I will eloborate.·
Post Edited (QuantumRI) : 11/7/2008 6:02:31 AM GMT
Comments
I am planning something similar with only 4 legs (12 servos).
My intention is to be able to climb a stair.
I can't answer your question, but I have another:
"Why do you work in floating point?"
You have already a SINE table in Eprom and·the ASM access metthod is fully described.
This would be much much faster to compute in integer.
From a doctorate thesis of the university of Brussels, I retain the following:
Level A: Global control; in my case an RC link.
Level B: Global attitude; give the "global" instructions to the legs to follow the instructions.
Level C: Leg control; for each leg, compute and control each servo, probably one cog per leg...
Good Luck
Roland