ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
I figured out how to calculate joint efforts from gravity using KDL's Python API.
The kdl.ChainDynParam class provides a JntToGravity method which will produce the joint torques resulting from gravity
Example:
import PyKDL as kdl
# Given: kdl_tree, base_link, end_link
chain = kdl_tree.getChain(base_link, end_link)
# Assuming: chain contains 3 rotational joints
grav_vector = kdl.Vector(0, 0, -9.81) # relative to kdl chain base link
dyn_kdl = kdl.ChainDynParam(chain, grav_vector)
jt_positions = kdl.JntArray(3)
jt_positions[0] = 0.0
jt_positions[1] = 0.0
jt_positions[2] = 0.0
grav_matrix = kdl.JntArray(3)
dyn_kdl.JntToGravity(jt_positions, grav_matrix)
gravity_compensating_jt_torques = [grav_matrix[i] for i in range(grav_matrix.rows())]
Orocos KDL repository: https://github.com/orocos/orocos_kinematics_dynamics
PyKDL Documentation (incomplete): http://docs.ros.org/diamondback/api/kdl/html/python/
2 | No.2 Revision |
I figured out how to calculate joint efforts from gravity using KDL's Python API.
The kdl.ChainDynParam class provides a JntToGravity method which will produce the joint torques resulting from gravityneeded to counteract the effects of gravity.
Example:
import PyKDL as kdl
# Given: kdl_tree, base_link, end_link
chain = kdl_tree.getChain(base_link, end_link)
# Assuming: chain contains 3 rotational joints
grav_vector = kdl.Vector(0, 0, -9.81) # relative to kdl chain base link
dyn_kdl = kdl.ChainDynParam(chain, grav_vector)
jt_positions = kdl.JntArray(3)
jt_positions[0] = 0.0
jt_positions[1] = 0.0
jt_positions[2] = 0.0
grav_matrix = kdl.JntArray(3)
dyn_kdl.JntToGravity(jt_positions, grav_matrix)
gravity_compensating_jt_torques = [grav_matrix[i] for i in range(grav_matrix.rows())]
Orocos KDL repository: https://github.com/orocos/orocos_kinematics_dynamics
PyKDL Documentation (incomplete): http://docs.ros.org/diamondback/api/kdl/html/python/