pinocchio示例 Inverse kinematics 报错： Boost.Python.ArgumentError: *** did not match C++ signature:

在运行pinocchio给出的示例Inverse kinematics(clik)时报错：

Traceback (most recent call last):
  File "test.py", line 29, in <module>
    q = pin.integrate(model)
Boost.Python.ArgumentError: Python argument types in
    pinocchio.pinocchio_pywrap.integrate(Model)
did not match C++ signature:
    integrate(pinocchio::ModelTpl<double, 0, pinocchio::JointCollectionDefaultTpl> model, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > q, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > v)

网上关于这个问题的解法大部分都是安装boost_python库 ，还有将std：：换成boost：：但是并没有解决，所以感觉不是环境问题，从代码入手查看，官网给出的示例代码如下：

    from __future__ import print_function
     
    import numpy as np
    import pinocchio
    
    model = pinocchio.buildSampleModelManipulator()
    data  = model.createData()
    
    JOINT_ID = 6
    xdes     = np.matrix([ 0.5,-0.5,0.5]).T
    
    q        = pinocchio.neutral(model)
    eps      = 1e-4
    IT_MAX   = 1000
    DT       = 1e-1
    
    i=0
    while True:
        pinocchio.forwardKinematics(model,data,q)
        x   = data.oMi[JOINT_ID].translation
        R   = data.oMi[JOINT_ID].rotation
        err = R.T*(x-xdes)
        if np.linalg.norm(err) < eps:
            print("Convergence achieved!")
            break
        if i >= IT_MAX:
            print("\nWarning: the iterative algorithm has not reached convergence to the desired precision")
            break
        J   = pinocchio.jointJacobian(model,data,q,JOINT_ID)[:3,:]
        v   = - np.linalg.pinv(J)*err
        q   = pinocchio.integrate(model,q,v*DT)
        if not i % 10:        print('error = %s' % err.T)
        i += 1
    
    print('\nresult: %s' % q.flatten().tolist())
    print('\nfinal error: %s' % err.T)

看到报错的地方是integrate()函数，没有思绪就先将之前的元素输出。

 

可以看到err和v的维度不对，err应该是一个3x1的矩阵，v应该是一个6x1的矩阵 ，可以看到R的维度是没有问题的，所以问题是x-xdes，输出看一下

明显不对，两个3x1的矩阵相减为什么会是一个3x3的矩阵。将相减部分用循环替换，发现还是报相同的错误，输出发现err还是3x3，乘法应该也有问题，将所有乘法换成点乘就可以运行了。修改代码如下：

from __future__ import print_function 
import numpy as np
import pinocchio as pin   
model = pin.buildSampleModelManipulator()
data  = model.createData()
   
JOINT_ID = 6
xdes     = np.matrix([ 0.5,-0.5,0.5]).T
   
q        = pin.neutral(model)
eps      = 1e-4
IT_MAX   = 1000
DT       = 1e-1

i=0
while True:
    pin.forwardKinematics(model,data,q)
    x   = data.oMi[JOINT_ID].translation
    R   = data.oMi[JOINT_ID].rotation
    ans=np.zeros((3,1))
    for i in range(len(xdes)):
        ans[i]=x[i]-xdes[i]
    # print("ans=",ans)
    err = np.dot(R.T,ans)
    # print("\n err=",err)
    if np.linalg.norm(err) < eps:
        print("Convergence achieved!")
        break
    if i >= IT_MAX:
        print("\nWarning: the iterative algorithm has not reached convergence to the desired precision")
        break
    J   = pin.jointJacobian(model,data,q,JOINT_ID)[:3,:]
    # print(J)
    v   = - np.dot(np.linalg.pinv(J),err)
    # print(v)
    # for i in range(len(q)):
    #     q[i] = q[i] + v[i]*DT
    q=pin.integrate(model, q, v * DT)
    if not i % 10:        print('error = %s' % err.T)
    i += 1
pin.forwardKinematics(model,data,q)
print(data.oMi[JOINT_ID].translation)   
print('\nresult: %s' % q.flatten().tolist())
print('\nfinal error: %s' % err.T)

运行结果如下：

代码用算出来的q重新计算了期望的x（即xdes），可以看出结果是正确的。