逆矩阵使用雅可比矩阵的伪逆矩阵,KDL代码中并没有单独的伪逆矩阵的求解器,而是直接求的伪逆矩阵和误差的乘积。二、正向运动学方程 正向运动学方程的x是关节状态,y是笛卡儿位姿。 2.1 ChainFkSolverPos_recursive::JntToCart JntToCart功能是根据关节状态q_in,算出该规划组内某个关节的笛卡儿位姿。过程就是求解正向...