In [1]:

    

import numpy as np
from human_moveit_config.human_model import HumanModel
import transformations
import timeit
import rospy
import time
from scipy.spatial import cKDTree
from copy import deepcopy
import tf
import time


    

In [2]:

    

def create_database(link, base, joint_names, nb_points=1):
    X = []
    for i in range(nb_points):
        state = human.get_random_state()
        fk = human.forward_kinematic(state, links=link, base=base)
        joints = []
        for j in joint_names:
            joints.append(state.position[state.name.index(j)])
        # append quaternion in both hemispheres
        for i in range(2):
            x = deepcopy(joints)
            x += fk[link][0]
            x += ((-1)**i * np.array(fk[link][1]) / 40).tolist()
        X.append(x)
    names = deepcopy(joint_names)
    names += [link + '_x', link + '_y', link + '_z']
    names += [link + '_qx', link + '_qy', link + '_qz', link + '_qw']
    np.savez_compressed('/tmp/database_' + link, names=names, data=X)


    

In [3]:

    

rospy.init_node('test')


    

In [4]:

    

human = HumanModel()
prefix = human.prefix


    

In [5]:

    

links = [prefix + '/shoulder_center', prefix + '/head', prefix + '/left_elbow', prefix + '/left_hand',
         prefix + '/right_elbow', prefix + '/right_hand']
bases = [prefix + '/base', prefix + '/shoulder_center', prefix + '/shoulder_center',
         prefix + '/left_elbow', prefix + '/shoulder_center', prefix + '/right_elbow']
joint_by_links = {}
for l in links:
    joint_by_links[l] = human.joint_by_links[l]
for s in ['right', 'left']:
    joint_by_links[prefix + '/' + s + '_elbow'] = (human.joint_by_links[prefix + '/' + s + '_shoulder'] +
                                                   joint_by_links[prefix + '/' + s + '_elbow'])


    

In [ ]:

    

nb_points = 10000
for i in range(len(links)):
    create_database(links[i], bases[i], joint_by_links[links[i]], nb_points=nb_points)


    

In [6]:

    

state = human.get_current_state()
state.position = [0] * len(state.name)
fk_test = human.forward_kinematic(state, links=links)
ik = human.inverse_kinematic(fk_test, tolerance=0.0001)
ik_res = human.forward_kinematic(ik, links=links)
avg_prec = 0
for l in links:
    avg_prec += np.linalg.norm(np.array(fk_test[l][0]) - np.array(ik_res[l][0]))
    avg_prec += np.linalg.norm(np.array(fk_test[l][1]) - np.array(ik_res[l][1]))
avg_prec /= len(links)
print 'precision: ' + str(avg_prec)


    

    




precision: 1.88733716722e-06

In [17]:

    

avg_time = 0
avg_prec = 0
for i in range(100):
    state = human.get_random_state()
    fk_test = human.forward_kinematic(state, links=links)
    start = time.time()
    ik = human.inverse_kinematic(fk_test, tolerance=0.001)
    avg_time += (time.time() - start)
    ik_res = human.forward_kinematic(ik, links=links)
    prec = 0
    for l in links:
        prec += np.linalg.norm(np.array(fk_test[l][0]) - np.array(ik_res[l][0]))
        prec += np.linalg.norm(np.array(fk_test[l][1]) - np.array(ik_res[l][1]))
    avg_prec += prec
avg_time /= 100
avg_prec /= (100 * len(links))

print 'time: ' + str(avg_time)
print 'precision: ' + str(avg_prec)


    

    




time: 0.0385030317307
precision: 0.00630696805122

In [ ]:

    

state = human.get_random_state()
fk_test = human.forward_kinematic(state, links=links)
ik = human.inverse_kinematic(fk_test)
test_link = links
ground_truth = human.forward_kinematic(state, links=test_link)
res_ik = human.forward_kinematic(ik, links=test_link)
for l in test_link:
    print l
    print ground_truth[l]
    print '-------------'
    print res_ik[l]
    print '##############'


    

In [ ]:

    

joint_by_links


    

In [ ]: