(inverse)-kinematics

Post all your hardware and software upgrades/projects here.

Re: (inverse)-kinematics

Postby michal » Fri Jun 07, 2013 6:11 am

Thanks! Yes, I'm using Bluetooth + Python. I made a lot of hardware and software modifications. I will make more posts about it soon.
michal
 
Posts: 36
Joined: Thu Jun 06, 2013 7:01 pm
Location: Delft, NL

Re: (inverse)-kinematics

Postby rpcook » Sat Jun 08, 2013 10:22 am

Michal, that's some awesome work. I see that you've implemented body rotations as well as lateral translations with your fixed feet code. Excellent.

How did you choose the location for the feet to maximise the range of motion of Hexy's body?

Hardware and software changes? Could you elaborate? I presume that the software changes were exclusively in the Python code as I can see the legs are moving through a series of fixed points, rather than a smooth motion through a path? What hardware changes were needed to get to this state?

Finally, can we have a go? Source code? :D
Rob
A record of my exploits and musings with Hexy and Sparki: http://robcook.eu, content available under a Creative Commons license.
rpcook
 
Posts: 121
Joined: Tue Sep 04, 2012 6:42 pm
Location: Luton, UK

Re: (inverse)-kinematics

Postby michal » Sun Jun 09, 2013 10:29 am

Here is how to calculate inverse kinematics for Hexy. Inverse kinematics calculates required servo angles from desired body and leg position/orientation.
For the calculations, I'm using numpy library
Code: Select all
import numpy as np


You start with defining your body:
Code: Select all
Hlength = 26
Tlength = 49
Flength = 55
bodyRadius = 100.0
bodyAngle = 50.0/180*np.pi
bodyAngles = [np.pi/2 - bodyAngle, np.pi/2, np.pi/2 + bodyAngle,
              3*np.pi/2 - bodyAngle, 3*np.pi/2, 3*np.pi/2 + bodyAngle]

d = bodyRadius 
body = np.array([[d*np.cos(bodyAngles[0]),d*np.sin(bodyAngles[0]),0],
                         [d*np.cos(bodyAngles[1]),d*np.sin(bodyAngles[1]),0],
                         [d*np.cos(bodyAngles[2]),d*np.sin(bodyAngles[2]),0],
                         [d*np.cos(bodyAngles[3]),d*np.sin(bodyAngles[3]),0],
                         [d*np.cos(bodyAngles[4]),d*np.sin(bodyAngles[4]),0],
                         [d*np.cos(bodyAngles[5]),d*np.sin(bodyAngles[5]),0]])

Image

Then you perform rotation and translation of the body to desired new coordinates. For that you need rotation matrix for X,Y,Z rotations. You can do the rotations separately or combine them in one big matrix as I did:
Remark: It DOES mater in which order you perform rotational transformation (matrix multiplication). For info see Wiki: http://en.wikipedia.org/wiki/Rotation_matrix
Code: Select all
def RotXYZ(point,a):
        Rxyz = [[np.cos(a[2])*np.cos(a[1]), np.cos(a[2])*np.sin(a[1])*np.sin(a[0])-np.sin(a[2])*np.cos(a[0]), np.cos(a[2])*np.sin(a[1])*np.cos(a[0])+np.sin(a[2])*np.sin(a[0])],
                [np.sin(a[2])*np.cos(a[1]), np.sin(a[2])*np.sin(a[1])*np.sin(a[0])+np.cos(a[2])*np.cos(a[0]), np.sin(a[2])*np.sin(a[1])*np.cos(a[0])-np.cos(a[2])*np.sin(a[0])],
                [-np.sin(a[1]), np.cos(a[1])*np.sin(a[0]), np.cos(a[1])*np.cos(a[0])]]
        return np.dot(Rxyz,point)


And you perform Rotation and Translation of the body:
Code: Select all
Tr = [0, 0, 75]
Rot = [-0.2*np.pi/6, 0.2*np.pi/6, -0.1*np.pi/2]

b = Tr + RotXYZ(body.T, Rot).T   #  ".T" means transpose - due to my definition of X,Y,Z coordinates in columns instead of rows

Image

Now I make a small trick to obtain an unique solution for hip angles. I need to create a second set of body points which are just slightly rotated in Rz at first.
Code: Select all
def RotZ(pos,angle):
        Rz = [[np.cos(angle),-np.sin(angle),0],
                [np.sin(angle),np.cos(angle),0],
                [0,0,1]]
        return np.dot(Rz,pos)

b2 = RotZ(body.T, 0.001).T
b2 = Tr + RotXYZ(b2.T, Rot).T

Remark: For clarity, the figure shows the Rz rotation 100x amplified
Image

Now you define/obtain your desired/previous foot position:
Code: Select all
f = [-50, -150, 0]

Image

Next step is to calculate projection P of the foot point F to the plane defined by the body B. For that you need to calculate normal vector of the B-plane. For details see Wiki: http://en.wikipedia.org/wiki/Plane_(geometry) and then project the point to the plane, see e.g. http://stackoverflow.com/questions/8942950/how-do-i-find-the-orthogonal-projection-of-a-point-onto-a-plane
Code: Select all
v0 = b[1] - b[0]  # 2 vectors from any 3 points defining the plane B
v1 = b[2] - b[0]
normal = np.cross(v0, v1)
normal /= np.linalg.norm(normal)
p = f - np.dot(normal, f - b[4])*normal

Image

Now you can calculate hip angle. For that you utilize the second set of body points from which we make a vector: B-B*. This vector is perpendicular to the hip servo zero angle. The second vector is from the projected foot point to to the body point: P-B. Now we calculate angle between those two vectors (see Wiki: http://en.wikipedia.org/wiki/Dot_product):
Remark: You might ask why I didn't use the center body point instead of this cumbersome B*. You must realize that solution for an angle between two vectors is always in range of 0 to 180 degrees. For zero hip servo angle the solution would be exactly 180 deg and any other angle would always be smaller than 180 deg. However, you would not knot to which side to turn, because the sign is always positive. In my calculation, the solution gives 90 degree when hip servo angle is 0 and if you offset these 90 degrees you get the needed servo angle.
Code: Select all
h0 = p - b[4]
h1 = b1[4] - b[4]
Hangle = (np.arccos(np.dot(h0,h1)/np.linalg.norm(h0)/np.linalg.norm(h1))) - np.pi/2

Image

Next step is to calculate knee servo position from know hip length (Hlength) and normalized vector h0
= distance Hlength from B[4] in direction h0
Code: Select all
k = b[4] + Hlength*h0/np.linalg.norm(h0)

Image

Now you need to calculate two lengths of K-F and B-F:
Code: Select all
kf = np.linalg.norm(k - f)
bf = np.linalg.norm(b[4] - f)

Image

The ankle and knee angles are then calculated using cosine law (see wiki: http://en.wikipedia.org/wiki/Cosine_law):
cos(gamma) = (a^2+b^2-c^2)/(2*a*b).
Ankle angle by using thigh and foot lengths (Tlength, Flength) and K-F length.
And knee angle as combination of 2 angles. And of course you have to offset them to obtain servo angles.
Code: Select all
Aangle = np.pi - np.arccos((Tlength**2 + Flength**2 - kf**2)/(2*Tlength*Flength))
Kangle = np.pi - np.arccos((Hlength**2 + kf**2 - bf**2)/(2*Hlength*kf)) + np.arccos((Tlength**2 + kf**2 - Flength**2)/(2*Tlength*kf))

Image

And so you get the solution:
Image
michal
 
Posts: 36
Joined: Thu Jun 06, 2013 7:01 pm
Location: Delft, NL

Re: (inverse)-kinematics

Postby rpcook » Thu Jun 13, 2013 8:24 pm

Michal. Wow. That's such an informative post!

I look forward to digesting this information in some detail this weekend. Love the graphs. Thanks!
Rob
A record of my exploits and musings with Hexy and Sparki: http://robcook.eu, content available under a Creative Commons license.
rpcook
 
Posts: 121
Joined: Tue Sep 04, 2012 6:42 pm
Location: Luton, UK

Re: (inverse)-kinematics

Postby michal » Sat Jun 15, 2013 3:52 pm

Here is complete class I made for controlling the robot by inverse kimenatics. You can append this at the end of robot.py

EDIT:
- speed optimization (about 3x faster now), but still uses numpy for several operations. Due to speed reasons.
- uses binary serial communication (see here).
Code: Select all
import numpy as np

#################################################################################################
# Inverse-kinematic robot Hexy
# by Michal G., 01-07-2013
# - speed optimization (about 3x faster now)
#  - it uses binary serial communication for data transfer, see http://forum.arcbotics.com/viewtopic.php?f=21&t=481#p2979
#################################################################################################
class kinematicRobot():
    def __init__(self, con, Dict):
        self.con = con
        self.Dict = self.reverseDict(Dict)
        self.bodyRadius = 100
        self.hip = 26
        self.thigh = 49
        self.foot = 55
        self.bodyAngle = 50.0/180*np.pi
        self.bodyAngles = [np.pi/2 - self.bodyAngle, np.pi/2, np.pi/2 + self.bodyAngle,
                          3*np.pi/2 - self.bodyAngle, 3*np.pi/2, 3*np.pi/2 + self.bodyAngle]

        self.CycleStep = 0  # counter for moving legs sychnronization
        self.MovingLegs = 3 # how many legs are moving at once
        self.FootPos = np.zeros((6,3))  # X,Y,Z position of all feet
        self.footDist = 160 # nominal distance of foot from body center. Empirical value
        for i in range(6):  # reset feet position
            self.FootPos[i] = [self.footDist*np.cos(self.bodyAngles[i]),self.footDist*np.sin(self.bodyAngles[i]),0]

        d = self.bodyRadius
        self.body = np.array([[d*math.cos(self.bodyAngles[0]),d*math.sin(self.bodyAngles[0]),0],
                              [d*math.cos(self.bodyAngles[1]),d*math.sin(self.bodyAngles[1]),0],
                              [d*math.cos(self.bodyAngles[2]),d*math.sin(self.bodyAngles[2]),0],
                              [d*math.cos(self.bodyAngles[3]),d*math.sin(self.bodyAngles[3]),0],
                              [d*math.cos(self.bodyAngles[4]),d*math.sin(self.bodyAngles[4]),0],
                              [d*math.cos(self.bodyAngles[5]),d*math.sin(self.bodyAngles[5]),0]]).T
        self.body2 = self.RotZ(self.body,0.001)

        if legsMirrored:
            self.sign = [1,1,1,-1,-1,-1]
        else:
            self.sign = [1,1,1,1,1,1]

    # internal - reverse servo dictionary
    def reverseDict(self,Dict):
        outDict = []
        for i in range(9):    # left side from front to back
            outDict.append(Dict[i][1])

        for i in range(3):     # right side from back to front
            outDict.append(Dict[15-3*i][1])
            outDict.append(Dict[16-3*i][1])
            outDict.append(Dict[17-3*i][1])
           
        outDict.append(Dict[18][1])  # head   
        return outDict

    # internal - Rotation X,Y,Z
    def RotXYZ(self,pos,a):
        Rxyz = [[math.cos(a[2])*math.cos(a[1]), math.cos(a[2])*math.sin(a[1])*math.sin(a[0])-math.sin(a[2])*math.cos(a[0]), math.cos(a[2])*math.sin(a[1])*math.cos(a[0])+math.sin(a[2])*math.sin(a[0])],
                [math.sin(a[2])*math.cos(a[1]), math.sin(a[2])*math.sin(a[1])*math.sin(a[0])+math.cos(a[2])*math.cos(a[0]), math.sin(a[2])*math.sin(a[1])*math.cos(a[0])-math.cos(a[2])*math.sin(a[0])],
                [-math.sin(a[1])              , math.cos(a[1])*math.sin(a[0])                                             , math.cos(a[1])*math.cos(a[0])]]
        return np.dot(Rxyz,pos)

    # internal - Rotation Z
    def RotZ(self,pos,angle):
        Rz = [[math.cos(angle),-math.sin(angle),0],
              [math.sin(angle),math.cos(angle),0],
              [0,0,1]]
        return np.dot(Rz,pos)

    # internal - build body and perform translation
    def buildBody(self, pos = [0,0,0], angle = [0,0,0]):
        b  = self.RotXYZ(self.body,angle).T + pos
        b2 = self.RotXYZ(self.body2,angle).T + pos
        return b, b2

    # internal - shift all legs in X,Y,Rz
    def setFeetPosInc(self, (x, y, Rz) = (0,0,0)):   
        self.FootPos = self.RotZ(self.FootPos.T, Rz*np.pi/180).T + [x, y, 0]

    # set number of legs moving at once (1 to 3)
    def setMovingLegs(self, iMovingLegs = 2):
        if iMovingLegs > 0 and iMovingLegs < 4:
            self.CycleStep = 0
            self.MovingLegs = iMovingLegs

    # initialization
    def initRobot(self, z = 75):
        # get body on the ground (z = 35 mm)
        b = np.array([self.bodyRadius, 35])
        k = np.array([self.bodyRadius + self.hip, 35])
        f = np.array([self.footDist, 0])
        kf = np.linalg.norm(k - f)
        bf = np.linalg.norm(b - f)
        ankleAngle = np.arccos((self.thigh**2 + self.foot**2 - kf**2)/(2*self.thigh*self.foot))
        kneeAngle = np.arccos((self.hip**2 + kf**2 - bf**2)/(2*self.hip*kf)) + np.arccos((self.thigh**2 + kf**2 - self.foot**2)/(2*self.thigh*kf))
        for i in range(6):
            self.con.servos[self.Dict[i*3+1]].setPos(deg = int(-kneeAngle/np.pi*180 + 180)*self.sign[i], move = False)
            self.con.servos[self.Dict[i*3+2]].setPos(deg = int(ankleAngle/np.pi*180 - 180 + ankleOffset)*self.sign[i], move = False)
        self.con.sendBinary()

        time.sleep(0.5)
        # reset hip position
        for i in range(6):
            self.con.servos[self.Dict[i*3+0]].setPos(deg = 0)
        self.con.sendBinary()   

        time.sleep(0.3)
        # get body to nominal height z
        b = np.array([self.bodyRadius, z])
        k = np.array([self.bodyRadius + self.hip, z])
        kf = np.linalg.norm(k - f)
        bf = np.linalg.norm(b - f)
        ankleAngle = np.arccos((self.thigh**2 + self.foot**2 - kf**2)/(2*self.thigh*self.foot))
        kneeAngle = np.arccos((self.hip**2 + kf**2 - bf**2)/(2*self.hip*kf)) + np.arccos((self.thigh**2 + kf**2 - self.foot**2)/(2*self.thigh*kf))
        for i in range(6):
            self.con.servos[self.Dict[i*3+1]].setPos(deg = int(-kneeAngle/np.pi*180 + 180)*self.sign[i], move = False)
            self.con.servos[self.Dict[i*3+2]].setPos(deg = int(ankleAngle/np.pi*180 - 180 + ankleOffset)*self.sign[i], move = False)
        self.con.sendBinary()   

    # update robot position - the main calculation loop
    def updateRobot(self, pos = (0,0,75), angle = (0,0,0), move = False, dPos = (0,0,0), send = True):
        """
        The idea is that during move the body stays still, but the legs move in opposite direction
        The body position and orientation is given by absolute pos and angle.
        If move is enabled, the legs are shifted by dPos = (dx,dy,dRz) each time
        """
        angle = map(lambda x: math.radians(x), angle) # angles from deg to rad

        # build body with desired position and orientation
        b, b2 = self.buildBody(pos, angle)

        # if move, shift all feet position by dPos = (dx,dy,dRz) increment
        if move:
            self.setFeetPosInc(dPos)           

        # calculate body normal vector
        v0 = b[1] - b[0]
        v1 = b[2] - b[0]
        normal = np.cross(v0, v1)
        normal /= math.sqrt(np.dot(normal,normal))

        doMove = False

        """
        1 Cycle step = one leg move up or down
        Cycle steps = 12/MovingLegs
        if MovingLegs = 2
            Cycle:  Leg:     Vertical move:
            0       0 and 3      Up
            1       0 and 3      Down
            2       1 and 4      Up
            3       1 and 4      Down
            4       2 and 5      Up
            5       2 and 5      Down
            0       0 and 3      Up
            ...
        """
        # if legs movement enabled or some legs in the air, update legs position
        if move or (self.CycleStep % 2 == 1):
            d = self.footDist
            i = self.CycleStep/2

            for j in range(self.MovingLegs):  # reset the position of the legs which should move this cycle (1 to 3 legs)
                self.FootPos[i + j*6/self.MovingLegs] = [d*math.cos(self.bodyAngles[i + j*6/self.MovingLegs]),d*math.sin(self.bodyAngles[i + j*6/self.MovingLegs]),0]

            self.CycleStep += 1
            self.CycleStep %= (12/self.MovingLegs)

            if (self.CycleStep % 2 == 1):  # go up with the legs
                doMove = True

        # calculate inverse kinematics for each leg
       
        for i in range(6):
            f = self.FootPos[i]
            f_b = f - b[i]
            p = f - np.dot(normal, f_b)*normal  # x = (q - (n.(q-p))*n , q = out of plane, p = plane point, n = normal
            h0 = p - b[i]
            h1 = b2[i] - b[i]
            h0norm = math.sqrt(np.dot(h0,h0))
            h1norm = math.sqrt(np.dot(h1,h1))
           
            hipAngle = math.acos(np.dot(h0,h1)/h0norm/h1norm)

            k = b[i] + self.hip*h0/h0norm
           
            k_f = k-f
            kfnorm = math.sqrt(np.dot(k_f,k_f))  # norm
           
            x = (self.thigh**2 + self.foot**2 - kfnorm**2)/(2*self.thigh*self.foot)
            if -1 <= x <= 1:
                ankleAngle = math.acos(x)
            else:
                ankleAngle = np.nan

            bfnorm = math.sqrt(np.dot(f_b,f_b))  # norm

            x = (self.hip**2 + kfnorm**2 - bfnorm**2)/(2*self.hip*kfnorm)
            y = (self.thigh**2 + kfnorm**2 - self.foot**2)/(2*self.thigh*kfnorm)
            if -1 <= x <= 1 and -1 <= y <= 1:
                kneeAngle = math.acos(x) + math.acos(y)
            else:
                kneeAngle = np.nan
               
            # move proper legs up
            if doMove and (self.CycleStep / 2 == i % (6/self.MovingLegs)):
                kneeAngle += np.pi/5.0
                ankleAngle -= np.pi/5.0

            # send updated angles to servos only if solution exists
            if np.isnan((hipAngle, kneeAngle, ankleAngle)).any():
                print "No solution for leg %d" % i
            else:
                self.con.servos[self.Dict[i*3+0]].setPos(deg = int(-hipAngle/np.pi*180 + 90), move = False)
                self.con.servos[self.Dict[i*3+1]].setPos(deg = int(-kneeAngle/np.pi*180 + 180)*self.sign[i], move = False)
                self.con.servos[self.Dict[i*3+2]].setPos(deg = int(ankleAngle/np.pi*180 - 180 + ankleOffset)*self.sign[i], move = False)

        if send:
            self.con.sendBinary()


How to use it
In GUI.py, in __init__ of class App you create a new variables
Code: Select all
self.Dict = [   # Setup names and servo assignment
            ("LF hip", 7),
            ("LF knee", 6),
            ("LF ankle", 5),
            ("LM hip", 11),
            ("LM knee", 10),
            ("LM ankle", 9),
            ("LB hip", 15),
            ("LB knee", 14),
            ("LB ankle", 13),
            ("RF hip", 24),
            ("RF knee", 25),
            ("RF ankle", 26),
            ("RM hip", 20),
            ("RM knee", 21),
            ("RM ankle", 22),
            ("RB hip", 16),
            ("RB knee", 17),
            ("RB ankle", 18),
            ("Head",31)
        ]

self.Robot = kinematicRobot(con, self.Dict)   # robot instance

self.pos = (0,0,75)  # (x,y,z) position of body [mm]
self.angle = (0,0,0) # (Rx,Ry,Rz) orientation of body [deg]
self.dPos  = (0,0,0) # (dx,dy,dRz) displacement of legs per one cycle
self.move = False  # no move now
self.once = False  # no position update now

Then when you want to initialize the position of the robot you call
Code: Select all
self.Robot.initRobot(z = 75) # z...initial body height between 35 and 98 mm


then in pool you call updateRobot.
Code: Select all
def poll(self):
      ...
      if self.once or self.move: 
                self.Robot.updateRobot(self.pos, self.angle, self.move, self.dPos)
                if self.move == False:
                    self.once = False
     ...
     self.master.after(1000/FPS, self.poll)

For FPS I recommend FPS= 20-30 as an optimal value between Hexy movement speed and reaction times of the servos. With binary communication and speed improvements you can achieve as much as about 50 FPS, but this might cause overload of Servotor32 serial buffer and stop response from the robot. I also recommend to use Bluetooth which by my experience causes less disturbance of the active servos.

If you only want to change body position/angle (with static legs) set
Code: Select all
self.pos = (30, 0, 75)  # example, lean forward
self.once = True

If you want to do the complete move (moving legs) set
Code: Select all
self.dPos = (5, 0, 0)  # example, move forward 5mm per cycle
self.move = True

To stop the movement, update the position once more such that all legs get to ground for sure:
Code: Select all
self.move = False
self.once = True

You can also change the number of simultaneously moving legs (between 1 and 3, default is 2). Try it, it's fun :D. See the video
Code: Select all
self.Robot.setMovingLegs(iMovingLegs = 2)

http://youtu.be/9sOlLHbRjdI

Thant's it. Enjoy! 8-)
Last edited by michal on Sun Jul 07, 2013 1:06 pm, edited 1 time in total.
michal
 
Posts: 36
Joined: Thu Jun 06, 2013 7:01 pm
Location: Delft, NL

Re: (inverse)-kinematics

Postby TooManySecrets » Sun Jun 16, 2013 10:00 am

Anyone up to connecting a ps3 controller or something for this?
Image
TooManySecrets
 
Posts: 79
Joined: Mon Jan 14, 2013 5:02 pm

Re: (inverse)-kinematics

Postby michal » Sun Jun 16, 2013 4:31 pm

I'm using Logitech Wireless Gamepad F710.
and pygame.joystick library to interface it in PoCoMo/GUI

It has 5 analog axes, 10 buttons and 1 hat which is more than enough for controlling hexy.
michal
 
Posts: 36
Joined: Thu Jun 06, 2013 7:01 pm
Location: Delft, NL

Re: (inverse)-kinematics

Postby TooManySecrets » Tue Jun 18, 2013 7:52 pm

Can you pls post a complete zip or rar file with all the modified files you are using (i meed the modefied pomoco)

That would help a lot.
Image
TooManySecrets
 
Posts: 79
Joined: Mon Jan 14, 2013 5:02 pm

Re: (inverse)-kinematics

Postby michal » Tue Jun 18, 2013 8:21 pm

Be patient. There are several reasons why I didn't do that yet:
1) My version of PoCoMo is adjusted to the hardware modifications I made. So it wouldn't work with your Hexy without modifications.
2) I'm bad in making comments inside code. Hence it would backfire on me with many questions like "What does this function does?", "My Hexy behaves strangely when I use your code, what am I doing wrong?"
3) Where's the fun if I give you all the solution immediately? I post here a lot of hints and even complete classes. If you don't want to use your brain cells, buy a radio-controlled car instead.

I first want to walk you through my modifications in the thread I'm writing. Once I will describe all major changes I will share the complete code.
michal
 
Posts: 36
Joined: Thu Jun 06, 2013 7:01 pm
Location: Delft, NL

Re: (inverse)-kinematics

Postby TooManySecrets » Tue Jun 18, 2013 8:58 pm

Thx Michal. Im sure im going to love all your changes and upgrades :)
Image
TooManySecrets
 
Posts: 79
Joined: Mon Jan 14, 2013 5:02 pm

PreviousNext

Return to Projects and Upgrades

Who is online

Users browsing this forum: No registered users and 2 guests

cron