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))
#---------------------------- 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)
#
##pos = fk(q = [0.0,-0.3,-0.3])['ee_pos']
#pos = fk(q = [0.785398, 1.28124, 0.0])['ee_pos']
#print('Plotto la posizione')
#print(pos)
#
#
# ---------------------- Solve Inverse dynamics ----------------------------
Idyn_string = pin.generate_inv_dyn(urdf)
#Create casadi function
Idyn = casadi.Function.deserialize(Idyn_string)
#print(Idyn)
# --------------------------- NLP formulation -----------------------------
#Dynamic system
nq = 3
qc = SX.sym('qc', nq)
qcdot = SX.sym('qcdot', nq)
qcddot = np.zeros(nq)
#tau = Idyn(q=qc,qdot= qcdot, qddot = qcddot)['tau']
#print(tau)
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
# Define a certain time grid
tgrid = [T / N * k for k in range(N)]
#Variables
nq = 6
# ---------------------- 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
# First arm variables
qc_f = SX.sym('qc_f', nq) #joint angles
# Second arm variables
qc_s = SX.sym('qc_s', nq) #joint angles
#Same zero acceleration and velocity for both