def start_position(self):
"""
Start the Sawyer to the initial position
"""
joint_command = {}
right = Limb('right')
joint_command['right_j0'] = -0.20
joint_command['right_j1'] = -0.58
joint_command['right_j2'] = 0.13
joint_command['right_j3'] = 1.36
joint_command['right_j4'] = -0.2
joint_command['right_j5'] = 2.24
joint_command['right_j6'] = 1.80
# joint_command['right_j0'] = -0.11
# joint_command['right_j1'] = 0.58
# joint_command['right_j2'] = -1.83
# joint_command['right_j3'] = 1.50
# joint_command['right_j4'] = 1.64
# joint_command['right_j5'] = 2.94
# joint_command['right_j6'] = -3.18
# joint_command['right_j0'] = -0.013
# joint_command['right_j1'] = 0.88
# joint_command['right_j2'] = -0.045
# joint_command['right_j3'] = -2.60
# joint_command['right_j4'] = -2.98
# joint_command['right_j5'] = -2.98
# joint_command['right_j6'] = -4.64
try:
print("Initializing starting position....")
right.move_to_joint_positions(joint_command)
except Exception as e:
print(e)
class KBEnv(object):
def __init__(self, path):
rospy.init_node('sawyer_arm_cntrl')
self.load_inits(path+'/pg_init.yaml')
self.limb = Limb()
self.all_jnts = copy.copy(self.limb.joint_names())
self.limb.set_joint_position_speed(0.2)
def load_inits(self, path):
stream = open(path, "r")
config = yaml.load(stream)
stream.close()
# these indices and names have to be in ascending order
self.jnt_indices = config['jnt_indices']
self.cmd_names = config['cmd_names']
self.init_pos = config['initial-position']
def chng_jnt_state(self, values, mode, iterations=20):
# Command Sawyer's joints to angles or velocity
robot = URDF.from_parameter_server()
kdl_kin_r = KDLKinematics(robot, 'base',
'right_gripper_r_finger_tip')
kdl_kin_l = KDLKinematics(robot, 'base',
'right_gripper_l_finger_tip')
q_jnt_angles = np.zeros(8)
q_jnt_angles[:7] = copy.copy(values)
q_jnt_angles[7] = 0.0
pose_r = kdl_kin_r.forward(q_jnt_angles)
pose_l = kdl_kin_l.forward(q_jnt_angles)
pose = 0.5*(pose_l + pose_r)
x, y, z = transformations.translation_from_matrix(pose)[:3]
print("goal cartesian: ", x, y, z)
timeout = 30.0
if mode == 4:
positions = dict()
i = 0
for jnt_name in self.all_jnts:
positions[jnt_name] = values[i]
i += 1
self.limb.move_to_joint_positions(positions, timeout)
else:
velocities = dict()
i = 0
for name in self.cmd_names:
velocities[name] = values[i]
i += 1
for _ in range(iterations):
self.limb.set_joint_velocities(velocities)
time.sleep(0.5)
def move_to_home(thetalist):
rightlimb = Limb()
waypoints = {}
waypoints['right_j0'] = thetalist[0]
waypoints['right_j1'] = thetalist[1]
waypoints['right_j2'] = thetalist[2]
waypoints['right_j3'] = thetalist[3]
waypoints['right_j4'] = thetalist[4]
waypoints['right_j5'] = thetalist[5]
waypoints['right_j6'] = thetalist[6]
waypoints['torso_t0'] = thetalist[7]
rightlimb.move_to_joint_positions(waypoints, timeout=20.0, threshold=0.05)
rospy.sleep(2.0)
def initialize_jnts(self):
print("Initializing joints...")
positions = dict()
limb = Limb()
calib_angles = [0.27, -3.27, 3.04, -1.60, -0.38, -1.66, 0.004]
all_jnts = copy.copy(limb.joint_names())
limb.set_joint_position_speed(0.2)
positions['head_pan'] = 0.0
enum_iter = enumerate(all_jnts, start=0)
for i, jnt_name in enum_iter:
positions[jnt_name] = calib_angles[i]
limb.move_to_joint_positions(positions)
print("Done Initializing joints!!")
def main():
try:
rospy.init_node('avalos_limb_py')
rate = rospy.Rate(100)
limb = Limb()
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
}
print positions
limb.move_to_joint_positions(positions)
#limb.move_to_neutral()
print "Move ok"
except rospy.ROSInterruptException:
rospy.logerr(
'Keyboard interrupt detected from the user. Exiting before trajectory completion.'
)
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 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.')
class Env(object):
def __init__(self, path, train_mode=True):
self.train_mode = train_mode
self.obs_lock = threading.Lock()
# path where the python script for agent and env reside
self.path = path
string = "sawyer_ros"
rospy.init_node(string + "_environment")
self.logp = None
if self.train_mode:
self.logpath = "AC" + '_' + time.strftime("%d-%m-%Y_%H-%M")
self.logpath = os.path.join(path + '/data', self.logpath)
if not (os.path.exists(self.logpath)):
# we should never have to create dir, as agent already done it
os.makedirs(self.logpath)
logfile = os.path.join(self.logpath, "ros_env.log")
self.logp = open(logfile, "w")
# Goal does not move and is specified by three points (for pose reasons)
self.goal_pos_x1 = None
self.goal_pos_y1 = None
self.goal_pos_z1 = None
self.goal_pos_x2 = None
self.goal_pos_y2 = None
self.goal_pos_z2 = None
self.goal_pos_x3 = None
self.goal_pos_y3 = None
self.goal_pos_z3 = None
self._load_inits(path)
self.cur_obs = np.zeros(self.obs_dim)
self.cur_act = np.zeros(self.act_dim)
self.cur_reward = None
self.goal_cntr = 0
self.limb = Limb()
self.all_jnts = copy.copy(self.limb.joint_names())
self.limb.set_joint_position_speed(0.2)
self.rate = rospy.Rate(10)
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
self.jnt_st_sub = rospy.Subscriber('/robot/joint_states',
JointState,
self._update_jnt_state,
queue_size=1)
self.jnt_cm_pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=None)
self.jnt_ee_sub = rospy.Subscriber('/robot/limb/right/endpoint_state',
EndpointState,
self.update_ee_pose,
queue_size=1)
'''
Three points on the object in "right_gripper_base" frame.
Obect is static in gripper frame. Needs to be calculated only once
during init time
'''
self.obj_pose1 = PoseStamped()
self.obj_pose1.header.frame_id = "right_gripper_base"
self.obj_pose1.pose.position.x = self.obj_x1
self.obj_pose1.pose.position.y = self.obj_y1
self.obj_pose1.pose.position.z = self.obj_z1
self.obj_pose2 = PoseStamped()
self.obj_pose2.header.frame_id = "right_gripper_base"
self.obj_pose2.pose.position.x = self.obj_x2
self.obj_pose2.pose.position.y = self.obj_y2
self.obj_pose2.pose.position.z = self.obj_z2
self.obj_pose3 = PoseStamped()
self.obj_pose3.header.frame_id = "right_gripper_base"
self.obj_pose3.pose.position.x = self.obj_x3
self.obj_pose3.pose.position.y = self.obj_y3
self.obj_pose3.pose.position.z = self.obj_z3
'''
All members of the observation vector have to be provided
'''
def _set_cur_obs(self, obs):
#self._print_env_log('Waiting for obs lock')
self.obs_lock.acquire()
try:
#self._print_env_log('Acquired obs lock')
self.cur_obs = copy.copy(obs)
finally:
#self._print_env_log('Released obs lock')
self.obs_lock.release()
def _get_cur_obs(self):
self.obs_lock.acquire()
try:
#self._print_env_log('Acquired obs lock')
obs = copy.copy(self.cur_obs)
finally:
#self._print_env_log('Released obs lock')
self.obs_lock.release()
return obs
def close_env_log(self):
self.logp.close()
self.logp = None
def _print_env_log(self, string):
if self.train_mode:
if self.logp is None:
return
self.logp.write("\n")
now = rospy.get_rostime()
t_str = time.strftime("%H-%M")
t_str += "-" + str(now.secs) + "-" + str(now.nsecs) + ": "
self.logp.write(t_str + string)
self.logp.write("\n")
def _load_inits(self, path):
if self.train_mode:
# Store versions of the main code required for
# test and debug after training
copyfile(path + "/init.yaml", self.logpath + "/init.yaml")
copyfile(path + "/ros_env.py", self.logpath + "/ros_env.py")
stream = open(path + "/init.yaml", "r")
config = yaml.load(stream)
stream.close()
self.distance_thresh = config['distance_thresh']
# limits for the uniform distribution to
# sample from when varying initial joint states during training
# joint position limits have to be in ascending order of joint number
# jnt_init_limits is a ordered list of [lower_limit, upper_limit] for
# each joint in motion
# limits for the joint positions
self.jnt_init_limits = config['jnt_init_limits']
self.cmd_mode = config['cmd_mode']
if self.cmd_mode == 'VELOCITY_MODE':
# limits for the joint positions
self.jnt_pos_limits = config['jnt_pos_limits']
self.vel_limit = config['vel_limit']
self.vel_mode = config['vel_mode']
else:
self.torque_limit = config['torque_limit']
'''
# The following are the names of Sawyer's joints that will move
# for the shape sorting cube task
# Any one or more of the following in ascending order -
# ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
# 'right_j5', 'right_j6']
# The last joint is the gripper finger joint and stays fixed
'''
self.debug_lvl = config['debug_lvl']
self.cmd_names = config['cmd_names']
self.init_pos = config['initial-position']
self.goal_obs_dim = config['goal_obs_dim']
'''
In TORQUE_MODE, jnt_obs_dim will be twice the size of the
number of joints being controlled (2 * self.act_dim), one each for
position and velocity.
The ordering in the observation vector is:
j0:pos, j1:pos, ..., jN:pos,
j0:vel, j1:vel ...., jN:vel, # Applicable only in torque mode
obj_coord:x1, obj_coord:y1, obj_coord:z1,
obj_coord:x2, obj_coord:y2, obj_coord:z2,
obj_coord:x3, obj_coord:y3, obj_coord:z3;
goal_coord:x1, goal_coord:y1, goal_coord:z1,
goal_coord:x2, goal_coord:y2, goal_coord:z2,
goal_coord:x3, goal_coord:y3, goal_coord:z3
'''
self.jnt_obs_dim = config['jnt_obs_dim']
self.obs_dim = self.goal_obs_dim + self.jnt_obs_dim
self.act_dim = config['act_dim']
'''
These indices have to be in ascending order
The length of this ascending order list >=1 and <=7 (values 0 to 6)
The last joint is the gripper finger joint and stays fixed
'''
self.jnt_indices = config['jnt_indices']
# test time goals specified in jnt angle space (for now)
if not self.train_mode:
self.test_goal = config['test_goal']
self.max_training_goals = config['max_training_goals']
self.batch_size = config['min_timesteps_per_batch']
self.goal_XYZs = config['goal_XYZs']
'''
The following 9 object coordinates are in "right_gripper_base" frame
They are relayed by the camera observer at init time.
They can be alternatively read from the pg_init.yaml file too
'''
self.obj_x1 = config['obj_x1']
self.obj_y1 = config['obj_y1']
self.obj_z1 = config['obj_z1']
self.obj_x2 = config['obj_x2']
self.obj_y2 = config['obj_y2']
self.obj_z2 = config['obj_z2']
self.obj_x3 = config['obj_x3']
self.obj_y3 = config['obj_y3']
self.obj_z3 = config['obj_z3']
# Callback invoked when EE pose update message is received
# This function will update the pose of object in gripper
def update_ee_pose(self, msg):
try:
tform = self.tf_buffer.lookup_transform("base",
"right_gripper_base",
rospy.Time(),
rospy.Duration(1.0))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
self._print_env_log("TF Exception, not storing update")
return
obs = self._get_cur_obs()
trans = (tform.transform.translation.x, tform.transform.translation.y,
tform.transform.translation.z)
rot = (tform.transform.rotation.x, tform.transform.rotation.y,
tform.transform.rotation.z, tform.transform.rotation.w)
mat44 = np.dot(transformations.translation_matrix(trans),
transformations.quaternion_matrix(rot))
pose44 = np.dot(xyz_to_mat44(self.obj_pose1.pose.position),
xyzw_to_mat44(self.obj_pose1.pose.orientation))
txpose = np.dot(mat44, pose44)
xyz = tuple(transformations.translation_from_matrix(txpose))[:3]
x, y, z = xyz
obs[self.jnt_obs_dim] = x
obs[self.jnt_obs_dim + 1] = y
obs[self.jnt_obs_dim + 2] = z
pose44 = np.dot(xyz_to_mat44(self.obj_pose2.pose.position),
xyzw_to_mat44(self.obj_pose2.pose.orientation))
txpose = np.dot(mat44, pose44)
xyz = tuple(transformations.translation_from_matrix(txpose))[:3]
x, y, z = xyz
obs[self.jnt_obs_dim + 3] = x
obs[self.jnt_obs_dim + 4] = y
obs[self.jnt_obs_dim + 5] = z
pose44 = np.dot(xyz_to_mat44(self.obj_pose3.pose.position),
xyzw_to_mat44(self.obj_pose3.pose.orientation))
txpose = np.dot(mat44, pose44)
xyz = tuple(transformations.translation_from_matrix(txpose))[:3]
x, y, z = xyz
obs[self.jnt_obs_dim + 6] = x
obs[self.jnt_obs_dim + 7] = y
obs[self.jnt_obs_dim + 8] = z
'''
self._print_env_log("EE coordinates: "
+ str(msg.pose.position.x) +
", " + str(msg.pose.position.y) + ", " +
str(msg.pose.position.z))
self._print_env_log("Obj location: "
+ str(obs[self.jnt_obs_dim:self.jnt_obs_dim+9]))
'''
self._set_cur_obs(obs)
def _update_jnt_state(self, msg):
'''
Only care for mesgs which have length 9
There is a length 1 message for the gripper finger joint
which we dont care about
'''
if len(msg.position) != 9:
return
obs = self._get_cur_obs()
enum_iter = enumerate(self.jnt_indices, start=0)
for i, index in enum_iter:
'''
Need to add a 1 to message index as it starts from head_pan
[head_pan, j0, j1, .. torso]
whereas joint_indices are starting from j0
'''
obs[i] = msg.position[index + 1]
if self.cmd_mode == "TORQUE_MODE":
obs[i + self.jnt_obs_dim / 2] = msg.velocity[index + 1]
self._set_cur_obs(obs)
'''
This function is called from reset only and during both training and testing
'''
def _init_jnt_state(self):
q_jnt_angles = copy.copy(self.init_pos)
# Command Sawyer's joints to pre-set angles and velocity
# JointCommand.msg mode: TRAJECTORY_MODE
positions = dict()
# Build some randomness in starting position
# between each subsequent iteration
enum_iter = enumerate(self.jnt_indices, start=0)
for i, index in enum_iter:
l_limit = self.jnt_init_limits[i][0]
u_limit = self.jnt_init_limits[i][1]
val = np.random.uniform(l_limit, u_limit)
q_jnt_angles[index] = val
string = "Initializing joint states to: "
enum_iter = enumerate(self.all_jnts, start=0)
for i, jnt_name in enum_iter:
positions[jnt_name] = q_jnt_angles[i]
string += str(positions[jnt_name]) + " "
self.limb.move_to_joint_positions(positions, 30)
self._print_env_log(string)
# sleep for a bit to ensure that the joints reach commanded positions
rospy.sleep(3)
def _action_clip(self, action):
if self.cmd_mode == "TORQUE_MODE":
return action
#return np.clip(action, -self.torque_limit, self.torque_limit)
else:
return np.clip(action, -self.vel_limit, self.vel_limit)
def _set_joint_velocities(self, actions):
if self.vel_mode == "raw":
velocities = dict()
enum_iter = enumerate(self.cmd_names, start=0)
for i, jnt in enum_iter:
velocities[jnt] = actions[i]
command_msg = JointCommand()
command_msg.names = velocities.keys()
command_msg.velocity = velocities.values()
command_msg.mode = JointCommand.VELOCITY_MODE
command_msg.header.stamp = rospy.Time.now()
self.jnt_cm_pub.publish(command_msg)
else:
# Command Sawyer's joints to angles as calculated by velocity*dt
positions = dict()
q_jnt_angles = copy.copy(self.init_pos)
obs_prev = self._get_cur_obs()
enum_iter = enumerate(self.jnt_indices, start=0)
for i, index in enum_iter:
timestep = self.dt + np.random.normal(0, 1)
val = obs_prev[i] + actions[i] * timestep
val = np.clip(val, self.jnt_pos_limits[i][0],
self.jnt_pos_limits[i][1])
q_jnt_angles[index] = val
enum_iter = enumerate(self.all_jnts, start=0)
for i, jnt_name in enum_iter:
positions[jnt_name] = q_jnt_angles[i]
self.limb.move_to_joint_positions(positions)
def _set_joint_torques(self, actions):
torques = dict()
enum_iter = enumerate(self.all_jnts, start=0)
for i, jnt_name in enum_iter:
torques[jnt_name] = 0.0
enum_iter = enumerate(self.cmd_names, start=0)
for i, jnt_name in enum_iter:
torques[jnt_name] = actions[i]
command_msg = JointCommand()
command_msg.names = torques.keys()
command_msg.effort = torques.values()
command_msg.mode = JointCommand.TORQUE_MODE
command_msg.header.stamp = rospy.Time.now()
self.jnt_cm_pub.publish(command_msg)
def step(self, action):
self.cur_act = copy.deepcopy(action)
# called to take a step with the provided action
# send the action as generated by policy (clip before sending)
clipped_acts = self._action_clip(action)
if self.cmd_mode == "TORQUE_MODE":
self._set_joint_torques(clipped_acts)
else:
self._set_joint_velocities(clipped_acts)
'''
NOTE: Observations are being constantly updated because
we are subscribed to the robot state publisher and also
subscribed to the ee topic which calculates
the pose of the goal and the block.
Sleep for some time, so that the action
execution on the robot finishes and we wake up to
pick up the latest observation
'''
# no sleep necessary if we send velocity integrated positions
if self.cmd_mode == "TORQUE_MODE" or self.vel_mode == "raw":
self.rate.sleep()
obs = self._get_cur_obs()
diff = self._get_diff(obs)
done = self._is_done(diff)
self.cur_reward = self._calc_reward(diff, done)
return obs, self.cur_reward, done
def _set_cartesian_test_goal(self):
self.goal_pos_x1 = self.test_goal[0][0]
self.goal_pos_y1 = self.test_goal[0][1]
self.goal_pos_z1 = self.test_goal[0][2]
self.goal_pos_x1 = self.test_goal[1][0]
self.goal_pos_y1 = self.test_goal[1][1]
self.goal_pos_z1 = self.test_goal[1][2]
self.goal_pos_x1 = self.test_goal[2][0]
self.goal_pos_y1 = self.test_goal[2][1]
self.goal_pos_z1 = self.test_goal[2][2]
def _set_random_training_goal(self):
k = self.goal_cntr
l = -0.01
u = 0.01
self.goal_pos_x1 = self.goal_XYZs[k][0][0] + np.random.uniform(l, u)
self.goal_pos_y1 = self.goal_XYZs[k][0][1] + np.random.uniform(l, u)
self.goal_pos_z1 = self.goal_XYZs[k][0][2] + np.random.uniform(l, u)
self.goal_pos_x2 = self.goal_XYZs[k][1][0] + np.random.uniform(l, u)
self.goal_pos_y2 = self.goal_XYZs[k][1][1] + np.random.uniform(l, u)
self.goal_pos_z2 = self.goal_XYZs[k][1][2] + np.random.uniform(l, u)
self.goal_pos_x3 = self.goal_XYZs[k][2][0] + np.random.uniform(l, u)
self.goal_pos_y3 = self.goal_XYZs[k][2][1] + np.random.uniform(l, u)
self.goal_pos_z3 = self.goal_XYZs[k][2][2] + np.random.uniform(l, u)
def reset(self):
# called Initially when the Env is initialized
# set the initial joint state and send the command
if not self.train_mode:
self._set_cartesian_test_goal()
else:
if self.goal_cntr == self.max_training_goals - 1:
self.goal_cntr = 0
else:
self.goal_cntr += 1
self._set_random_training_goal()
cur_obs = self._get_cur_obs()
# Update cur_obs with the new goal
od = self.jnt_obs_dim
cur_obs[od + 9] = self.goal_pos_x1
cur_obs[od + 10] = self.goal_pos_y1
cur_obs[od + 11] = self.goal_pos_z1
cur_obs[od + 12] = self.goal_pos_x2
cur_obs[od + 13] = self.goal_pos_y2
cur_obs[od + 14] = self.goal_pos_z2
cur_obs[od + 15] = self.goal_pos_x3
cur_obs[od + 16] = self.goal_pos_y3
cur_obs[od + 17] = self.goal_pos_z3
self._set_cur_obs(cur_obs)
# this call will result in sleeping for 3 seconds
self._init_jnt_state()
# send the latest observations
return self._get_cur_obs()
def _get_diff(self, obs):
od = self.jnt_obs_dim
diff = [
abs(obs[od + 0] - obs[od + 9]),
abs(obs[od + 1] - obs[od + 10]),
abs(obs[od + 2] - obs[od + 11]),
abs(obs[od + 3] - obs[od + 12]),
abs(obs[od + 4] - obs[od + 13]),
abs(obs[od + 5] - obs[od + 14]),
abs(obs[od + 6] - obs[od + 15]),
abs(obs[od + 7] - obs[od + 16]),
abs(obs[od + 8] - obs[od + 17])
]
return diff
def _is_done(self, diff):
# all elements of d are positive values
done = all(d <= self.distance_thresh for d in diff)
if done:
self._print_env_log(" Reached the goal!!!! ")
return done
def _calc_reward(self, diff, done):
l2 = np.linalg.norm(np.array(diff))
l2sq = l2**2
alpha = 1e-6
w_l2 = -1e-3
w_u = -1e-2
w_log = -1.0
reward = 0.0
dist_cost = l2sq
precision_cost = np.log(l2sq + alpha)
cntrl_cost = np.square(self.cur_act).sum()
reward += w_l2 * dist_cost + w_log * precision_cost + w_u * cntrl_cost
string = "l2sq: {}, log of l2sq: {} contrl_cost: {} reward: {}".format(
dist_cost, precision_cost, cntrl_cost, reward)
self._print_env_log(" Current Reward: " + string)
return reward
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.')