In [ ]:

    

%matplotlib inline
import matplotlib
import numpy as np
import matplotlib.pyplot as plt
import sys
sys.path.append('../../rai/rai/ry')
import libry as lry
import time
import copy


    

In [ ]:

    

K = lry.Config()
D = K.view()


    

In [ ]:

    

K.clear();
K.addFile('human.g')


    

In [ ]:

    

def multiTask(num_steps=1000, alpha=1., sigma=0.01):
    y_target = [-0.2, -0.4, 1.1]
    
    # init joint state to zero vector
    q = np.zeros(K.getJointDimension())
    K.setJointState(q)
    
    # get position and position Jacobian of right hand
    F = K.feature(lry.FS.position, ["|handR"])
    y, J = F.eval(K)
    

    y0 = copy.deepcopy(y)
    W = np.identity(K.getJointDimension())
    
    for i in range(num_steps):
        Phi = []
        PhiJ = []
        
        # track circle
        yt = y_target + .2 * np.array([np.cos(i/20.), 0, np.sin(i/20.)])
        
        Phi = (yt-y)/sigma
        PhiJ = (J/sigma)
        
        # 1st task: joint should stay close to zero



        # 2nd task: left hand should point upwards

        

        # 3rd task: robot should look at right hand


        
        q += 0.1 * np.dot(np.dot(np.linalg.inv(np.dot(np.transpose(PhiJ), PhiJ) + W), np.transpose(PhiJ)), Phi)
              
        
        
        K.setJointState(q)
        y, J = F.eval(K)
        time.sleep(0.04)


    

In [ ]:

    

multiTask()