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.
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:
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.