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):
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 path_to_urdf = "../../urdf/gantry.urdf" root = "gantry_link_base" tip = "gantry_tool0" gantry = u2c.URDFparser() gantry.from_file(path_to_urdf) gantry_rbdl = rbdl.loadModel(path_to_urdf) jointlist, names, q_max, q_min = gantry.get_joint_info(root, tip) n_joints = gantry.get_n_joints(root, tip) q = np.zeros(n_joints) qdot = np.zeros(n_joints) tau = np.zeros(n_joints) fd_rbdl_aba = np.zeros(n_joints) fd_rbdl_crba = np.zeros(n_joints) gravity = [0., 0., -9.81] fd_sym_aba = gantry.get_forward_dynamics_aba(root, tip, gravity=gravity) fd_sym_crba = gantry.get_forward_dynamics_crba(root, tip, gravity=gravity) error_rbdl_u2c_crba = np.zeros(n_joints)
# so that data coming from different equipment, at different sample rates,
# is properly handled.
filterFreq = 7.5
# The inverse-kinematics data is filtered using a 2nd order Butterworth filter
# in the forwards and backwards direction (so there is no phase introduced).
# This is the 3db frequency of
#Read in the model
# The model name convention follows the one in OpenSim:
# gait: model intended for walking simulations
# 9: DoF
# 12: Number of muscles. In this case torque muscles
model = rbdl.loadModel("gait912.lua",
kwargs={
"floating_base": True,
"verbose": True
})
print("DoF: ", model.q_size)
q_size = model.q_size
qdot_size = model.qdot_size
#Read in the experimental data
qVIn = genfromtxt("qIK.csv", delimiter=",")
#Column 0: is time
#Columns 1 ,..., N: correspond to q0, ..., qN
fVIn = genfromtxt("grf.ff", delimiter=",") #0th column is time
#Column 0: is time
#Columns 1, 2, 3: Right foot: CoP
#Columns 4, 5, 6: Right foot: Ground Forces
def estimateValidationTorques(self):
""" calculate torques of trajectory from validation measurements and identified params """
# TODO: don't duplicate simulation code
# TODO: get identified params directly into idyntree (new KinDynComputations class does not
# have inverse dynamics yet, so we have to go over a new urdf file for now)
import os
v_data = np.load(self.validation_file)
dynComp = iDynTree.DynamicsComputations()
if self.opt['estimateWith'] == 'urdf':
params = self.model.xStdModel
else:
params = self.model.xStd
outfile = self.model.urdf_file + '.tmp.urdf'
self.urdfHelpers.replaceParamsInURDF(input_urdf=self.model.urdf_file,
output_urdf=outfile,
new_params=params)
if self.opt['useRBDL']:
import rbdl
self.model.rbdlModel = rbdl.loadModel(outfile,
floating_base=self.opt['floatingBase'],
verbose=False)
self.model.rbdlModel.gravity = np.array(self.model.gravity)
else:
dynComp.loadRobotModelFromFile(outfile)
os.remove(outfile)
old_skip = self.opt['skipSamples']
self.opt['skipSamples'] = 8
self.tauEstimatedValidation = None # type: np._ArrayLike
for m_idx in self.progress(range(0, v_data['positions'].shape[0], self.opt['skipSamples'] + 1)):
if self.opt['useRBDL']:
torques = self.model.simulateDynamicsRBDL(v_data, m_idx, None, params)
else:
torques = self.model.simulateDynamicsIDynTree(v_data, m_idx, dynComp, params)
if self.tauEstimatedValidation is None:
self.tauEstimatedValidation = torques
else:
self.tauEstimatedValidation = np.vstack((self.tauEstimatedValidation, torques))
if self.opt['skipSamples'] > 0:
self.tauMeasuredValidation = v_data['torques'][::self.opt['skipSamples'] + 1]
self.Tv = v_data['times'][::self.opt['skipSamples'] + 1]
else:
self.tauMeasuredValidation = v_data['torques']
self.Tv = v_data['times']
# add simulated base forces also to measurements
if self.opt['floatingBase']:
self.tauMeasuredValidation = \
np.concatenate((self.tauEstimatedValidation[:, :6], self.tauMeasuredValidation), axis=1)
#TODO: add contact forces to estimation, so far validation is only correct for fixed-base!
print(Fore.RED+'No proper validation for floating base yet!'+Fore.RESET)
self.opt['skipSamples'] = old_skip
self.val_error = sla.norm(self.tauEstimatedValidation - self.tauMeasuredValidation) \
* 100 / sla.norm(self.tauMeasuredValidation)
print("Relative validation error: {}%".format(self.val_error))
self.val_residual = np.mean(sla.norm(self.tauEstimatedValidation-self.tauMeasuredValidation, axis=1))
print("Absolute validation error: {} Nm".format(self.val_residual))
torque_limits = []
for joint in self.model.jointNames:
torque_limits.append(self.model.limits[joint]['torque'])
self.val_nrms = helpers.getNRMSE(self.tauMeasuredValidation, self.tauEstimatedValidation, limits=torque_limits)
print("NRMS validation error: {}%".format(self.val_nrms))
# Configuracion articular deseada
qdes = np.array([1.0, 1.0, 1.0, 1.0, 1.0])
# Velocidad articular deseada
dqdes = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
# Aceleracion articular deseada
ddqdes = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
# =============================================================
# Posicion resultante de la configuracion articular deseada
xdes = robot_fkine_m(qdes)[0:3, 3]
# Copiar la configuracion articular en el mensaje a ser publicado
jstate.position = q
pub.publish(jstate)
# Modelo RBDL
modelo = rbdl.loadModel('../urdf/robot.urdf')
ndof = modelo.q_size # Grados de libertad
zeros = np.zeros(ndof) # Vector de ceros
# Frecuencia del envio (en Hz)
freq = 20
dt = 1.0 / freq
rate = rospy.Rate(freq)
# Simulador dinamico del robot
robot = Robot(q, dq, ndof, dt)
# Bucle de ejecucion continua
t = 0.0
# Se definen las ganancias del controlador
# Configuracion articular deseada
qdes = np.array([-0.732, 1.101, 0.13, -1.896, -1.589, 0.134, 0.])
# Velocidad articular deseada
dqdes = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
# Aceleracion articular deseada
ddqdes = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
# =============================================================
# Posicion resultante de la configuracion articular deseada
xdes = fkine(qdes)[0:3, 3]
# Copiar la configuracion articular en el mensaje a ser publicado
jstate.position = q
pub.publish(jstate)
# Modelo RBDL
modelo = rbdl.loadModel('../urdf/robot_gazebo.urdf')
ndof = modelo.q_size # Grados de libertad
zeros = np.zeros(ndof) # Vector de ceros
# Frecuencia del envio (en Hz)
freq = 20
dt = 1.0 / freq
rate = rospy.Rate(freq)
# Simulador dinamico del robot
robot = Robot(q, dq, ndof, dt)
# Bucle de ejecucion continua
t = 0.0
# Se definen las ganancias del controlador
#current_path = os.path.abspath(__file__)
path = os.path.abspath(__file__)
dir_path = os.path.dirname(path)
load_torques = False
torques_saved_filename = 'torques_init_traj.npy'
T_init = 3
T_traj = 5
T_impedance_zero = 10
freq = 100
# ROBOT MODEL for trying ID
robot_urdf_file = os.environ["ROBOTOLOGY_ROOT"]+'/configs/ADVR_shared/bigman/urdf/bigman.urdf'
robot_model = rbdl.loadModel(robot_urdf_file, verbose=False, floating_base=False)
#LH_name = 'LWrMot3'
#RH_name = 'RWrMot3'
default_joint_stiffness = np.array([8000., 5000., 8000., 5000., 5000., 2000.,
8000., 5000., 5000., 5000., 5000., 2000.,
5000., 8000., 5000.,
5000., 8000., 5000., 5000., 300., 2000., 300.,
300., 300.,
5000., 8000., 5000., 5000., 300., 2000., 300.])
default_joint_damping = np.array([30., 50., 30., 30., 30., 5.,
30., 50., 30., 30., 30., 5.,
30., 50., 30.,
30., 50., 30., 30., 1., 5., 1.,
1., 1.,
30., 50., 30., 30., 1., 5., 1.])
pd_tau_weights = np.array([0.80, 0.50, 0.80, 0.50, 0.50, 0.20,
q = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
# Velocidad inicial
dq = np.array([0., 0., 0., 0., 0., 0.])
# Configuracion articular deseada
qdes = np.array([-2.26, 0.086, -0.26, -1.39, -0.55, 0.0])
# =============================================================
# Posicion resultante de la configuracion articular deseada
xdes = fkine(qdes)[0:3, 3]
# Copiar la configuracion articular en el mensaje a ser publicado
jstate.position = q
pub.publish(jstate)
# Modelo RBDL
modelo = rbdl.loadModel(
'/home/diegopalma/Documents/ROSProjects/lab_ws/src/projectfr/urdf/robotfr.urdf'
)
ndof = modelo.q_size # Grados de libertad
# Frecuencia del envio (en Hz)
freq = 20
dt = 1.0 / freq
rate = rospy.Rate(freq)
# Simulador dinamico del robot
robot = Robot(q, dq, ndof, dt)
# Se definen las ganancias del controlador
#valores = 0.1*np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
valores = 1 * np.array([5, 5, 5, 5, 5, 5])
Kp = np.diag(valores)
# Configuracion articular deseada
qdes = np.array([0.85, 0, 0.28, 0.2, -0.85, -0.93])
# Velocidad articular deseada
dqdes = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
# Aceleracion articular deseada
ddqdes = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
# =============================================================
# Posicion resultante de la configuracion articular deseada
xdes = fkine(qdes)[0:3, 3]
# Copiar la configuracion articular en el mensaje a ser publicado
jstate.position = q
pub.publish(jstate)
# Modelo RBDL
modelo = rbdl.loadModel('../urdf/robot_gazebo_proyecto.urdf')
ndof = modelo.q_size # Grados de libertad
zeros = np.zeros(ndof) # Vector de ceros
# Frecuencia del envio (en Hz)
freq = 1000
dt = 1.0 / freq
rate = rospy.Rate(freq)
# Simulador dinamico del robot
robot = Robot(q, dq, ndof, dt)
# Bucle de ejecucion continua
t = 0.0
u = np.zeros(ndof) # Reemplazar por la ley de control
M = np.zeros([ndof, ndof])
import pybullet as pb 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) ur_chain = ur_tree.getChain(root,tip) #rbdl urmodel = rbdl.loadModel(path_to_urdf) #u2c gantry = u2c.URDFparser() gantry.from_file(path_to_urdf) #pybullet sim = pb.connect(pb.DIRECT) pbmodel = 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) q_kdl = kdl.JntArray(n_joints)
q_int = x[:nDof]
qd_int = x[nDof:]
qdd = np.zeros((2))
dxdt = np.zeros(x.shape)
rbdl.ForwardDynamics(lua_model, q_int, qd_int, c, qdd)
dxdt[:nDof] = qd_int[:]
dxdt[nDof:] = qdd[:]
return dxdt
if __name__ == "__main__":
#load the lua model
lua_model = rbdl.loadModel(
"/home/patrick/Documents/cartPenudlum_template/model.lua")
#load a data file
data = pd.read_csv(
"/home/patrick/Documents/cartPenudlum_template/build/RES/meshup_cart_pendulum_count_0000.csv"
)
data = data.to_numpy()
time = data[:, 0]
#assign the values
q0 = data[:, 0]
q1 = data[:, 1]
qd0 = data[:, 2]
qd1 = data[:, 3]
tau0 = data[:, 4]
# set joint positions
if q is not None:
q = np.array(q)
robot.set_joint_positions(q, joint_ids)
else:
print('got None')
# step in the simulation
world.step(sleep_dt=dt)
######################################################
# IK using RBDL and own damped-least-squares inverse #
######################################################
elif solver_flag == 4:
# load model using rbdl
model = rbdl.loadModel(urdf, verbose=False, floating_base=False)
rbdl_link_id = model.GetBodyId('iiwa_link_7') # end_effector_name)
rbdl_id = [model.GetBodyId(name) - 1 for name in chain_name]
print("RBDL Link ID: {}".format(rbdl_id))
print("RBDL Q size: {}".format(model.q_size))
print("RBDL number of DoFs: {}".format(model.dof_count))
J = np.zeros((3, model.dof_count))
def position_pd(x, xd, v, vd=np.zeros(3), ad=np.zeros(3), kp=100, kd=None):
# if damping is not specified, make it critically damped
if kd is None:
kd = 2.0 * np.sqrt(kp)
# return PD error
return kp * (xd - x) + kd * (vd - v) + ad
import kdl_parser_py.urdf as kdlurdf import pybullet as pb root = 'base_link' tip = 'tool0' path_to_urdf = '../../urdf/ur5_mod.urdf' #get robot models #kdl ok, ur_tree = kdlurdf.treeFromFile(path_to_urdf) ur5_kdl = ur_tree.getChain(root,tip) #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)
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
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)
robot = Cogimon(
init_pos=init_pos,
init_ori=init_ori,
)
urdf_file = robot.model_xml.encode()
# Reset robot
robot_state = robot.reset(physicsClientId=pb_client)
# Robot Model
robot_model = RobotModel(urdf_file=robot.model_xml.encode(),
floating_base=not robot.is_fixed_base)
# Rbdl Model
rbdl_model = rbdl.loadModel(urdf_file,
verbose=False,
floating_base=not robot.is_fixed_base)
# ######## #
# Check FK #
# ######## #
q_des = robot.n_ordered_joints
print('q_robot', robot.n_ordered_joints)
print('q_model', robot_model.q_size)
print('q_rbdl', rbdl_model.q_size, rbdl_model.qdot_size)
print('\n****' * 3)
print('Body names:')
print('-----------')
for name in robot.get_body_parts_names():
print(name)
#Rospy and msg definitions
import rospy
from kuka_iiwa_14_prismatic_gripper.msg import end_effector
from sensor_msgs.msg import JointState
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from std_msgs.msg import Header
from std_msgs.msg import Duration
from std_msgs.msg import Float64
import quaternion
#RBDL model
import rbdl
model = rbdl.loadModel(
"/home/gabriele/Documents/NRP/GazeboRosPackages/src/kuka_iiwa_14_prismatic_gripper/scripts/iiwa.urdf"
)
# Msg "end_effector.msg" structure
# Header header
# float64[] orientation (euler angles) - position
# float64[] velocity
# float64 time
def talker():
pub = rospy.Publisher('ee_data', end_effector, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(100) # 100hz
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
kuka_model = rbdl.loadModel(
"/home/lmjohann/urdf2casadi/examples/urdf/kuka.urdf")
root = "calib_kuka_arm_base_link"
tip = "kuka_arm_7_link"
kuka = u2c.URDFparser()
kuka.from_file("/home/lmjohann/urdf2casadi/examples/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)
tau_rbdl = np.zeros(n_joints)
fd_rbdl = np.zeros(n_joints)
q = [None] * n_joints
qdot = [None] * n_joints
tau = [None] * n_joints
gravity = [0., 0., -9.81]
fd_sym = kuka.get_forward_dynamics_crba(root, tip, gravity)
error = np.zeros(n_joints)
