def __init__(self, name, mtm_name): pose_str = name + 'pose' button_str = name + 'button' force_str = name + 'force_feedback' self._active = False self._scale = 0.0009 self.pose = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0)) self.base_frame = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0)) self.tip_frame = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0)) self.grey_button_pressed = False # Used as Position Engage Clutch self.white_button_pressed = False # Used as Gripper Open Close Binary Angle self._force = DeviceFeedback() self._force.force.x = 0 self._force.force.y = 0 self._force.force.z = 0 self._force.position.x = 0 self._force.position.y = 0 self._force.position.z = 0 self._pose_sub = rospy.Subscriber(pose_str, PoseStamped, self.pose_cb, queue_size=10) self._button_sub = rospy.Subscriber(button_str, DeviceButtonEvent, self.buttons_cb, queue_size=10) self._force_pub = rospy.Publisher(force_str, DeviceFeedback, queue_size=1) # External Clutch Buttons Requested by ICL extra_footPedal_str = '/footPedal' self._extra_pedal_cb = rospy.Subscriber(extra_footPedal_str, Vector3, self.extra_footpedal_cb, queue_size=1) self._engageMTM = True self._externalFootPedalMsg = Vector3() self._geomagicButtonMsg = DeviceButtonEvent() print('BINDING GEOMAGIC DEVICE: ', name, 'TO MOCK MTM DEVICE: ', mtm_name) self._mtm_handle = ProxyMTM(mtm_name) self._msg_counter = 0
def main(): # Inicializo nodo - haptic_jointpose - rospy.init_node('testeo_forces', anonymous=False) # Moveit commander, robot commander y planning scene moveit_commander.roscpp_initialize(sys.argv) # Subscribo topics /Geomagic/button y /Geomagic/joint_status sub_botones = rospy.Subscriber("/Geomagic/button", DeviceButtonEvent, Callback_botones) # Subscribo topic fuerzas /wrench sub_force = rospy.Subscriber("/wrench", WrenchStamped, Callback_force) # Publish force feedback pub = rospy.Publisher('/Geomagic/force_feedback', DeviceFeedback, queue_size=1) r = rospy.Rate(10) # Instancio MoveGroupCommander, para UR5e - manipulator - (Robot planning group) # Publico topic /move_group/display_planned_path # Inicializo joint vars df=DeviceFeedback() df.force=Vector3() ############################ # Main while rospy running # ############################ while not rospy.is_shutdown(): r.sleep() #print df ###################### # Push button action # ###################### df.force.x=force_x.data df.force.y=force_y.data df.force.z=force_z.data print(df) if(boton_gris.data == 1): print('Boton activado') df.force.x=1 df.force.y=1 df.force.z=1 pub.publish(df)
def __init__(self, name): pose_topic_name = name + 'pose' twist_topic_name = name + 'twist' button_topic_name = name + 'button' force_topic_name = name + 'force_feedback' self._active = False self._scale = 0.0002 self.pose = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0)) self.twist = PyKDL.Twist() # This offset is to align the pitch with the view frame R_off = Rotation.RPY(0.0, 0, 0) self._T_baseoffset = Frame(R_off, Vector(0, 0, 0)) self._T_baseoffset_inverse = self._T_baseoffset.Inverse() self._T_tipoffset = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0)) self.clutch_button_pressed = False # Used as Position Engage Clutch self.gripper_button_pressed = False # Used as Gripper Open Close Binary Angle self._force = DeviceFeedback() self._force.force.x = 0 self._force.force.y = 0 self._force.force.z = 0 self._force.position.x = 0 self._force.position.y = 0 self._force.position.z = 0 self._pose_sub = rospy.Subscriber(pose_topic_name, PoseStamped, self.pose_cb, queue_size=1) self._twist_sub = rospy.Subscriber(twist_topic_name, Twist, self.twist_cb, queue_size=1) ###### Commented self._button_sub = rospy.Subscriber(button_topic_name, DeviceButtonEvent, self.buttons_cb, queue_size=1) self._force_pub = rospy.Publisher(force_topic_name, DeviceFeedback, queue_size=1) self.switch_psm = False self._button_msg_time = rospy.Time.now() self._switch_psm_duration = rospy.Duration(0.5) print('Creating Geomagic Device Named: ', name, ' From ROS Topics') self._msg_counter = 0
def main(): # Inicializo nodo - haptic_jointpose - rospy.init_node('haptic_jointpose', anonymous=False) r = rospy.Rate(10) #Rate 500Hz # Pub /Geomagic/force_feedback ,/scaled_pos_traj_controller/command jt_pub_ur5e = rospy.Publisher('/scaled_pos_traj_controller/command', JointTrajectory, queue_size=1, latch=True) pub_force = rospy.Publisher('/Geomagic/force_feedback', DeviceFeedback, queue_size=1, latch=True) # Subscribo topics /Geomagic/button y /Geomagic/joint_status sub_botones = rospy.Subscriber("/Geomagic/button", DeviceButtonEvent, Callback_botones) sub_joints = rospy.Subscriber("/Geomagic/joint_states", JointState, Callback_joints) # Subscribo forces de UR5e /wrench sub_force = rospy.Subscriber("/wrench", WrenchStamped, Callback_force) # Subscribo valocidad robot /speed_scaling_factor sub_speed = rospy.Subscriber("/speed_scaling_factor", Float64, Callback_speed) # Joint msg jt_ur5e = JointTrajectory() jt_ur5e.points = [JointTrajectoryPoint()] jt_ur5e.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] # Inicializo joint vars joint_goal = [ pi / 2, -0.2689 * 2, 0.6397 + pi / 2, -pi + pi / 5, -pi / 2, pi ] joint_haptic = [0, 0, 0, 0, 0, 0] joint_des = math.radians(0) #Desfase por acceleration y velocities # TIME FROM START CONTROL rt_control = 1 # 0-> lento y fluido 1-> CUIDADO time_from_start if rt_control == 1: # Control por tiempo en nano segundo (CUIDADO nsecs muy bajo) jt_ur5e.points[0].time_from_start.nsecs = 500000000 tfs = jt_ur5e.points[0].time_from_start.nsecs / 1000000000.0 else: jt_ur5e.points[0].time_from_start.secs = 1 tfs = jt_ur5e.points[0].time_from_start.secs # Force vars f_const = 10 # Force div f_cap = 1 # Force sensitivity df = DeviceFeedback() df.lock = [False] force_flag = 0 # force_flaj != 0 , 1 -> Disable force btn # Force init df.force.x = 0 df.force.y = 0 df.force.z = 0 pub_force.publish(df) rospy.sleep(2) jt_ur5e.points[0].positions = joint_goal #################### # Print robot info # #################### print('\033[1;32;38m ##### Ready ##### \033[1;37;0m \n') r.sleep() print '\033[1;33;38m Robot Speed: \033[1;37;0m', vel.data * 100, '%' #assert vel.data<0.41, "Velocidad muy elevada" print '\033[1;33;38m Time From Start: \033[1;37;0m', tfs, 's' if tfs < 0.5: print '\033[1;31;38m WARN: Time From Start < 0.5s \033[1;37;0m' print '\033[1;37;38m ==================== \033[1;37;0m' ''' # Pose inicial (Movimiento desde pose apagado) jt_ur5e.points[0].positions=[pi/2,-pi/2,pi/2+pi/4,-pi+pi/5,-pi/2,pi] jt_pub_ur5e.publish(jt_ur5e) rospy.sleep(3) ''' ############################ # Main while rospy running # ############################ while not rospy.is_shutdown(): # Activar/Desactivar fuerzas if (force_flag == 0 and boton_blanco.data == 1): force_flag = 1 print( '\033[1;34;38m Control de fuerzas \033[1;32;38m ' 'activado \033[1;37;0m') while (boton_blanco.data == 1): r.sleep() elif (force_flag == 1 and boton_blanco.data == 1): force_flag = 0 print( '\033[1;34;38m Control de fuerzas \033[1;31;38m ' 'desactivado \033[1;37;0m') df.force.x = 0 df.force.y = 0 df.force.z = 0 pub_force.publish(df) while (boton_blanco.data == 1): r.sleep() ################## # Forces control # ################## if (force_flag == 1): df.force.x = force_x.data / f_const if (df.force.x < f_cap and df.force.x > -f_cap): df.force.x = 0 df.force.y = -force_z.data / f_const if (df.force.y < f_cap and df.force.y > -f_cap): df.force.y = 0 df.force.z = force_y.data / f_const if (df.force.z < f_cap + 0.7 and df.force.z > -f_cap): df.force.z = 0 if (force_x.data > df.force.x + 1 or force_y.data > df.force.y + 1 or force_z.data > df.force.z + 1): pub_force.publish(df) if (force_x.data < df.force.x - 1 or force_y.data < df.force.y - 1 or force_z.data < df.force.z - 1): pub_force.publish(df) ###################### # Push button action # ###################### if (boton_gris.data == 1): ac = 0 ve = 0 jt_ur5e.points[0].accelerations = [ac, ac, ac, ac, ac, ac] jt_ur5e.points[0].velocities = [ve, ve, ve, ve, ve, ve] joint_haptic = [ waist.data, shoulder.data, elbow.data, yaw.data, pitch.data, roll.data ] #print joint_haptic joint_goal[0] = joint_haptic[0] + pi / 2 + joint_des joint_goal[1] = -joint_haptic[1] - 0.2689 + joint_des joint_goal[2] = -joint_haptic[2] + pi / 2 + joint_des joint_goal[3] = joint_haptic[4] + pi / 5 + joint_des joint_goal[4] = joint_haptic[3] - pi - pi / 2 + joint_des joint_goal[5] = joint_haptic[5] + joint_des ##### Final if grey button ##### jt_ur5e.points[0].positions = joint_goal jt_pub_ur5e.publish(jt_ur5e) r.sleep()
def main(): # Inicializo nodo - haptic_jointpose - rospy.init_node('haptic_jointpose', anonymous=False) # Moveit commander, robot commander y planning scene moveit_commander.roscpp_initialize(sys.argv) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() # Subscribo topics /Geomagic/button y /Geomagic/joint_status sub_botones = rospy.Subscriber("/Geomagic/button", DeviceButtonEvent, Callback_botones) sub_joints = rospy.Subscriber("/Geomagic/joint_states", JointState, Callback_joints) # Subscribo topic fuerzas /wrench sub_force = rospy.Subscriber("/wrench", WrenchStamped, Callback_force) # Publish force feedback pub = rospy.Publisher('/Geomagic/force_feedback', DeviceFeedback, queue_size=1) #r = rospy.Rate(10) # Instancio MoveGroupCommander, para UR5e - manipulator - (Robot planning group) move_group = moveit_commander.MoveGroupCommander("manipulator") # Publico topic /move_group/display_planned_path display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) # Inicializo joint vars joint_goal = move_group.get_current_joint_values() joint_haptic = [0, 0, 0, 0, 0, 0] joint_goal = [ pi / 2, -0.2689 * 2, 0.6397 + pi / 2, -pi + pi / 5, -pi / 2 + pi, pi ] df = DeviceFeedback() ############################ # Main while rospy running # ############################ iii = 1 while not rospy.is_shutdown(): ###################### # Push button action # ###################### if (boton_gris.data == 1): joint_haptic = [ waist.data, shoulder.data, elbow.data, yaw.data, pitch.data, roll.data ] print joint_haptic #r.sleep() joint_goal[0] = joint_goal[0] joint_goal[1] = joint_goal[1] - 0.05 joint_goal[2] = joint_goal[2] joint_goal[3] = joint_goal[3] joint_goal[4] = joint_goal[4] joint_goal[5] = joint_goal[5] ##### Final if grey button ##### # The go command can be called with joint values, poses, or without any # parameters if you have already set the pose or joint target for the group move_group.go(joint_goal, wait=True)
########################################## # Joint trayectory control UR5e Geomagic # ########################################## ############ # Var init # ############ ################# # Main function # ################# # Inicializo nodo - haptic_jointpose - rospy.init_node('testing', anonymous=False) r = rospy.Rate(10) pub = rospy.Publisher('/Geomagic/force_feedback', DeviceFeedback, queue_size=1) df = DeviceFeedback() df.force = Vector3() df.lock = [False] while not rospy.is_shutdown(): df.force.x = 0 df.force.y = 0 df.force.z = 0 pub.publish(df) r.sleep()
def main(): # Inicializo nodo - haptic_jointpose - rospy.init_node('haptic_jointpose', anonymous=False) r = rospy.Rate(500) #Rate Hz # Pub /Geomagic/force_feedback ,/scaled_pos_traj_controller/command jt_pub_ur5e = rospy.Publisher('/scaled_pos_traj_controller/command', JointTrajectory, queue_size=1, latch=True) pub_force = rospy.Publisher('/Geomagic/force_feedback', DeviceFeedback, queue_size=1, latch=True) # Subscribo topics /Geomagic/button y /Geomagic/joint_status sub_botones = rospy.Subscriber("/Geomagic/button", DeviceButtonEvent, Callback_botones) sub_joints = rospy.Subscriber("/Geomagic/joint_states", JointState, Callback_joints) # Subscribo forces de UR5e /wrench sub_force = rospy.Subscriber("/wrench", WrenchStamped, Callback_force) sub_pose = rospy.Subscriber('Geomagic/pose', PoseStamped, Callback_pose) # Subscribo valocidad robot /speed_scaling_factor sub_speed = rospy.Subscriber("/speed_scaling_factor", Float64, Callback_speed) # Joint msg /trayectory_msg/JointTrajectory.msg jt_ur5e = JointTrajectory() jt_ur5e.points = [JointTrajectoryPoint()] jt_ur5e.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] # Inicializo joint vars joint_goal = [ pi / 2, -math.radians(15.4) * 2, 0.6397 + pi / 2, -pi + pi / 5, -pi / 2, pi ] #init joint_haptic = [0, 0, 0, 0, 0, 0] joint_des = math.radians(0) #Desfase por acceleration y velocities base_mult = 1.5 ac = 0 #accelerations ve = 0 #velocities flg = 0 #Flag reajuste time_from_start jt_ur5e.points[0].accelerations = [ac, ac, ac, ac, ac, ac] jt_ur5e.points[0].velocities = [ve, ve, ve, ve, ve, ve] # TIME FROM START CONTROL # Control por tiempo en nano segundos (CUIDADO nsecs muy bajo) jt_ur5e.points[0].time_from_start.nsecs = 2000000000 tfs = jt_ur5e.points[0].time_from_start.nsecs / 1000000000.0 # Force vars f_const = 8 # Force mult (Multiplicador intensidad fuerza) f_cap = 5 # Force sensitivity (Marge de error de fuerza) df = DeviceFeedback() df.lock = [False] force_flag = 0 # force_flag != 0 , 1 -> Disable force btn # Force init df.force.x = 0 df.force.y = 0 df.force.z = 0 pub_force.publish(df) rospy.sleep(3) jt_ur5e.points[0].positions = joint_goal #################### # Print robot info # #################### print('\033[1;32;38m ##### Ready ##### \033[1;37;0m \n') r.sleep() print '\033[1;33;38m Robot Speed: \033[1;37;0m', vel.data * 100, '%' assert vel.data < 0.61, "Velocidad muy elevada" print '\033[1;33;38m Time From Start: \033[1;37;0m', tfs, 's' if tfs < 0.5: print '\033[1;31;38m WARN: Time From Start < 0.5s \033[1;37;0m' print '\033[1;37;38m ==================== \033[1;37;0m' ############################ # Main while rospy running # ############################ f_count = 0 while not rospy.is_shutdown(): # Activar/Desactivar fuerzas if (force_flag == 0 and boton_blanco.data == 1): force_flag = 1 print( '\033[1;34;38m Control de fuerzas \033[1;32;38m ' 'activado \033[1;37;0m') while (boton_blanco.data == 1): r.sleep() elif (force_flag == 1 and boton_blanco.data == 1): force_flag = 0 print( '\033[1;34;38m Control de fuerzas \033[1;31;38m ' 'desactivado \033[1;37;0m') df.force.x = 0 df.force.y = 0 df.force.z = 0 pub_force.publish(df) while (boton_blanco.data == 1): r.sleep() ################## # Forces control # (BETA) ################## if (force_flag == 1): # Print fuerza output_stream.write(' Fuerza aplicada: %.4f\r' % -force_z.data) output_stream.flush() ''' #Rigth-Left haptic df.force.x=force_x.data/f_const if(df.force.x<f_cap and df.force.x>-f_cap ): df.force.x=0 #Up-Down haptic df.force.y=-force_z.data/f_const if(df.force.y<f_cap and df.force.y>-f_cap ): df.force.y=0 #Front-Back haptic df.force.z=force_y.data/f_const if(df.force.z<f_cap+0.7 and df.force.z>-f_cap ): df.force.z=0 #Publish diferencia mayor de 1 if(force_x.data>df.force.x+1 or force_y.data>df.force.y+1 or force_z.data>df.force.z+1): pub_force.publish(df) if(force_x.data<df.force.x-1 or force_y.data<df.force.y-1 or force_z.data<df.force.z-1): pub_force.publish(df) ''' ############### # Force calcs # ############### force_per = (-force_z.data - f_cap) * 0.04 * f_const if force_per < 0: force_per = 0 rot = R.from_quat( [pose_x.data, pose_y.data, pose_z.data, pose_w.data]) (X, Y, Z) = rot.as_euler('zyx', degrees=False) ##### Fuerzas ejes XYZ ##### #Fuerza x Fx = Y / (pi / 2) #Fuerza y if (Z < pi / 2 and Z > -pi / 2): Fy = -Z / (pi / 2) else: if Z < -pi / 2: Fy = (Z * 2 / pi + 2) - abs(Y / (pi / 2)) if Fy < 0: Fy = 0 else: Fy = (Z * 2 / pi - 2) + abs(Y / (pi / 2)) if Fy > 0: Fy = 0 #Fuerza z if (Z < pi / 2 and Z > -pi / 2): Fz = 1 - abs(Z / (pi / 2)) - abs(Y / (pi / 2)) if Fz < 0: Fz = 0 else: Fz = -(abs(Z) * 2 / pi - 1) # Porcentajes intensidad fuerzas px = Fx / (abs(Fx) + abs(Fy) + abs(Fz)) py = Fy / (abs(Fx) + abs(Fy) + abs(Fz)) pz = Fz / (abs(Fx) + abs(Fy) + abs(Fz)) df.force.x = px * force_per df.force.y = py * force_per df.force.z = pz * force_per # Publicacion fuerzas (Se debe disminuir la frecuencia) if f_count == 5: f_count = 0 pub_force.publish(df) f_count = f_count + 1 ###################### # Push button action # ###################### if (boton_gris.data == 1): #First time btn push if (flg == 0): # Reescala time_from_start jt_ur5e.points[0].time_from_start.nsecs = 500000000 #0.5 sec tfs = jt_ur5e.points[0].time_from_start.nsecs / 1000000000.0 print '\033[1;33;38m Time From Start: \033[1;37;0m', tfs, 's' flg = 1 r.sleep() #Joint pose haptic joint_haptic = [ waist.data, shoulder.data, elbow.data, yaw.data, pitch.data, roll.data ] #Set joint value joint_goal[0] = joint_haptic[0] * base_mult + pi / 2 + joint_des joint_goal[1] = -joint_haptic[1] - math.radians(15.4) + joint_des joint_goal[2] = -joint_haptic[2] + pi / 2 + joint_des joint_goal[3] = joint_haptic[4] + pi / 5 + joint_des joint_goal[4] = joint_haptic[3] - pi - pi / 2 + joint_des joint_goal[5] = joint_haptic[5] + joint_des ##### Final if grey button ##### jt_ur5e.points[0].positions = joint_goal jt_pub_ur5e.publish(jt_ur5e) r.sleep()