def move_arm_to(X, Z, rad=0, side=1):
pub = rospy.Publisher("/ur5/jointsPosTargetCommand",
ManipulatorJoints,
queue_size=1)
#Parameters
Y = 0
a1 = 0.5
a2 = 0.425
a3 = 0.392
r = m.sqrt(X**2 + Y**2)
d = m.sqrt((Z - a1)**2 + r**2)
#Angulos
if side == 1: theta1 = -np.pi / 2
else: theta1 = np.pi / 2
theta3 = m.acos((d**2 - a3**2 - a2**2) / 2 * a2 * a3)
theta2 = m.atan2(r, Z - a1) - m.atan2(a3 * m.sin(theta3),
a2 + a3 * m.cos(theta3))
theta4 = -(theta3 + theta2) + rad
theta5 = -np.pi / 2
theta6 = 0
msg = ManipulatorJoints()
msg.joint_variable = [theta1, theta2, theta3, theta4, theta5, theta6]
print(msg)
pub.publish(msg)
rospy.spin()
def __init__(self):
# Variaveis que controlam a rotina de toque
self.touch = 0
self.state = -1
# Variavel que alerta sobre a forca
self.forceFlag = 0
# Comandos a serem enviados para as juntas
self.joint1 = 0.0
self.joint2 = 0.0
self.joint3 = 0.0
self.joint4 = 0.0
self.joint5 = 0.0
self.joint6 = 0.0
# Posicao desejada das juntas
self.desired_joint1 = 0.0
self.desired_joint2 = 0.0
self.desired_joint3 = 0.0
self.desired_joint4 = 0.0
self.desired_joint5 = 0.0
self.desired_joint6 = 0.0
# Publica em jointsPosTargetCommand
self.pub_jointsPos = rospy.Publisher('/ur5/jointsPosTargetCommand',
ManipulatorJoints,
queue_size=1)
self.subpinto = rospy.Subscriber('/aai_rosi_pose', Pose,
self.callback_pose)
# Subscreve em jointsPositionCurrentState, forceTorqueSensorOutput e aai_rosi_pose
self.sub_joints = rospy.Subscriber('/ur5/jointsPositionCurrentState',
ManipulatorJoints,
self.callback_joints)
self.sub_force = rospy.Subscriber('/ur5/forceTorqueSensorOutput',
TwistStamped, self.callback_force)
# Define a frequencia de publicacao
node_sleep_rate = rospy.Rate(10)
# Loop principal do algoritmo
while not rospy.is_shutdown():
if rospy.get_param('touch_mode'):
if self.state == -1:
self.state = 0
traj = ManipulatorJoints()
traj.header.stamp = rospy.Time.now()
traj.joint_variable = [
self.joint1, self.joint2, self.joint3, self.joint4,
self.joint5, self.joint6
]
# Publica a mensagem
self.pub_jointsPos.publish(traj)
# Pausa
node_sleep_rate.sleep()
else:
pass
def __init__(self):
rospy.loginfo("Starting node key_node")
rospy.init_node('arm_exe', anonymous=True)
self.key_publisher = rospy.Publisher('/ur5/jointsPosTargetCommand',
ManipulatorJoints)
# pub = rospy.Publisher('/ur5/jointsPosTargetCommand', ManipulatorJoints)
rate = rospy.Rate(10) #10hz
msg = ManipulatorJoints()
msg.joint_variable = command_array
# rate.sleep()
for i in range(4):
self.key_publisher.publish(msg)
rate.sleep()
def envia_comando_manipulador(self, vetor_com_angulos_das_juntas):
'''
Recebe vetor contendo os angulos de todas as juntas do manipulador
Publica isso no topico '/ur5/jointsPosTargetCommand'
'''
# Registra o publisher
pub = rospy.Publisher('/ur5/jointsPosTargetCommand',
ManipulatorJoints,
queue_size=1)
# Ajeita informacoes recebidas para o formato de msg do topico
comando = ManipulatorJoints()
comando.joint_variable = vetor_com_angulos_das_juntas
# Publica
pub.publish(comando)
def __init__(self):
# Posicao relativa (x, y) do ponto mais proximo ao robo
self.near_x = 5
self.near_y = 0
# Angulo desejado
self.angle = 0
# Posicao desejada das juntas
self.joint1 = 0
self.joint2 = 0
self.joint3 = 0
self.joint4 = 0
self.joint5 = 0
self.joint6 = 0
# Topicos em que vai se subscrever e publicar
self.sub_hokuyo = rospy.Subscriber('/sensor/hokuyo', HokuyoReading, self.callback_hokuyo)
self.sub_joints = rospy.Subscriber('/ur5/jointsPositionCurrentState', ManipulatorJoints, self.callback_joints)
self.pub_jointsPos = rospy.Publisher('/ur5/jointsPosTargetCommand', ManipulatorJoints, queue_size=1)
# Frequencia de publicacao
node_sleep_rate = rospy.Rate(5)
# Mensagem de inicializacao
rospy.loginfo('Coleta do Transportador de Correia iniciado')
# Loop principal, responsavel pelos procedimentos chaves do programa
while not rospy.is_shutdown():
# Caso esteja na tarefa do toque, esse no nao deve publicar nada
if rospy.get_param('touch_mode'):
continue
else:
traj = ManipulatorJoints()
traj.header.stamp = rospy.Time.now()
traj.joint_variable = [self.joint1, self.joint2, self.joint3, self.joint4, self.joint5, self.joint6]
# Publica a mensagem
self.pub_jointsPos.publish(traj)
# Pausa
node_sleep_rate.sleep()
def __init__(self):
# Comandos a serem enviados para as juntas
self.joint1 = 0
self.joint2 = 0
self.joint3 = 0
self.joint4 = 0
self.joint5 = 0
self.joint6 = 0
# Publica em jointsPosTargetCommand
self.pub_jointsPos = rospy.Publisher('/ur5/jointsPosTargetCommand',
ManipulatorJoints,
queue_size=1)
# Subscreve em jointsPositionCurrentState
self.sub_joints = rospy.Subscriber('/ur5/jointsPositionCurrentState',
ManipulatorJoints,
self.callback_joints)
# Define a frequencia de publicacao
node_sleep_rate = rospy.Rate(10)
# Loop principal do algoritmo
while not rospy.is_shutdown():
#
traj = ManipulatorJoints()
traj.header.stamp = rospy.Time.now()
traj.joint_variable = [
self.joint1, self.joint2, self.joint3, self.joint4,
self.joint5, self.joint6
]
# Publica a mensagem
self.pub_jointsPos.publish(traj)
# Pausa
node_sleep_rate.sleep()
def interpreter(changeReadValue):
global terrainCorrection
global x, y, z, memory
global yawCorrection
if (changeReadValue.data == 1 and memory != changeReadValue.data):
ray = ManipulatorJoints()
pub = rospy.Publisher('/ur5/jointsPosTargetCommand',
ManipulatorJoints,
queue_size=1)
ray.joint_variable = [
yawCorrection, terrainCorrection, 0, 0,
math.radians(-90), 0
]
pub.publish(ray)
ang = 0
rospy.sleep(1.5)
while (z < 0.2 or ang < 0.3) and ang < 2:
ray.joint_variable = [
yawCorrection + math.radians(90), terrainCorrection + ang, 0,
-ang,
math.radians(-90), 0
]
ang += 0.001
pub.publish(ray)
rospy.sleep(0.2)
print '1:', ang
while ang > 0:
print '2:', ang
ang -= 0.001
ray.joint_variable = [
yawCorrection + math.radians(90), terrainCorrection + ang, 0,
-ang,
math.radians(-90), 0
]
pub.publish(ray)
rospy.sleep(0.2)
ray.joint_variable = [yawCorrection, terrainCorrection, 0, 0, 0, 0]
pub.publish(ray)
for inn in range(10000):
flug = rospy.Publisher('/cpmo', Int8, queue_size=1)
flug.publish(1)
for inn in range(10000):
flug = rospy.Publisher('/cpmo', Int8, queue_size=1)
flug.publish(0)
elif (changeReadValue.data == 0):
ray = ManipulatorJoints()
pub = rospy.Publisher('/ur5/jointsPosTargetCommand',
ManipulatorJoints,
queue_size=1)
ray.joint_variable = (yawCorrection, terrainCorrection, 0, 0, 0, 0)
pub.publish(ray)
elif (changeReadValue.data == 2):
ray = ManipulatorJoints()
pub = rospy.Publisher('/ur5/jointsPosTargetCommand',
ManipulatorJoints,
queue_size=1)
ray.joint_variable = (yawCorrection + math.radians(90), 0, 0,
math.radians(-20), math.radians(-90), 0)
pub.publish(ray)
memory = changeReadValue.data