def main():
try:
rospy.init_node('avalos_limb_py')
limb = Limb()
print "Joint Name:"
print limb.joint_names()
print "\n" + "Joint Angles:"
print limb.joint_angles()
print "\n" + "Joint Velocities:"
print limb.joint_velocities()
print "\n" + "Endpoint Pose:"
print limb.endpoint_pose()
print "\n" + "Endpoint Velocity:"
print limb.endpoint_velocity()
except rospy.ROSInterruptException:
rospy.logerr(
'Keyboard interrupt detected from the user. Exiting before trajectory completion.'
)
def inicio():
running_status = True
limb = Limb()
state = "false"
while not rospy.is_shutdown():
if running_status:
j_p=limb.joint_angles()
state = "true"
data = {'head_pan':round(180*j_p['right_j0']/3.14,1),
'right_j0':round(180*j_p['right_j0']/3.14,1),
'right_j1':round(180*j_p['right_j1']/3.14,1),
'right_j2':round(180*j_p['right_j2']/3.14,1),
'right_j3':round(180*j_p['right_j3']/3.14,1),
'right_j4':round(180*j_p['right_j4']/3.14,1),
'right_j5':round(180*j_p['right_j5']/3.14,1),
'right_j6':round(180*j_p['right_j6']/3.14,1) }
success = device_client.publishEvent(
"joint_angle",
"json",
{'d': data},
qos=0,
on_publish=my_on_publish_callback)
#print data
time.sleep(0.3)
if not success:
print("Not connected to WatsonIoTP")
def sendIOT():
#rospy.init_node('avalos_limb_py')
while not rospy.is_shutdown():
limb = Limb()
j_p=limb.joint_angles()
j_v=limb.joint_velocities()
p_p=limb.endpoint_pose()
v_p=limb.endpoint_velocity()
if running_status:
joint_p = {'right_j0': j_p['right_j0'],
'right_j1': j_p['right_j1'],
'right_j2': j_p['right_j2'],
'right_j3': j_p['right_j3'],
'right_j4': j_p['right_j4'],
'right_j5': j_p['right_j5'],
'right_j6': j_p['right_j6']}
joint_v = {'right_j0': j_v['right_j0'],
'right_j1': j_v['right_j1'],
'right_j2': j_v['right_j2'],
'right_j3': j_v['right_j3'],
'right_j4': j_v['right_j4'],
'right_j5': j_v['right_j5'],
'right_j6': j_v['right_j6']}
success = device_client.publishEvent(
"joint_angle",
"json",
{'j_p': joint_p,'j_v': joint_v},
qos=0,
on_publish=my_on_publish_callback())
#print data
time.sleep(0.1)
if not success:
print("Not connected to WatsonIoTP")
def main():
rospy.init_node("parry")
global arm
global listener
arm = Limb()
clash_pub = rospy.Publisher('clash', Float64, queue_size=1)
twist_sub = rospy.Subscriber('/vive/twist1',
TwistStamped,
twist_callback,
queue_size=1)
listener = tf.TransformListener()
old_tracker_pose = 0
# set the end effector twist
sword_twist = Twist()
arm_twist = Twist()
no_twist = Twist()
no_twist.linear.x, no_twist.linear.y, no_twist.linear.z = 0, 0, 0
no_twist.angular.x, no_twist.angular.y, no_twist.angular.z = 0, 0, 0
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
# (controller_pos, controller_quat) = listener.lookupTransform('world', 'controller', rospy.Time(0))
(tracker_pos,
tracker_quat) = listener.lookupTransform('world', 'tracker',
rospy.Time(0))
# get sword pose
(sword_position,
_) = listener.lookupTransform('world', 'sword_tip', rospy.Time(0))
# get arm position
armpose = arm.endpoint_pose()
arm_position = armpose['position']
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
tracker_pos = np.array(tracker_pos)
arm_position = np.array(arm_position)
sword_position = np.array(sword_position)
#offset tracker position so that robot sword meets at mid point
tracker_pos[2] += sword_zoffset
#
displacement = tracker_pos - sword_position
# set velocity in the direction of the displacement
disp_mag = np.linalg.norm(displacement)
clash_pub.publish(disp_mag)
tracker_pos_mag = np.linalg.norm(tracker_pos)
# print("distance between tracker and world {}".format(tracker_pos_mag))
# print("The distance between the arm and tracker is {}".format(disp_mag))
#print("distance is {}".format(tracker_pos_mag))
arm_twist.linear.x = 0.30 * displacement[0] / disp_mag
arm_twist.linear.y = 0.30 * displacement[1] / disp_mag
arm_twist.linear.z = 0.30 * displacement[2] / disp_mag
arm_twist.angular.x = 0.3 * np.random.choice(end_effector_directions)
arm_twist.angular.y = 0
arm_twist.angular.z = 0
pos_diff = np.linalg.norm(old_tracker_pose) - tracker_pos_mag
print("tracker twist is {}".format(tracker_twist))
# if user sword is less than 1.25m to robot
# and distance between robot arm and user sword is less than 0.15m
# and user sword is moving towards robot...
if tracker_pos_mag < 1.25 and disp_mag > 0.07:
#pass
move(arm_twist)
if tracker_twist < 0.15:
arm.set_joint_positions(arm.joint_angles())
elif tracker_pos_mag > 1.25:
#pass
arm.set_joint_positions(home_config)
else:
#pass
move(no_twist)
old_tracker_pose = tracker_pos_mag
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 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 Fencer():
def __init__(self):
self.Blist = sw.Blist
self.M = sw.M
self.Slist = mr.Adjoint(self.M).dot(self.Blist)
self.eomg = 0.01 # positive tolerance on end-effector orientation error
self.ev = 0.001 # positive tolerance on end-effector position error
self.arm = None
self.listener = None
self.home_config = {
'right_j6': -1.3186796875,
'right_j5': 0.5414912109375,
'right_j4': 2.9682451171875,
'right_j3': 1.7662939453125,
'right_j2': -3.0350302734375,
'right_j1': 1.1202939453125,
'right_j0': -0.0001572265625
}
self.tracker_position = None
self.arm_position = None
self.sword_position = None
self.sword_zoffset = 0.25
self.end_effector_directions = [-1.0, 1.0]
self.tracker_twist = 50
self.end_effector_vel = np.zeros(6)
# velocities need to be passed in as a dictionary
self.joint_vels_dict = {}
# set the end effector twist
self.sword_twist = Twist()
self.arm_twist = Twist()
self.no_twist = Twist()
self.no_twist.linear.x = 0
self.no_twist.linear.y = 0
self.no_twist.linear.z = 0
self.no_twist.angular.x = 0
self.no_twist.angular.y = 0
self.no_twist.angular.z = 0
self.arm = Limb()
self.clash_pub = rospy.Publisher('clash', Float64, queue_size=1)
self.twist_sub = rospy.Subscriber('/vive/twist1',
TwistStamped,
self.twist_callback,
queue_size=1)
self.tf_listener = tf.TransformListener()
self.rate = rospy.Rate(10.0)
def move(self, twist_msg):
# have to switch the order of linear and angular velocities in twist
# message so that it comes in the form needed by the modern_robotics library
self.end_effector_vel[0] = 0 #twist_msg.angular.x
self.end_effector_vel[1] = 0 #twist_msg.angular.y
self.end_effector_vel[2] = 0 #twist_msg.angular.z
self.end_effector_vel[3] = twist_msg.linear.x
self.end_effector_vel[4] = twist_msg.linear.y
self.end_effector_vel[5] = twist_msg.linear.z
arm_angles_dict = self.arm.joint_angles()
thetalist0 = []
for i in range(len(arm_angles_dict)):
thetalist0.append(arm_angles_dict['right_j' + str(i)])
J = mr.JacobianSpace(self.Slist, np.array(thetalist0))
pinv_J = np.linalg.pinv(J)
# print("The shape of the end effector velocity vector is {}".format(self.end_effector_vel.shape))
# print("The shape of the Jacobian Pseudo Inverse matrix is {}".format(pinv_J.shape))
joint_vels = np.dot(pinv_J, self.end_effector_vel)
for i, vel in enumerate(joint_vels):
self.joint_vels_dict['right_j' + str(i)] = vel
# set joint velocities
self.arm.set_joint_velocities(self.joint_vels_dict)
def twist_callback(self, msg):
# get magnitude of tracker twist
self.tracker_twist = np.linalg.norm([msg.twist.linear.x, \
msg.twist.linear.y, \
msg.twist.linear.z])
def fence(self):
old_tracker_pose = 0
while not rospy.is_shutdown():
try:
# (controller_pos, controller_quat) = listener.lookupTransform('world', 'controller', rospy.Time(0))
(self.tracker_position,
_) = self.tf_listener.lookupTransform('world', 'tracker',
rospy.Time(0))
# get sword pose
(self.sword_position,
_) = self.tf_listener.lookupTransform('world', 'sword_tip',
rospy.Time(0))
# get arm position
armpose = self.arm.endpoint_pose()
self.arm_position = armpose['position']
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
self.tracker_position = np.array(self.tracker_position)
self.arm_position = np.array(self.arm_position)
self.sword_position = np.array(self.sword_position)
#offset tracker position so that robot sword meets at mid point
self.tracker_position[2] += self.sword_zoffset
#
displacement = self.tracker_position - self.sword_position
# set velocity in the direction of the displacement
disp_mag = np.linalg.norm(displacement)
self.clash_pub.publish(disp_mag)
tracker_pos_mag = np.linalg.norm(self.tracker_position)
# print("distance between tracker and world {}".format(tracker_pos_mag))
# print("The distance between the arm and tracker is {}".format(disp_mag))
#print("distance is {}".format(tracker_pos_mag))
self.arm_twist.linear.x = 0.30 * displacement[0] / disp_mag
self.arm_twist.linear.y = 0.30 * displacement[1] / disp_mag
self.arm_twist.linear.z = 0.30 * displacement[2] / disp_mag
self.arm_twist.angular.x = 0.3 * np.random.choice(
self.end_effector_directions)
self.arm_twist.angular.y = 0
self.arm_twist.angular.z = 0
pos_diff = np.linalg.norm(old_tracker_pose) - tracker_pos_mag
print("tracker twist is {}".format(self.tracker_twist))
# if user sword is less than 1.25m to robot
# and distance between robot arm and user sword is less than 0.15m
# and user sword is moving towards robot...
if tracker_pos_mag < 1.25 and disp_mag > 0.07:
#pass
self.move(self.arm_twist)
if self.tracker_twist < 0.15:
self.arm.set_joint_positions(self.arm.joint_angles())
elif tracker_pos_mag > 1.25:
#pass
self.arm.set_joint_positions(self.home_config)
else:
#pass
self.move(self.no_twist)
old_tracker_pose = tracker_pos_mag
self.rate.sleep()
class Fencer():
def __init__(self):
self.Blist = sw.Blist
self.M = sw.M
self.Slist = mr.Adjoint(self.M).dot(self.Blist)
self.eomg = 0.01 # positive tolerance on end-effector orientation error
self.ev = 0.001 # positive tolerance on end-effector position error
self.arm = None
self.listener = None
self.home_config = {
'right_j6': -1.3186796875,
'right_j5': 0.5414912109375,
'right_j4': 2.9682451171875,
'right_j3': 1.7662939453125,
'right_j2': -3.0350302734375,
'right_j1': 1.1202939453125,
'right_j0': -0.0001572265625
}
self.tracker_position = None
self.tracker_pos_mag = None
self.arm_position = None
self.sword_position = None
self.attack_position = None
self.tag_position = None
self.disp_mag = None
self.attack_disp_mag = None
self.fence_region = 1.50
self.fence_speed = 0.50
self.sword_zoffset = 0.25
self.end_effector_directions = [-1.0, 1.0]
self.tracker_twist = 50
self.end_effector_vel = np.zeros(6)
# velocities need to be passed in as a dictionary
self.joint_vels_dict = {}
# set the end effector twist
self.sword_twist = Twist()
self.arm_twist = Twist()
self.no_twist = Twist()
self.no_twist.linear.x = 0
self.no_twist.linear.y = 0
self.no_twist.linear.z = 0
self.no_twist.angular.x = 0
self.no_twist.angular.y = 0
self.no_twist.angular.z = 0
self.arm = Limb()
print("going to home configuration")
self.arm.set_joint_positions(self.home_config)
self.clash_pub = rospy.Publisher('clash', Float64, queue_size=1)
self.twist_sub = rospy.Subscriber('/vive/twist1',
TwistStamped,
self.twist_callback,
queue_size=1)
self.tf_listener = tf.TransformListener()
self.rate = rospy.Rate(10.0)
def move(self, twist_msg):
# have to switch the order of linear and angular velocities in twist
# message so that it comes in the form needed by the modern_robotics library
self.end_effector_vel[0] = 0 #twist_msg.angular.x
self.end_effector_vel[1] = 0 #twist_msg.angular.y
self.end_effector_vel[2] = 0 #twist_msg.angular.z
self.end_effector_vel[3] = twist_msg.linear.x
self.end_effector_vel[4] = twist_msg.linear.y
self.end_effector_vel[5] = twist_msg.linear.z
arm_angles_dict = self.arm.joint_angles()
thetalist0 = []
for i in range(len(arm_angles_dict)):
thetalist0.append(arm_angles_dict['right_j' + str(i)])
J = mr.JacobianSpace(self.Slist, np.array(thetalist0))
pinv_J = np.linalg.pinv(J)
# print("The shape of the end effector velocity vector is {}".format(self.end_effector_vel.shape))
# print("The shape of the Jacobian Pseudo Inverse matrix is {}".format(pinv_J.shape))
joint_vels = np.dot(pinv_J, self.end_effector_vel)
for i, vel in enumerate(joint_vels):
self.joint_vels_dict['right_j' + str(i)] = vel
# set joint velocities
self.arm.set_joint_velocities(self.joint_vels_dict)
def twist_callback(self, msg):
# get magnitude of tracker twist
self.tracker_twist = np.linalg.norm([msg.twist.linear.x, \
msg.twist.linear.y, \
msg.twist.linear.z])
def fence(self):
while not rospy.is_shutdown():
try:
# (controller_pos, controller_quat) = listener.lookupTransform('world', 'controller', rospy.Time(0))
(self.tracker_position,
_) = self.tf_listener.lookupTransform('world', 'tracker',
rospy.Time(0))
# get sword pose
(self.sword_position,
_) = self.tf_listener.lookupTransform('world', 'sword_tip',
rospy.Time(0))
# get tag position
# (self.tag_position, _) = self.tf_listener.lookupTransform('world', 'ar_marker_0', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
self.tracker_position = None
self.sword_position = None
self.tag_position = None
self.arm.set_joint_positions(self.home_config)
continue
# get arm position
armpose = self.arm.endpoint_pose()
self.arm_position = armpose['position']
self.tracker_position = np.array(self.tracker_position)
self.arm_position = np.array(self.arm_position)
self.sword_position = np.array(self.sword_position)
self.tag_position = np.array(self.tag_position)
#offset tracker position so that robot sword meets at mid point
self.tracker_position[2] += self.sword_zoffset
print("tracker twist is {}".format(self.tracker_twist))
if self.tracker_twist > 0.20: # user moving sword
self.defend()
else:
self.attack()
self.rate.sleep()
def defend(self):
if self.tracker_position is None:
self.arm.set_joint_positions(self.home_config)
return
else:
displacement = self.tracker_position - self.sword_position
# set velocity in the direction of the displacement
self.disp_mag = np.linalg.norm(displacement)
self.clash_pub.publish(self.disp_mag)
self.tracker_pos_mag = np.linalg.norm(self.tracker_position)
# print("distance between tracker and world {}".format(tracker_pos_mag))
self.arm_twist.linear.x = self.fence_speed * displacement[
0] / self.disp_mag
self.arm_twist.linear.y = self.fence_speed * displacement[
1] / self.disp_mag
self.arm_twist.linear.z = self.fence_speed * displacement[
2] / self.disp_mag
self.arm_twist.angular.x = self.fence_speed * np.random.choice(
self.end_effector_directions)
self.arm_twist.angular.y = 0
self.arm_twist.angular.z = 0
if self.tracker_pos_mag < self.fence_region and self.disp_mag > 0.07:
#pass
self.move(self.arm_twist)
if self.disp_mag < 0.40:
self.arm.set_joint_positions(self.arm.joint_angles())
time.sleep(1)
elif self.tracker_pos_mag > self.fence_region:
self.arm.set_joint_positions(self.home_config)
else:
#pass
self.move(self.no_twist)
def attack(self):
#self.arm.set_joint_positions(self.home_config)
# define new attack point
x_point = np.random.uniform(low=self.tracker_position[0] + 0.10,
high=self.tracker_position[0] + 0.10)
y_point = np.random.uniform(low=self.tracker_position[1] - 0.50,
high=self.tracker_position[1])
z_point = np.random.uniform(low=self.tracker_position[2],
high=self.tracker_position[2] + 0.40)
self.attack_position = np.array([x_point, y_point, z_point])
# if self.tag_position is None:
# self.arm.set_joint_positions(self.home_config)
# return
# else:
# displacement = self.tag_position - self.arm_position
displacement = self.attack_position - self.arm_position
# set velocity in the direction of the displacement
self.attack_disp_mag = np.linalg.norm(displacement)
self.clash_pub.publish(self.disp_mag)
self.arm_twist.linear.x = self.fence_speed * displacement[
0] / self.attack_disp_mag
self.arm_twist.linear.y = self.fence_speed * displacement[
1] / self.attack_disp_mag
self.arm_twist.linear.z = self.fence_speed * displacement[
2] / self.attack_disp_mag
self.arm_twist.angular.x = 0
self.arm_twist.angular.y = self.fence_speed * np.random.choice(
self.end_effector_directions)
self.arm_twist.angular.z = 0
self.move(self.arm_twist)
if self.disp_mag < 0.40:
self.arm.set_joint_positions(self.arm.joint_angles())
time.sleep(1)
elif self.tracker_pos_mag > self.fence_region:
self.arm.set_joint_positions(self.home_config)
else:
#pass
#self.move(self.no_twist)
self.arm.set_joint_positions(self.home_config)
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.')
