In [ ]:

    

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


    

In [ ]:

    

# Load config, file and viewer
K = lry.Config()
K.clear();
K.addFile('pegArm.g')
K.sortFrames();
D = K.view()


    

In [ ]:

    

# Initialize variables, set start state
noise = .01
gravity = True
n = K.getJointDimension()
tau = .01
N = 500
T = N*tau

qdot = np.zeros(n)
q = np.zeros(n) + 0.4

K.setJointState(q)

q_goal = np.zeros(n)
q0 = copy.deepcopy(q)

q_hist = np.zeros((N, n))


    

In [ ]:

    

for i in range(N):
    t = i * tau
    u = np.zeros(n)  # no controller torques

    # b) compute desired position, velocity and acceleration with sine motion profile
    
    
    # c) PD controller
    
    
    # d) PID controller
    
    
    # e) Inverse dynamics feedforward control
    M, F = K.equationOfMotion(qdot, gravity);

    
    
    # dynamic simulation (simple Euler integration of the system dynamics, look into the code)
    qdot = K.stepDynamics(qdot, u, tau, noise, gravity);
    time.sleep(tau)
    q = K.getJointState()
    print (" t=" + str(tau*t) + "sec E=" + str(K.getEnergy(qdot)) + "  q = " + str(q))
    q_hist[i] = q


    

In [ ]:

    

# plot q
plt.plot(q_hist)
plt.show()


    

In [ ]: