def __init__(self, robot=RobotUR(), gripper_topic='switch_on_off', random_state_strategy='optimal'):
self.robot = robot # Robot we want to control
self.gripper_topic = gripper_topic # Gripper topic
self.gripper_publisher = rospy.Publisher(self.gripper_topic, Bool) # Publisher for the gripper topic
self.image_controller = ImageController(image_topic='/usb_cam2/image_raw')
self.environment_image = None
self.random_state_strategy = random_state_strategy
def __init__(self, initialPose):
super(InteractRobotPyQt, self).__init__()
# loadUi('reglage.ui', self)
# Start the ROS node
rospy.init_node('interact_robot')
self.robot = RobotUR()
objectifAtteint = self.robot.go_to_joint_state(
initialPose) # On met le robot en position initiale
# objectifAtteint = self.robot.go_to_joint_state([0, 0, 0, 0, 0, 0])
self.pose_goal = self.robot.get_current_pose(
).pose # On récupère ses coord articulaires et cartésiennes
# Définition de l'IHM
vLayout = QVBoxLayout()
# Partie cartésienne
titre = QLabel("Coordonnées cartésiennes")
titre.setFont(QtGui.QFont("Arial", 14, QtGui.QFont.Black))
titre.setAlignment(QtCore.Qt.AlignCenter)
vLayout.addWidget(titre)
vLayout.addWidget(QLabel("Position"))
self.reglPoseX = Reglage(-1, 1, "x : ", self.pose_goal.position.x)
vLayout.addWidget(self.reglPoseX)
self.reglPoseY = Reglage(-1, 1, "y : ", self.pose_goal.position.y)
vLayout.addWidget(self.reglPoseY)
self.reglPoseZ = Reglage(-0.5, 1, "z : ", self.pose_goal.position.z)
vLayout.addWidget(self.reglPoseZ)
vLayout.addWidget(QLabel("Orientation"))
q = self.pose_goal.orientation
phi, theta, psi = euler_from_quaternion([q.w, q.x, q.y, q.z])
self.reglOrientPhi = Reglage(-3.14, 3.14, "phi : ", phi)
vLayout.addWidget(self.reglOrientPhi)
self.reglOrientTheta = Reglage(-3.14, 3.14, "theta : ", theta)
vLayout.addWidget(self.reglOrientTheta)
self.reglOrientPsi = Reglage(-3.14, 3.14, "psi : ", psi)
vLayout.addWidget(self.reglOrientPsi)
bouton = QPushButton('Send move cartesian', self)
bouton.clicked.connect(self.changerPoseCartesian)
vLayout.addWidget(bouton)
# Partie angulaire
titre = QLabel("Coordonnées angulaires")
titre.setFont(QtGui.QFont("Arial", 14, QtGui.QFont.Black))
titre.setAlignment(QtCore.Qt.AlignCenter)
vLayout.addWidget(titre)
self.lesReglAngulaires = self.creerWidgetsReglageAng(
initialPose, vLayout)
self.setLayout(vLayout)
#!/usr/bin/env python
from math import pi
import rospy
from ur_icam_description.robotUR import RobotUR
if __name__ == '__main__':
try:
# Start the ROS node
rospy.init_node('node_test_collision_moveit')
myRobot = RobotUR()
initialJointsState = [0, -pi / 2, pi / 2, -pi / 2, -pi / 2, 0]
myRobot.go_to_joint_state(initialJointsState)
# On ajoute les obstacles
myRobot.add_obstacle_box('left_cube', size=(1, 1, 1), position=(0.75, 0.5, 0))
myRobot.add_obstacle_box('table', size=(2, 2, 0.03), position=(0, 0, 0))
myRobot.add_obstacle_box('wall', size=(0.5, 0.5, 2), position=(0.6, -0.4, 0))
# myRobot.add_obstacle_table('table_cafe', size=(0.3, 0.01, 0.4), position=(0.6, 0, 0.4))
except rospy.ROSInterruptException:
print ("Program interrupted before completion")
#!/usr/bin/env python
# coding: utf-8
import copy
from math import pi
import rospy
from geometry_msgs.msg import Pose
from ur_icam_description.robotUR import RobotUR
if __name__ == '__main__':
myRobot = RobotUR()
rospy.init_node('robotUR')
print("============ Printing robot current pose")
print(myRobot.get_current_pose())
print("============ Printing robot state")
print(myRobot.robot.get_current_state())
print moveit_msgs.msg.MotionPlanRequest().max_velocity_scaling_factor
# Move over the middle cube
pose_goal.position.y = 0
myRobot.go_to_pose_goal(pose_goal)
# Move just above the middle cube(s)
pose_goal.position.z = z + h
myRobot.go_to_pose_goal(pose_goal)
# Open the gripper
myGripper.open()
rospy.sleep(1) # Pour laisser le temps au contact de se défaire
# Go up to initial pose
myRobot.go_to_joint_state(initialJointsState)
if __name__ == '__main__':
try:
# Start the ROS node
rospy.init_node('stack_cubes')
myGripper = Robotiq85Gripper()
myRobot = RobotUR()
myRobot.velocity_factor(0.1)
myRobot.acceleration_factor(0.1)
initialJointsState = [0, -pi / 2, pi / 2, -pi / 2, -pi / 2, 0]
myRobot.go_to_joint_state(initialJointsState)
myRobot.velocity_factor(0.5)
myRobot.acceleration_factor(0.5)
pick_and_place(0.6, -0.2, -0.1, 0.05, initialJointsState) # Left cube
myRobot.velocity_factor(1)
myRobot.acceleration_factor(1)
pick_and_place(0.6, 0.2, -0.1, 0.05 + 0.05, initialJointsState) # Right cube, h is 0.05 more
except rospy.ROSInterruptException:
print ("Program interrupted before completion")
class InteractRobotPyQt(QWidget):
def __init__(self, initialPose):
super(InteractRobotPyQt, self).__init__()
# loadUi('reglage.ui', self)
# Start the ROS node
rospy.init_node('interact_robot')
self.robot = RobotUR()
objectifAtteint = self.robot.go_to_joint_state(
initialPose) # On met le robot en position initiale
# objectifAtteint = self.robot.go_to_joint_state([0, 0, 0, 0, 0, 0])
self.pose_goal = self.robot.get_current_pose(
).pose # On récupère ses coord articulaires et cartésiennes
# Définition de l'IHM
vLayout = QVBoxLayout()
# Partie cartésienne
titre = QLabel("Coordonnées cartésiennes")
titre.setFont(QtGui.QFont("Arial", 14, QtGui.QFont.Black))
titre.setAlignment(QtCore.Qt.AlignCenter)
vLayout.addWidget(titre)
vLayout.addWidget(QLabel("Position"))
self.reglPoseX = Reglage(-1, 1, "x : ", self.pose_goal.position.x)
vLayout.addWidget(self.reglPoseX)
self.reglPoseY = Reglage(-1, 1, "y : ", self.pose_goal.position.y)
vLayout.addWidget(self.reglPoseY)
self.reglPoseZ = Reglage(-0.5, 1, "z : ", self.pose_goal.position.z)
vLayout.addWidget(self.reglPoseZ)
vLayout.addWidget(QLabel("Orientation"))
q = self.pose_goal.orientation
phi, theta, psi = euler_from_quaternion([q.w, q.x, q.y, q.z])
self.reglOrientPhi = Reglage(-3.14, 3.14, "phi : ", phi)
vLayout.addWidget(self.reglOrientPhi)
self.reglOrientTheta = Reglage(-3.14, 3.14, "theta : ", theta)
vLayout.addWidget(self.reglOrientTheta)
self.reglOrientPsi = Reglage(-3.14, 3.14, "psi : ", psi)
vLayout.addWidget(self.reglOrientPsi)
bouton = QPushButton('Send move cartesian', self)
bouton.clicked.connect(self.changerPoseCartesian)
vLayout.addWidget(bouton)
# Partie angulaire
titre = QLabel("Coordonnées angulaires")
titre.setFont(QtGui.QFont("Arial", 14, QtGui.QFont.Black))
titre.setAlignment(QtCore.Qt.AlignCenter)
vLayout.addWidget(titre)
self.lesReglAngulaires = self.creerWidgetsReglageAng(
initialPose, vLayout)
self.setLayout(vLayout)
def creerWidgetsReglageAng(self, initialPose, vLayout):
lesReglAngulaires = []
for i in range(6):
reglAng = Reglage(-3.14, 3.14, "q{} : ".format(i + 1),
initialPose[i])
reglAng.slider.valueChanged.connect(self.changerPoseAngular)
vLayout.addWidget(reglAng)
lesReglAngulaires.append(reglAng)
return lesReglAngulaires
def majPoseAngular(self):
for angle, reglage in zip(self.robot.get_current_joint(),
self.lesReglAngulaires):
reglage.majEditEtSlider(angle)
def majPoseCartesian(self):
p = self.robot.get_current_pose().pose
self.reglPoseX.majEditEtSlider(p.position.x)
self.reglPoseY.majEditEtSlider(p.position.y)
self.reglPoseZ.majEditEtSlider(p.position.z)
o = p.orientation
phi, theta, psi = euler_from_quaternion([o.w, o.x, o.y, o.z])
self.reglOrientPhi.majEditEtSlider(phi)
self.reglOrientTheta.majEditEtSlider(theta)
self.reglOrientPsi.majEditEtSlider(psi)
def changerPoseAngular(self):
angPose = []
for w in self.lesReglAngulaires:
angPose.append(w.value())
self.robot.go_to_joint_state(angPose)
self.majPoseCartesian()
def changerPoseCartesian(self):
self.pose_goal.position.x = self.reglPoseX.value()
self.pose_goal.position.y = self.reglPoseY.value()
self.pose_goal.position.z = self.reglPoseZ.value()
phi = self.reglOrientPhi.value()
theta = self.reglOrientTheta.value()
psi = self.reglOrientPsi.value()
orient = quaternion_from_euler(phi, theta, psi)
self.pose_goal.orientation.w = orient[0]
self.pose_goal.orientation.x = orient[1]
self.pose_goal.orientation.y = orient[2]
self.pose_goal.orientation.z = orient[3]
self.robot.go_to_pose_goal(self.pose_goal)
self.majPoseAngular()
#!/usr/bin/env python
# license removed for brevity
import rospy
from std_msgs.msg import Float32
from std_msgs.msg import Bool
from ur_icam_description.robotUR import RobotUR
import random
# Global variable to calibrate distance between the robot and the table
PICK_MOVEMENT_DISTANCE = 0.24
# Global variable for myRobot
MY_ROBOT = RobotUR()
def talker():
distance_pub = rospy.Publisher('/distance', Float32)
gripper_pub = rospy.Publisher('/object_gripped', Bool)
rospy.init_node('arduino', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
distance = MY_ROBOT.get_current_pose().pose.position.z - PICK_MOVEMENT_DISTANCE
rospy.loginfo("Measure distance: {}".format(distance))
distance_pub.publish(distance)
# object_gripped = random.random() > 0.4
# rospy.loginfo("Object_gripped: {}".format(object_gripped))
# gripper_pub.publish(object_gripped)
#!/usr/bin/env python
# coding: utf-8
import geometry_msgs.msg
from math import pi
from ur_icam_description.robotUR import RobotUR
import rospy
#
# Permet de positionner le robot à un endroit précis à l'aide de coord cartésiennes
#
if __name__ == '__main__':
robot = RobotUR()
rospy.init_node('robotUR')
objectifAtteint = robot.go_to_joint_state(
[0, -pi / 2, pi / 2, -pi / 2, -pi / 2, 0])
# On teste le positionnement par rapprot à des coordonnées cartésiennes
pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.x = 0.0
pose_goal.orientation.y = 0.707
pose_goal.orientation.z = 0.0
pose_goal.orientation.w = 0.707
pose_goal.position.x = 0.5
pose_goal.position.y = 0.1
pose_goal.position.z = 0.0
robot.go_to_pose_goal(pose_goal)