#==============================================================================
if var_pl != 0:
plt.figure(1)
plt.clf()
plt.plot(T, x_opt)
plt.step(T, vertcat(DM.nan(1), u_opt))
plt.show()
#==============================================================================
# Animation the joint space trajectories
# Plot the trajectory using the 2DoF pendulum URDF
# with the prismatic joint at 0.3m fixed position
#==============================================================================
if var_ani != 0:
nj = 2
j_name = []
for j in range(nj):
tmp = "J%02d" % (j + 1)
j_name.append(tmp)
q_pl = np.zeros([N + 1, 2])
q_pl[:, 0] = phi_opt + np.pi
qdot_pl = np.zeros([N + 1, 2])
qdot_pl[:, 0] = omega_opt
pose = fn.RobotPose(j_name, q_pl, qdot_pl, [], int(1 / h))
pose.interpolate(30)
js.posePublisher(pose)
print len(lbg)
# print type(len(joint_str-nj))
if M == 1:
zerovec = np.zeros([N_stage[0] + 1, len(joint_str) - nj])
else:
zerovec = np.zeros([N_tot + 1, len(joint_str) - nj])
q_opt = np.concatenate((q_opt, zerovec), axis=1)
qdot_opt = np.concatenate((qdot_opt, zerovec), axis=1)
qddot_opt = np.concatenate((qddot_opt, zerovec), axis=1)
tau_opt = np.concatenate((tau_opt, zerovec), axis=1)
# print np.shape(q_opt)
slomo = 1 # if slomo < 1.0 than the motion is slowed down
# print np.shape(q_opt)
pose = fn.RobotPose(name=joint_str,
q=q_opt,
qdot=qdot_opt,
qddot=qddot_opt,
tau=tau_opt,
rate=int(slomo * 1 / dt_opt[0]))
# print joint_str
# print pose.name
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)
# 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()
print "The optimized time step size: h_opt = %s" % h_opt.flatten().tolist()
print "The optimized final time: Tf_opt = %s" % Tf_opt.flatten().tolist()
#==============================================================================
if var_ani != 0:
nj = 2
j_name = []
for j in range(nj):
tmp = "J%02d" % (j + 1)
j_name.append(tmp)
tau_opt = np.vstack((tau_opt, tau_opt[-1, :]))
# print np.shape(q_opt)
# print np.shape(qdot_opt)
# print np.shape(qddot_opt)
# print np.shape(tau_opt)
pose = fn.RobotPose(j_name, q_opt, qdot_opt, tau_opt, int(1 / h_opt))
# pose.interpolate(rviz_rate)
if var_rec != 0:
# os.system("ffmpeg -f x11grab -s 1900x1080 -r 25 -i :0.0 -qscale 5 ~/screenGrab.mpeg")
# subprocess.call(['ffmpeg','-f','x11grab','-s','1900x1080','r','25','i',':0.0','-qscale','5','screenGrab.mpeg'])
# subprocess.call("ffmpeg -f x11grab -s 1900x1080 -r 25 -i :0.0 -qscale 5 ~/screenGrab.mpeg", shell=True)
# subproc = subprocess.Popen("ffmpeg -f x11grab -s 1900x1080 -r 25 -i :0.0 -qscale 5 screenGrab.mpeg", shell=True)
js.posePublisher(pose)
# subproc.kill()
# os.system("q")
else:
js.posePublisher(pose)
#==============================================================================
# DEBUGGING AREA
#==============================================================================
# TESTED: True
path = '/home/user/catkin_ws/src/tameshiwari/python/OCP_centauro_7dof_arm/results'
# filename = 'RobotPose_centauro_max_momentum_7dof_v1_2019-11-29T14:08:09.mat' # OLD but probably the same
filename = 'RobotPose_centauro_max_momentum_7dof_v1_2019-12-04T10:09:13.mat' # NEW but probably the same
#######################################
# LOAD FILE
#######################################
matfile = sio.loadmat(path+'/'+filename)
matfile['name'] = [str(x).strip() for x in matfile['name']]
# matfile['']
pose = fn.RobotPose(
name=matfile['name'],
q=matfile['q'],
qdot=matfile['qdot'],
# tau=matfile['tau'],
rate=matfile['rate']
)
rostopic = '/xbotcore/ros_command'
# rostopic = '/pose_state'
pubrate = 120.0
slowrate = 1.0
poserate = pubrate*slowrate
pose.interpolate(poserate)
if rostopic == '/pose_state':
T = 0.5
else:
T = 3
# 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(name=['j_arm2_1', 'j_arm2_4'],
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
print "The optimized time step size: h_opt = %s" % h_opt.flatten().tolist()
print "The optimized final time: Tf_opt = %s" % Tf_opt.flatten().tolist()
if np.shape(pose.qdot)[0] > 1:
state_str.velocity = pose.qdot[iteration, :]
if nj_extra > 0:
state_str.velocity = np.concatenate(
(state_str.velocity, zerovec))
if np.shape(pose.tau)[0] > 1:
state_str.effort = pose.tau[iteration, :]
if nj_extra > 0:
state_str.effort = np.concatenate((state_str.effort, zerovec))
# rospy.loginfo(state_str) # use for debugging
pub.publish(state_str)
iteration += 1
rate.sleep()
if __name__ == '__main__':
try:
path = '/home/user/catkin_ws/results/'
filename = 'RobotPose_centauro_max_momentum_3dof_2019-11-15T11:27:39.mat'
matfile = sio.loadmat(path + filename)
matfile['name'] = [str(x) for x in matfile['name']]
pose = fn.RobotPose(name=matfile['name'],
q=matfile['q'],
qdot=matfile['qdot'],
tau=matfile['tau'],
rate=matfile['rate'])
posePublisher(pose)
except rospy.ROSInterruptException:
pass
def invKin(fk=None,frame=None,j_str=None,q_init=None,p_init=None,p_des=None,T=None,animate=True):
if not fk:
print "forward kinematics function not provided"
else:
forKin = fk
frame = frame or 'arm1_8'
j_str = j_str or config.JointNames('arm1').getName()
j_lim = config.JointBounds(j_str)
q_lb = j_lim.getLowerBound()
q_ub = j_lim.getUpperBound()
T = T or 1
nq = forKin.numel_in()
q = SX.sym('q',nq)
if q_init is not None:
p_init = forKin(q=q_init)['ee_pos'].full()
elif p_init is None and q_init is None:
print "ERROR: no initial q or p provided"
p_des = p_des or [1.0, 0.0, 1.35]
if isinstance(p_des,list):
p_des = np.array(p_des)
elif not isinstance(p_des,np.ndarray):
print "ERROR: in p_des creation"
p_des_sym = forKin(q=q)['ee_pos']
p_des_num = SX(p_des)
J = dot(p_des_sym-p_des_num,p_des_sym-p_des_num)
nlp = dict(f=J, x=q)
opts = {"print_time":False}
opts["ipopt"] = {"print_level":0}
solver = nlpsol('solver','ipopt',nlp,opts)
sol = solver(x0=q_init, lbx=q_lb, ubx=q_ub)
q_sol = sol['x'].full().transpose()
q_motion = np.append(np.array(q_init),q_sol.flatten()).reshape(2,-1)
q_dot = np.zeros([2,nq])
print "######## RUNNING INVERSE KINEMATICS CHECK ########\n"
print "Final joint configuration: %s [rad]" %q_sol.flatten()
print "The desired xyz end-effector position: %s" %p_des.tolist()
p_sol = np.around(forKin(q=q_sol)['ee_pos'].full().flatten(),2)
print "The achieved xyz end-effector position: %s" %p_sol.tolist()
e_des = sqrt(sol['f'].full().flatten())
print "The achieved ||des-res||_2 error: %s [mm]\n" %round(e_des*1000,0)
tolerance = 0.005 # 5 millimeter
tolerance = 0.05 # 5 centimeter
if solver.stats()['return_status'] == 'Solve_Succeeded' and e_des <= tolerance:
print "######## INVERSE KINEMATICS SUCCEEDED ########\n"
cont = True
else:
print "######## ERROR ######## \n\nDesired end point is not in task space \n\n######## ERROR ########\n"
cont = False
if animate:
pose = fn.RobotPose(name=j_str,q=q_motion,qdot=q_dot,rate=1/float(T))
pose.interpolate(30.0)
js.posePublisher(pose)
return cont, q_sol.flatten()
v_Nz = v_N[2]
print "The optimized final EE velocity along the z-axis: %s [m/s]" % v_Nz
print "The optimized final joint velocties: %s [rad/s]" % qdot_opt[
impact_ind, :]
p_N = forKin(q=q_opt[impact_ind, :])['ee_pos']
print "The optimized final EE cartesian position: %s [m]" % p_N
print "This position is w.r.t. origin of the world frame"
print "The optimized final momentum h_iota: %s [kg m/s]" % h_iota_lin
print "The optimized final momentum ||h_iota||_2: %s [kg m/s]" % h_iota_resultant
slomo = 1 # if slomo < 1.0 than the motion is slowed down
print np.shape(q_opt)
pose = fn.RobotPose(name=joint_str,
q=q_opt,
qdot=qdot_opt,
qddot=qddot_opt,
tau=tau_opt,
rate=int(slomo * 1 / dt_opt[0]))
print joint_str
print pose.name
Tf_opt = dt_opt * np.array(N_stage).reshape(-1, 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)
# pose.plot_qdot(lb=qdot_lb,ub=qdot_ub,limits=True,Tvec=T_opt)
pose.interpolate(30)
save = False
if save:
pose.saveMat()
