Controller.py 11.4 KB
Newer Older
1
#!/usr/bin/env python
2
DEBUG = True
3
4
VERBOSE = True

Cadmus's avatar
Cadmus committed
5
## Python libraries
Lab Computer's avatar
Lab Computer committed
6
from copy import deepcopy
7
import time
8
import sys
9

Cadmus's avatar
Cadmus committed
10
11
## 3rd party libraries
# Scipy (1.2.3)
Lab Computer's avatar
Lab Computer committed
12
from scipy.spatial.transform import Rotation
13

Cadmus's avatar
Cadmus committed
14
# Numpy (1.16.6)
Lab Computer's avatar
Lab Computer committed
15
16
import numpy as np

Cadmus's avatar
Cadmus committed
17
# ROS Libraries
Jayant Khatkar's avatar
Jayant Khatkar committed
18
import rospy
19
from std_msgs.msg import Bool
Jayant Khatkar's avatar
Jayant Khatkar committed
20
21
22
23
from trajectory_msgs.msg import (
    JointTrajectoryPoint,
    JointTrajectory
)
24
from actionlib_msgs.msg import GoalStatusArray
25
26
27

# MOVEit
import moveit_commander
28
from moveit_msgs.msg._RobotTrajectory import RobotTrajectory
29

30
31
32
## Our libraries
from extrudex.msg._ExtruderControl import ExtruderControl
from extrudex.srv._Hotend import Hotend
33
from utils import load_plan, JTraj2ROS
34

Cadmus's avatar
Cadmus committed
35
36
37
38
class Controller(object):
    """
    Controller for multiple robot arms to execute lists of configurations with timestamps.
    The timing of the arms include extrusion of filament.
Lab Computer's avatar
Lab Computer committed
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55

    Frame names:
    - base_link
    - shoulder_link
    - upper_arm_link
    - forearm_link
    - wrist_1_link
    - wrist_2_link
    - wrist_3_link

    Joint names:
    - shoulder_pan_joint
    - shoulder_lift_joint
    - elbow_joint
    - wrist_1_joint
    - wrist_2_joint
    - wrist_3_joint
Cadmus's avatar
Cadmus committed
56
    """
57
    def __init__(self,
Cadmus's avatar
Cadmus committed
58
        close_deg=1.0,
59
60
61
62
63
        timeScale=1.0,
        blocking=True,
        extruderTemp_C=190.0,
        hotend_srv='hotend',
        retractionDist_mm=12.0,
64
        retractionSpeed_mmpmin=4800.0,
65
66
        disable_extruder=False,
        robot=0):
Jayant Khatkar's avatar
Jayant Khatkar committed
67
        """
Cadmus's avatar
Cadmus committed
68
69
70
        Creates a controller to multiple robot arms. (TODO)

        close_deg: Euclidean distance of configurations within which two configurations are considered "close"
71
        timeScale: Scaling factor of time
72
73

        robot can be 0 (if only one robot is turned on (not using 2xbringup), 1 for r1, 2 for r2
Jayant Khatkar's avatar
Jayant Khatkar committed
74
        """
75
76
77
78
79
80
81
82
83
        ## Initialise ROS things
        self.node = rospy.init_node('py_controller',
            anonymous = False)
        self.extrusion_pub = rospy.Publisher('extrusion_cmd',ExtruderControl,
            queue_size=2)
        self.extruderReady_sub = rospy.Subscriber('ready',Bool,self._extruderReady_cb,
            queue_size=1)
        self.scaledPosTraj_pub = rospy.Publisher('scaled_pos_traj_controller/command',JointTrajectory,
            queue_size=1)
84
85
        self.motion_feedback_sub = rospy.Subscriber('scaled_pos_traj_controller/follow_joint_trajectory/status',GoalStatusArray,
            self._update_motion_status)
86
87

        time.sleep(0.1)
88
89
        if not disable_extruder:
            print("Waiting for Hotend service")
90
            rospy.wait_for_service(hotend_srv)
91
92
93
94
            try:
                self.extruderSetTemp_srv = rospy.ServiceProxy(hotend_srv,Hotend)
            except rospy.ServiceException as e:
                print('Service call failed: {}'.format(e))
Jayant Khatkar's avatar
Jayant Khatkar committed
95

96
97
98
        # initialise MoveIt commander
        moveit_commander.roscpp_initialize(sys.argv)

99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
        if robot == 0:
            self.robot = moveit_commander.RobotCommander()
            self.scene = moveit_commander.PlanningSceneInterface()
            self.group = moveit_commander.MoveGroupCommander('manipulator')

        else:
            self.robot = moveit_commander.RobotCommander(
                    "/r" + str(robot) + "/robot_description",
                    ns="r" + str(robot)
                    )
            self.scene = moveit_commander.PlanningSceneInterface(ns="r"+ str(robot))
            self.group = moveit_commander.MoveGroupCommander(
                    'manipulator',
                    robot_description="r" + str(robot) + "/robot_description",
                    ns="r" + str(robot)
                    )
115

116
117
        self.group.set_max_velocity_scaling_factor(timeScale)
        self.group.set_max_acceleration_scaling_factor(timeScale**2)
118

119
120
121
122
123
        self.close_deg = close_deg
        self.timeScale = timeScale
        self.retractionDist_mm = retractionDist_mm
        self.retractionTime_s = retractionDist_mm/retractionSpeed_mmpmin * 60.0
        self.extruderReady = False
124
        self.traj_complete = False
125
126

        # Start extruder
127
        if not disable_extruder:
128
129
130
            self.set_extruder_temp(extruderTemp_C)
            if blocking:
                self.wait_extruder_ready()
131

132
133

    ## Arm methods
Cadmus's avatar
Cadmus committed
134
    def get_joint_names(self):
135
        """
Cadmus's avatar
Cadmus committed
136
        Returns the list of joint names in the order the controller uses
137
        """
Cadmus's avatar
Cadmus committed
138
        return self.group.get_active_joints()
139
140


Cadmus's avatar
Cadmus committed
141
    def empty_traj(self):
142
        """
Cadmus's avatar
Cadmus committed
143
        Creates an empty JointTrajectory with joint names of the arm prefilled.
144

Cadmus's avatar
Cadmus committed
145
146
147
148
149
        Returns JointTrajectory object.
        """
        t = JointTrajectory()
        t.joint_names = self.get_joint_names()
        return t
150
151


152
    def exec_ctraj(self,jtraj, wait=True, contour=None):
153
154
155
156
157
158
159
        """
        Using the
        jtraj: JTrajectory object to execute
        Returns True if the JointTrajectory is executed; False otherwise.
        """
        plan = RobotTrajectory()
        plan.joint_trajectory = JTraj2ROS(jtraj)
160
161
162
163
164
165
166
167
168
169

        # NOTE: This can only be done safely becuase assuming that the current
        # joint position is already very close to where we want it to be
        cj = self.group.get_current_joint_values()
        sj = plan.joint_trajectory.points[0].positions
        if any([0.01 < abs(cj[i]-sj[i]) for i in range(6)]):
            print("Potentially would've failed here, Retrying")
            #time.sleep(0.1)
            self.group.go(plan.joint_trajectory.points[0].positions, wait=True)
            return False
170

171
172
        if contour is not None:
            print("THIS NEXT TRAJ REQUIRES EXTRUSION")
173
            self.preprint_load()
174
175
            self.extrude(contour.Ext[-1], jtraj.time[-1])

176
        #rospy.loginfo('Sending trajectory to UR5e')
177
178
179
180
181
182
        self.traj_complete = False
        self.group.execute(plan, wait=False)

        if wait:
            s = time.time()
            t = jtraj.time[-1]
183
184
            while not self.traj_complete or time.time() - s < t - 1:
                if time.time() - s > t + 1:
185
186
187
188
                    Exception("traj_complete failed")
                    break
            self.traj_complete = False

189
        if contour is not None:
190
            self.postprint_retract()
191

192
193
194
        return True


195
    def exec_traj(self,traj):
Jayant Khatkar's avatar
Jayant Khatkar committed
196
        """
Cadmus's avatar
Cadmus committed
197
        Executes a JointTrajectory object
198

Cadmus's avatar
Cadmus committed
199
200
201
202
203
        NOTE: If the arm is not at the first JointTrajectoryPoint, then
              the trajectory is not executed.

        traj: JointTrajectory object to execute
        Returns True if the JointTrajectory is executed; False otherwise.
Jayant Khatkar's avatar
Jayant Khatkar committed
204
        """
205
        rospy.loginfo('Sending trajectory to UR5e')
206

207
        # Scale trajectory's temporal aspect
Lab Computer's avatar
Lab Computer committed
208
209
        scaledTraj = deepcopy(traj)
        for (iPoint,point) in enumerate(traj.points):
210
            # Scale timestamps
Lab Computer's avatar
Lab Computer committed
211
            t = point.time_from_start.secs + 1e-9*point.time_from_start.nsecs
212
            scaledT = t/self.timeScale
Lab Computer's avatar
Lab Computer committed
213
            secs = int(scaledT)
214
            nsecs = int((scaledT-secs)*1e9)
Lab Computer's avatar
Lab Computer committed
215
216
217
            scaledTraj.points[iPoint].time_from_start.secs = secs
            scaledTraj.points[iPoint].time_from_start.nsecs = nsecs

218
219
220
221
222
223
224
            # Scale velocity
            scaledTraj.points[iPoint].velocities = [v*self.timeScale for v in scaledTraj.points[iPoint].velocities]

            # Scale acceleration
            timeScale2 = self.timeScale**2
            scaledTraj.points[iPoint].accelerations = [a*timeScale2 for a in scaledTraj.points[iPoint].accelerations]

225
        plan = RobotTrajectory()
Lab Computer's avatar
Lab Computer committed
226
        plan.joint_trajectory = scaledTraj
227
        self.group.execute(plan, wait=True)
Jayant Khatkar's avatar
Jayant Khatkar committed
228

Cadmus's avatar
Cadmus committed
229
        return True
230
231


232
    def move_to(self,conf_next,near_deg = 1.0):
Cadmus's avatar
Cadmus committed
233
234
235
236
        """
        Moves the arm to the specified configuration by linearly
        interpolating the joint angles.

Lab Computer's avatar
Lab Computer committed
237
        conf_next: 6x numpy array of joint angles in radians
Cadmus's avatar
Cadmus committed
238
239
        near_deg: joint distance limit in degrees [default 1.0];
                  if None, no check is done.
240
        """
241
        if DEBUG:
Cadmus's avatar
Cadmus committed
242
            print('Controller::move_to()')
243

Cadmus's avatar
Cadmus committed
244
        if near_deg is not None:
245
            # List of joint angles
Lab Computer's avatar
Lab Computer committed
246
            conf_curr = np.array(self.get_joints())
247

248
            # Make sure the new joint angles are not too different
Lab Computer's avatar
Lab Computer committed
249
            dConf = conf_next-conf_curr
Cadmus's avatar
Cadmus committed
250
251
252
            dist_rad = np.linalg.norm(dConf)
            dist_deg = np.rad2deg(dist_rad)
            if dist_deg > near_deg:
Lab Computer's avatar
Lab Computer committed
253
                rospy.logwarn('Attempted to move a joint distance of {} > {} deg'.format(dist_conf_deg,jointLim_deg))
254
                return False
Jayant Khatkar's avatar
Jayant Khatkar committed
255

Lab Computer's avatar
Lab Computer committed
256
        self.group.go(conf_next)
257
        return True
Jayant Khatkar's avatar
Jayant Khatkar committed
258

259

260
    def is_close(self,trajPoint):
261
        """
Lab Computer's avatar
Lab Computer committed
262
        Check if the arm is currently close to the configuration specified.
Cadmus's avatar
Cadmus committed
263
        Ignores timestamp.
264

Cadmus's avatar
Cadmus committed
265
        trajPoint: JointTrajectoryPoint configuration
266
        """
Lab Computer's avatar
Lab Computer committed
267
268
269
270
271
272
273
274
275
        # TODO: Check if the joint order is the same
        conf_ref = np.array(trajPoint.positions)
        conf_curr = np.array(self.get_joints())

        dConf = conf_ref-conf_curr
        dist_rad = np.linalg.norm(dConf)
        dist_deg = np.rad2deg(dist_rad)

        return dist_deg <= self.close_deg
Lab Computer's avatar
Lab Computer committed
276
277


Cadmus's avatar
Cadmus committed
278
    def get_joints(self):
Jayant Khatkar's avatar
Jayant Khatkar committed
279
        """
Cadmus's avatar
Cadmus committed
280
        Return configuration of the arm in a list
Jayant Khatkar's avatar
Jayant Khatkar committed
281
        """
Cadmus's avatar
Cadmus committed
282
        return self.group.get_current_joint_values()
Jayant Khatkar's avatar
Jayant Khatkar committed
283
284


285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
    ## Extruder methods
    def extrude(self,ext_mm,t_s):
        """
        Send raw message to move extrusion.

        ext_mm: float
        t_s: float
        """
        if DEBUG:
            print('Controller::extrude({},{})'.format(ext_mm,t_s))
        msg = ExtruderControl()
        msg.delta_e = ext_mm
        msg.delta_t = t_s
        self.extrusion_pub.publish(msg)


    def postprint_retract(self):
        self.extrude(-self.retractionDist_mm,self.retractionTime_s)
303
        time.sleep(self.retractionTime_s)
304
305
306
307


    def preprint_load(self):
        self.extrude(self.retractionDist_mm,self.retractionTime_s)
308
        time.sleep(self.retractionTime_s)
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346


    def printFilament(self,ext_mm,t_s):
        """
        Does a sequence of print with retractions.

        ext_mm: Tx
        t_s: Tx
        """
        nStride = ext_mm.shape[0]
        self.preprint_load()

        for iStride in range(nStride):
            self.extrude(self,ext_mm,t_s)
            time.sleep(t_s)

        self.postprint_retract()


    def set_extruder_temp(self,temp_C):
        if VERBOSE:
            rospy.loginfo('Setting extruder temperature to {} C'.format(temp_C))
        self.extruderTemp_C = temp_C
        self.extruderSetTemp_srv(self.extruderTemp_C)


    def wait_extruder_ready(self):
        if VERBOSE:
            rospy.loginfo('Waiting for extruder to heat up...')
        while not self.extruderReady:
            time.sleep(0.1)
        if VERBOSE:
            rospy.loginfo('Done!')


    def _extruderReady_cb(self,ready_boolMsg):
        self.extruderReady = ready_boolMsg.data

347
348
349
350
    def _update_motion_status(self, msg):
        if len(msg.status_list)>0:
            self.traj_complete = (msg.status_list[-1].status==3 or
                msg.status_list[-1].status==2)
351
352
            if msg.status_list[-1].status >= 4 and msg.status_list[-1].status!=6:
                print("Unexpected Trajectory Status")
353
354
355
                print("STATUS: {}".format(msg.status_list[-1].status))
                print("For info on status meaning, go to:\n" + \
                    "http://docs.ros.org/en/melodic/api/actionlib_msgs/html/msg/GoalStatus.html")
356

357

358
def getR_ET():
359
    return Rotation.from_euler('XYZ',[-45,-90,0],
360
361
362
        degrees=True)


363
if __name__=='__main__':
Cadmus's avatar
Cadmus committed
364
    con = Controller()
365
366

# TODO: Add confirmation checks
Cadmus's avatar
Cadmus committed
367
368
369
# display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
#     moveit_msgs.msg.DisplayTrajectory,
#     queue_size=20)