Fail to compute CartToJnt of ChainIdSolver_RNE

[YummyOu]

I would like to compute the torques of some movement and by using PyKDL.ChainIdSolver_RNE. But something went wrong when compute CartToJnt.

from __future__ import print_function

from urdf_parser_py.urdf import URDF
from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model
import PyKDL
import math

class KdlIssue:
    def __init__(self):

        robot = URDF.from_xml_file("/home/amigo/ur5.urdf")
        tree = kdl_tree_from_urdf_model(robot)

        self._chain = tree.getChain("base", "tool0")

        print(self._chain.getNrOfJoints(), "joints")
        print(self._chain.getNrOfSegments(), "segments")

        # Param
        self._gravity = PyKDL.Vector(0, 0, -9.81)
        self._wrenches = [PyKDL.Wrench(),PyKDL.Wrench(),PyKDL.Wrench(),PyKDL.Wrench(),PyKDL.Wrench(),PyKDL.Wrench()]

        self.tau_gc = PyKDL.JntArray(self._chain.getNrOfJoints())

    def computeInvDym(self, q, qd, qdd):

        # gcSolver = PyKDL.ChainDynParam(self._chain, self._gravity)
        RNE_IdSolver = PyKDL.ChainIdSolver_RNE(self._chain, self._gravity)
        ret = RNE_IdSolver.CartToJnt(q, qd, qdd, self._wrenches, self.tau_gc)

        if ret < 0:
            print("CartToJnt Failed")
            return -1

    def move_robot(self):

        q = PyKDL.JntArray(6)
        qd = PyKDL.JntArray(6)
        qdd = PyKDL.JntArray(6)

        q[0] = 0 # 1.24
        q[1] = -1.74
        q[2] = 1.6
        q[3] = -1.39
        q[4] = 4.71
        q[5] = 0

        # start = rospy.Time.now().to_sec()
        t = 0

        # sin() curve
        A = 0.4
        T = math.pi
        offset = [0, -1.74, 1.6, -1.39, 4.71, 0]

        print('-------------------- start -----------------------')

        while t < T:

            t += 0.0001

            # compute cur_q
            cur_q = A * math.cos(2 * math.pi * t / T) + offset[5]
            # differential q: compute current vel
            cur_vel = -2 * math.pi / T * A * math.sin(2 * math.pi * t  / T)
            # differential v: compute current acc
            cur_acc = - math.pow(2 * math.pi / T, 2) * A * math.cos(2 * math.pi * t / T)

            q[5] = cur_q
            qd[5] = cur_vel
            qdd[5] = cur_acc

            q[0] = cur_q
            qd[0] = cur_vel
            qdd[0] = cur_acc

            if self.computeInvDym(q, qd, qdd) == -1:
                break

            print(self.tau_gc)

if __name__ == "__main__":
    kdl = KdlIssue()
    kdl.move_robot()

runmove_robot()and print:

Unknown attribute: iyx
Unknown attribute: izx
Unknown attribute: izy
6 joints
8 segments
-------------------- start -----------------------
CartToJnt Failed

I could successfully use inverse kinematics functions.

I also would like to ask:

• In this case, the graveity vector is PyKDL.Vector(0, 0, -9.81)?
• Now there are no external force for each torque, which means self._wrenches is zero matrix?

[MatthijsBurgh]

@YummyOu I have updated your code patch. Next time, please provide an example that I is executable. So Including imports, a __main__ , etc. In your case you also use other libraries and even a URDF file. Please provide these or provide information about where to find them.

By only checking if the return value is smaller than zero, you don't check the actual value. Which already provides a lot of information. It is -4, which corresponds to size mismatch. A small search in the code would have shown you https://github.com/orocos/orocos_kinematics_dynamics/blob/7c424ffe77e11b2b7761cd8bb7f91f3abce923df/orocos_kdl/src/chainidsolver_recursive_newton_euler.cpp#L50

Which shows that the wrenches should be of the size of the number of segments, not joints. So by changing that line to

self._wrenches = [PyKDL.Wrench()] * self._chain.getNrOfSegments()

Your problem is solved.

[MatthijsBurgh]

On the remaining questions:

• Yes your gravity vector is correct. Gravity is pulling down.
• Yes, in case no external forces are applied, wrenches is all zero.

[YummyOu]

Thank you so much! It works!

self._wrenches = [PyKDL.Wrench()] * self._chain.getNrOfSegments()