def main():
try:
rospy.init_node('avalos_limb_py')
#Frecuency for Sawyer robot
f = 100
rate = rospy.Rate(f)
#Define topic
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
# Class to record
data = Data()
#Class limb to acces information sawyer
limb = Limb()
#Initial position
limb.move_to_neutral()
time.sleep(3)
# Position init
initial = limb.joint_angles()
print "Posicion inicial terminada"
#begin to record data
data.writeon("cin_directa.txt")
time.sleep(0.5)
limb.move_to_joint_positions({
"right_j6": 0.0,
"right_j5": 0.0,
"right_j4": 0.0,
"right_j3": 0.0,
"right_j2": 0.0,
"right_j1": 0.0,
"right_j0": 0.0
})
limb.move_to_joint_positions({
"right_j6": initial["right_j6"],
"right_j5": initial["right_j5"],
"right_j4": initial["right_j4"],
"right_j3": initial["right_j3"],
"right_j2": initial["right_j2"],
"right_j1": initial["right_j1"],
"right_j0": initial["right_j0"]
})
time.sleep(1)
data.writeoff()
print "FINISH"
initial = limb.joint_angles()
p0 = np.array([
initial["right_j0"], initial["right_j1"], initial["right_j2"],
initial["right_j3"], initial["right_j4"], initial["right_j5"],
initial["right_j6"]
])
# Posiition end
p1 = [0, 0, 0, 0, 0, 0, 0]
p2 = p0
# Knost vector time. We assum the process will take 10 sec
k = np.array([0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1])
tiempo_estimado = 7
k_t = tiempo_estimado * k
k_pt = np.array([k_t[0], k_t[5], k_t[-1]])
# Set inperpolation in linear form
k_j0 = sp.interpolate.interp1d(k_pt, [p0[0], p1[0], p2[0]],
kind='linear')(k_t)
k_j1 = sp.interpolate.interp1d(k_pt, [p0[1], p1[1], p2[1]],
kind='linear')(k_t)
k_j2 = sp.interpolate.interp1d(k_pt, [p0[2], p1[2], p2[2]],
kind='linear')(k_t)
k_j3 = sp.interpolate.interp1d(k_pt, [p0[3], p1[3], p2[3]],
kind='linear')(k_t)
k_j4 = sp.interpolate.interp1d(k_pt, [p0[4], p1[4], p2[4]],
kind='linear')(k_t)
k_j5 = sp.interpolate.interp1d(k_pt, [p0[5], p1[5], p2[5]],
kind='linear')(k_t)
k_j6 = sp.interpolate.interp1d(k_pt, [p0[6], p1[6], p2[6]],
kind='linear')(k_t)
# Obtain all point following the interpolated points
j0 = path_simple_cub_v0(k_j0, k_t, f)
j1 = path_simple_cub_v0(k_j1, k_t, f)
j2 = path_simple_cub_v0(k_j2, k_t, f)
j3 = path_simple_cub_v0(k_j3, k_t, f)
j4 = path_simple_cub_v0(k_j4, k_t, f)
j5 = path_simple_cub_v0(k_j5, k_t, f)
j6 = path_simple_cub_v0(k_j6, k_t, f)
l = len(j0)
print "l"
print l
# Vector for velocity
v_j0 = np.zeros(l)
v_j1 = np.zeros(l)
v_j2 = np.zeros(l)
v_j3 = np.zeros(l)
v_j4 = np.zeros(l)
v_j5 = np.zeros(l)
v_j6 = np.zeros(l)
# Vector for acceleration
a_j0 = np.zeros(l)
a_j1 = np.zeros(l)
a_j2 = np.zeros(l)
a_j3 = np.zeros(l)
a_j4 = np.zeros(l)
a_j5 = np.zeros(l)
a_j6 = np.zeros(l)
# Vector for jerk
jk_j0 = np.zeros(l)
jk_j1 = np.zeros(l)
jk_j2 = np.zeros(l)
jk_j3 = np.zeros(l)
jk_j4 = np.zeros(l)
jk_j5 = np.zeros(l)
jk_j6 = np.zeros(l)
for i in xrange(l - 1):
v_j0[i] = (j0[i + 1] - j0[i]) * f
v_j1[i] = (j1[i + 1] - j1[i]) * f
v_j2[i] = (j2[i + 1] - j2[i]) * f
v_j3[i] = (j3[i + 1] - j3[i]) * f
v_j4[i] = (j4[i + 1] - j4[i]) * f
v_j5[i] = (j5[i + 1] - j5[i]) * f
v_j6[i] = (j6[i + 1] - j6[i]) * f
v_j0[-1] = v_j0[-2]
v_j1[-1] = v_j0[-2]
v_j2[-1] = v_j0[-2]
v_j3[-1] = v_j0[-2]
v_j4[-1] = v_j0[-2]
v_j5[-1] = v_j0[-2]
v_j6[-1] = v_j0[-2]
for i in xrange(l - 1):
a_j0[i] = (v_j0[i + 1] - v_j0[i]) * f
a_j1[i] = (v_j1[i + 1] - v_j1[i]) * f
a_j2[i] = (v_j2[i + 1] - v_j2[i]) * f
a_j3[i] = (v_j3[i + 1] - v_j3[i]) * f
a_j4[i] = (v_j4[i + 1] - v_j4[i]) * f
a_j5[i] = (v_j5[i + 1] - v_j5[i]) * f
a_j6[i] = (v_j6[i + 1] - v_j6[i]) * f
a_j0[-2] = a_j0[-3]
a_j1[-2] = a_j1[-3]
a_j2[-2] = a_j2[-3]
a_j3[-2] = a_j3[-3]
a_j4[-2] = a_j4[-3]
a_j5[-2] = a_j5[-3]
a_j6[-2] = a_j6[-3]
a_j0[-1] = a_j0[-2]
a_j1[-1] = a_j1[-2]
a_j2[-1] = a_j2[-2]
a_j3[-1] = a_j3[-2]
a_j4[-1] = a_j4[-2]
a_j5[-1] = a_j5[-2]
a_j6[-1] = a_j6[-2]
for i in xrange(l - 1):
jk_j0[i] = (a_j0[i + 1] - a_j0[i]) * f
jk_j1[i] = (a_j1[i + 1] - a_j1[i]) * f
jk_j2[i] = (a_j2[i + 1] - a_j2[i]) * f
jk_j3[i] = (a_j3[i + 1] - a_j3[i]) * f
jk_j4[i] = (a_j4[i + 1] - a_j4[i]) * f
jk_j5[i] = (a_j5[i + 1] - a_j5[i]) * f
jk_j6[i] = (a_j6[i + 1] - a_j6[i]) * f
jk_j0[-3] = jk_j0[-4]
jk_j1[-3] = jk_j1[-4]
jk_j2[-3] = jk_j2[-4]
jk_j3[-3] = jk_j3[-4]
jk_j4[-3] = jk_j4[-4]
jk_j5[-3] = jk_j5[-4]
jk_j6[-3] = jk_j6[-4]
jk_j0[-2] = jk_j0[-3]
jk_j1[-2] = jk_j1[-3]
jk_j2[-2] = jk_j2[-3]
jk_j3[-2] = jk_j3[-3]
jk_j4[-2] = jk_j4[-3]
jk_j5[-2] = jk_j5[-3]
jk_j6[-2] = jk_j6[-3]
jk_j0[-1] = jk_j0[-2]
jk_j1[-1] = jk_j1[-2]
jk_j2[-1] = jk_j2[-2]
jk_j3[-1] = jk_j3[-2]
jk_j4[-1] = jk_j4[-2]
jk_j5[-1] = jk_j5[-2]
jk_j6[-1] = jk_j6[-2]
j = np.array([j0, j1, j2, j3, j4, j5, j6])
v = np.array([v_j0, v_j1, v_j2, v_j3, v_j4, v_j5, v_j6])
a = np.array([a_j0, a_j1, a_j2, a_j3, a_j4, a_j5, a_j6])
jk = np.array([jk_j0, jk_j1, jk_j2, jk_j3, jk_j4, jk_j5, jk_j6])
save_matrix(j, "data_p.txt", f)
save_matrix(v, "data_v.txt", f)
save_matrix(a, "data_a.txt", f)
save_matrix(jk, "data_y.txt", f)
#Build message
my_msg = JointCommand()
# POSITION_MODE
my_msg.mode = 4
my_msg.names = [
"right_j0", "right_j1", "right_j2", "right_j3", "right_j4",
"right_j5", "right_j6"
]
data.writeon("cin_trayectoria.txt")
time.sleep(0.5)
for i in xrange(l):
my_msg.position = [j0[i], j1[i], j2[i], j3[i], j4[i], j5[i], j6[i]]
my_msg.velocity = [
v_j0[i], v_j1[i], v_j2[i], v_j3[i], v_j4[i], v_j5[i], v_j6[i]
]
my_msg.acceleration = [
a_j0[i], a_j1[i], a_j2[i], a_j3[i], a_j4[i], a_j5[i], a_j6[i]
]
pub.publish(my_msg)
rate.sleep()
time.sleep(1)
data.writeoff()
print "Programa terminado "
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user.')
def main():
try:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('avalos_limb_py', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = 'right_arm'
group = moveit_commander.MoveGroupCommander(group_name)
#frecuency for Sawyer robot
f = 100
alfa = 0.9
rate = rospy.Rate(f)
# add gripper
gripper = intera_interface.Gripper('right_gripper')
gripper.calibrate()
gripper.open()
moveit_robot_state = RobotState()
joint_state = JointState()
joint_state.header = Header()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
#Define topic
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
limb = Limb()
limb.move_to_neutral()
limb.move_to_joint_positions({"right_j6": 2})
group.clear_pose_targets()
group.set_start_state_to_current_state()
# We can get the joint values from the group and adjust some of the values:
pose_goal = geometry_msgs.msg.Pose()
# Orientation
pose_goal.orientation.x = -1
pose_goal.orientation.y = 0.0
pose_goal.orientation.z = 0.0
pose_goal.orientation.w = 0.0
q0 = np.array([])
q1 = np.array([])
q2 = np.array([])
q3 = np.array([])
q4 = np.array([])
q5 = np.array([])
q6 = np.array([])
# Cartesian position - Carga '01'
pose_goal.position.x = 0.758552
pose_goal.position.y = -0.3435
pose_goal.position.z = 0.25
group.set_pose_target(pose_goal)
carga1 = group.plan().joint_trajectory.points
n = len(carga1)
joint_state.position = [
carga1[-1].positions[0], carga1[-1].positions[1],
carga1[-1].positions[2], carga1[-1].positions[3],
carga1[-1].positions[4], carga1[-1].positions[5],
carga1[-1].positions[6]
]
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)
tmp = np.array([])
if (n > 8):
tmp = np.append(tmp, 0)
k = int(math.sqrt(n) + 2)
r = int((n - 1) / float(k))
for i in range(k):
print i
tmp = np.append(tmp, int(r * (i + 1) - 1))
tmp = np.append(tmp, n - 1)
else:
for i in range(n):
print i
tmp = np.append(tmp, i)
print "tmp:", tmp
for i in range(len(tmp)):
q0 = np.append(q0, carga1[int(tmp[i])].positions[0])
q1 = np.append(q1, carga1[int(tmp[i])].positions[1])
q2 = np.append(q2, carga1[int(tmp[i])].positions[2])
q3 = np.append(q3, carga1[int(tmp[i])].positions[3])
q4 = np.append(q4, carga1[int(tmp[i])].positions[4])
q5 = np.append(q5, carga1[int(tmp[i])].positions[5])
q6 = np.append(q6, carga1[int(tmp[i])].positions[6])
print "q000", q0
# Cartesian position - Carga '00'
#pose_goal.position.x = 0.85
#pose_goal.position.y = -0.4
pose_goal.position.z = -0.01
group.set_pose_target(pose_goal)
carga0 = group.plan().joint_trajectory.points
q0 = np.append(q0, carga0[-1].positions[0])
q1 = np.append(q1, carga0[-1].positions[1])
q2 = np.append(q2, carga0[-1].positions[2])
q3 = np.append(q3, carga0[-1].positions[3])
q4 = np.append(q4, carga0[-1].positions[4])
q5 = np.append(q5, carga0[-1].positions[5])
q6 = np.append(q6, carga0[-1].positions[6])
#'''
q = np.array([q0, q1, q2, q3, q4, q5, q6])
print "q001", q0
m_time, t_min_tiempo = min_time(q)
start = time.time()
opt = Opt_avalos(q, f, 0.9)
v_time = opt.full_time()
j_1, v_1, a_1, jk_1 = generate_path_cub(q, v_time, f)
ext_1 = len(j_1[0, :])
end = time.time()
print('Process Time:', end - start)
v_jk = sqrt(np.mean(np.square(jk_1)))
print("Opt Time:", v_time)
print("Min Time:", m_time)
#print('Optimizacion:',opt.result())
max_v = np.amax(np.absolute(v_1))
max_ac = np.amax(np.absolute(a_1))
max_jk = np.amax(np.absolute(jk_1))
print "Max Velo:", max_v
print "Max Acel:", max_ac
print "Max Jerk:", max_jk
#raw_input('Iniciar_CT_execute?')
#Build message
my_msg = JointCommand()
# POSITION_MODE
my_msg.mode = 4
my_msg.names = [
"right_j0", "right_j1", "right_j2", "right_j3", "right_j4",
"right_j5"
] #,"right_j6"]
print("Control por trayectoria iniciado.")
#time.sleep(0.25)
q0 = np.array([])
q1 = np.array([])
q2 = np.array([])
q3 = np.array([])
q4 = np.array([])
q5 = np.array([])
q6 = np.array([])
q0 = np.append(q0, carga0[-1].positions[0])
q1 = np.append(q1, carga0[-1].positions[1])
q2 = np.append(q2, carga0[-1].positions[2])
q3 = np.append(q3, carga0[-1].positions[3])
q4 = np.append(q4, carga0[-1].positions[4])
q5 = np.append(q5, carga0[-1].positions[5])
q6 = np.append(q6, carga0[-1].positions[6])
joint_state.position = [
carga1[-1].positions[0], carga1[-1].positions[1],
carga1[-1].positions[2], carga1[-1].positions[3],
carga1[-1].positions[4], carga1[-1].positions[5],
carga1[-1].positions[6]
]
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)
# Cartesian position - Base '01'
pose_goal.position.x = 0.80791
pose_goal.position.y = 0.4461
pose_goal.position.z = 0.2501
group.set_pose_target(pose_goal)
base1 = group.plan().joint_trajectory.points
n = len(base1)
joint_state.position = [
base1[-1].positions[0], base1[-1].positions[1],
base1[-1].positions[2], base1[-1].positions[3],
base1[-1].positions[4], base1[-1].positions[5],
base1[-1].positions[6]
]
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)
tmp = np.array([])
if (n > 14):
tmp = np.append(tmp, 0)
k = int(math.sqrt(n) + 3)
r = int((n - 1) / float(k))
for i in range(k):
print i
tmp = np.append(tmp, int(r * (i + 1) - 1))
tmp = np.append(tmp, n - 1)
else:
for i in range(n):
print i
tmp = np.append(tmp, i)
print "tmp:", tmp
for i in range(len(tmp)):
q0 = np.append(q0, base1[int(tmp[i])].positions[0])
q1 = np.append(q1, base1[int(tmp[i])].positions[1])
q2 = np.append(q2, base1[int(tmp[i])].positions[2])
q3 = np.append(q3, base1[int(tmp[i])].positions[3])
q4 = np.append(q4, base1[int(tmp[i])].positions[4])
q5 = np.append(q5, base1[int(tmp[i])].positions[5])
q6 = np.append(q6, base1[int(tmp[i])].positions[6])
print "q000", q0
# Cartesian position - Base '00'
#pose_goal.position.x = 0.90
#pose_goal.position.y = 0.38
pose_goal.position.z = 0.01
group.set_pose_target(pose_goal)
base0 = group.plan().joint_trajectory.points
q0 = np.append(q0, base0[-1].positions[0])
q1 = np.append(q1, base0[-1].positions[1])
q2 = np.append(q2, base0[-1].positions[2])
q3 = np.append(q3, base0[-1].positions[3])
q4 = np.append(q4, base0[-1].positions[4])
q5 = np.append(q5, base0[-1].positions[5])
q6 = np.append(q6, base0[-1].positions[6])
q = np.array([q0, q1, q2, q3, q4, q5, q6])
print "q001", q0
#print q[0]
#return 0
m_time, t_min_tiempo = min_time(q)
start = time.time()
opt = Opt_avalos(q, f, alfa)
v_time = opt.full_time()
j_2, v_2, a_2, jk_2 = generate_path_cub(q, v_time, f)
ext_2 = len(j_2[0, :])
end = time.time()
print('Process Time:', end - start)
#save_matrix(j,"data_p.txt",f)
#save_matrix(v,"data_v.txt",f)
#save_matrix(a,"data_a.txt",f)
#save_matrix(jk,"data_y.txt",f)
v_jk = sqrt(np.mean(np.square(jk_2)))
print("Opt Time:", v_time)
print("Min Time:", m_time)
#print('Optimizacion:',opt.result())
max_v = np.amax(np.absolute(v_2))
max_ac = np.amax(np.absolute(a_2))
max_jk = np.amax(np.absolute(jk_2))
print "Max Velo:", max_v
print "Max Acel:", max_ac
print "Max Jerk:", max_jk
q0 = np.array([])
q1 = np.array([])
q2 = np.array([])
q3 = np.array([])
q4 = np.array([])
q5 = np.array([])
q6 = np.array([])
q0 = np.append(q0, base0[-1].positions[0])
q1 = np.append(q1, base0[-1].positions[1])
q2 = np.append(q2, base0[-1].positions[2])
q3 = np.append(q3, base0[-1].positions[3])
q4 = np.append(q4, base0[-1].positions[4])
q5 = np.append(q5, base0[-1].positions[5])
q6 = np.append(q6, base0[-1].positions[6])
# Cartesian position - Carga '01'
pose_goal.position.x = 0.7708552
pose_goal.position.y = -0.394135
pose_goal.position.z = 0.24
group.set_pose_target(pose_goal)
carga1 = group.plan().joint_trajectory.points
n = len(carga1)
tmp = np.array([])
if (n > 10):
tmp = np.append(tmp, 0)
k = int(math.sqrt(n) + 2)
r = int((n - 1) / float(k))
for i in range(k):
print i
tmp = np.append(tmp, int(r * (i + 1) - 1))
tmp = np.append(tmp, n - 1)
else:
for i in range(n):
print i
tmp = np.append(tmp, i)
print "tmp:", tmp
for i in range(len(tmp)):
q0 = np.append(q0, carga1[int(tmp[i])].positions[0])
q1 = np.append(q1, carga1[int(tmp[i])].positions[1])
q2 = np.append(q2, carga1[int(tmp[i])].positions[2])
q3 = np.append(q3, carga1[int(tmp[i])].positions[3])
q4 = np.append(q4, carga1[int(tmp[i])].positions[4])
q5 = np.append(q5, carga1[int(tmp[i])].positions[5])
q6 = np.append(q6, carga1[int(tmp[i])].positions[6])
q = np.array([q0, q1, q2, q3, q4, q5, q6])
print "q001", q0
#print q[0]
#return 0
m_time, t_min_tiempo = min_time(q)
start = time.time()
opt = Opt_avalos(q, f, 0.8)
v_time = opt.full_time()
j_3, v_3, a_3, jk_3 = generate_path_cub(q, v_time, f)
ext_3 = len(j_3[0, :])
end = time.time()
print('Process Time:', end - start)
#save_matrix(j,"data_p.txt",f)
#save_matrix(v,"data_v.txt",f)
#save_matrix(a,"data_a.txt",f)
#save_matrix(jk,"data_y.txt",f)
v_jk = sqrt(np.mean(np.square(jk_3)))
print("Opt Time:", v_time)
print("Min Time:", m_time)
#print('Optimizacion:',opt.result())
max_v = np.amax(np.absolute(v_3))
max_ac = np.amax(np.absolute(a_3))
max_jk = np.amax(np.absolute(jk_3))
print "Max Velo:", max_v
print "Max Acel:", max_ac
print "Max Jerk:", max_jk
raw_input('Iniciar_CT_execute?')
#Build message
for n in xrange(ext_1):
my_msg.position = [
j_1[0][n], j_1[1][n], j_1[2][n], j_1[3][n], j_1[4][n],
j_1[5][n]
] #,j_1[6][n]]
my_msg.velocity = [
v_1[0][n], v_1[1][n], v_1[2][n], v_1[3][n], v_1[4][n],
v_1[5][n]
] #,v_1[6][n]]
my_msg.acceleration = [
a_1[0][n], a_1[1][n], a_1[2][n], a_1[3][n], a_1[4][n],
a_1[5][n]
] #,a_1[6][n]]
pub.publish(my_msg)
rate.sleep()
print("Control por trayectoria terminado.")
time.sleep(0.25)
gripper.close()
print("Control por trayectoria iniciado.")
#time.sleep(0.25)
for n in xrange(ext_2):
my_msg.position = [
j_2[0][n], j_2[1][n], j_2[2][n], j_2[3][n], j_2[4][n],
j_2[5][n]
] #,j_2[6][n]]
my_msg.velocity = [
v_2[0][n], v_2[1][n], v_2[2][n], v_2[3][n], v_2[4][n],
v_2[5][n]
] #,v_2[6][n]]
my_msg.acceleration = [
a_2[0][n], a_2[1][n], a_2[2][n], a_2[3][n], a_2[4][n],
a_2[5][n]
] #,a_2[6][n]]
pub.publish(my_msg)
rate.sleep()
print("Control por trayectoria terminado.")
gripper.open()
time.sleep(0.5)
#data.writeoff()
print("Programa terminado.")
for n in xrange(ext_3):
my_msg.position = [
j_3[0][n], j_3[1][n], j_3[2][n], j_3[3][n], j_3[4][n],
j_3[5][n]
] #,j_3[6][n]]
my_msg.velocity = [
v_3[0][n], v_3[1][n], v_3[2][n], v_3[3][n], v_3[4][n],
v_3[5][n]
] #,v_3[6][n]]
my_msg.acceleration = [
a_3[0][n], a_3[1][n], a_3[2][n], a_3[3][n], a_3[4][n],
a_3[5][n]
] #,a_3[6][n]]
pub.publish(my_msg)
rate.sleep()
print("Control por trayectoria terminado.")
gripper.open()
#data.writeoff()
print("Programa terminado.")
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user.')
def move2cartesian(position=None, orientation=None, relative_pose=None, in_tip_frame=False, joint_angles=[],
tip_name='right_hand', linear_speed=0.6, linear_accel=0.6, rotational_speed=1.57,
rotational_accel=1.57, timeout=None, neutral=False):
"""
Move the robot arm to the specified configuration.
Call using:
$ rosrun intera_examples go_to_cartesian_pose.py [arguments: see below]
-p 0.4 -0.3 0.18 -o 0.0 1.0 0.0 0.0 -t right_hand
--> Go to position: x=0.4, y=-0.3, z=0.18 meters
--> with quaternion orientation (0, 1, 0, 0) and tip name right_hand
--> The current position or orientation will be used if only one is provided.
-q 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0
--> Go to joint angles: 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0 using default settings
--> If a Cartesian pose is not provided, Forward kinematics will be used
--> If a Cartesian pose is provided, the joint angles will be used to bias the nullspace
-R 0.01 0.02 0.03 0.1 0.2 0.3 -T
-> Jog arm with Relative Pose (in tip frame)
-> x=0.01, y=0.02, z=0.03 meters, roll=0.1, pitch=0.2, yaw=0.3 radians
-> The fixed position and orientation paramters will be ignored if provided
"""
try:
#rospy.init_node('go_to_cartesian_pose_py')
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options = traj_options, limb = limb)
wpt_opts = MotionWaypointOptions(max_linear_speed=linear_speed,
max_linear_accel=linear_accel,
max_rotational_speed=rotational_speed,
max_rotational_accel=rotational_accel,
max_joint_speed_ratio=1.0)
waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb)
joint_names = limb.joint_names()
if joint_angles and len(joint_angles) != len(joint_names):
rospy.logerr('len(joint_angles) does not match len(joint_names!)')
return None
if neutral == True:
limb.move_to_neutral()
else:
if (position is None and orientation is None and relative_pose is None):
if joint_angles:
# does Forward Kinematics
waypoint.set_joint_angles(joint_angles, tip_name, joint_names)
else:
rospy.loginfo("No Cartesian pose or joint angles given. Using default")
waypoint.set_joint_angles(joint_angles=None, active_endpoint=tip_name)
else:
endpoint_state = limb.tip_state(tip_name)
if endpoint_state is None:
rospy.logerr('Endpoint state not found with tip name %s', tip_name)
return None
pose = endpoint_state.pose
if relative_pose is not None:
if len(relative_pose) != 6:
rospy.logerr('Relative pose needs to have 6 elements (x,y,z,roll,pitch,yaw)')
return None
# create kdl frame from relative pose
rot = PyKDL.Rotation.RPY(relative_pose[3],
relative_pose[4],
relative_pose[5])
trans = PyKDL.Vector(relative_pose[0],
relative_pose[1],
relative_pose[2])
f2 = PyKDL.Frame(rot, trans)
# and convert the result back to a pose message
if in_tip_frame:
# end effector frame
pose = posemath.toMsg(posemath.fromMsg(pose) * f2)
else:
# base frame
pose = posemath.toMsg(f2 * posemath.fromMsg(pose))
else:
if position is not None and len(position) == 3:
pose.position.x = position[0]
pose.position.y = position[1]
pose.position.z = position[2]
if orientation is not None and len(orientation) == 4:
pose.orientation.x = orientation[0]
pose.orientation.y = orientation[1]
pose.orientation.z = orientation[2]
pose.orientation.w = orientation[3]
poseStamped = PoseStamped()
poseStamped.pose = pose
if not joint_angles:
# using current joint angles for nullspace bais if not provided
joint_angles = limb.joint_ordered_angles()
waypoint.set_cartesian_pose(poseStamped, tip_name, joint_angles)
else:
waypoint.set_cartesian_pose(poseStamped, tip_name, joint_angles)
rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string())
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=timeout)
if result is None:
rospy.logerr('Trajectory FAILED to send')
return
if result.result:
rospy.loginfo('Motion controller successfully finished the trajectory!')
else:
rospy.logerr('Motion controller failed to complete the trajectory with error %s',
result.errorId)
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user. Exiting before trajectory completion.')
def main():
try:
#rospy.init_node('avalos_limb_py')
#moveit_commander.roscpp_initialize(sys.argv)
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = 'right_arm'
group = moveit_commander.MoveGroupCommander(group_name)
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# We can get the name of the reference frame for this robot:
planning_frame = group.get_planning_frame()
#frecuency for Sawyer robot
f=100
rate = rospy.Rate(f)
# add gripper
if(True):
gripper = intera_interface.Gripper('right_gripper')
gripper.calibrate()
gripper.open()
moveit_robot_state = RobotState()
joint_state = JointState()
joint_state.header = Header()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6']
#Define topic
pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10)
# Class to record
#data=Data()
#Class limb to acces information sawyer
limb = Limb()
limb.move_to_neutral()
neutral=[-7.343481724930712e-07,-1.1799709303615113,2.7170121530417646e-05,2.1799982536216014,-0.00023687847544184848,0.5696772114967752,3.1411912264073045]
base=[0.4045263671875, 0.3757021484375, -2.31678515625, 1.4790908203125, 2.14242578125, 2.1983291015625, 0.8213740234375]
s6_down=[0.1123427734375, 0.398875, -2.4554755859375, 0.4891044921875, 2.4769873046875, 1.575130859375, 1.531140625]
s6_up=[0.1613271484375, 0.3916650390625, -2.441814453125, 0.6957587890625, 2.515578125, 1.679708984375, 1.459033203125]
obj_up=[-0.74389453125, 0.153580078125, -1.7190927734375, 0.7447021484375, 1.72510546875, 1.5934130859375, 0.317576171875]
obj_down=[-0.7055927734375, 0.5030830078125, -1.7808125, 0.7994287109375, 2.0973154296875, 1.35018359375, 0.259451171875]
centro=[0.0751162109375, 0.1868447265625, -1.93045703125, 1.425337890625, 1.7726181640625, 1.904037109375, 0.4765615234375]
#points_1=path(neutral,centro,5)
#points_2=path(centro,objeto,5)
#points=path_full([neutral,objeto,centro,s6,centro,neutral],[5,5,5,5,5])
#points=path_full([neutral,obj_up,obj_down,obj_up,centro,s6,centro,neutral],[3,3,3,3,3,3,3])
alfa=0.5
p1=path_full([neutral,obj_up,obj_down],[2,3,2])
p2=path_full([obj_down,obj_up,centro,base,s6_up,s6_down],[3,3,3,3,3,2])
p3=path_full([s6_down,base,centro,obj_up],[3,3])
opt_1=Opt_avalos(p1,f,alfa)
opt_2=Opt_avalos(p2,f,alfa)
opt_3=Opt_avalos(p3,f,alfa)
v_time1=opt_1.full_time()
v_time2=opt_2.full_time()
v_time3=opt_3.full_time()
j,v,a,jk=generate_path_cub(p1,v_time1,f)
ext=len(j[0,:])
#Build message
my_msg=JointCommand()
# POSITION_MODE
my_msg.mode=4
my_msg.names=["right_j0","right_j1","right_j2","right_j3","right_j4","right_j5","right_j6"]
for n in xrange(ext):
my_msg.position=[j[0][n],j[1][n],j[2][n],j[3][n],j[4][n],j[5][n],j[6][n]]
my_msg.velocity=[v[0][n],v[1][n],v[2][n],v[3][n],v[4][n],v[5][n],v[6][n]]
my_msg.acceleration=[a[0][n],a[1][n],a[2][n],a[3][n],a[4][n],a[5][n],a[6][n]]
pub.publish(my_msg)
rate.sleep()
gripper.close()
time.sleep(1)
j,v,a,jk=generate_path_cub(p2,v_time2,f)
ext=len(j[0,:])
#Build message
my_msg=JointCommand()
# POSITION_MODE
my_msg.mode=4
my_msg.names=["right_j0","right_j1","right_j2","right_j3","right_j4","right_j5","right_j6"]
for n in xrange(ext):
my_msg.position=[j[0][n],j[1][n],j[2][n],j[3][n],j[4][n],j[5][n],j[6][n]]
my_msg.velocity=[v[0][n],v[1][n],v[2][n],v[3][n],v[4][n],v[5][n],v[6][n]]
my_msg.acceleration=[a[0][n],a[1][n],a[2][n],a[3][n],a[4][n],a[5][n],a[6][n]]
pub.publish(my_msg)
rate.sleep()
gripper.open()
j,v,a,jk=generate_path_cub(p3,v_time3,f)
ext=len(j[0,:])
#Build message
my_msg=JointCommand()
# POSITION_MODE
my_msg.mode=4
my_msg.names=["right_j0","right_j1","right_j2","right_j3","right_j4","right_j5","right_j6"]
for n in xrange(ext):
my_msg.position=[j[0][n],j[1][n],j[2][n],j[3][n],j[4][n],j[5][n],j[6][n]]
my_msg.velocity=[v[0][n],v[1][n],v[2][n],v[3][n],v[4][n],v[5][n],v[6][n]]
my_msg.acceleration=[a[0][n],a[1][n],a[2][n],a[3][n],a[4][n],a[5][n],a[6][n]]
pub.publish(my_msg)
rate.sleep()
'''
group.clear_pose_targets()
group.set_start_state_to_current_state()s
# We can get the joint values from the group and adjust some of the values:
pose_goal = geometry_msgs.msg.Pose()
# Orientation
pose_goal.orientation.x = -1
pose_goal.orientation.y = 0.0
pose_goal.orientation.z = 0.0
pose_goal.orientation.w = 0.0
q0=np.array([])
q1=np.array([])
q2=np.array([])
q3=np.array([])
q4=np.array([])
q5=np.array([])
q6=np.array([])
# Cartesian position
pose_goal.position.x = -0.1
pose_goal.position.y = 0.35
pose_goal.position.z = 0.05
pose_goal=Pose(Point(-0.1,0.6,0.05),Quaternion(-1,0,0,0))
group.set_pose_target(pose_goal)
a=group.plan()
points=a.joint_trajectory.points
n=len(points)
joint_state.position = [points[n-1].positions[0], points[n-1].positions[1], points[n-1].positions[2], points[n-1].positions[3],points[n-1].positions[4], points[n-1].positions[5], points[n-1].positions[6]]
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)
for i in range(n):
q0=np.append(q0, points[i].positions[0])
q1=np.append(q1, points[i].positions[1])
q2=np.append(q2, points[i].positions[2])
q3=np.append(q3, points[i].positions[3])
q4=np.append(q4, points[i].positions[4])
q5=np.append(q5, points[i].positions[5])
q6=np.append(q6, points[i].positions[6])
print "q000",q0
# Cartesian position
pose_goal.position.x = 0.43
pose_goal.position.y = -0.4
pose_goal.position.z = 0.2
group.set_pose_target(pose_goal)
a=group.plan()
points=a.joint_trajectory.points
n=len(points)
joint_state.position = [points[n-1].positions[0], points[n-1].positions[1], points[n-1].positions[2], points[n-1].positions[3],points[n-1].positions[4], points[n-1].positions[5], points[n-1].positions[6]]
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)
for i in range(n-1): # Si se repite un numero en posicion entra en un bug
q0=np.append(q0, points[i+1].positions[0])
q1=np.append(q1, points[i+1].positions[1])
q2=np.append(q2, points[i+1].positions[2])
q3=np.append(q3, points[i+1].positions[3])
q4=np.append(q4, points[i+1].positions[4])
q5=np.append(q5, points[i+1].positions[5])
q6=np.append(q6, points[i+1].positions[6])
q=np.array([q0,q1,q2,q3,q4,q5,q6])
print "q001",q0
print q[0]
#return 0
alfa=0.5
start = time.time()
opt=Opt_avalos(q,f,alfa)
v_time=opt.full_time()
j,v,a,jk=generate_path_cub(q,v_time,f)
ext=len(j[0,:])
end = time.time()
print('Process Time:', end-start)
print ext
#save_matrix(j,"data_p.txt",f)
#save_matrix(v,"data_v.txt",f)
#save_matrix(a,"data_a.txt",f)
#save_matrix(jk,"data_y.txt",f)
v_jk=sqrt(np.mean(np.square(jk)))
print("Opt Time:",v_time)
print("Min Time:",m_time)
#print('Optimizacion:',opt.result())
print('Costo Tiempo:',len(j[0])/float(100.0))
print('Costo Jerk:', v_jk)
# Position init
#raw_input('Iniciar_CT_initial_position?')
#limb.move_to_joint_positions({"right_j6": ik1[6],"right_j5": ik1[5], "right_j4": ik1[4], "right_j3": ik1[3], "right_j2":
#ik1[2],"right_j1": ik1[1],"right_j0": ik1[0]})
raw_input('Iniciar_CT_execute?')
#Build message
my_msg=JointCommand()
# POSITION_MODE
my_msg.mode=4
my_msg.names=["right_j0","right_j1","right_j2","right_j3","right_j4","right_j5","right_j6"]
#data.writeon(str(alfa)+"trayectoria.txt")
print("Control por trayectoria iniciado.")
#time.sleep(0.25)
for n in xrange(ext):
my_msg.position=[j[0][n],j[1][n],j[2][n],j[3][n],j[4][n],j[5][n],j[6][n]]
my_msg.velocity=[v[0][n],v[1][n],v[2][n],v[3][n],v[4][n],v[5][n],v[6][n]]
my_msg.acceleration=[a[0][n],a[1][n],a[2][n],a[3][n],a[4][n],a[5][n],a[6][n]]
#pub.publish(my_msg)
rate.sleep()
print("Control por trayectoria terminado.")
time.sleep(0.25)
#data.writeoff()
'''
print("Programa terminado.")
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user.')
def main():
try:
rospy.init_node('avalos_limb_py')
#frecuency for Sawyer robot
f = 100
rate = rospy.Rate(f)
# add gripper
gripper = intera_interface.Gripper('right_gripper')
gripper.calibrate()
gripper.open()
#Define topic
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
# Class to record
data = Data()
#Class limb to acces information sawyer
limb = Limb()
#Initial position
limb.move_to_neutral()
ik_pose_1 = np.array([0.45, 0.70, -0.2, 0.70, 0.0, 0.70, 0.0])
ik1 = np.array([
0.69514791, 0.64707899, 1.92295654, 0.01856896, -0.96413306,
-0.92232169, 4.16828131
])
ik_pose_2 = np.array([0.80, 0.30, 0.3, 0.70, 0.0, 0.70, 0.0])
ik2 = np.array([
5.97103602e-01, -9.58042104e-02, 1.70891311e+00, -9.48490850e-01,
-1.16882802e-03, 3.46304028e-01, 3.15488999e+00
])
ik_pose_3 = np.array([0.50, 0.00, 0.65, 0.70, 0.0, 0.70, 0.0])
ik3 = np.array([
0.63314606, -0.74365565, 1.14243681, -1.8618414, -0.84360096,
1.44537886, 3.41899404
])
ik_pose_4 = np.array([0.80, -0.30, 0.3, 0.70, 0.0, 0.70, 0.0])
ik4 = np.array([
-0.31256034, -0.32949631, 2.12830197, -1.18751871, -0.45027567,
1.32627167, 2.95775708
])
ik_pose_5 = np.array([0.45, -0.70, -0.20, 0.70, 0.0, 0.70, 0.0])
ik5 = np.array([
-1.55624859, 0.70439442, 2.52311113, -0.08708148, -0.94109983,
1.61554785, 2.54289819
])
#ik_service_client_full(ik_pose_1)
#ik_service_client_full(ik_pose_2)
#ik_service_client_full(ik_pose_3)
#ik_service_client_full(ik_pose_4)
#ik_service_client_full(ik_pose_5)
#initial=limb.joint_angles()
# Define KNOTS. Set inperpolation in linear form
limb.move_to_joint_positions({
"right_j6": ik1[6],
"right_j5": ik1[5],
"right_j4": ik1[4],
"right_j3": ik1[3],
"right_j2": ik1[2],
"right_j1": ik1[1],
"right_j0": ik1[0]
})
q=np.array([[ik1[0],ik2[0],ik3[0],ik4[0],ik5[0]], \
[ik1[1],ik2[1],ik3[1],ik4[1],ik5[1]], \
[ik1[2],ik2[2],ik3[2],ik4[2],ik5[2]], \
[ik1[3],ik2[3],ik3[3],ik4[3],ik5[3]], \
[ik1[4],ik2[4],ik3[4],ik4[4],ik5[4]], \
[ik1[5],ik2[5],ik3[5],ik4[5],ik5[5]], \
[ik1[6],ik2[6],ik3[6],ik4[6],ik5[6]] \
])
t_min, t_min_tiempo = min_time(q)
print t_min_tiempo
tasa = 1 / 0.2
knots_sec = np.round(t_min * tasa, 0)
t_k2 = np.arange(knots_sec[-1])
k_j0 = sp.interpolate.interp1d(
knots_sec, [ik1[0], ik2[0], ik3[0], ik4[0], ik5[0]],
kind='linear')(t_k2)
k_j1 = sp.interpolate.interp1d(
knots_sec, [ik1[1], ik2[1], ik3[1], ik4[1], ik5[1]],
kind='linear')(t_k2)
k_j2 = sp.interpolate.interp1d(
knots_sec, [ik1[2], ik2[2], ik3[2], ik4[2], ik5[2]],
kind='linear')(t_k2)
k_j3 = sp.interpolate.interp1d(
knots_sec, [ik1[3], ik2[3], ik3[3], ik4[3], ik5[3]],
kind='linear')(t_k2)
k_j4 = sp.interpolate.interp1d(
knots_sec, [ik1[4], ik2[4], ik3[4], ik4[4], ik5[4]],
kind='linear')(t_k2)
k_j5 = sp.interpolate.interp1d(
knots_sec, [ik1[5], ik2[5], ik3[5], ik4[5], ik5[5]],
kind='linear')(t_k2)
k_j6 = sp.interpolate.interp1d(
knots_sec, [ik1[6], ik2[6], ik3[6], ik4[6], ik5[6]],
kind='linear')(t_k2)
q = np.array([k_j0, k_j1, k_j2, k_j3, k_j4, k_j5, k_j6])
alfa = 0.15
start = time.time()
opt = Opt_2_avalos(q, f, alfa)
v_time = opt.full_time()
j, v, a, jk = generate_path_cub(q, v_time, f)
ext = len(j[0, :])
end = time.time()
print('Process Time:', end - start)
print ext
save_matrix(j, "data_p.txt", f)
save_matrix(v, "data_v.txt", f)
save_matrix(a, "data_a.txt", f)
save_matrix(jk, "data_y.txt", f)
print("Vector Time", v_time)
#print('Optimizacion:',opt.result())
print('Costo Tiempo:', opt.value_time())
print('Costo Jerk:', opt.value_jerk())
# Position init
limb.move_to_joint_positions({
"right_j6": ik1[6],
"right_j5": ik1[5],
"right_j4": ik1[4],
"right_j3": ik1[3],
"right_j2": ik1[2],
"right_j1": ik1[1],
"right_j0": ik1[0]
})
#raw_input('Cerrar?')
#time.sleep(4)
#gripper.close()
'''
raw_input('Iniciar_CD?')
data.writeon("directa.txt")
#time.sleep(0.25)
limb.move_to_joint_positions({"right_j6": ik2[6],"right_j5": ik2[5], "right_j4": ik2[4], "right_j3": ik2[3], "right_j2": ik2[2],"right_j1": ik2[1],"right_j0": ik2[0]})
#raw_input('Continuar?')
limb.move_to_joint_positions({"right_j6": ik3[6],"right_j5": ik3[5], "right_j4": ik3[4], "right_j3": ik3[3], "right_j2": ik3[2],"right_j1": ik3[1],"right_j0": ik3[0]})
#raw_input('Continuar?')
limb.move_to_joint_positions({"right_j6": ik4[6],"right_j5": ik4[5], "right_j4": ik4[4], "right_j3": ik4[3], "right_j2": ik4[2],"right_j1": ik4[1],"right_j0": ik4[0]})
#raw_input('Continuar?')
limb.move_to_joint_positions({"right_j6": ik5[6],"right_j5": ik5[5], "right_j4": ik5[4], "right_j3": ik5[3], "right_j2": ik5[2],"right_j1": ik5[1],"right_j0": ik5[0]})
time.sleep(0.25)
data.writeoff()
print("Control por cinematica directa terminado.")
'''
raw_input('Iniciar_CT_initial_position?')
limb.move_to_joint_positions({
"right_j6": ik1[6],
"right_j5": ik1[5],
"right_j4": ik1[4],
"right_j3": ik1[3],
"right_j2": ik1[2],
"right_j1": ik1[1],
"right_j0": ik1[0]
})
raw_input('Iniciar_CT_execute?')
#Build message
my_msg = JointCommand()
# POSITION_MODE
my_msg.mode = 4
my_msg.names = [
"right_j0", "right_j1", "right_j2", "right_j3", "right_j4",
"right_j5", "right_j6"
]
data.writeon(str(alfa) + "trayectoria.txt")
print("Control por trayectoria iniciado.")
#time.sleep(0.25)
for n in xrange(ext):
my_msg.position = [
j[0][n], j[1][n], j[2][n], j[3][n], j[4][n], j[5][n], j[6][n]
]
my_msg.velocity = [
v[0][n], v[1][n], v[2][n], v[3][n], v[4][n], v[5][n], v[6][n]
]
my_msg.acceleration = [
a[0][n], a[1][n], a[2][n], a[3][n], a[4][n], a[5][n], a[6][n]
]
pub.publish(my_msg)
rate.sleep()
print("Control por trayectoria terminado.")
time.sleep(0.25)
data.writeoff()
print("Programa terminado.")
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user.')
def main():
try:
rospy.init_node('avalos_limb_py')
#Frecuency for Sawyer robot
f = 100
rate = rospy.Rate(f)
#Define topic
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
#Class limb to acces information sawyer
limb = Limb()
#Initial position
limb.move_to_neutral()
print "posicion inicial terminada"
# Position init
initial = limb.joint_angles()
pi = [
initial["right_j0"], initial["right_j1"], initial["right_j2"],
initial["right_j3"], initial["right_j4"], initial["right_j5"],
initial["right_j6"]
]
# Posiition end
pe = [0, 0, 0, 0, 0, 0, 0]
# Knost vector time. We assum the process will take 10 sec
k_t = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
# Set knots points for each joint
k_j0 = np.linspace(pi[0], pe[0], num=11)
k_j1 = np.linspace(pi[1], pe[1], num=11)
k_j2 = np.linspace(pi[2], pe[2], num=11)
k_j3 = np.linspace(pi[3], pe[3], num=11)
k_j4 = np.linspace(pi[4], pe[4], num=11)
k_j5 = np.linspace(pi[5], pe[5], num=11)
k_j6 = np.linspace(pi[6], pe[6], num=11)
# Length time that will depend of frecuecy
l = k_t[-1] * f
new_t = np.linspace(k_t[0], k_t[-1], l)
# Obtain all point following the interpolated points
j0 = sp.interpolate.interp1d(k_t, k_j0, kind='cubic')(new_t)
j1 = sp.interpolate.interp1d(k_t, k_j1, kind='cubic')(new_t)
j2 = sp.interpolate.interp1d(k_t, k_j2, kind='cubic')(new_t)
j3 = sp.interpolate.interp1d(k_t, k_j3, kind='cubic')(new_t)
j4 = sp.interpolate.interp1d(k_t, k_j4, kind='cubic')(new_t)
j5 = sp.interpolate.interp1d(k_t, k_j5, kind='cubic')(new_t)
j6 = sp.interpolate.interp1d(k_t, k_j6, kind='cubic')(new_t)
# Vector for velocity
v_j0 = np.zeros(l)
v_j1 = np.zeros(l)
v_j2 = np.zeros(l)
v_j3 = np.zeros(l)
v_j4 = np.zeros(l)
v_j5 = np.zeros(l)
v_j6 = np.zeros(l)
# Vector for acceleration
a_j0 = np.zeros(l)
a_j1 = np.zeros(l)
a_j2 = np.zeros(l)
a_j3 = np.zeros(l)
a_j4 = np.zeros(l)
a_j5 = np.zeros(l)
a_j6 = np.zeros(l)
# Vector for acceleration
jk_j0 = np.zeros(l)
jk_j1 = np.zeros(l)
jk_j2 = np.zeros(l)
jk_j3 = np.zeros(l)
jk_j4 = np.zeros(l)
jk_j5 = np.zeros(l)
jk_j6 = np.zeros(l)
for i in xrange(l - 1):
v_j0[i] = (j0[i + 1] - j0[i]) * f
v_j1[i] = (j1[i + 1] - j1[i]) * f
v_j2[i] = (j2[i + 1] - j2[i]) * f
v_j3[i] = (j3[i + 1] - j3[i]) * f
v_j4[i] = (j4[i + 1] - j4[i]) * f
v_j5[i] = (j5[i + 1] - j5[i]) * f
v_j6[i] = (j6[i + 1] - j6[i]) * f
v_j0[-1] = v_j0[-2]
v_j1[-1] = v_j1[-2]
v_j2[-1] = v_j2[-2]
v_j3[-1] = v_j3[-2]
v_j4[-1] = v_j4[-2]
v_j5[-1] = v_j5[-2]
v_j6[-1] = v_j6[-2]
for i in xrange(l - 1):
a_j0[i] = (v_j0[i + 1] - v_j0[i]) * f
a_j1[i] = (v_j1[i + 1] - v_j1[i]) * f
a_j2[i] = (v_j2[i + 1] - v_j2[i]) * f
a_j3[i] = (v_j3[i + 1] - v_j3[i]) * f
a_j4[i] = (v_j4[i + 1] - v_j4[i]) * f
a_j5[i] = (v_j5[i + 1] - v_j5[i]) * f
a_j6[i] = (v_j6[i + 1] - v_j6[i]) * f
a_j0[-1] = a_j0[-2]
a_j1[-1] = a_j1[-2]
a_j2[-1] = a_j2[-2]
a_j3[-1] = a_j3[-2]
a_j4[-1] = a_j4[-2]
a_j5[-1] = a_j5[-2]
a_j6[-1] = a_j6[-2]
for i in xrange(l - 1):
jk_j0[i] = (a_j0[i + 1] - a_j0[i]) * f
jk_j1[i] = (a_j1[i + 1] - a_j1[i]) * f
jk_j2[i] = (a_j2[i + 1] - a_j2[i]) * f
jk_j3[i] = (a_j3[i + 1] - a_j3[i]) * f
jk_j4[i] = (a_j4[i + 1] - a_j4[i]) * f
jk_j5[i] = (a_j5[i + 1] - a_j5[i]) * f
jk_j6[i] = (a_j6[i + 1] - a_j6[i]) * f
jk_j0[-1] = jk_j0[-2]
jk_j1[-1] = jk_j1[-2]
jk_j2[-1] = jk_j2[-2]
jk_j3[-1] = jk_j3[-2]
jk_j4[-1] = jk_j4[-2]
jk_j5[-1] = jk_j5[-2]
jk_j6[-1] = jk_j6[-2]
j = np.array([j0, j1, j2, j3, j4, j5, j6])
v = np.array([v_j0, v_j1, v_j2, v_j3, v_j4, v_j5, v_j6])
a = np.array([a_j0, a_j1, a_j2, a_j3, a_j4, a_j5, a_j6])
jk = np.array([jk_j0, jk_j1, jk_j2, jk_j3, jk_j4, jk_j5, jk_j6])
save_matrix(j, "data_p.txt", f)
save_matrix(v, "data_v.txt", f)
save_matrix(a, "data_a.txt", f)
save_matrix(jk, "data_y.txt", f)
#Build message
my_msg = JointCommand()
# POSITION_MODE
my_msg.mode = 4
my_msg.names = [
"right_j0", "right_j1", "right_j2", "right_j3", "right_j4",
"right_j5", "right_j6"
]
for i in xrange(l):
my_msg.position = [j0[i], j1[i], j2[i], j3[i], j4[i], j5[i], j6[i]]
my_msg.velocity = [
v_j0[i], v_j1[i], v_j2[i], v_j3[i], v_j4[i], v_j5[i], v_j6[i]
]
my_msg.acceleration = [
a_j0[i], a_j1[i], a_j2[i], a_j3[i], a_j4[i], a_j5[i], a_j6[i]
]
pub.publish(my_msg)
rate.sleep()
print "Move ok"
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user.')
def main():
try:
#rospy.init_node('avalos_limb_py')
#moveit_commander.roscpp_initialize(sys.argv)
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = 'right_arm'
group = moveit_commander.MoveGroupCommander(group_name)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# We can get the name of the reference frame for this robot:
planning_frame = group.get_planning_frame()
#frecuency for Sawyer robot
f = 100
rate = rospy.Rate(f)
# add gripper
if (False):
gripper = intera_interface.Gripper('right_gripper')
gripper.calibrate()
gripper.open()
moveit_robot_state = RobotState()
joint_state = JointState()
joint_state.header = Header()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
#Define topic
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
# Class to record
#data=Data()
#Class limb to acces information sawyer
limb = Limb()
limb.move_to_neutral()
group.clear_pose_targets()
group.set_start_state_to_current_state()
# We can get the joint values from the group and adjust some of the values:
pose_goal = geometry_msgs.msg.Pose()
# Orientation
pose_goal.orientation.x = -1
pose_goal.orientation.y = 0.0
pose_goal.orientation.z = 0.0
pose_goal.orientation.w = 0.0
q0 = np.array([])
q1 = np.array([])
q2 = np.array([])
q3 = np.array([])
q4 = np.array([])
q5 = np.array([])
q6 = np.array([])
# Cartesian position
pose_goal.position.x = -0.1
pose_goal.position.y = 0.35
pose_goal.position.z = 0.05
pose_goal = Pose(Point(-0.1, 0.6, 0.05), Quaternion(-1, 0, 0, 0))
group.set_pose_target(pose_goal)
a = group.plan()
points = a.joint_trajectory.points
n = len(points)
joint_state.position = [
points[n - 1].positions[0], points[n - 1].positions[1],
points[n - 1].positions[2], points[n - 1].positions[3],
points[n - 1].positions[4], points[n - 1].positions[5],
points[n - 1].positions[6]
]
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)
for i in range(n):
q0 = np.append(q0, points[i].positions[0])
q1 = np.append(q1, points[i].positions[1])
q2 = np.append(q2, points[i].positions[2])
q3 = np.append(q3, points[i].positions[3])
q4 = np.append(q4, points[i].positions[4])
q5 = np.append(q5, points[i].positions[5])
q6 = np.append(q6, points[i].positions[6])
print "q000", q0
# Cartesian position
pose_goal.position.x = 0.43
pose_goal.position.y = -0.4
pose_goal.position.z = 0.2
group.set_pose_target(pose_goal)
a = group.plan()
points = a.joint_trajectory.points
n = len(points)
joint_state.position = [
points[n - 1].positions[0], points[n - 1].positions[1],
points[n - 1].positions[2], points[n - 1].positions[3],
points[n - 1].positions[4], points[n - 1].positions[5],
points[n - 1].positions[6]
]
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)
for i in range(
n - 1): # Si se repite un numero en posicion entra en un bug
q0 = np.append(q0, points[i + 1].positions[0])
q1 = np.append(q1, points[i + 1].positions[1])
q2 = np.append(q2, points[i + 1].positions[2])
q3 = np.append(q3, points[i + 1].positions[3])
q4 = np.append(q4, points[i + 1].positions[4])
q5 = np.append(q5, points[i + 1].positions[5])
q6 = np.append(q6, points[i + 1].positions[6])
#'''
q = np.array([q0, q1, q2, q3, q4, q5, q6])
print "q001", q0
print q[0]
#return 0
alfa = 0.5
start = time.time()
opt = Opt_avalos(q, f, alfa)
v_time = opt.full_time()
j, v, a, jk = generate_path_cub(q, v_time, f)
ext = len(j[0, :])
end = time.time()
print('Process Time:', end - start)
print ext
#save_matrix(j,"data_p.txt",f)
#save_matrix(v,"data_v.txt",f)
#save_matrix(a,"data_a.txt",f)
#save_matrix(jk,"data_y.txt",f)
v_jk = sqrt(np.mean(np.square(jk)))
print("Opt Time:", v_time)
print("Min Time:", m_time)
#print('Optimizacion:',opt.result())
print('Costo Tiempo:', len(j[0]) / float(100.0))
print('Costo Jerk:', v_jk)
# Position init
#raw_input('Iniciar_CT_initial_position?')
#limb.move_to_joint_positions({"right_j6": ik1[6],"right_j5": ik1[5], "right_j4": ik1[4], "right_j3": ik1[3], "right_j2":
#ik1[2],"right_j1": ik1[1],"right_j0": ik1[0]})
raw_input('Iniciar_CT_execute?')
#Build message
my_msg = JointCommand()
# POSITION_MODE
my_msg.mode = 4
my_msg.names = [
"right_j0", "right_j1", "right_j2", "right_j3", "right_j4",
"right_j5", "right_j6"
]
#data.writeon(str(alfa)+"trayectoria.txt")
print("Control por trayectoria iniciado.")
#time.sleep(0.25)
for n in xrange(ext):
my_msg.position = [
j[0][n], j[1][n], j[2][n], j[3][n], j[4][n], j[5][n], j[6][n]
]
my_msg.velocity = [
v[0][n], v[1][n], v[2][n], v[3][n], v[4][n], v[5][n], v[6][n]
]
my_msg.acceleration = [
a[0][n], a[1][n], a[2][n], a[3][n], a[4][n], a[5][n], a[6][n]
]
pub.publish(my_msg)
rate.sleep()
print("Control por trayectoria terminado.")
time.sleep(0.25)
#data.writeoff()
print("Programa terminado.")
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user.')
