In [1]:
%matplotlib inline
import matplotlib
import numpy as np
from numpy.linalg import inv
import matplotlib.pyplot as plt
import sys
sys.path.append('../../rai/lib')
import libry as lry
Loading the environment:
In [2]:
K = lry.Config()
D = K.view()
K.clear();
K.addFile('human.g')
1st task: Robot reaching towards y_target:
In [3]:
n = K.getJointDimension()
q = K.getJointState()
w = 1e-4
W = w * np.identity(n) # W is equal the ID_n matrix times scalar w
input("initial posture, press Enter to continue...")
y_target = [-0.2, -0.4, 1.1]
for i in range(5):
# 1st task
F = K.feature(lry.FS.position, ["|handR"]) # "handR" is the name of the right hand ("handL" for the left hand)
y, J = F.eval(K)
# compute joint updates
q += inv(J.T @ J + W) @ J.T @ (y_target - y)
# NOTATION: J.T is the transpose of J; @ is matrix multiplication (dot product)
# sets joint angles AND computes all frames AND updates display
K.setJointState(q)
# optional: pause and watch OpenGL
input("Press Enter to continue...")
In [4]:
# reset joint state to zeros
q = np.zeros(K.getJointDimension())
K.setJointState(q)
In [ ]: