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()