Gravity Compensation with PyKDL always gives Zero

[ReykCS]

I am trying to calculate the necessary gravity compensation for a KDL chain in Python. However, everything i tried results in a zero vector and I don't know what i missed.

The code I was testing with is the following:

import PyKDL
import math


kdlChain = PyKDL.Chain()

joint1 = PyKDL.Joint(8)
frame1 = PyKDL.Frame(PyKDL.Vector(0.0, 1.0, 0.0))
kdlChain.addSegment(PyKDL.Segment(joint1, frame1))

joint2 = PyKDL.Joint(PyKDL.Joint.RotZ)
frame2 = PyKDL.Frame(PyKDL.Vector(0.0, 2.0, 0.0))
kdlChain.addSegment(PyKDL.Segment(joint2, frame2))

joint3 = PyKDL.Joint(PyKDL.Joint.RotZ)
frame3 = PyKDL.Frame(PyKDL.Rotation.EulerZYX(0.0, 0.0, -math.pi / 2)) * PyKDL.Frame(PyKDL.Vector(0.0, 0.0, 2.0))
kdlChain.addSegment(PyKDL.Segment(joint3, frame3))

joint4 = PyKDL.Joint(PyKDL.Joint.RotZ)
frame4 = PyKDL.Frame(PyKDL.Rotation.EulerZYX(0.0, 0.0, math.pi / 2)) * PyKDL.Frame(PyKDL.Vector(1.0, 1.0, 0.0))
kdlChain.addSegment(PyKDL.Segment(joint4, frame4))

jointAngles = PyKDL.JntArray(3)
jointAngles[0] = 0
jointAngles[1] = 0
jointAngles[2] = 0

grav_vector = PyKDL.Vector(0, 0, -9.81)

dyn_kdl = PyKDL.ChainDynParam(kdlChain, grav_vector)

grav_matrix = PyKDL.JntArray(3)

test = dyn_kdl.JntToGravity(jointAngles, grav_matrix)

gravity_compensation_torques = [grav_matrix[i] for i in range(grav_matrix.rows())]

print(jointAngles, grav_vector, grav_matrix, gravity_compensation_torques, test)

The first part where the chain is build comes from this medium post. The second part where the necessary torques should be calculated is from here

I already tried to modulate the chain, change the gravity vector for testing multiple directions and changing the joint angles. Nothing worked, the resulting grav matrix keeps being filled with zeros.

I expect to somehow get a vector that describes the necessary torques for all joints to compensate the exact force described in grav vector. However, i still don't get how I can include the weights of the joint and segments, maybe this is the key for solving this problem.

[MatthijsBurgh]

This seems to be tested in cpp.

orocos_kinematics_dynamics/orocos_kdl/tests/solvertest.cpp

Lines 1216 to 1224 in 41ca686

	ret = DynSolver.JntToGravity(q, taugrav);
	if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
	CPPUNIT_ASSERT(Equal(0.000, taugrav(0), eps));
	CPPUNIT_ASSERT(Equal(-36.672, taugrav(1), eps));
	CPPUNIT_ASSERT(Equal(4.315, taugrav(2), eps));
	CPPUNIT_ASSERT(Equal(-11.205, taugrav(3), eps));
	CPPUNIT_ASSERT(Equal(0.757, taugrav(4), eps));
	CPPUNIT_ASSERT(Equal(1.780, taugrav(5), eps));
	CPPUNIT_ASSERT(Equal(0.000, taugrav(6), eps));

Which would indicate the bug is in PyKDL.

@ReykCS could you maybe translate your code to cpp and test it. When that does work we have located the bug. Otherwise the issue is more complex.

P.S. what version of (Py)KDL are you using and how did you install it?

[MatthijsBurgh]

@ReykCS ping ;)