RBDL vs Pinnochio

RBDL

Pinnochio

Fixed-Base

Model

import rbdl

import pinocchio

Load model from urdf file:

model = rbdl.loadModel(urdf_filename)

model = pinocchio.buildModelFromUrdf(urdf_filename)robot = pinocchio.RobotWrapper(model)

Robot State

q = np.random.rand(model.q_size)qd = np.random.rand(model.qdot_size) qdd = np.zeros(model.qdot_size)

q = pinocchio.randomConfiguration(model)qd = np.random.rand(model.nv) qdd = np.zeros(model.nv) robot.forwardKinematics(q, qd, qdd)robot.computeJointJacobians(q)

Kinematics & Dynamics

CoM State

com_position = np.zeros(3)com_velocity = np.zeros(3)com_acceleration = np.zeros(3)angular_momentum = np.zeros(3)change_of_angular_momentum = np.zeros(3) mass = rbdl.CalcCenterOfMass(model, q, qd, qdd, com_position, com_velocity, com_acceleration, angular_momentum, change_of_angular_momentum)linear_momentum = mass*com_velocitychange_of_linear_momentum = mass*com_acceleration

mass = pinocchio.computeTotalMass(pinocchio_model)com_position = robot.com(q)com_velocity = robot.vcom(q, qd)com_acceleration = robot.acom(q, qd, qdd)linear_momentum = robot.centroidalMomentum(q, qd).linearangular_momentum = robot.centroidalMomentum(q, qd).angularchange_of_linear_momentum = robot.centroidalMomentumVariation(q, qd, qdd).linearchange_of_angular_momentum = robot.centroidalMomentumVariation(q, qd, qdd).angular

Inertia Matrix

inertia_matrix = np.zeros((model.dof_count,model.dof_count))rbdl.CompositeRigidBodyAlgorithm(model, q, inertia_matrix)

inertia_matrix = robot.mass(q)

Nonlinear Term = coriolis + centrifugal + gravity

nonlinear_effects = np.zeros(model.dof_count)rbdl.NonlinearEffects(model, q, qd, nonlinear_effects)

nonlinear_effects = robot.nle(q, qd)

Centroidal Momentum Matrix

N/A

CMM = pinocchio.computeCentroidalMap(model, data, q)

Placement (Position & Rotation)

position = rbdl.CalcBodyToBaseCoordinates(model, q, joint_id, np.zeros(3))rotation = rbdl.CalcBodyWorldOrientation(model, q, joint_id).T

position = robot.framePlacement(q, frame_id).translationrotation = robot.framePlacement(q, frame_id).rotation

Velocity

global_vel_angular_linear = rbdl.CalcPointVelocity6D(model, q, qd, joint_id, np.zeros(3))

local_vel_linear_angular = robot.frameVelocity(q,qd,frame_id)

Acceleration

global_classic_acc_angular_linear = rbdl.CalcPointAcceleration6D(model, q, qd, qdd, joint_id, np.zeros(3))

local_classic_acc_linear_angular = robot.frameClassicAcceleration(frame_id).vector

Jacobian

global_J_angular_linear = np.zeros((6, model.dof_count))rbdl.CalcPointJacobian6D(model, q, joint_id, np.zeros(3), J_angular_linear)

global_J_linear_angular = robot.getFrameJacobian(frame_id, pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED)

Floating-base

When you have a floating base robot, RBDL and Pinocchio has different convensions for q, v and a:

RBDL

q = [global_base_position, global_base_quaternion_x, global_base_quaternion_y, global_base_quaternion_z, joint_positions, global_base_quaternion_w]v = [global_base_velocity_linear, local_base_velocity_angular, joint_velocities]a = [global_base_acceleration_linear, local_base_acceleration_angular, joint_accelerations]

Pinocchio

q = [global_base_position, global_base_quaternion, joint_positions]v = [local_base_velocity_linear, local_base_velocity_angular, joint_velocities]a = [local_base_spacial_acceleration_linear, local_base_spacial_acceleration_angular, joint_accelerations]

Classic acceleration & Spacial acceleration

local_base_spacial_acc_linear = local_base_classic_acc_linear - np.cross(local_base_vel_angular,local_base_vel_linear)local_base_spacial_acc_angular = local_base_classic_acc_angular

Classic acceleration:

Spacial acceleration:

Relations