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 __init__(self):
# Subscriber estado atual do manipulador
self.sub = rospy.Subscriber('/ur5/jointsPositionCurrentState',
ManipulatorJoints,
self.callback_EstadoManipulador)
# Publisher do estado estado atual para JointState
self.pub = rospy.Publisher('joint_states', JointState, queue_size=10)
# Inicializacao da variavel para receber o valor das juntas do manipulador
self.states = ManipulatorJoints()
# Inicializacao da variavel para receber os valores das juntas
self.juntas = JointState()
self.juntas.header = Header()
self.juntas.header.stamp = rospy.Time.now()
self.juntas.name = [
'joint_ur5_j1', 'joint_ur5_j2', 'joint_ur5_j3', 'joint_ur5_j4',
'joint_ur5_j5', 'joint_ur5_j6'
]
self.juntas.position = []
self.juntas.velocity = []
self.juntas.effort = []
# Frequencia de execucao do no
self.rate = rospy.Rate(10)
# Loop a ser executado para atualizacao da posicao das juntas (entre planejador e simulador)
while not rospy.is_shutdown():
self.juntas.header.stamp = rospy.Time.now()
self.juntas.position = self.states.joint_variable
self.pub.publish(self.juntas)
self.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):
super(MoveGroupPythonInteface, self).__init__()
#self.pontos=[]
#self.juntas = []
self.seq_points = []
#self.trajetoria = JointState()
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface', anonymous=True)
#self.serv = rospy.Service('planning', Planning, self.handle_planning)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "manipulator"
move_group = moveit_commander.MoveGroupCommander(group_name)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.states = ManipulatorJoints()
# Publisher das posicoes da trajetoria
self.pub_manipulator = rospy.Publisher('/ur5/jointsPosTargetCommand',
ManipulatorJoints,
queue_size=1)
# Subscriber odom
self.sub_odom = rospy.Subscriber('/odom', Odometry, self.callback_Odom)
# Subscriber dos estados das juntas
self.sub_manipulatorStates = rospy.Subscriber(
'/ur5/jointsPositionCurrentState', ManipulatorJoints,
self.callback_Mstates)
# Subscriber dos estados das juntas planejado
self.joint_states = rospy.Subscriber(
'/move_group/fake_controller_joint_states',
JointState,
self.callback_Jstates,
queue_size=100)
# Obtem o frame de referencia
planning_frame = move_group.get_planning_frame()
#print "============ Planning frame: %s" % planning_frame
# Obtem o link do efetuador
eef_link = move_group.get_end_effector_link()
#print "============ End effector link: %s" % eef_link
# Obtem o nome do grupo definido no moveit config
group_names = robot.get_group_names()
#print "============ Available Planning Groups:", robot.get_group_names()
# Misc variables
self.rpy = 0
self.yawOrientation = 0
self.robot = robot
self.scene = scene
self.move_group = move_group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
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 __init__(self):
self.imu = Imu()
self.states = ManipulatorJoints()
self.forces = TwistStamped()
self.theta = [0, 0, 0, 0, 0, 0]
self.orientacao = [0.0, 0.0, 0.0]
self.pub_manipulator = rospy.Publisher('/ur5/jointsPosTargetCommand',
ManipulatorJoints,
queue_size=1)
self.sub_manipulatorStates = rospy.Subscriber(
'/ur5/jointsPositionCurrentState', ManipulatorJoints,
self.callback_Mstates)
self.sub_manipulatorForces = rospy.Subscriber(
'/ur5/forceTorqueSensorOutput', TwistStamped,
self.callback_Mforces)
self.sub_imu = rospy.Subscriber('/sensor/imu', Imu, self.callback_Imu)
# defining the eternal loop frequency
node_sleep_rate = rospy.Rate(10)
# eternal loop (until second order)
while not rospy.is_shutdown():
#manipulator = ManipulatorJoints()
orientation_q = self.imu.orientation
orientation_list = [
orientation_q.x, orientation_q.y, orientation_q.z,
orientation_q.w
]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
self.orientacao = [roll, pitch, yaw]
#angulos = []
#for i in range(0, 6):
# s = "Escolha o angulos theta" + str(i) + "\n"
# ang = float(input(s))
# angulos.append(ang) # adding the element
angulos = [math.pi - yaw, 0, 0, 0, 0, 0]
print(angulos)
self.theta = angulos
self.pub_manipulator.publish(joint_variable=self.theta)
#theta = np.array(self.theta)
#theta = angulos
#self.theta = np.array(theta).tolist()
node_sleep_rate.sleep()
def __init__(self):
super(MoveGroupPythonInteface, self).__init__()
self.pontos = []
self.juntas = []
self.seq_points = []
self.trajetoria = JointState()
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface', anonymous=True)
self.serv = rospy.Service('planning', Planning, self.handle_planning)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "manipulator"
move_group = moveit_commander.MoveGroupCommander(group_name)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.states = ManipulatorJoints()
# Publisher das posicoes da trajetoria
self.pub_manipulator = rospy.Publisher('/ur5/jointsPosTargetCommand',
ManipulatorJoints,
queue_size=1)
# Subscriber dos estados das juntas
self.sub_manipulatorStates = rospy.Subscriber(
'/ur5/jointsPositionCurrentState', ManipulatorJoints,
self.callback_Mstates)
# Subscriber dos estados das juntas planejado
self.joint_states = rospy.Subscriber(
'/move_group/fake_controller_joint_states',
JointState,
self.callback_Jstates,
queue_size=100)
# Obtem o frame de referencia
planning_frame = move_group.get_planning_frame()
#print "============ Planning frame: %s" % planning_frame
# Obtem o link do efetuador
eef_link = move_group.get_end_effector_link()
#print "============ End effector link: %s" % eef_link
# Obtem o nome do grupo definido no moveit config
group_names = robot.get_group_names()
#print "============ Available Planning Groups:", robot.get_group_names()
#orientation_constraint = OrientationConstraint()
#orientation_constraint.link_name = "tool_pointer"
#orientation_constraint.orientation.w = 1.0
#orientation_constraint.absolute_x_axis_tolerance = 0.1
#orientation_constraint.absolute_y_axis_tolerance = 0.1
#orientation_constraint.absolute_z_axis_tolerance = 0.1
#orientation_constraint.weight = 1
#constraint_0 = moveit_msgs.msg.JointConstraint()
#constraint_0.joint_name = 'base_link'
#constraint_0.header.frame_id
##constraint_0.position = joint_goal[0]
#constraint_0.orientation.w = 1.0
#constraint_0.tolerance_above = 0.1
#constraint_0.tolerance_below = 0.1
#constraint_0.weight = 1.0
#joint_limits = moveit_msgs.msg.Constraints()
#joint_limits.joint_constraints.append(constraint_0)
#joint_limits.joint_constraints.append(constraint_1)
#joint_limits.joint_constraints.append(constraint_2)
#move_group.set_path_constraints(joint_limits)
#print "============ Printing robot state"
#print robot.get_current_state()
#print ""
# Misc variables
#self.box_name = ''
self.robot = robot
self.scene = scene
self.move_group = move_group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
# uint32 seq
# time stamp
# string frame_id
# float32[] joint_variable
import rospy
import numpy as np
from rosi_defy.msg import RosiMovement
from rosi_defy.msg import RosiMovementArray
from rosi_defy.msg import ManipulatorJoints
from sensor_msgs.msg import Joy
from pynput.keyboard import Listener, Key
joint = 0
command_array = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
msg = ManipulatorJoints()
class KeyboardPublisher:
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)
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
def __init__(self):
self.force = 0
self.laser = 0
self.image = None
self.cX = 0
self.cY = 0
self.bridge = cv_bridge.CvBridge()
cv2.namedWindow("window", 1)
self.imu = Imu()
self.states = ManipulatorJoints()
self.forces = TwistStamped()
self.theta = [
0, math.pi / 15, math.pi / 15, -math.pi / 7.5, -math.pi / 2, 0
]
self.orientacao = [0.0, 0.0, 0.0]
self.pub_manipulator = rospy.Publisher('/ur5/jointsPosTargetCommand',
ManipulatorJoints,
queue_size=1)
self.sub_manipulatorStates = rospy.Subscriber(
'/ur5/jointsPositionCurrentState', ManipulatorJoints,
self.callback_Mstates)
#self.sub_manipulatorForces = rospy.Subscriber('/ur5/forceTorqueSensorOutput', TwistStamped, self.callback_Mforces)
self.sub_imu = rospy.Subscriber('/sensor/imu', Imu, self.callback_Imu)
self.sub_image = rospy.Subscriber('sensor/ur5toolCam', Image,
self.callback_Image)
self.sub_laser = rospy.Subscriber('sensor/hokuyo', HokuyoReading,
self.callback_Laser)
self.sub_force = rospy.Subscriber('ur5/forceTorqueSensorOutput',
TwistStamped, self.callback_Force)
# defining the eternal loop frequency
node_sleep_rate = rospy.Rate(10)
# eternal loop (until second order)
while not rospy.is_shutdown():
#manipulator = ManipulatorJoints()
orientation_q = self.imu.orientation
orientation_list = [
orientation_q.x, orientation_q.y, orientation_q.z,
orientation_q.w
]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
self.orientacao = [roll, pitch, yaw]
angulos = []
distX = 320 - self.cX
kp = -0.0005
#for i in range(0, 6):
# s = "Escolha o angulos theta" + str(i) + "\n"
# ang = float(input(s))
# angulos.append(ang) # adding the element
#print(self.laser)
#self.theta = [0,0,0,0,0,0]
#self.theta[0] = kp*distX - math.pi/2 - yaw
print(self.force)
if self.force < 0.15:
self.theta[0] = -math.pi / 2 - yaw
if self.states < 1.2:
self.theta[1] += 0.004
self.theta[2] -= 0.007
self.theta[3] += 0.003
else:
self.theta[2] += 0.003
self.theta[3] -= 0.003
#self.theta[4] = -math.pi/2
#self.theta[5] = 0
#angulos = theta
#size = self.image.shape
#print(size)
#print(angulos)
angulos = self.theta
self.pub_manipulator.publish(joint_variable=angulos)
#theta = np.array(self.theta)
#theta = angulos
#self.theta = np.array(theta).tolist()
node_sleep_rate.sleep()