c="r",
marker="+")
plt.show()
Tf_opt = dt_opt * np.array(N_stage).reshape(-1, 1)
if M == 1:
T_opt = np.zeros([N_stage[0] + 1, 1])
T_opt[0:N_stage[0] + 1] = np.matlib.linspace(0, Tf_opt[0], N_stage[0] + 1)
else:
T_opt = np.zeros([N_tot + 1, 1])
T_opt[0:N_stage[0] + 1] = np.matlib.linspace(0, Tf_opt[0], N_stage[0] + 1)
T_opt[N_cum[0]:N_cum[1] + 1] = np.matlib.linspace(Tf_opt[0],
Tf_opt[0] + Tf_opt[1],
N_stage[1] + 1)
solver_param = fn.SolverParam(solver.stats(), N_tot + 1, dt_opt, T_opt, Tf_opt,
sol['f'].full())
evaluate = True
if evaluate:
evaluation = fn.RobotEvaluation()
evaluation.addSolverParam(solver_param.dict)
evaluation.addBoundedParam('q', 'joint_position', np.transpose(q_opt),
q_lbopt, q_ubopt)
evaluation.addBoundedParam('qdot', 'joint_velocity',
np.transpose(qdot_opt), qdot_lbopt, qdot_ubopt)
evaluation.addBoundedParam('qddot', 'joint_acceleration',
np.transpose(qddot_opt), qddot_lbopt,
qddot_ubopt)
tau_ubopt = np.zeros([N_tot + 1, nj])
tau_ubopt[0:N_cum[0]] = np.matlib.repmat(
W_tau[0] * np.asarray(tau_ub).reshape(1, -1), N_cum[0], 1)
tau_ubopt[N_cum[0]:] = np.matlib.repmat(
# print q_opt[126]
#==============================================================================
# RETRIEVE CONFIGURATION TRAJECTORY
#==============================================================================
xyz = np.zeros([N_tot + 1, 3])
for j in range(N_tot + 1):
xyz[j, :] = forKin(q=q_opt[j, :])['ee_pos'].full().reshape(-1, 3)
#==============================================================================
# CREATE SOLVERPARAM & ROBOTPOSE CLASSES, THEN SAVE TO FILE IF DESIRED
#==============================================================================
# CREATE A CLASS FOR THIS WHERE WE STORE Q,QDOT,QDDOT,TAU AND ALL THE OPTIMIZER STUFF
solver_param = fn.SolverParam(solver.stats(), N, h_opt, T, Tf, sol['f'].full())
pose = fn.RobotPose(q=q_opt,
qdot=qdot_opt,
qddot=qddot_opt,
tau=tau_opt,
rate=int(1 / h_opt[0]))
if var_save:
solver_param.saveMat()
pose.saveMat()
#==============================================================================
# PRINT STATEMENTS
#==============================================================================
print "The initial time step size: h_0 = %s" % h0_vec
print "The initial final time: Tf_0 = %s" % Tf0_vec.flatten()
# plt.show(block=False)
if var_pl:
plt.show()
Tf_opt = h_opt*np.array(N_stage).reshape(-1,1)
if M == 1:
T_opt = np.zeros([N_stage[0]+1,1])
T_opt[0:N_stage[0]+1] = np.matlib.linspace(0,Tf_opt[0],N_stage[0]+1)
else:
T_opt = np.zeros([N_tot+1,1])
T_opt[0:N_stage[0]+1] = np.matlib.linspace(0,Tf_opt[0],N_stage[0]+1)
T_opt[N_cum[0]:N_cum[1]+1] = np.matlib.linspace(Tf_opt[0],Tf_opt[0]+Tf_opt[1],N_stage[1]+1)
data_points = N_tot + 1
solver_param = fn.SolverParam(solver.stats(),data_points,h_opt,T,Tf,sol['f'].full())
if var_save:
evaluation = fn.RobotEvaluation()
evaluation.addSolverParam(solver_param.dict)
evaluation.addBoundedParam('q','joint_position',np.transpose(q_opt),q_lbopt,q_ubopt)
evaluation.addBoundedParam('qdot','joint_velocity',np.transpose(qdot_opt),qdot_lbopt,qdot_ubopt)
evaluation.addBoundedParam('qddot','joint_acceleration',np.transpose(qddot_opt),qddot_lbopt,qddot_ubopt)
tau_ubopt = np.zeros([nj,data_points])
tau_ubopt[:,0:N_cum[0]] = np.matlib.repmat(W_tau[0]*tau_lim[0,:].reshape(-1,1),1,N_cum[0])
tau_ubopt[:,N_cum[0]:] = np.matlib.repmat(W_tau[1]*tau_lim[0,:].reshape(-1,1),1,N_stage[1]+1)
tau_lbopt = -1*tau_ubopt
evaluation.addBoundedParam('tau','joint_effort',np.transpose(tau_opt),tau_lbopt,tau_ubopt)
evaluation.addBoundedParam('Tf','final_time',Tf_opt,Tf_lb,Tf_ub)
evaluation.addParam('time',np.transpose(T_opt))
evaluation.addParam('impact_postion',p_end)
evaluation.addParam('stage_interval',N_stage)