def lowerArmBasicMove(self, cur_pos, speed):
# define some parameters:
frequency = 100 # customize this to change the vibration speed
repetation = 20
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
rate = rospy.Rate(frequency)
pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio',
Float64,
latch=True,
queue_size=10)
# set the joint angles for the first press position
command1 = JointCommand()
command1.names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
command1.position = [
cur_pos['right_j0'], cur_pos['right_j1'], cur_pos['right_j2'],
cur_pos['right_j3'] - 0.05, cur_pos['right_j4'] - 0.05,
cur_pos['right_j5'] + 0.1, cur_pos['right_j6']
]
command1.mode = 1
# set the joint angles for the second press position
command2 = JointCommand()
command2.names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
command2.position = [
cur_pos['right_j0'], cur_pos['right_j1'], cur_pos['right_j2'],
cur_pos['right_j3'], cur_pos['right_j4'], cur_pos['right_j5'],
cur_pos['right_j6']
]
command2.mode = 1
# looping to create vibrating motion
pub_speed_ratio.publish(speed)
for i in range(0, repetation):
self.forcePress(self.vibration_force, 0.1)
#for j in range(0, 10):
# pub.publish(command1)
# rate.sleep()
#rate.sleep()
for k in range(0, 10):
pub.publish(command2)
rate.sleep()
rate.sleep()
def movetozero(self):
rospy.init_node('arm_low_level_control', anonymous=True)
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio',
Float64,
latch=True,
queue_size=10)
command = JointCommand()
command.names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
command.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
command.mode = 1
pub_speed_ratio.publish(0.2)
start_time = rospy.get_time()
end_time = rospy.get_time()
rospy.loginfo(
">>>>>>>>>> moving the arm to zero joint position >>>>>>>>>>>")
while end_time - start_time < 8:
pub.publish(command)
end_time = rospy.get_time()
def basicAccelerationMove(self, pos, speed):
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio',
Float64,
latch=True,
queue_size=10)
sub = rospy.Subscriber('/robot/joint_states', JointState,
self.joints_callback)
command = JointCommand()
command.names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
command.position = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
command.acceleration = [0.3, 0.05, 0.05, 0.05, 0.05, 0.05, 0.1]
command.mode = 1
#pub_speed_ratio.publish(0.2)
rate = rospy.Rate(10)
control_diff_record = 10.0
control_diff_temp = 0.0
threshold = 0.02
# terminate the control once the arm moved to the desired joint space within the threshold
while control_diff_record > threshold:
pub.publish(command)
for x, y in zip(self.jointAngles, command.acceleration):
control_diff_temp = abs(x - y) + control_diff_temp
control_diff_record = control_diff_temp
control_diff_temp = 0.0
rate.sleep()
rospy.loginfo(
">>>>>>>>>> The robot is at the target position <<<<<<<<<<:")
def basicPositionMoveForPositioning(self, pos, speed):
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
sub = rospy.Subscriber('/robot/joint_states', JointState,
self.joints_callback)
rate = rospy.Rate(180)
# define the speed publisher
pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio',
Float64,
latch=True,
queue_size=10)
command = JointCommand()
command.names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
command.position = [
pos['right_j0'], pos['right_j1'], pos['right_j2'], pos['right_j3'],
pos['right_j4'], pos['right_j5'], pos['right_j6']
]
command.mode = 1
# customize the value to change speed
pub_speed_ratio.publish(speed)
# terminate the control once the arm moved to the desired joint space within the threshold
pub.publish(command)
rate.sleep()
rospy.loginfo(
">>>>>>>>>> The robot is at the target position <<<<<<<<<<:")
def main():
try:
rospy.init_node('avalos_limb_py')
rate = rospy.Rate(100)
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
#limb = Limb()
my_msg = JointCommand()
# POSITION_MODE
my_msg.mode = 1
my_msg.names = [
"right_j0", "right_j1", "right_j2", "right_j3", "right_j4",
"right_j5", "right_j6"
]
my_msg.position = [0, 0, 0, 0, 0, 0, 0]
for x in xrange(100):
pub.publish(my_msg)
rate.sleep()
print "Move ok"
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user.')
def talker2():
rospy.init_node('joint_state_publisher_0')
pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10)
rate = rospy.Rate(10) # 10hz
hello_str = JointCommand()
hello_str.header = Header()
hello_str.header.stamp = rospy.Time.now()
hello_str.mode = 1
hello_str.names = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
'right_j4', 'right_j5', 'right_j6']
# hello_str.position = [-0.20288, 0.61753, -1.74399, -1.32579, 1.09791, -1.74968, 0.51062]
# hello_str.position = [-1.20675, 0.467, -3.04262, -0.39422, 2.96331, -2.21461, 2.87877]
positions = [[-0.20288, 0.61753, -1.74399, -1.32579, 1.09791, -1.74968, 0.51062], [-0.36308, 0.48712, -1.88532, -1.08158, 1.30146, -1.8188, 0.22098], [-0.39752, 0.44012, -1.93739, -0.98365, 1.25842, -1.75613, 0.24877], [-0.43197, 0.39311, -1.98946, -0.88573, 1.21539, -1.69347, 0.27656], [-0.46642, 0.3461, -2.04154, -0.7878, 1.17236, -1.6308, 0.30435], [-0.50087, 0.29909, -2.09361, -0.68987, 1.12933, -1.56814, 0.33214], [-0.53532, 0.25209, -2.14568, -0.59195, 1.08629, -1.50547, 0.35993], [-0.56976, 0.20508, -2.19776, -0.49402, 1.04326, -1.4428, 0.38772], [-0.60421, 0.15807, -2.24983, -0.3961, 1.00023, -1.38014, 0.41551], [-0.63936, 0.16653, -2.29415, -0.3935, 1.11641, -1.42953, 0.56089], [-0.67451, 0.17499, -2.33848, -0.39091, 1.2326, -1.47893, 0.70627], [-0.70966, 0.18346, -2.3828, -0.38832, 1.34879, -1.52832, 0.85165], [-0.74481, 0.19192, -2.42712, -0.38573, 1.46497, -1.57772, 0.99702], [-0.77996, 0.20038, -2.47145, -0.38314, 1.58116, -1.62711, 1.1424], [-0.81511, 0.20884, -2.51577, -0.38055, 1.69735, -1.67651, 1.28778], [-0.85026, 0.2173, -2.56009, -0.37796, 1.81354, -1.7259, 1.43316], [-0.88541, 0.22576, -2.60442, -0.37536, 1.92972, -1.7753, 1.57853], [-0.92056, 0.23422, -2.64874, -0.37277, 2.04591, -1.82469, 1.72391], [-0.95571, 0.24268, -2.69306, -0.37018, 2.1621, -1.87409, 1.86929], [-0.99086, 0.25114, -2.73739, -0.36759, 2.27828, -1.92348, 2.01467], [-1.02601, 0.2596, -2.78171, -0.365, 2.39447, -1.97288, 2.16004], [-1.06116, 0.26806, -2.82603, -0.36241, 2.51066, -2.02227, 2.30542], [-1.0963, 0.27652, -2.87036, -0.35981, 2.62684, -2.07167, 2.4508], [-1.13145, 0.28498, -2.91468, -0.35722, 2.74303, -2.12106, 2.59618], [-1.1666, 0.29344, -2.959, -0.35463, 2.85922, -2.17045, 2.74155], [-1.20175, 0.30191, -3.00333, -0.35204, 2.9754, -2.21985, 2.88693]]
hello_str.velocity = []
hello_str.effort = []
# pub.publish(hello_str)
# time.sleep(10)
step = 0
while not rospy.is_shutdown():
hello_str.header.stamp = rospy.Time.now()
hello_str.position = positions[step]
pub.publish(hello_str)
step +=1
if(step >= len(positions)):
break
time.sleep(2)
rate.sleep()
def _pub_joint_positions(self, positions):
command_msg = JointCommand()
command_msg.names = self.arm_joint_names
command_msg.position = positions
command_msg.mode = JointCommand.POSITION_MODE
command_msg.header.stamp = rospy.Time.now()
self.joint_pub.publish(command_msg)
def bring2stretched(self): _robot = JointCommand() _robot.mode = 1 _robot.names = ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6'] _robot.position = [0,0,-1.61,0,0,0,0] for i in range(1000): self.pub_joints.publish(_robot) time.sleep(0.01)
def _send_pos_command(self, pos):
self._try_enable()
command = JointCommand()
command.mode = JointCommand.POSITION_MODE
command.names = self._limb.joint_names()
command.position = pos
self._cmd_publisher.publish(command)
def move_with_impedance(self, waypoints, duration=1.5):
"""
Moves from curent position to final position while hitting waypoints
:param waypoints: List of arrays containing waypoint joint angles
:param duration: trajectory duration
"""
self._try_enable()
jointnames = self.limb.joint_names()
prev_joint = np.array([self.limb.joint_angle(j) for j in jointnames])
waypoints = np.array([prev_joint] + waypoints)
spline = CSpline(waypoints, duration)
start_time = rospy.get_time() # in seconds
finish_time = start_time + duration # in seconds
time = rospy.get_time()
while time < finish_time:
pos, velocity, acceleration = spline.get(time - start_time)
command = JointCommand()
command.mode = JointCommand.POSITION_MODE
command.names = jointnames
command.position = pos
command.velocity = np.clip(velocity, -max_vel_mag, max_vel_mag)
command.acceleration = np.clip(acceleration, -max_accel_mag,
max_accel_mag)
self._cmd_publisher.publish(command)
self.control_rate.sleep()
time = rospy.get_time()
for i in xrange(10):
command = JointCommand()
command.mode = JointCommand.POSITION_MODE
command.names = jointnames
command.position = waypoints[-1]
self._cmd_publisher.publish(command)
self.control_rate.sleep()
def move_to_ja(self, waypoints, duration=1.5):
"""
:param waypoints: List of joint angle arrays. If len(waypoints) == 1: then go directly to point.
Otherwise: take trajectory that ends at waypoints[-1] and passes through each intermediate waypoint
:param duration: Total time trajectory will take before ending
"""
self._try_enable()
jointnames = self._limb.joint_names()
prev_joint = np.array([self._limb.joint_angle(j) for j in jointnames])
waypoints = np.array([prev_joint] + waypoints)
spline = CSpline(waypoints, duration)
start_time = rospy.get_time() # in seconds
finish_time = start_time + duration # in seconds
time = rospy.get_time()
while time < finish_time:
pos, velocity, acceleration = spline.get(time - start_time)
command = JointCommand()
command.mode = JointCommand.POSITION_MODE
command.names = jointnames
command.position = pos
command.velocity = np.clip(velocity, -max_vel_mag, max_vel_mag)
command.acceleration = np.clip(acceleration, -max_accel_mag, max_accel_mag)
self._cmd_publisher.publish(command)
self._control_rate.sleep()
time = rospy.get_time()
for i in xrange(10):
command = JointCommand()
command.mode = JointCommand.POSITION_MODE
command.names = jointnames
command.position = waypoints[-1]
self._cmd_publisher.publish(command)
self._control_rate.sleep()
def basicTrajMove(self, positions, speed, traj_length, speed_rate):
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
sub = rospy.Subscriber('/robot/joint_states', JointState,
self.joints_callback)
rate = rospy.Rate(speed_rate)
# define the speed publisher
pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio',
Float64,
latch=True,
queue_size=10)
command = JointCommand()
command.names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
command.mode = 1
# customize the value to change speed
pub_speed_ratio.publish(speed)
control_diff_record = 10.0
control_diff_temp = 0.0
threshold = 0.015
# terminate the control once the arm moved to the desired joint space within the threshold
counter = 0
while control_diff_record > threshold and counter < traj_length - 1:
if positions[counter] == False:
continue
command.position = [
positions[counter]['right_j0'], positions[counter]['right_j1'],
positions[counter]['right_j2'], positions[counter]['right_j3'],
positions[counter]['right_j4'], positions[counter]['right_j5'],
positions[counter]['right_j6']
]
pub.publish(command)
for x, y in zip(self.jointAngles, command.position):
control_diff_temp = abs(x - y) + control_diff_temp
control_diff_record = control_diff_temp
control_diff_temp = 0.0
rate.sleep()
counter = counter + 1
rospy.loginfo(
">>>>>>>>>> The robot is at the target position <<<<<<<<<<:")
def basicPositionMove(self, pos, speed):
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
sub = rospy.Subscriber('/robot/joint_states', JointState,
self.joints_callback)
rate = rospy.Rate(180)
# define the speed publisher
pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio',
Float64,
latch=True,
queue_size=10)
command = JointCommand()
command.names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
command.position = [
pos['right_j0'], pos['right_j1'], pos['right_j2'], pos['right_j3'],
pos['right_j4'], pos['right_j5'], pos['right_j6']
]
command.mode = 1
# customize the value to change speed
pub_speed_ratio.publish(speed)
control_diff_record = 10.0
control_diff_temp = 0.0
threshold = 0.02
# terminate the control once the arm moved to the desired joint space within the threshold
start_time = rospy.get_time()
end_time = rospy.get_time()
while control_diff_record > threshold:
end_time = rospy.get_time()
if end_time - start_time > 5:
break
pub.publish(command)
for x, y in zip(self.jointAngles, command.position):
control_diff_temp = abs(x - y) + control_diff_temp
control_diff_record = control_diff_temp
control_diff_temp = 0.0
rate.sleep()
rospy.loginfo(
">>>>>>>>>> The robot is at the target position <<<<<<<<<<:")
def basicPositionMove(self, pos, speed, initial):
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
sub = rospy.Subscriber('/robot/joint_states', JointState,
self.joints_callback)
# define the speed publisher
pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio',
Float64,
latch=True,
queue_size=10)
command = JointCommand()
command.names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
command.position = [
pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]
]
command.mode = 1
# customize the value to change speed
pub_speed_ratio.publish(speed)
# terminate the control once the arm moved to the desired joint space within the threshold
rate = rospy.Rate(1000)
control_diff_record = 10.0
control_diff_temp = 0.0
threshold = 0.02
if not initial:
# terminate the control once the arm moved to the desired joint space within the threshold
for i in range(0, 200):
pub.publish(command)
rate.sleep()
else:
for i in range(0, 10000):
rospy.logerr("returning to the first point")
pub.publish(command)
rate.sleep()
rospy.loginfo(
">>>>>>>>>> The robot is at the target position <<<<<<<<<<:")
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:
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 set_move():
F = 100 #Frecuencia de envio
rate = rospy.Rate(F) # hz
limb = intera_interface.Limb('right')
limb.move_to_neutral()
ik_service_client_full([0.662, 0.450, -0.040, 0.717, 0, 0.717, 0])
print "Posicion neutral terminada"
time.sleep(1)
p1 = np.array([0.662, 0.450, -0.040, 0.717, 0, 0.717, 0])
p2 = np.array([0.662, 0.200, 0.290, 0.717, 0, 0.717, 0])
p3 = np.array([0.662, -0.150, 0.290, 0.717, 0, 0.717, 0])
p4 = np.array([0.662, -0.450, -0.040, 0.717, 0, 0.717, 0])
p5 = np.array([0.662, -0.150, 0.290, 0.717, 0, 0.717, 0])
p6 = np.array([0.662, 0.200, 0.290, 0.717, 0, 0.717, 0])
p7 = np.array([0.662, 0.450, -0.040, 0.717, 0, 0.717, 0])
data = Data()
data.writeon("IK_data.txt")
# Control de posicion por cinematica inversa
time.sleep(0.75)
[succes, q1] = ik_service_client_full(p1)
[succes, q2] = ik_service_client_full(p2)
[succes, q3] = ik_service_client_full(p3)
[succes, q4] = ik_service_client_full(p4)
[succes, q5] = ik_service_client_full(p5)
[succes, q6] = ik_service_client_full(p6)
[succes, q7] = ik_service_client_full(p7)
time.sleep(0.75)
data.writeoff()
print "Control de Posicion Cinematica Inversa terminada"
names = [
"right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5",
"right_j6"
]
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
# Control de trayectoria cinematica inversa
q = np.matrix([q1, q2, q3, q4, q5, q6, q7])
print q
[t, t_rec] = min_time(q, F)
k = 1.25 #Factor de Tiempo
t_points = [k * x for x in t]
print "t_points", t_points
#print "Tiempo de recorrido",t_rec,"s"
[j, ext] = generate_path_cub(q, t_points, F)
[v, ext] = generate_vel(j, F)
[a, ext] = generate_acel(v, F)
[jk, v_jk, ext] = generate_jerk(a, F)
v_t = 6 * (t_points[-1] - t_points[0])
raw_input('Iniciar?')
print "Valor tiempo: ", v_t, "Valor jerk", v_jk
save_matrix(j, "save_data_p.txt", F)
save_matrix(v, "save_data_v.txt", F)
save_matrix(a, "save_data_a.txt", F)
save_matrix(jk, "save_data_y.txt", F)
ik_service_client_full([0.662, 0.450, -0.040, 0.717, 0, 0.717, 0])
time.sleep(1)
my_msg = JointCommand()
my_msg.mode = 4
my_msg.names = [
"right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5",
"right_j6"
]
data.writeon("S_data.txt")
time.sleep(0.75)
if (my_msg.mode == 4):
for n in range(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()
time.sleep(0.75)
return True
def move_move_no(limb, group, target, speed_ratio=None, accel_ratio=None, timeout=None):
trajectory_publisher = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size = 1)
JointCommandMessage = JointCommand()
JointCommandMessage.mode = 4
if speed_ratio is None:
speed_ratio = 0.3
if girigiri_aiiiiii:
speed_ratio = 0.8
if accel_ratio is None:
accel_ratio = 0.1
if girigiri_aiiiiii:
accel_ratio = 0.2
plan = group.plan(target)
rospy.sleep(1)
JointCommandMessage.names = plan.joint_trajectory.joint_names
start_time = rospy.get_time()
position_array = []
velocity_array = []
acceleration_array = []
time_array = []
for point in plan.joint_trajectory.points:
position = point.positions
velocity = point.velocities
acceleration = point.accelerations
position_array.append(position)
velocity_array.append(velocity)
acceleration_array.append(acceleration)
desire_time = point.time_from_start.to_sec()
time_array.append(desire_time)
s_array = time_array
wp_array = position_array
path = ta.SplineInterpolator(s_array, wp_array)
s_sampled = np.linspace(0, time_array[len(time_array) - 1], 100)
q_sampled = path.eval(s_sampled)
# --------------------------- plotting the interpolator --------------------------
# plt.plot(s_sampled, q_sampled)
# plt.hold(True)
# plt.plot(time_array, position_array, 'kd')
# plt.legend(["joint_1","joint_2","joint_3","joint_4","joint_5","joint_6","joint_7"])
# plt.hold(False)
# plt.show()
# exit()
# ---------------------------- end of plotting ----------------------------------
# Create velocity bounds, then velocity constraint object
dof = 7
vlim_ = np.ones(dof) * 5 #* speed_ratio
vlim = np.vstack((-vlim_, vlim_)).T
# Create acceleration bounds, then acceleration constraint object
alim_ = np.ones(dof) * 2 * accel_ratio
alim = np.vstack((-alim_, alim_)).T
pc_vel = constraint.JointVelocityConstraint(vlim)
pc_acc = constraint.JointAccelerationConstraint(
alim, discretization_scheme=constraint.DiscretizationType.Interpolation)
# Setup a parametrization instance
instance = algo.TOPPRA([pc_vel, pc_acc], path, solver_wrapper='seidel')
# Retime the trajectory, only this step is necessary.
t0 = time.time()
jnt_traj, aux_traj = instance.compute_trajectory(0, 0)
print("TOPPRA Parameterization time: {:} secs".format(time.time() - t0))
ts_sample = np.linspace(0, jnt_traj.get_duration(), 800)
position_output = jnt_traj.eval(ts_sample)
velocity_output = jnt_traj.evald(ts_sample)
acceleration_output = jnt_traj.evaldd(ts_sample)
# --------------------------- plotting the algorithm output ---------------------------
# plt.subplot(3,1,1)
# plt.plot(ts_sample, position_output)
# plt.subplot(3,1,2)
# plt.plot(ts_sample, velocity_output)
# plt.subplot(3,1,3)
# plt.plot(ts_sample, acceleration_output)
# plt.show()
# --------------------------- end plot the algorithm output ---------------------------
start_time = rospy.get_time()
for i in range(len(ts_sample) - 1):
JointCommandMessage.position = position_output[i]
JointCommandMessage.velocity = velocity_output[i]
JointCommandMessage.acceleration = acceleration_output[i]
desire_time = ts_sample[i + 1]
while (rospy.get_time() - start_time) < desire_time:
trajectory_publisher.publish(JointCommandMessage)
JointCommandMessage.position = position_output[-1]
JointCommandMessage.velocity = velocity_output[-1]
JointCommandMessage.acceleration = acceleration_output[-1]
trajectory_publisher.publish(JointCommandMessage)
def _ctl_loop(self):
rate = rospy.Rate(self._ctl_freq)
#
# trajectory_options = TrajectoryOptions()
# trajectory_options.interaction_control = False
# trajectory_options.interpolation_type = TrajectoryOptions.JOINT
last_vel = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
last_acc = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
rec_data = []
rec_seq = 0
def compute_vel(measured, desired):
tol = 0.008
mp = measured[0, :]
dp = desired[0, :]
mv = measured[1, :]
dv = desired[1, :]
cycle = self._des_provider[
self._active_des_provider]._cycles_since_active
if cycle == 1:
max_steps = (3.5 * np.abs(dp - mp) * self._ctl_freq +
50).astype(np.int)
max_steps[np.abs(dp - mp) < tol] = 1
self._des_provider[
self._active_des_provider]._HACK_MAX_STEPS = max_steps
else:
max_steps = self._des_provider[
self._active_des_provider]._HACK_MAX_STEPS
# max_steps = 500
steps = np.minimum(cycle, max_steps).astype(np.float64)
weights = (steps / max_steps)
dgain = 0.0
pgain = 4.0
ep = (mp * (1.0 - weights) + weights * dp) - mp
ep[np.abs(dp - mp) < tol] = 0.0
ev = dv - (mv * (1.0 - weights) + weights * dv)
sv = pgain * ep + dgain * ev
sv = np.maximum(np.minimum(sv, 4.0), -4.0)
return sv
while not rospy.is_shutdown():
des_joint_state, des_gripper_state = self.get_active_desired_states(
state_ids=('joint', 'gripper'))
self.run_cycle()
timestamp = rospy.Time.now()
if des_joint_state is not None:
MODE = self._des_provider[self._active_des_provider]._HACK_MODE
joint_msg = JointCommand()
joint_msg.header.stamp = timestamp
joint_msg.names = self._joint_names
acc = (self._joint_state[1, :] - last_vel) / 0.01
jerk = (acc - last_acc) / 0.01
vel = compute_vel(self._joint_state, des_joint_state)
if MODE == JointCommand.VELOCITY_MODE:
# if np.any(np.abs(vel[-1]) > 10.0):
# print("pos msr:{} des:{}\tvel msr:{} des:{}\tset: {} acc: {} jerk: {}".format(self._joint_state[0,-1],des_joint_state[0,-1],self._joint_state[1,-1],des_joint_state[1,-1],vel[-1],acc[-1], jerk[-1]))
if self.HACK_do_print:
# print("pos msr:{} des:{}\tvel msr:{} des:{}\tset: {} acc: {} jerk: {}".format(self._joint_state[0,-1],des_joint_state[0,-1],self._joint_state[1,-1],des_joint_state[1,-1],vel[-1],acc[-1], jerk[-1]))
with np.printoptions(precision=6,
suppress=True,
floatmode='fixed',
sign=' '):
# print("pm:{},\npd:{},\nvm:{},\nvd:{},\nvs:{},".format(np.array2string(self._joint_state[0,:]),np.array2string(des_joint_state[0,:]),np.array2string(self._joint_state[1,:]),np.array2string(des_joint_state[1,:]),np.array2string(vel)))
# print(np.array2string(np.array([self._joint_state[0,:], des_joint_state[0,:], self._joint_state[1,:], des_joint_state[1,:], vel])))
rec_data.append(
np.array([
self._joint_state[0, :],
des_joint_state[0, :],
self._joint_state[1, :],
des_joint_state[1, :], vel
]))
joint_msg.mode = JointCommand.VELOCITY_MODE
joint_msg.position = []
joint_msg.velocity = vel
joint_msg.acceleration = []
joint_msg.effort = []
elif MODE == JointCommand.POSITION_MODE:
print("NOOOOO:(")
joint_msg.mode = JointCommand.POSITION_MODE
joint_msg.position = des_joint_state[0, :]
joint_msg.velocity = []
joint_msg.acceleration = []
joint_msg.effort = []
self._joint_command_pub.publish(joint_msg)
if self._joint_state is not None and np.all(
np.abs(self._joint_state[0, :] -
des_joint_state[0, :]) < 0.008):
self.set_reached()
if self.HACK_do_print:
self.HACK_do_print = False
# np.save('data_{}_{:03d}'.format(self._name,rec_seq),np.array(rec_data))
rec_seq += 1
rec_data = []
last_vel = self._joint_state[1, :]
last_acc = acc
if des_gripper_state is not None:
gripper_msg = IOComponentCommand()
gripper_msg.time = timestamp
gripper_msg.op = "set"
gripper_msg.args = '{"signals": {"position_m": {"data": [' + str(
des_gripper_state) + '], "format": {"type": "float"}}}}'
self._gripper_command_pub.publish(gripper_msg)
rate.sleep()
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:
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 __init__(self, limb="right"):
rospy.init_node("custom_impedance_controller")
rospy.on_shutdown(self.clean_shutdown)
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
self._rs.enable()
if not self._rs.state().enabled:
raise RuntimeError("Robot did not enable...")
self._limb = intera_interface.Limb(limb)
print("Robot enabled...")
# control parameters
self._rate = rospy.Rate(800) # Hz
self._j_names = [n for n in self._limb.joint_names()]
self._duration = 1.5
self._desired_joint_pos = NEUTRAL_JOINT_ANGLES.copy()
self._fit_interp()
#synchronizes commands
self._global_lock = Lock()
# create cuff disable publisher
cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction'
self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)
self._cmd_publisher = rospy.Publisher(
'/robot/limb/{}/joint_command'.format(limb),
JointCommand,
queue_size=100)
print("Running. Ctrl-c to quit")
rospy.Subscriber("desired_joint_pos", JointState, self._set_des_pos)
rospy.Subscriber("interp_duration", Float32, self._set_duration)
rospy.Subscriber("imp_ctrl_active", Int64, self._imp_ctrl_active)
self._ctrl_active = True
self._node_running = True
self._rate.sleep()
self._start_time = None
while self._node_running:
if self._ctrl_active:
self._pub_cuff_disable.publish()
self._global_lock.acquire()
if self._start_time is None:
self._start_time = rospy.get_time()
elif self._start_time + self._duration < rospy.get_time():
self._fit_interp()
self._start_time = rospy.get_time()
t = min(rospy.get_time() - self._start_time,
self._duration) #T \in [0, self._duration]
pos, velocity, accel = [
x.squeeze() for x in self._interp.get(t)
]
command = JointCommand()
command.mode = JointCommand.TRAJECTORY_MODE
command.names = self._j_names
command.position = pos
command.velocity = np.clip(velocity, -max_vel_mag, max_vel_mag)
command.acceleration = np.clip(accel, -max_accel_mag,
max_accel_mag)
self._cmd_publisher.publish(command)
self._global_lock.release()
self._rate.sleep()
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.')
"""
joint7_desired = joint_f7.send_motion(robot.joint_data[7], 1.2, 1.,
time.time() - start_time7)
#en7a = enable2nextcycle(freq7a, time.time()-start_time7a)
#en7b = enable2nextcycle(freq7b, time.time()-start_time7b)
joint7_pos = copy.copy(robot.joint_data[7])
effort7 = copy.copy(robot.effort_data[7])
velocity7 = copy.copy(robot.vel_data[7])
else:
joint7_pos = 0.
effort7 = 0.
joint7_desired = 0.
velocity7 = 0.
command.position = [
joint1_desired, joint2_desired, joint3_desired, joint4_desired,
joint5_desired, joint6_desired, joint7_desired
]
robot.pub_joints.publish(command)
df_tmp = pd.DataFrame([[
joint1_pos, joint2_pos, joint3_pos, joint4_pos, joint5_pos,
joint6_pos, joint7_pos, velocity1, velocity2, velocity3, velocity4,
velocity5, velocity6, velocity7, joint1_desired, joint2_desired,
joint3_desired, joint4_desired, joint5_desired, joint6_desired,
joint7_desired, effort1, effort2, effort3, effort4, effort5,
effort6, effort7
]],
columns=column_index)
df = df.append(df_tmp, ignore_index=True)
rate.sleep()
print itera
#print joint4_desired
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.')
