ARCOS KDL

Our implementation of a very simple Kinematic Dynamic Library. For more information go to the official documentation. Because this is still in development you will need to generate the documentation yourself. Go to the documentation section for that.

For the API documentation visit this page.

Building

Execute:

cargo build --lib

Testing

Execute:

cargo test

Documentation

Execute:

cargo doc

For testing a default chain was created. To compare against orocos-kdl, it can be created with:

from PyKDL import Frame, FrameVel, Rotation, Vector, Segment
from PyKDL import Chain, JntArray, Joint, JntArrayVel
from PyKDL import ChainFkSolverPos_recursive, ChainFkSolverVel_recursive

def create_segment(x, y, z, roll, pitch, yaw, joint_type=Joint.RotZ):
    return Segment(Joint(joint_type),
                   Frame(Rotation.RPY(roll, pitch, yaw), Vector(x,y,z)))

def create_angles(angle_list):
    joint_array = JntArray(len(angle_list))
    for index,angle in enumerate(angle_list):
        joint_array[index] = angle
    return joint_array

# Define the segments
segments = [
    create_segment(0.1, -0.2, 0.3, -0.1, -0.2, 0.3, Joint.None),
    create_segment(0.2, 0.5, 0.1, 0.58, -1.2, 2.3),
    create_segment(0.02, 0.3, -0.81, 3.0, -1.7, 1.3),
    create_segment(0.4, -0.27, 0.19, 1.57 ,-1.57, 0.0),
    create_segment(0.24, -0.18, 0.16, 0.01, 0.0, -0.01),
    create_segment(1.0, -0.5, -0.01, 0.05, -1.02, 0.34),
    create_segment(0.02, 0.58, -0.87, 0.95, -1.74, 0.68),
    create_segment(0.2, 0.1, -0.1, -0.76, -0.94, 0.25)
]

# Create the chain
chain = Chain()
for segment in segments:
    chain.addSegment(segment)

# Create the solver
solver = ChainFkSolverPos_recursive(chain)
diff_solver = ChainFkSolverVel_recursive(chain)

# Get the result
result = Frame()
diff_result = FrameVel()
joint_array = create_angles([0.1, -0.95, 0.57, 0.68, -0.27, 0.39, 0.47])
qdots_array = create_angles([1.0, -0.65, 0.87, 0.01, -0.41, -0.98, 0.24])
solver.JntToCart(joint_array, result)
diff_solver.JntToCart(JntArrayVel(joint_array, qdots_array), diff_result)
print(result)
print(diff_result.deriv())