-
Notifications
You must be signed in to change notification settings - Fork 0
/
ur10_1_lm_robot_manipulator.py
executable file
·212 lines (170 loc) · 9.46 KB
/
ur10_1_lm_robot_manipulator.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
#!/usr/bin/env python
import sys
import copy
import rospy
from std_msgs.msg import Header
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
import geometry_msgs.msg
import moveit_msgs.msg
from std_msgs.msg import String
import moveit_commander
import tf.transformations as tf
from geometry_msgs.msg import Pose, Quaternion
#from kinematics_utils import *
from leap_motion.msg import leapcobotright
class CmdTrajectory():
def __init__(self):
self.robot_pose_updated = False
## LEAP MOTION CONFIG
self.leap_motion_right_hand_sub = rospy.Subscriber('/leapmotion/data_right', leapcobotright, self.send_leap_motion_trajectory, queue_size=1)
self.set_leap_motion_reference_position = False
self.leap_motion_reference_position = geometry_msgs.msg.Pose().position
self.robot_reference = geometry_msgs.msg.Pose()
self.start_leap_motion_control = False
self.PLANNING_GROUP_GRIPPER = "gripper"
self.PLANNING_GROUP_ARM = "manipulator"
self.PLANNING_NS = "/ur10_1/"
self.executing = False
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
self.robot = moveit_commander.RobotCommander("%srobot_description"%self.PLANNING_NS, ns="/ur10_1/")
self.arm = moveit_commander.move_group.MoveGroupCommander(self.PLANNING_GROUP_ARM,"%srobot_description"%self.PLANNING_NS, ns="/ur10_1/")
self.gripper = moveit_commander.move_group.MoveGroupCommander(self.PLANNING_GROUP_GRIPPER, "%srobot_description"%self.PLANNING_NS, ns="/ur10_1/")
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory, queue_size=10)
def send_leap_motion_trajectory(self, frame):
# Gestos:
# Punyo: Para de enviar nuevas instrucciones
# Ok: Comienza a enviar instrucciones
# Pinza: Cierra o abre el gripper
# Movimiento de munyeca: Rota el Gripper [para la simulacion no se activan]
# Gesto de ROCK: Configura el frame de referencia de Leap Motion
# Antes de comenzar, es bueno situar el robot en una posicion en donde trabajara
# Obtiene la posicion de la palma de la mano
palm_pos = frame.right_hand_palmpos
if frame.right_hand_fist:
self.start_leap_motion_control = False
if frame.right_hand_thumb_up:
self.start_leap_motion_control = True
if self.start_leap_motion_control:
# Configura la posicion de referencia en Leap Motion,
# cada vez que reconozca el gesto de ROCK&ROLL
if frame.right_hand_set_origin_frame_detected:
self.set_leap_motion_reference_position = True
self.leap_motion_reference_position.x = palm_pos.x
self.leap_motion_reference_position.y = palm_pos.y
self.leap_motion_reference_position.z = palm_pos.z
# Solamente si la referencia de lm esta configurada
if self.set_leap_motion_reference_position:
if frame.right_hand_pinch:
#self.send_gripper_cmd(frame.right_hand_pinch_value)
self.send_gripper_cmd(self.gripper, frame.right_hand_pinch_value)
else:
self.send_gripper_cmd(self.gripper, 0.0)
if not self.executing:
self.executing = True
desired_pos = self.get_transformed_position(palm_pos)
#print("desired_pos (xyz): "+ str(desired_pos.x) + ", " + str(desired_pos.y) + ", " + str(desired_pos.z))
pos_x = self.robot_reference.position.x + desired_pos.x * 0.001
pos_y = self.robot_reference.position.y + desired_pos.z * 0.001
pos_z = self.robot_reference.position.z + desired_pos.y * 0.001
print("desired_pos (xyz) * 0.001: "+ str(desired_pos.x*0.001) + ", " + str(desired_pos.y*0.001) + ", " + str(desired_pos.z*0.001))
print("robot_reference (xyz): "+ str(self.robot_reference.position.x) + ", " + str(self.robot_reference.position.y) + ", " + str(self.robot_reference.position.z))
current_pose = self.arm.get_current_pose().pose.position
#print("robot_current_pose (xyz): "+ str(current_pose.x) + ", " + str(current_pose.y) + ", " + str(current_pose.z))
#print pos_x
#print pos_y
#print pos_z
self.send_trajectory(self.arm, round(pos_x, 3), round(pos_y, 3), round(pos_z, 3))
self.executing = False
def get_transformed_position(self, palm_pos):
desired_pos = geometry_msgs.msg.Pose().position
pos_x = abs(self.leap_motion_reference_position.x - palm_pos.x)
pos_y = abs(self.leap_motion_reference_position.y - palm_pos.y)
pos_z = abs(self.leap_motion_reference_position.z - palm_pos.z)
if palm_pos.x > self.leap_motion_reference_position.x:
desired_pos.x = pos_x
if palm_pos.x < self.leap_motion_reference_position.x:
desired_pos.x = -pos_x
if palm_pos.x == self.leap_motion_reference_position.x:
desired_pos.x = 0
if palm_pos.y > self.leap_motion_reference_position.y:
desired_pos.y = pos_y
if palm_pos.y < self.leap_motion_reference_position.y:
desired_pos.y = -pos_y
if palm_pos.y == self.leap_motion_reference_position.y:
desired_pos.y = 0
if palm_pos.z > self.leap_motion_reference_position.z:
desired_pos.z = -pos_z
if palm_pos.z < self.leap_motion_reference_position.z:
desired_pos.z = pos_z
if palm_pos.z == self.leap_motion_reference_position.z:
desired_pos.z = 0
return desired_pos
def send_gripper_cmd(self, group, gripper_distance):
self.gripper.set_joint_value_target("robotiq_85_left_knuckle_joint", round(gripper_distance, 2))
self.gripper.go(wait = True)
print('\033[93m[' + str(gripper_distance) + ']\033[0m')
def set_robot_reference(self):
## Puede que se anyada las orientaciones... primero posiciones
self.robot_reference.position = self.arm.get_current_pose().pose.position
# Set init pose with articular values
def set_init_pose(self):
self.arm.set_named_target("home")
#self.arm.set_pose_target([0.239932090164, -0.631949506861, 0.62055460172, 0.0173452269234, -0.69900917270, 0.0169188757643, 0.714702085642], "ee_link")
self.arm.go(wait=True)
pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.x = 0.69907354139
pose_goal.orientation.y = 0.0170341855653
pose_goal.orientation.z = 0.714637584943
pose_goal.orientation.w = -0.0172957346397
pose_goal.position.x = 0.239932090164
pose_goal.position.y = -0.631949506861
pose_goal.position.z = 0.62055460172
self.arm.set_pose_target(pose_goal)
plan = self.arm.go(wait=True)
# Calling `stop()` ensures that there is no residual movement
self.arm.stop()
# It is always good to clear your targets after planning with poses.
# Note: there is no equivalent function for clear_joint_value_targets()
self.arm.clear_pose_targets()
# We can get the joint values from the group and adjust some of the values:
#joint_goal = self.arm.get_current_joint_values()
#joint_goal[0] = 2.176069313532091
#joint_goal[1] = -1.5180115396780627
#joint_goal[2] = -1.6710373677127652
#joint_goal[3] = -1.5109484721040696
#joint_goal[4] = 1.588992493059897
#joint_goal[5] = -1.0139681460138625
# 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
#self.arm.go(joint_goal, wait=True)
# Calling ``stop()`` ensures that there is no residual movement
#self.arm.stop()
#self.arm.go(wait=True)
#[2.176069313532091, -1.5180115396780627, -1.6710373677127652, -1.5109484721040696, 1.588992493059897, -1.0139681460138625]
#self.arm.go(wait = True)
self.gripper.set_named_target("gripper_open")
self.gripper.go(wait = True)
#print(self.gripper.get_current_joint_values())
#print(self.gripper.get_joints())
print(self.arm.get_current_joint_values())
print(self.arm.get_current_pose())
# def send_trajectory(self,group, pos_x, pos_y, pos_z, rot_x, rot_y, rot_z, rot_w):
def send_trajectory(self, group, pos_x, pos_y, pos_z):
group.set_pose_target([pos_x, pos_y, pos_z, 0.714702085642, 0.0169188757643, -0.69900917270, 0.0173452269234], "ee_link")
group.go(wait=True)
print('\033[93m[' + str(pos_x) + ', ' + str(pos_y) + ', ' + str(pos_z) + ']\033[0m')
if __name__ == '__main__':
## First initialize moveit_commander and rospy.
print("============ Starting dual arms moveit")
moveit_commander.roscpp_initializer.roscpp_initialize(sys.argv)
rospy.init_node('ur10_1_two_arm_moveit_leap_motion',
anonymous=True)
rospy.sleep(2)
cmd = CmdTrajectory()
cmd.set_init_pose()
cmd.set_robot_reference()
while not rospy.is_shutdown():
rospy.sleep(0.1)