def build_ilqr_problem(self, grad_type=0):
if grad_type == 0:
self.ilqr = PyLQR_iLQRSolver(T=self.T,
plant_dyn=self.plant_dyn,
cost=self.instaneous_cost,
use_autograd=False)
# not use finite difference, assign the gradient functions
self.ilqr.plant_dyn_dx = self.plant_dyn_dx
self.ilqr.plant_dyn_du = self.plant_dyn_du
self.ilqr.cost_dx = self.cost_dx
self.ilqr.cost_du = self.cost_du
self.ilqr.cost_dxx = self.cost_dxx
self.ilqr.cost_duu = self.cost_duu
self.ilqr.cost_dux = self.cost_dux
elif grad_type == 1:
self.ilqr = PyLQR_iLQRSolver(T=self.T,
plant_dyn=self.plant_dyn,
cost=self.instaneous_cost,
use_autograd=True)
else:
# finite difference
self.ilqr = PyLQR_iLQRSolver(T=self.T,
plant_dyn=self.plant_dyn,
cost=self.instaneous_cost,
use_autograd=False)
return
def build_ilqr_problem(self):
self.ilqr_solver = PyLQR_iLQRSolver(T=self.T,
plant_dyn=self.plant_dyn,
cost=self.instaneous_cost,
verbose=self.verbose)
return
class Manipulator_X():
def __init__(self, T=20, weight=[1.0, 1.0], verbose=True):
#initialize model
self.chain = PyKDL.Chain()
self.chain.addSegment(
PyKDL.Segment(
"Base", PyKDL.Joint(PyKDL.Joint.None),
PyKDL.Frame(PyKDL.Vector(0.0, 0.0, 0.042)),
PyKDL.RigidBodyInertia(
0.08659, PyKDL.Vector(0.00025, 0.0, -0.02420),
PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(
PyKDL.Segment(
"Joint1", PyKDL.Joint(PyKDL.Joint.RotZ),
PyKDL.Frame(PyKDL.Vector(0.0, -0.019, 0.028)),
PyKDL.RigidBodyInertia(
0.00795, PyKDL.Vector(0.0, 0.019, -0.02025),
PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(
PyKDL.Segment(
"Joint2", PyKDL.Joint(PyKDL.Joint.RotY),
PyKDL.Frame(PyKDL.Vector(0.0, 0.019, 0.0405)),
PyKDL.RigidBodyInertia(
0.09312, PyKDL.Vector(0.00000, -0.00057, -0.02731),
PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(
PyKDL.Segment(
"Joint3", PyKDL.Joint(PyKDL.Joint.RotZ),
PyKDL.Frame(PyKDL.Vector(0.024, -0.019, 0.064)),
PyKDL.RigidBodyInertia(
0.19398, PyKDL.Vector(-0.02376, 0.01864, -0.02267),
PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(
PyKDL.Segment(
"Joint4",
PyKDL.Joint("minus_RotY", PyKDL.Vector(0, 0, 0),
PyKDL.Vector(0, -1, 0), PyKDL.Joint.RotAxis),
PyKDL.Frame(PyKDL.Vector(0.064, 0.019, 0.024)),
PyKDL.RigidBodyInertia(
0.09824, PyKDL.Vector(-0.02099, 0.0, -0.01213),
PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(
PyKDL.Segment(
"Joint5", PyKDL.Joint(PyKDL.Joint.RotX),
PyKDL.Frame(PyKDL.Vector(0.0405, -0.019, 0.0)),
PyKDL.RigidBodyInertia(
0.09312, PyKDL.Vector(-0.01319, 0.01843, 0.0),
PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(
PyKDL.Segment(
"Joint6",
PyKDL.Joint("minus_RotY", PyKDL.Vector(0, 0, 0),
PyKDL.Vector(0, -1, 0), PyKDL.Joint.RotAxis),
PyKDL.Frame(PyKDL.Vector(0.064, 0.019, 0.0)),
PyKDL.RigidBodyInertia(
0.09824, PyKDL.Vector(-0.02099, 0.0, 0.01142),
PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(
PyKDL.Segment(
"Joint7", PyKDL.Joint(PyKDL.Joint.RotX),
PyKDL.Frame(PyKDL.Vector(0.14103, 0.0, 0.0)),
PyKDL.RigidBodyInertia(
0.26121, PyKDL.Vector(-0.09906, 0.00146, -0.00021),
PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.min_position_limit = [
-160.0, -90.0, -160.0, -90.0, -160.0, -90.0, -160.0
]
self.max_position_limit = [
160.0, 90.0, 160.0, 90.0, 160.0, 90.0, 160.0
]
self.min_joint_position_limit = PyKDL.JntArray(7)
self.max_joint_position_limit = PyKDL.JntArray(7)
for i in range(0, 7):
self.min_joint_position_limit[
i] = self.min_position_limit[i] * pi / 180
self.max_joint_position_limit[
i] = self.max_position_limit[i] * pi / 180
self.fksolver = PyKDL.ChainFkSolverPos_recursive(self.chain)
self.iksolver = PyKDL.ChainIkSolverVel_pinv(self.chain)
self.iksolverpos = PyKDL.ChainIkSolverPos_NR_JL(
self.chain, self.min_joint_position_limit,
self.max_joint_position_limit, self.fksolver, self.iksolver, 100,
1e-6)
#parameter
self.T = T
self.weight_u = weight[0]
self.weight_x = weight[1]
self.verbose = verbose
self.nj = self.chain.getNrOfJoints()
self.q_init = np.zeros(self.nj)
self.q_out = np.zeros(self.nj)
ret, self.dest, self.q_out = self.generate_dest()
self.fin_position = self.dest.p
return
def generate_dest(self):
dest = PyKDL.Frame()
ret = -3
jointpositions = PyKDL.JntArray(self.nj)
while dest.p.z() <= 0 or ret < 0:
'''
jointpositions[0] = rd.randrange(0,11)/10*(self.max_joint_position_limit[0]-self.min_joint_position_limit[0])+self.min_joint_position_limit[0]
jointpositions[1] = rd.randrange(0,11)/10*(self.max_joint_position_limit[1]-self.min_joint_position_limit[1])+self.min_joint_position_limit[1]
jointpositions[2] = rd.randrange(0,11)/10*(self.max_joint_position_limit[2]-self.min_joint_position_limit[2])+self.min_joint_position_limit[2]
jointpositions[3] = rd.randrange(0,11)/10*(self.max_joint_position_limit[3]-self.min_joint_position_limit[3])+self.min_joint_position_limit[3]
jointpositions[4] = rd.randrange(0,11)/10*(self.max_joint_position_limit[4]-self.min_joint_position_limit[4])+self.min_joint_position_limit[4]
jointpositions[5] = rd.randrange(0,11)/10*(self.max_joint_position_limit[5]-self.min_joint_position_limit[5])+self.min_joint_position_limit[5]
jointpositions[6] = 0
'''
jointpositions[0] = 0.3
jointpositions[1] = -0.5
jointpositions[3] = -0.6
jointpositions[5] = -0.5
jointpositions[6] = -0.8
self.fksolver.JntToCart(self.NpToJnt(self.q_init), dest)
print('initial position cartesian')
print(dest)
kinematics_status = self.fksolver.JntToCart(jointpositions, dest)
ret = self.iksolverpos.CartToJnt(self.NpToJnt(self.q_init), dest,
self.NpToJnt(self.q_out))
#print(ret)
print('destination postion cartesian')
print dest
return ret, dest, jointpositions
def getInitPosition(self):
init = PyKDL.Frame()
self.fksolver.JntToCart(self.NpToJnt(self.q_init), init)
return init
def NpToJnt(self, q):
temp = PyKDL.JntArray(self.nj)
for j in range(self.nj):
temp[j] = q[j]
return temp
def JntToNp(self, q):
temp = np.zeros(self.nj)
for j in range(self.nj):
temp[j] = q[j]
return temp
def plant_dyn(self, x, u, t=None, aux=None):
x_new = x + u #+ rd.gauss(0,3e-8)*np.ones(u.shape)
return x_new
def getPosition(self, x):
temp_dest = PyKDL.Frame()
self.fksolver.JntToCart(self.NpToJnt(x), temp_dest)
x_position = temp_dest
print('whole')
print(x_position)
x_position = temp_dest.p
print('temp_dest.p')
print(x_position)
return x_position
def instaneous_cost(self, x, u, t, aux):
if t < self.T - 1:
#return u.dot(u)*self.weight_u
temp_dest = PyKDL.Frame()
self.fksolver.JntToCart(self.NpToJnt(self.plant_dyn(x, u)),
temp_dest)
x_position = temp_dest.p
return PyKDL.dot(
self.fin_position - x_position, self.fin_position -
x_position) * self.weight_x + u.dot(u) * self.weight_u
else:
temp_dest = PyKDL.Frame()
#self.fksolver.JntToCart(self.NpToJnt(x+u+rd.gauss(0,1e-8)*np.ones(u.shape)), temp_dest)
self.fksolver.JntToCart(self.NpToJnt(self.plant_dyn(x, u)),
temp_dest)
x_position = temp_dest.p
return PyKDL.dot(
self.fin_position - x_position, self.fin_position -
x_position) * self.weight_x + u.dot(u) * self.weight_u
def build_ilqr_problem(self):
self.ilqr_solver = PyLQR_iLQRSolver(T=self.T,
plant_dyn=self.plant_dyn,
cost=self.instaneous_cost,
verbose=self.verbose)
return
def solve_ilqr_problem(self):
u_init = []
#sum = self.q_init
sum = np.zeros(self.nj)
#delta = (self.JntToNp(self.q_out)-self.q_init)/self.T
delta = (self.JntToNp(self.q_out) - np.zeros(self.nj)) / self.T
if self.verbose:
print "delta", delta
for t in range(self.T):
temp = np.zeros(self.nj)
#param = rd.random()+0.5
param = rd.gauss(10, 1)
for i in range(self.nj):
if t < self.T - 1:
temp[i] = param * delta[i]
sum[i] += temp[i]
else:
temp[i] = self.q_out[i] - sum[i]
u_init.append(temp)
#x_init = self.q_init
x_init = self.set_x_init()
if self.ilqr_solver is not None:
self.res = self.ilqr_solver.ilqr_iterate(x_init,
u_init,
n_itrs=50,
tol=1e-6)
return
#####################################################
# FIXXXX ITTTTT !!!!!!!
#
#########################################################
def set_x_init(self):
return np.zeros(self.nj)
def plot_ilqr_result(self):
if self.res is not None:
#draw cost evolution and phase chart
fig = plt.figure(figsize=(16, 8), dpi=80)
ax_cost = fig.add_subplot(121)
n_itrs = len(self.res['J_hist'])
ax_cost.plot(np.arange(n_itrs),
self.res['J_hist'],
'r',
linewidth=3.5)
f = open("log/ilqr_result.txt", 'a')
f.write("ilqr_result\n")
for i in np.arange(n_itrs):
f.write(str(self.res['J_hist'][i]))
f.write("\n")
f.close()
ax_cost.set_xlabel('Number of Iterations', fontsize=20)
ax_cost.set_ylabel('Trajectory Cost')
ax_phase = fig.add_subplot(122)
theta = self.res['x_array_opt'][:, 0]
theta_dot = self.res['x_array_opt'][:, 1]
ax_phase.plot(theta, theta_dot, 'k', linewidth=3.5)
ax_phase.set_xlabel('theta (rad)', fontsize=20)
ax_phase.set_ylabel('theta_dot (rad/s)', fontsize=20)
ax_phase.set_title('Phase Plot', fontsize=20)
#draw the destination point
ax_phase.hold(True)
ax_phase.plot([theta[-1]], [theta_dot[-1]], 'b*', markersize=16)
#print self.res['x_array_opt']
plt.show(10)
return
class InversePendulumProblem():
def __init__(self, T=150):
#parameters
self.m_ = 1
self.l_ = .5
self.b_ = .1
self.lc_ = .5
self.g_ = 9.81
self.dt_ = 0.01
self.I_ = self.m_ * self.l_**2
self.ilqr = None
self.res = None
self.T = T
self.Q = 100
self.R = .01
#terminal Q to regularize final speed
self.Q_T = .1
return
def plant_dyn(self, x, u, t, aux):
xdd = (u[0] - self.m_*self.g_*self.lc_*np.sin(x[0]) - self.b_*x[1])/self.I_
# dont need term +0.5*(qdd**2)*self.dt_?
x_new = x + self.dt_ * np.array([x[1], xdd])
return x_new
def plant_dyn_dx(self, x, u, t, aux):
dfdx = np.array([
[1, self.dt_],
[-self.m_*self.g_*self.lc_*np.cos(x[0])*self.dt_/self.I_, 1-self.b_*self.dt_/self.I_]
])
return dfdx
def plant_dyn_du(self, x, u, t, aux):
dfdu = np.array([
[0],
[self.dt_/self.I_]
])
return dfdu
def cost_dx(self, x, u, t, aux):
if t < self.T:
dldx = np.array(
[2*(x[0]-np.pi)*self.Q, 0]
).T
else:
#terminal cost
dldx = np.array(
[2*(x[0]-np.pi)*self.Q, 2*x[1]*self.Q_T]
).T
return dldx
def cost_du(self, x, u, t, aux):
dldu = np.array(
[2*u[0]*self.R]
)
return dldu
def cost_dxx(self, x, u, t, aux):
if t < self.T:
dldxx = np.array([
[2, 0],
[0, 0]
]) * self.Q
else:
dldxx = np.array([
[2* self.Q, 0],
[0, 2*x[1]*self.Q_T]
])
return dldxx
def cost_duu(self, x, u, t, aux):
dlduu = np.array([
[2*self.R]
])
return dlduu
def cost_dux(self, x, u, t, aux):
dldux = np.array(
[0, 0]
)
return dldux
def instaneous_cost(self, x, u, t, aux):
if t < self.T:
return (x[0] - np.pi)**2 * self.Q + u[0]**2 * self.R
else:
return (x[0] - np.pi)**2 * self.Q + x[1]**2*self.Q_T + u[0]**2 * self.R
#grad_types = ['user', 'autograd', 'fd']
def build_ilqr_problem(self, grad_type=0):
if grad_type==0:
self.ilqr = PyLQR_iLQRSolver(T=self.T, plant_dyn=self.plant_dyn, cost=self.instaneous_cost, use_autograd=False)
#not use finite difference, assign the gradient functions
self.ilqr.plant_dyn_dx = self.plant_dyn_dx
self.ilqr.plant_dyn_du = self.plant_dyn_du
self.ilqr.cost_dx = self.cost_dx
self.ilqr.cost_du = self.cost_du
self.ilqr.cost_dxx = self.cost_dxx
self.ilqr.cost_duu = self.cost_duu
self.ilqr.cost_dux = self.cost_dux
elif grad_type==1:
self.ilqr = PyLQR_iLQRSolver(T=self.T, plant_dyn=self.plant_dyn, cost=self.instaneous_cost, use_autograd=True)
else:
#finite difference
self.ilqr = PyLQR_iLQRSolver(T=self.T, plant_dyn=self.plant_dyn, cost=self.instaneous_cost, use_autograd=False)
return
def solve_ilqr_problem(self, x0=None, u_init=None, n_itrs=150, verbose=True):
#prepare initial guess
if u_init is None:
u_init = np.zeros((self.T, 1))
if x0 is None:
x0 = np.array([np.random.rand()*2*np.pi - np.pi, 0])
if self.ilqr is not None:
self.res = self.ilqr.ilqr_iterate(x0, u_init, n_itrs=n_itrs, tol=1e-6, verbose=verbose)
return
def plot_ilqr_result(self):
if self.res is not None:
#draw cost evolution and phase chart
fig = plt.figure(figsize=(16, 8), dpi=80)
ax_cost = fig.add_subplot(121)
n_itrs = len(self.res['J_hist'])
ax_cost.plot(np.arange(n_itrs), self.res['J_hist'], 'r', linewidth=3.5)
ax_cost.set_xlabel('Number of Iterations', fontsize=20)
ax_cost.set_ylabel('Trajectory Cost')
ax_phase = fig.add_subplot(122)
theta = self.res['x_array_opt'][:, 0]
theta_dot = self.res['x_array_opt'][:, 1]
ax_phase.plot(theta, theta_dot, 'k', linewidth=3.5)
ax_phase.set_xlabel('theta (rad)', fontsize=20)
ax_phase.set_ylabel('theta_dot (rad/s)', fontsize=20)
ax_phase.set_title('Phase Plot', fontsize=20)
ax_phase.plot([theta[-1]], [theta_dot[-1]], 'b*', markersize=16)
plt.show()
return
class TwoR_Robot:
def __init__(self, T=150):
# parameters
self.nDoF = 2
self.dt = 0.01 # s
""" arm model parameters """
self.x0 = np.array([np.deg2rad(5),
np.deg2rad(10), 0., 0.]) # initial state vector
self.l = np.array([0.3, 0.33]) # m. link length
self.m = np.array([1.4, 1.1]) # kg. link mass
self.I = np.array(
[0.025,
0.045]) # kg m^2. MOI about the CG of each of the link, Izz
self.s = np.array([0.11,
0.16]) # m. distance of CG from each of the joints
""" pre-compute some constants """
self.d1 = self.I[0] + self.I[1] + self.m[1] * self.l[0]**2
self.d2 = self.m[1] * self.l[0] * self.s[1]
self.dt_ = 0.01
self.ilqr = None
self.res = None
self.T = T
self.Q = 100 * np.eye(self.nDoF)
self.R = .01 * np.eye(self.nDoF)
# terminal Q to regularize final speed
self.Q_T = .1 * np.eye(self.nDoF)
self.target = np.array(
[0.4, 0.2]
) # cartesian position of target. At the target, the robot has to stop and thus zero velocity
self.target_thetas = self.inv_kin(
self.target
) # provides the joint angles associated with cart. target position
def plant_dyn(self, x, u, t, aux):
q, dq = x[0:self.nDoF], x[self.nDoF:2 * self.nDoF]
M = np.array([[
self.d1 + 2 * self.d2 * np.cos(q[1]),
self.I[1] + self.d2 * np.cos(q[1])
], [self.I[1] + self.d2 * np.cos(q[1]), self.I[1]]])
# centripital and Coriolis effects
C = np.array([[-dq[1] * (2 * dq[0] + dq[1])], [dq[0]**2]
]) * self.d2 * np.sin(q[1])
# joint friction
B = np.array([[.05, .025], [.025, .05]])
# calculate forward dynamics
kk = np.dot(B, dq).reshape(-1, 1)
ddq = np.linalg.pinv(M) @ (u.reshape(-1, 1) - C - kk)
# transfer to next time step
new_dq = dq + self.dt_ * ddq.reshape(-1)
new_q = q + self.dt_ * new_dq.reshape(-1)
x_new = np.hstack((new_q, new_dq))
return x_new
"""
def plant_dyn_dx(self, x, u, t, aux):
dfdx = np.array([
[1, self.dt_],
[-self.m_ * self.g_ * self.lc_ * np.cos(x[0]) * self.dt_ / self.I_, 1 - self.b_ * self.dt_ / self.I_]
])
return dfdx
def plant_dyn_du(self, x, u, t, aux):
dfdu = np.array([
[0],
[self.dt_ / self.I_]
])
return dfdu
def cost_dx(self, x, u, t, aux):
if t < self.T:
dldx = np.array(
[2 * (x[0] - np.pi) * self.Q, 0]
).T
else:
# terminal cost
dldx = np.array(
[2 * (x[0] - np.pi) * self.Q, 2 * x[1] * self.Q_T]
).T
return dldx
def cost_du(self, x, u, t, aux):
dldu = np.array(
[2 * u[0] * self.R]
)
return dldu
def cost_dxx(self, x, u, t, aux):
if t < self.T:
dldxx = np.array([
[2, 0],
[0, 0]
]) * self.Q
else:
dldxx = np.array([
[2 * self.Q, 0],
[0, 2 * x[1] * self.Q_T]
])
return dldxx
def cost_duu(self, x, u, t, aux):
dlduu = np.array([
[2 * self.R]
])
return dlduu
def cost_dux(self, x, u, t, aux):
dldux = np.array(
[0, 0]
)
return dldux
"""
def forward_kin(self, q):
x1, y1 = self.l[0] * np.cos(q[0]), self.l[0] * np.sin(q[0])
x2, y2 = self.l[1] * np.cos(q[0] + q[1]), self.l[1] * np.sin(q[0] +
q[1])
return x1, x2, y1, y2
def inv_kin(self, target=None):
if target is None:
target = self.target
a = target[0]**2 + target[1]**2 - self.l[0]**2 - self.l[1]**2
b = 2 * self.l[0] * self.l[1]
q2 = np.arccos(a / b)
c = np.arctan2(target[1], target[0])
q1 = c - np.arctan2(self.l[1] * np.sin(q2),
(self.l[0] + self.l[1] * np.cos(q2)))
return np.array([q1, q2])
def instaneous_cost(self, x, u, t, aux):
q, dq = x[0:self.nDoF], x[self.nDoF:2 * self.nDoF]
# x1, x2, y1, y2 = self.forward_kin(q)
# eef_pos = np.array([x1+x2, y1+y2])
# pos_err = eef_pos - self.target
pos_err = q - self.target_thetas
if t < self.T:
return pos_err.T @ self.Q @ pos_err + u.T @ self.R @ u
else:
return pos_err.T @ self.Q @ pos_err + u.T @ self.R @ u + dq.T @ self.Q_T @ dq
# grad_types = ['user', 'autograd', 'fd']
def build_ilqr_problem(self, grad_type=0):
if grad_type == 0:
self.ilqr = PyLQR_iLQRSolver(T=self.T,
plant_dyn=self.plant_dyn,
cost=self.instaneous_cost,
use_autograd=False)
# not use finite difference, assign the gradient functions
self.ilqr.plant_dyn_dx = self.plant_dyn_dx
self.ilqr.plant_dyn_du = self.plant_dyn_du
self.ilqr.cost_dx = self.cost_dx
self.ilqr.cost_du = self.cost_du
self.ilqr.cost_dxx = self.cost_dxx
self.ilqr.cost_duu = self.cost_duu
self.ilqr.cost_dux = self.cost_dux
elif grad_type == 1:
self.ilqr = PyLQR_iLQRSolver(T=self.T,
plant_dyn=self.plant_dyn,
cost=self.instaneous_cost,
use_autograd=True)
else:
# finite difference
self.ilqr = PyLQR_iLQRSolver(T=self.T,
plant_dyn=self.plant_dyn,
cost=self.instaneous_cost,
use_autograd=False)
return
def solve_ilqr_problem(self,
x0=None,
u_init=None,
n_itrs=150,
verbose=True):
# prepare initial guess
if u_init is None:
u_init = np.ones((self.T, self.nDoF))
if x0 is None:
x0 = np.array([0.11, 0.12, 0.0, 0.0])
if self.ilqr is not None:
self.res = self.ilqr.ilqr_iterate(x0,
u_init,
n_itrs=n_itrs,
tol=1e-6,
verbose=verbose)
return self.res
def plot_ilqr_result(self):
if self.res is not None:
# draw cost evolution and phase chart
fig = plt.figure(figsize=(16, 8), dpi=80)
ax_cost = fig.add_subplot(121)
n_itrs = len(self.res['J_hist'])
ax_cost.plot(np.arange(n_itrs),
self.res['J_hist'],
'r',
linewidth=3.5)
ax_cost.set_xlabel('Number of Iterations', fontsize=20)
ax_cost.set_ylabel('Trajectory Cost')
ax_phase = fig.add_subplot(122)
theta = self.res['x_array_opt'][:, 0:self.nDoF]
theta_dot = self.res['x_array_opt'][:, self.nDoF:self.nDoF * 2]
ax_phase.plot(theta, theta_dot, 'k', linewidth=3.5)
ax_phase.set_xlabel('theta (rad)', fontsize=20)
ax_phase.set_ylabel('theta_dot (rad/s)', fontsize=20)
ax_phase.set_title('Phase Plot', fontsize=20)
ax_phase.plot([theta[-1]], [theta_dot[-1]], 'b*', markersize=16)
plt.figure()
plt.grid()
plt.plot(range(len(theta)), theta, label='angle')
plt.legend()
plt.pause(0.02)
plt.figure()
plt.grid()
plt.plot(range(len(theta)), theta_dot, label='ang_velocity')
plt.legend()
plt.pause(0.02)
return
def animation(self):
if self.res is not None:
theta = self.res['x_array_opt'][:, 0:self.nDoF]
plt.figure()
for i in range(theta.shape[0]):
plt.clf()
plt.grid()
plt.xlim([-.3, 0.7])
plt.ylim([-0.3, 0.4])
plt.plot(self.target[0], self.target[1], 'r*')
x1, x2, y1, y2 = self.forward_kin(theta[i])
plt.plot([0, x1], [0, y1], 'b')
plt.plot([x1, x1 + x2], [y1, y1 + y2], 'b')
plt.pause(0.02)