from urdf_parser_py.urdf import URDF, Pose
import os
import urdf2casadi.urdfparser as u2c
import pybullet as pb
path_to_urdf = "../../urdf/snake_robot.urdf"
root = "base_link"
tip = "link16"
#get robot models
#rbdl
snake_rbdl = rbdl.loadModel(path_to_urdf)
#u2c
snake = u2c.URDFparser()
snake.from_file(path_to_urdf)
#pybullet
sim = pb.connect(pb.DIRECT)
snake_pb = pb.loadURDF(path_to_urdf,
useFixedBase=True,
flags=pb.URDF_USE_INERTIA_FROM_FILE)
pb.setGravity(0, 0, -9.81)
#joint info
jointlist, names, q_max, q_min = snake.get_joint_info(root, tip)
n_joints = snake.get_n_joints(root, tip)
#u2c & pybullet
q = [None] * n_joints
from urdf_parser_py.urdf import URDF, Pose
import os
import urdf2casadi.urdfparser as u2c
import pybullet as pb
path_to_urdf = "/home/lmjohann/urdf2casadi/examples/urdf/ur5_mod.urdf"
root = "base_link"
tip = "tool0"
#get robot models
#rbdl
ur5_rbdl = rbdl.loadModel(path_to_urdf)
#u2c
ur5 = u2c.URDFparser()
ur5.from_file(path_to_urdf)
#pybullet
sim = pb.connect(pb.DIRECT)
ur5_pb = pb.loadURDF(path_to_urdf,
useFixedBase=True,
flags=pb.URDF_USE_INERTIA_FROM_FILE)
pb.setGravity(0, 0, -9.81)
#joint info
jointlist, names, q_max, q_min = ur5.get_joint_info(root, tip)
n_joints = ur5.get_n_joints(root, tip)
#u2c & pybullet
q = [None] * n_joints
path_to_urdf = "../../urdf/gantry.urdf" root = "gantry_link_base" tip = "gantry_tool0" #get robot models #kdl ok, ur_tree = kdlurdf.treeFromFile(path_to_urdf) gantry_kdl = ur_tree.getChain(root,tip) #rbdl gantry_rbdl = rbdl.loadModel(path_to_urdf) #u2c gantry = u2c.URDFparser() gantry.from_file(path_to_urdf) #pybullet sim = pb.connect(pb.DIRECT) gantry_pb = pb.loadURDF(path_to_urdf, useFixedBase=True, flags = pb.URDF_USE_INERTIA_FROM_FILE) pb.setGravity(0, 0, -9.81) #joint info jointlist, names, q_max, q_min = gantry.get_joint_info(root, tip) n_joints = gantry.get_n_joints(root, tip) #declarations #kdl
import rbdl
import numpy as np
import casadi as cs
from urdf_parser_py.urdf import URDF, Pose
import os
import urdf2casadi.urdfparser as u2c
root = "calib_kuka_arm_base_link"
tip = "kuka_arm_7_link"
urmodel = rbdl.loadModel("../../urdf/kuka.urdf")
kuka = u2c.URDFparser()
kuka.from_file("../../urdf/kuka.urdf")
jointlist, names, q_max, q_min = kuka.get_joint_info(root, tip)
n_joints = kuka.get_n_joints(root, tip)
q_rbdl = np.zeros(n_joints)
qdot_rbdl = np.zeros(n_joints)
qddot_rbdl = np.zeros(n_joints)
id_rbdl = np.zeros(n_joints)
q = [None] * n_joints
qdot = [None] * n_joints
qddot = [None] * n_joints
gravity = [0., 0., -9.81]
id_sym = kuka.get_inverse_dynamics_rnea(root, tip, gravity)
error = np.zeros(n_joints)
def u2c2np(asd):
def _load_urdf(urdf_path: str):
'''Function that creates a parser and load the path'''
robot_parser = u2c.URDFparser()
robot_parser.from_file(urdf_path)
return robot_parser
def ur5():
ur5 = u2c.URDFparser()
path_to_urdf = (os.path.dirname(os.path.abspath(__file__)) +
"/urdf/ur5_mod.urdf")
ur5.from_file(path_to_urdf)
return ur5