#==============================================================================
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)
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()