search

roboticslab-uc3m / teo-main

TEO full-sized humanoid robot: super/meta repository.
http://roboticslab.uc3m.es/roboticslab/robot/teo-humanoid
GNU Lesser General Public License v2.1
4 stars 1 forks source link

Review and Unify TEO kinematic model #38

Closed jgvictores closed 5 years ago

jgvictores commented 6 years ago

Review and Unify TEO kinematic model.

The first step is for table and drawing to agree. Following the workflow for assets, this means we:

  1. First edit dh-drawing.svg and dh-table.ods.
  2. Then update the corresponding generated assets and resources of other repositories.

Related with most documentation issues. At time of writing, blocked by #23 and #39.

Edit: Very small tool that (may) be nice for debugging: https://github.com/arcoslab/roboview (http://wiki.icub.org/wiki/KDL-simple)

jgvictores commented 6 years ago

Related regarding DH and tracking its history:

jgvictores commented 6 years ago

A general view of all we have related to DH is nice too: https://github.com/search?q=org%3Aroboticslab-uc3m+DH

jgvictores commented 6 years ago

More refs: https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/77

jgvictores commented 6 years ago

Also: https://github.com/roboticslab-uc3m/teo-main/issues/25

jgvictores commented 5 years ago

Blocked by https://github.com/roboticslab-uc3m/teo-developer-manual/issues/54

jgvictores commented 5 years ago

No longer blocked by https://github.com/roboticslab-uc3m/teo-developer-manual/issues/54

jgvictores commented 5 years ago

For https://github.com/roboticslab-uc3m/teo-developer-manual/issues/40 I've been using https://github.com/arcoslab/roboview (in fact, have a small PR at https://github.com/arcoslab/roboview/pull/2), which enables some python yarp+kdl+viewer. Could be better, but here's a little script for the Trunk (I may make it more generic for the rest of the limbs):

from PyKDL import *
from math import pi

def degToRad(deg):
    return deg*pi/180.0

l0 = 193.2 / 1000.0 # [mm] to [m]

linkOffset = [0.0, 0.0]
linkA = [0.0, 0.0]
linkAlpha = [-90.0, 0.0]
linkD = [l0, 0.0]

segments = [
    Segment(Joint(Joint.RotZ),
            Frame().DH(linkA[0],degToRad(linkAlpha[0]),linkD[0],degToRad(linkOffset[0]))),
    Segment(Joint(Joint.RotZ),
            Frame().DH(linkA[1],degToRad(linkAlpha[1]),linkD[1],degToRad(linkOffset[1])))
]

# Human-inspired limits
limits_min = [-30.0, -90.0]
limits_max = [ 30.0,  10.0]

limits_min = [degToRad(v) for v in limits_min]
limits_max = [degToRad(v) for v in limits_max]

Here's a small screenshot of what is achieved:

jgvictores commented 5 years ago

Not very related, but I just found: https://github.com/robotology/icub-model-generator

jgvictores commented 5 years ago

Not very related, but I just found: https://github.com/robotology/icub-model-generator

I was in fact looking for this: http://wiki.icub.org/wiki/ICubFowardKinematics_right If you open http://wiki.icub.org/images/6/65/ICubFwdKinNew.zip you can see the MATLAB code to generate the 3d plot.

jgvictores commented 5 years ago

I'm pretty happy with https://github.com/arcoslab/roboview. However, seeing this is done in Python, I can't avoid to think about some Jupyter notebook... ^^

jgvictores commented 5 years ago

Updated script that reads directly from lengths.csv and any dh-*.csv file.

from PyKDL import *
from math import pi
import csv

lengthsFileName = '/home/yo/repos/teo-developer-manual/csv/lengths.csv'
dhFileName = '/home/yo/repos/teo-developer-manual/csv/dh-rightArm.csv'

segments = [] # required for roboview

def degToRad(deg):
    return deg*pi/180.0

#-- initPoss
# Small hack: first replace two digits, e.g. else can replace q1 before q13
twoDigitJoints = {'q'+str(i):'0.0' for i in range(10,28)} # dof
oneDigitjoints = {'q'+str(i):'0.0' for i in range(1,10)} # dof

#-- lengths
lengthsFile = open(lengthsFileName, 'r')
reader = csv.reader(lengthsFile, delimiter=',')
next(reader, None)  # skip the header
lengths = {row[0]:str(float(row[1])/1000.0) for row in reader} # [mm] to [m]
print('lengths: ',lengths)

#-- dh params
dhFile = open(dhFileName, 'r')
reader = csv.reader(dhFile, delimiter=',')

#-- populate segments
next(reader, None)  # skip the header
for row in reader:
    print('row: ',row)

    linkOffset = row[1]
    for first, second in twoDigitJoints.items():
        linkOffset = str(linkOffset).replace(first, second)
    for first, second in oneDigitjoints.items():
        linkOffset = str(linkOffset).replace(first, second)
    linkOffset = eval(linkOffset)

    linkD = row[2]
    for first, second in twoDigitJoints.items():
        linkD = str(linkD).replace(first, second)
    for first, second in oneDigitjoints.items():
        linkD = str(linkD).replace(first, second)
    for first, second in lengths.items():
        linkD = str(linkD).replace(first, second)
    linkD = eval(linkD)

    linkA = row[3]
    for first, second in lengths.items():
        linkA = str(linkA).replace(first, second)
    linkA = eval(linkA)

    linkAlpha = eval(row[4])

    print('* linkOffset ', linkOffset)
    print('* linkD ', linkD)
    print('* linkA ', linkA)
    print('* linkAlpha ', linkAlpha)
    s = Segment(Joint(Joint.RotZ),
                Frame().DH(linkA,degToRad(linkAlpha),linkD,degToRad(linkOffset)))
    segments.append(s)

#-- populate limits (hard-coded for now)
limits_min = [-180.0] * len(segments)
limits_max = [180.0] * len(segments)

limits_min = [degToRad(v) for v in limits_min]
limits_max = [degToRad(v) for v in limits_max]

Compare for rightArm (will change, just to see at time of writing): Screenshot from 2019-04-09 15-21-48

jgvictores commented 5 years ago

WIP (blocked by) https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/177

jgvictores commented 5 years ago

WIP (blocked by) roboticslab-uc3m/kinematics-dynamics#177

Closed https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/177 so unblocked!

jgvictores commented 5 years ago

Can now use kdl-from-csv.py from roboticslab-uc3m/kinematics-dynamics/scripts/python/ for updating https://github.com/roboticslab-uc3m/teo-configuration-files

jgvictores commented 5 years ago

WIP at https://github.com/roboticslab-uc3m/teo-configuration-files/issues/17 and https://github.com/roboticslab-uc3m/teo-configuration-files/pull/20

jgvictores commented 5 years ago

Also see https://github.com/roboticslab-uc3m/teo-configuration-files/issues/7

jgvictores commented 5 years ago

Done, mainly at:

jgvictores commented 5 years ago

Unblocks https://github.com/roboticslab-uc3m/teo-main/issues/16 (dynamics)