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.')