三、位置级逆运动学求解器（ChainIkSolverPos_NR（牛顿‑拉夫逊法）)

1. 整体流程1.1 迭代代码chainiksolverpos_nr.cpp/** * Find an output joint pose \a q_out, given a starting joint pose * \a q_init and a desired cartesian pose \a p_in * * @return: * E_NOERROR=solution converged to eps in maxiter * E_DEGRADED=solution converged to eps in maxiter, but solution is * degraded in quality (e.g. pseudo-inverse in iksolver is singular) * E_IKSOLVER_FAILED=velocity solver failed * E_NO_CONVERGE=solution did not converge (e.g. large displacement, low iterations) */// q_init 输入的初始关节角度// p_in 输入的目标末端位姿 包含位置（3x1）和姿态（3x3）信息// q_out 输出的求解后的关节角度intChainIkSolverPos_NR::CartToJnt(constJntArrayq_init,constFramep_in,JntArrayq_out){if(nj!=chain.getNrOfJoints())return(error=E_NOT_UP_TO_DATE);if(q_init.rows()!=nj||q_out.rows()!=nj)return(error=E_SIZE_MISMATCH);q_out=q_init;unsignedinti;for(i=0;imaxiter;i++){// 1. 正运动学计算 根据当前 q_out，算出末端位姿 fif(E_NOERRORfksolver.JntToCart(q_out,f))return(error=E_FKSOLVERPOS_FAILED);// 2. 计算位姿误差 目标位姿 p_in 减去当前位姿 f// diff 函数处理了旋转矩阵的差值（通过轴角表示），得到一个 Twistdelta_twist=diff(f,p_in);// 3. 调用速度级 IK 求解器计算关节增量 求解 J * delta_q = delta_twistconstintrc=iksolver.CartToJnt(q_out,delta_twist,delta_q);if(E_NOERRORrc)return(error=E_IKSOLVER_FAILED);// we chose to continue if the child solver returned a positive// "error", which may simply indicate a degraded solution// 4. 更新关节角度 q_new = q_old + delta_qAdd(q_out,delta_q,q_out);// 5. 收敛判断if(Equal(delta_twist,Twist::Zero(),eps))// converged, but possibly with a degraded solution// 注意：即使收敛，如果内部速度求解器返回了正数（如 E_DEGRADED），// 这里会传递该状态，表示解算成功但质量欠佳（例如奇异）return(rcE_NOERROR?E_DEGRADED:E_NOERROR);}return(error=E_MAX_ITERATIONS_EXCEEDED);// failed to converge}1.2 迭代流程函数CartToJnt(q_init,p_in,q_out):检查关节数是否匹配，若不匹配返回错误 初始化 q_out=q_initfori=0to maxiter-1:1.正运动学：f=fksolver.JntToCart(q_out)，若失败返回错误2.计算位姿误差：delta_twist=diff(f,p_in)// 轴角表示3.调用速度级IK求解器：rc=iksolver.CartToJnt(q_out,delta_twist,delta_q)若 rc 表示严重错误（E_NOERRORrc），返回 E_IKSOLVER_FAILED4.更新关节：q_out+=delta_q5.收敛判断：若 delta_twist 接近零（小于 eps），则 若 rcE_NOERROR（即速度求解器返回退化解），返回 E_DEGRADED 否则返回 E_NOERROR 循环结束，返回 E_MAX_ITERATIONS_EXCEEDED2 IK求解器2.1 代码chainiksolvervel_pinv.cpp/** * Find an output joint velocity \a qdot_out, given a starting joint pose * \a q_init and a desired cartesian velocity \a v_in * * @return * E_NOERROR=solution converged to eps in maxiter * E_SVD_FAILED=SVD computation failed * E_CONVERGE_PINV_SINGULAR=solution converged but (pseudo)inverse is singular * * @note if E_CONVERGE_PINV_SINGULAR returned then converged and can * continue motion, but have degraded solution * * @note If E_SVD_FAILED returned, then getSvdResult() returns the error code * from the SVD algorithm. */intChainIkSolverVel_pinv::CartToJnt(constJntArrayq_in,constTwistv_in,JntArrayqdot_out){if(nj!=chain.getNrOfJoints())return(error=E_NOT_UP_TO_DATE);if(nj!=q_in.rows()||nj!=qdot_out.rows())return(error=E_SIZE_MISMATCH);//Let the ChainJntToJacSolver calculate the jacobian "jac" for//the current joint positions "q_in"error=jnt2jac.JntToJac(q_in,jac);if(errorE_NOERROR)returnerror;doublesum;unsignedinti,j;// Initialize near zero singular value counternrZeroSigmas=0;//Do a singular value decomposition of "jac" with maximum//iterations "maxiter", put the results in "U", "S" and "V"//jac = U*S*VtsvdResult=svd.calculate(jac,U,S,V,maxiter);if(0!=svdResult){qdot_out.data.setZero();return(error=E_SVD_FAILED);}// We have to calculate qdot_out = jac_pinv*v_in// Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):// qdot_out = V*S_pinv*Ut*v_in//first we calculate Ut*v_infor(i=0;ijac.columns();i++){sum=0.0;for(j=0;jjac.rows();j++