#!/usr/bin/env julia using RigidBodyDynamics using StaticArrays include("models.jl") function julia_test(model) robot = parse_urdf(Float64, PATH * "$model.urdf") if model != "lwr" && model != "tiago" for joint in out_joints(root_body(robot), robot) floatingjoint = Joint(string(joint), frame_before(joint), frame_after(joint), QuaternionFloating{Float64}()) replace_joint!(robot, joint, floatingjoint) end end println("Julia Test") println(" model: ", model) println(" nq: ", num_positions(robot)) println(" nv: ", num_velocities(robot)) state = MechanismState(robot) M = mass_matrix(state) println("FD: M after CRBA: ", M) tau = inverse_dynamics(state, state.v) println("ID: tau after RNEA: ", transpose(tau)) end for model in MODELS julia_test(model) end