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))
###############################################################################
# ------------------------------ 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 -----------------------------#
import rospy
import mpc_fatigue.pynocchio_casadi as pin
import matplotlib.pyplot as plt
import numpy as np
import two_pilz_talker_inv_dyn_6DOF as ta
import two_pilz_talker_inv_kin_6DOF as ti
#---------------------------- Load the urdf ---------------------------------
pilz_first = rospy.get_param('/robot_description_1')
pilz_second = rospy.get_param('/robot_description_2')
# ---------------------- 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)
#PURPOSE --> SOLVE INVERSE KINEMATIC
# --------------------------- Load libraries ---------------------------------
from casadi import *
import rospy
import mpc_fatigue.pynocchio_casadi as pin
import talker_inv_kin_pilz_3DOF 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)
#Look inside the fk function
print(fk)
#Variable q
qc = SX.sym('qc', 3)
pos_link_5 = fk(q = qc)['ee_pos']
pos_des_link_5 = SX([10.0,0.1,0.3])
J = dot(pos_link_5 - pos_des_link_5, pos_link_5 - pos_des_link_5)
#Nlp problem
RightConst = True
Plot = True
###############################################################################
# ------------------------------ 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 -----------------------------#
###############################################################################
###############################################################################
# ------------------------------ Functions -----------------------------------#
###############################################################################
if Check.Plot == True or Check.OCP == True:
#---------------------------- Load the urdf --------------------------------#
pilz_first = rospy.get_param('/robot_description_1')
pilz_second = rospy.get_param('/robot_description_2')
# ---------------------- 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)
def talker(q1, q2, F, box):
#Load URDF
pilz_first_urdf = rospy.get_param('/robot_description_1')
pilz_second_urdf = rospy.get_param('/robot_description_2')
# ---------------------- 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_urdf, '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_urdf,
'prbt_link_5')
#Create casadi function
fk_second = casadi.Function.deserialize(fk_second_string)
#Define the talker parameters
if True:
pub = rospy.Publisher(
'topic_position_from_invkin', JointState,
queue_size=100) # This can be seen in rostopic list
rospy.init_node(
'node_position_from_invkin') #This can be seen in rosnode list
hello_str = JointState()
hello_str.header = Header()
hello_str.header.stamp = rospy.Time.now()
hello_str.name = [
'prbt_joint_1', 'prbt_joint_2', 'prbt_joint_3', 'prbt_joint_4',
'prbt_joint_5', 'prbt_joint_6', 'sec_prbt_joint_1',
'sec_prbt_joint_2', 'sec_prbt_joint_3', 'sec_prbt_joint_4',
'sec_prbt_joint_5', 'sec_prbt_joint_6'
]
hello_str.position = []
hello_str.velocity = []
hello_str.effort = []
rate = rospy.Rate(10) # 10hz
i = 0
qsize = np.size(q1[0])
fsize = np.size(F[0])
#Define the marker plotter
if True:
topic = 'visualization_marker_array'
publisher_1 = rospy.Publisher(topic, MarkerArray, queue_size=100)
publisher_2 = rospy.Publisher(topic, MarkerArray, queue_size=100)
publisher_3 = rospy.Publisher(topic, MarkerArray, queue_size=100)
#rospy.init_node('register')
markerArray = MarkerArray()
marker_f = Marker()
marker_s = Marker()
weight = Marker()
marker_f.header.frame_id = "/prbt_base"
marker_s.header.frame_id = "/prbt_base"
weight.header.frame_id = "/prbt_base"
marker_f.ns = "Fz_forces"
marker_s.ns = "Fz_forces"
weight.ns = "Fz_forces"
marker_f.type = marker_f.ARROW
marker_s.type = marker_s.ARROW
weight.type = marker_s.ARROW
marker_f.action = marker_f.ADD
marker_s.action = marker_s.ADD
weight.action = weight.ADD
marker_f.id = 0
marker_s.id = 1
weight.id = 2
#Scales
marker_f.scale.x = 0.2
marker_s.scale.x = 0.2
weight.scale.x = 0.2
marker_f.scale.y = 0.02
marker_s.scale.y = 0.02
weight.scale.y = 0.02
marker_f.scale.z = 0.02
marker_s.scale.z = 0.02
weight.scale.z = 0.02
marker_f.color.a = 1.0
marker_f.color.r = 1.0
marker_f.color.g = 1.0
marker_f.color.b = 1.0
marker_s.color.a = 1.0
marker_s.color.r = 1.0
marker_s.color.g = 1.0
marker_s.color.b = 1.0
weight.color.a = 1.0
weight.color.r = 1.0
weight.color.g = 1.0
weight.color.b = 1.0
marker_f.pose.orientation.x = 0.0
marker_f.pose.orientation.y = 1.0
marker_f.pose.orientation.z = 0.0
marker_f.pose.orientation.w = -1.0
marker_s.pose.orientation.x = 0.0
marker_s.pose.orientation.y = 1.0
marker_s.pose.orientation.z = 0.0
marker_s.pose.orientation.w = -1.0
weight.pose.orientation.x = 0.0
weight.pose.orientation.y = 1.0
weight.pose.orientation.z = 0.0
weight.pose.orientation.w = 1.0
while not rospy.is_shutdown():
if i < qsize:
if i < fsize:
qf = [
q1[0][i], q1[1][i], q1[2][i], q1[3][i], q1[4][i], q1[5][i]
]
qs = [
q2[0][i], q2[1][i], q2[2][i], q2[3][i], q2[4][i], q2[5][i]
]
pos_Fzf = fk_first(q=qf)['ee_pos']
pos_Fzs = fk_second(q=qs)['ee_pos']
#Marker scales
marker_f.scale.x = 0.2 * F[0][i] / F[2]
marker_s.scale.x = 0.2 * F[1][i] / F[2]
weight.scale.x = 0.2 * (F[1][i] + F[0][i]) / F[2]
# Marker f position
marker_f.pose.position.x = pos_Fzf[0] + 0.1
marker_f.pose.position.y = pos_Fzf[1]
marker_f.pose.position.z = pos_Fzf[2]
marker_s.pose.position.x = pos_Fzs[0] - 0.1
marker_s.pose.position.y = pos_Fzs[1]
marker_s.pose.position.z = pos_Fzs[2]
weight.pose.position.x = box[0][i]
weight.pose.position.y = box[1][i]
weight.pose.position.z = box[2][i]
pub.publish(hello_str)
publisher_1.publish([marker_f])
publisher_2.publish([marker_s])
publisher_3.publish([weight])
rate.sleep()
hello_str.header.stamp = rospy.Time.now()
hello_str.position = [
q1[0][i], q1[1][i], q1[2][i], q1[3][i], q1[4][i], q1[5][i],
q2[0][i], q2[1][i], q2[2][i], q2[3][i], q2[4][i], q2[5][i]
]
else:
marker_f.action = marker_f.DELETE
marker_s.action = marker_s.DELETE
weight.action = weight.DELETE
publisher_1.publish([marker_f])
publisher_2.publish([marker_s])
publisher_3.publish([weight])
break
i += 1
