kindyn = cas_kin_dyn.CasadiKinDyn(urdf)
joint_str = config.JointNames('arm1').getName()
# print joint_str
joint_num = [1, 2, 3, 4, 5, 6]
joint_str = [
joint_str[j]
for j in [i for i, x in enumerate(joint_str) if int(x[-1]) in joint_num]
]
nj = len(joint_str)
nq = 3
# print joint_str
# JOINT LIMITS
joint_lim = config.JointBounds(joint_str)
q_lb = joint_lim.getLowerBound()
q_ub = joint_lim.getUpperBound()
# print q_lb
# print q_ub
# q_lb = [-3.312, 0.04, -2.465]
# q_ub = [1.615, 0.04, 0.28]
qdot_ub = joint_lim.getVelocityBound()
factor = 3.0
qdot_ub = [x * factor for x in qdot_ub]
qdot_lb = [-x for x in qdot_ub]
# nj_ones =
qddot_ub = 100 * np.ones(nj)
qddot_ub = qddot_ub.flatten().tolist()
qddot_lb = [-x for x in qddot_ub]
tau_ub = joint_lim.getTorqueBound() # [143.00 143.00]
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()