class Centauro_features:
#Define the joints that will be controlled by the python script
joint_name = [
'j_arm1_1', 'j_arm1_2', 'j_arm1_3', 'j_arm1_4', 'j_arm1_5', 'j_arm1_6',
'j_arm1_7', 'j_arm2_1', 'j_arm2_2', 'j_arm2_3', 'j_arm2_4', 'j_arm2_5',
'j_arm2_6', 'j_arm2_7'
]
#Define joint position lower bound
joint_angle_lb = np.array([
-3.312, 0.020, -2.552, -2.465, -2.569, -1.529, -2.565, -3.3458,
-3.4258, -2.5614, -2.4794, -2.5394, -1.5154, -2.5554
])
#Define joint position upper bound
joint_angle_ub = np.array([
1.615, 3.431, 2.566, 0.280, 2.562, 1.509, 2.569, 1.6012, -0.0138,
2.5606, 0.2886, 2.5546, 1.5156, 2.5686
])
#Define joint velocity limit
joint_velocity_lim = np.array([
3.86, 3.86, 6.06, 6.06, 11.72, 11.72, 20.35, 3.86, 3.86, 6.06, 6.06,
11.72, 11.72, 20.35
])
#Define torque limit
joint_torque_lim = np.array([
147.00, 147.00, 147.00, 147.00, 55.00, 55.00, 28.32, 147.00, 147.00,
147.00, 147.00, 55.00, 55.00, 28.32
])
# Compute forward kinematics
fk_la = casadi.Function.deserialize(
pin.generate_forward_kin(centauro, end_LA))
fk_ra = casadi.Function.deserialize(
pin.generate_forward_kin(centauro, end_RA))
# Jacobians
jac_la = casadi.Function.deserialize(
pin.generate_jacobian(centauro, end_LA))
jac_ra = casadi.Function.deserialize(
pin.generate_jacobian(centauro, end_RA))
# Inverse dynamics
Idyn = casadi.Function.deserialize(pin.generate_inv_dyn(centauro))
if Check.Plot == True or Check.OCP == True:
#---------------------------- Load the urdf --------------------------------#
pilzLR = rospy.get_param('/robot_description_1')
pilzRR = rospy.get_param('/robot_description_2')
end_point = 'end_effector'
# Forward kinematics
fk_LR = casadi.Function.deserialize(
pin.generate_forward_kin(pilzLR, end_point))
fk_RR = casadi.Function.deserialize(
pin.generate_forward_kin(pilzRR, end_point))
# Jacobians
jac_LR = casadi.Function.deserialize(
pin.generate_jacobian(pilzLR, end_point))
jac_RR = casadi.Function.deserialize(
pin.generate_jacobian(pilzRR, end_point))
# Inverse dynamics
Idyn_LR = casadi.Function.deserialize(pin.generate_inv_dyn(pilzLR))
Idyn_RR = casadi.Function.deserialize(pin.generate_inv_dyn(pilzRR))
###############################################################################
# --------------------------- Initial conditions -----------------------------#
###############################################################################
# Define time
T = 2.
N = 50
h = T / N
# ---------------------- Solve forward kinematics pilz 1 --------------------- #Postion of link 5 in cartesian space (Solution to direct kinematics) fk_first_string = pin.generate_forward_kin(pilz_first, 'prbt_link_5') #Create casadi function fk_first = casadi.Function.deserialize(fk_first_string) # ---------------------- Solve forward kinematics pilz 2 --------------------- #Postion of link 5 in cartesian space (Solution to direct kinematics) fk_second_string = pin.generate_forward_kin(pilz_second, 'prbt_link_5') #Create casadi function fk_second = casadi.Function.deserialize(fk_second_string) # ---------------------- Solve Jacobian of link_5 ---------------------------- #Jacobian for link_5 (Solution to direct kinematics) jac_first_string = pin.generate_jacobian(pilz_first, 'prbt_link_5') jac_second_string = pin.generate_jacobian(pilz_second, 'prbt_link_5') #Create casadi function jac_first = casadi.Function.deserialize(jac_first_string) jac_second = casadi.Function.deserialize(jac_second_string) # ---------------------- Solve Inverse dynamics Pilz 1 and 2 ----------------- Idyn_first_string = pin.generate_inv_dyn(pilz_first) Idyn_second_string = pin.generate_inv_dyn(pilz_second) #Create casadi function Idyn_first = casadi.Function.deserialize(Idyn_first_string) Idyn_second = casadi.Function.deserialize(Idyn_second_string) # --------------------------- NLP formulation ------------------------------- #Variables nq = 6
# ------------------------------ Functions -----------------------------------#
###############################################################################
if Check.Plot == True or Check.OCP == True:
#---------------------------- Load the urdf --------------------------------#
pilzLR = rospy.get_param('/robot_description_1')
pilzRR = rospy.get_param('/robot_description_2')
end_point = 'end_effector'
# Forward kinematics
fk_LR = casadi.Function.deserialize(pin.generate_forward_kin(pilzLR, end_point))
fk_RR = casadi.Function.deserialize(pin.generate_forward_kin(pilzRR, end_point))
# Jacobians
jac_LR = casadi.Function.deserialize(pin.generate_jacobian(pilzLR, end_point))
jac_RR = casadi.Function.deserialize(pin.generate_jacobian(pilzRR, end_point))
# Inverse dynamics
Idyn_LR = casadi.Function.deserialize(pin.generate_inv_dyn(pilzLR))
Idyn_RR = casadi.Function.deserialize(pin.generate_inv_dyn(pilzRR))
###############################################################################
# --------------------------- Initial conditions -----------------------------#
###############################################################################
# Define time
T = 2.
N = 50
h = T/N
import numpy as np
import talker_inv_dyn_pilz_6DOF as ta
import talker_inv_kin_pilz_6DOF as ti
#---------------------------- Load the urdf ---------------------------------
urdf = rospy.get_param('/robot_description')
# ---------------------- Solve forward kinematics ----------------------------
#Postion of link 5 in cartesian space (Solution to direct kinematics)
fk_string = pin.generate_forward_kin(urdf, 'prbt_link_5')
#Create casadi function
fk = casadi.Function.deserialize(fk_string)
# ---------------------- Solve Jacobian of link_5 ----------------------------
#Jacobian for link_5 (Solution to direct kinematics)
jac_string = pin.generate_jacobian(urdf, 'prbt_link_5')
#Create casadi function
jac_dict = casadi.Function.deserialize(jac_string)
# ---------------------- Solve Inverse dynamics ----------------------------
Idyn_string = pin.generate_inv_dyn(urdf)
#Create casadi function
Idyn = casadi.Function.deserialize(Idyn_string)
# --------------------------- NLP formulation -----------------------------
#Variables
nq = 6
qc = SX.sym('qc', nq) #joint angles
qcdot = SX.sym('qcdot', nq) # joint velocities
qcddot = np.zeros(nq) # Joint acceleration
# ---------------------- Solve forward kinematics pilz 1 --------------------#
end_point = 'prbt_link_5' #prbt_link_5 #end_effector
#Postion of link 5 in cartesian space (Solution to direct kinematics)
fk_first_string = pin.generate_forward_kin(pilz_first, end_point)
#Create casadi function
fk_first = casadi.Function.deserialize(fk_first_string)
# ---------------------- Solve forward kinematics pilz 2 --------------------#
#Postion of link 5 in cartesian space (Solution to direct kinematics)
fk_second_string = pin.generate_forward_kin(pilz_second, end_point)
#Create casadi function
fk_second = casadi.Function.deserialize(fk_second_string)
# ---------------------- Solve Jacobian of link_5 ---------------------------#
#Jacobian for link_5 (Solution to direct kinematics)
jac_first_string = pin.generate_jacobian(pilz_first, end_point)
jac_second_string = pin.generate_jacobian(pilz_second, end_point)
#Create casadi function
jac_first = casadi.Function.deserialize(jac_first_string)
jac_second = casadi.Function.deserialize(jac_second_string)
# ---------------------- Solve Inverse dynamics Pilz 1 and 2 ----------------#
Idyn_first_string = pin.generate_inv_dyn(pilz_first)
Idyn_second_string = pin.generate_inv_dyn(pilz_second)
#Create casadi function
Idyn_first = casadi.Function.deserialize(Idyn_first_string)
Idyn_second = casadi.Function.deserialize(Idyn_second_string)
###############################################################################
# --------------------------- Initial conditions -----------------------------#
###############################################################################